双轴反作用轮定点自平衡杆动力学模型

最新推荐文章于 2025-04-22 14:06:39 发布

Quark Star

最新推荐文章于 2025-04-22 14:06:39 发布

阅读量4.7k

点赞数 13

本文链接：https://blog.csdn.net/qq_45518988/article/details/119325381

版权

前言：笔者曾经看到过一篇控制系统实践论文，论文描述的是一种自平衡杆系统的实现。该系统的实体如右图所示，借助两电机的驱动动量轮获得反作用力矩，并在地面摩擦力的共同作用下实现定点直立平衡。论文中控制系统使用的控制算法是线性系统中的状态反馈方法。

动力学建模上，论文作者对其中一个动量轮与摆杆构成的双刚体系统进行动力学分析，然后将这个过程独立地运用在另一个轮的分析上。然而这样处理的前提是当一个轮转动时，另一个轮静止。而实际系统仍然能够使杆直立平衡是因为摆杆的摆动幅度不大。因此不考虑双轮的耦合效应，近似线性化处理就能达到设计目的。在此基础上，笔者突发奇想：能不能将系统的平衡范围扩大？就是摆杆除了定点直立平衡，还能像陀螺一样定点进动（进动时并不需要杆与支架整体旋转，而是动量轮在旋转）。本人做了一些动力学上的推导，从推导结果发现控制系统的设计难度大大超出了我的预计。由于笔者不是物理或力学专业出身，推导过程并不严格，如有错误敬请指出。

摘要：本文首先采用静态欧拉角来描述双轴反作用轮定点自平衡杆系统的运动。然后根据拉格朗日力学对此刚体系统进行动力学建模。考虑到原始动力学方程过于冗长复杂，因此进行了适当的简化，将进动这一状态变量省略了。最后得到简化的动力学模型。其间表达式的代入、变形等使用了Mat lab程序辅助。

1. 系统的运动学描述

2.系统的动力学建模

3.代码解释

1. 系统的运动学描述

如图1所示，下端削尖直杆上部两反作用轮轮轴相互垂直且与杆垂直布置，分别固定于直杆上的直流电机驱动。整个系统可以看做是由三部分构成的刚体系统。摆杆(包括电机的外壳和定子部分)为刚体1，两反作用轮(包括电机的转子部分)为刚体2、3。竖直放置时刚体质心到水平支撑面间的距离分别为 $l_{1}$ , $l_{2}$ , $l_{3}$ 。刚体质量分别为 $m_{1}, m_{2}, m_{3}$ 。刚体对应三个轴的转动惯量（简化分析起见，本文假设刚体的本体系坐标轴与惯性主轴重合了）为 $I_{1x},I_{1y},I_{1z}$ ， $I_{2x},I_{2y},I_{2z}$ ， $I_{3x},I_{3y},I_{3z}$ 。

三个刚体在各自本体系下的惯性张量分别为 $J_{1}=diag(I_{1x},I_{1y},I_{1z})$ , $J_{2}=diag(I_{2x},I_{2y},I_{2z})$ , $J_{3}=diag(I_{3x},I_{3y},I_{3z})$ ，角速度矢量分别为 $\Omega_{1}=(\omega_{x'_{1}} ,\omega_{y'_{1}} ,\omega_{z'_{1}} )$ ， $\Omega _{2}=(\omega_{x'_{2}},\omega_{y'_{2}},\omega_{z'_{2}})$ ， $\Omega _{3}=(\omega_{x'_{3}},\omega_{y'_{3}},\omega_{z'_{3}})$ 。

采用欧拉角描述摆杆在空间坐标系中的姿态。如图2所示，静止坐标系 $o-xyz$ 称为空间坐标系，运动坐标系 $o-x'y'z'$ 与摆杆上的本体系重合。章动θ、自转ψ、进动φ。整个系统运动部件的自由度是5 （摆杆的欧拉角3个自由度、反作用轮各一个）。摆杆本体系上各轴角速度与欧拉角的关系为：

$\left\{\begin{matrix} \omega_{x_{1}'}= \dot{\varphi}\sin \theta\sin \psi+\dot{\theta}\cos\psi\\ \omega_{y_{1}'}= \dot{\varphi}\sin \theta\cos \psi-\dot{\theta}\sin\psi\\ \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \omega_{z_{1}'}= \dot{\varphi}\sin \theta+\dot{\psi}\\ \end{matrix}\right.$ $(1)$

反作用轮1转轴 $z_{2}'$ 与摆杆坐标轴 $y_{1}'$ 平行反向，相对电机定子或摆杆的转动角用α表示。相应角速度与摆杆角速度之间的关系为：

$\left\{\begin{matrix} \! \! \! \! \! \! \! \! \! \! \! \omega_{x_{2}'}=\omega_{x_{1}'}\\ \! \! \! \! \! \! \! \! \! \! \! \omega_{y_{2}'}=\omega_{z_{1}'}\\ \omega_{z_{2}'}=\dot{\alpha}-\omega_{y_{1}'}\\ \end{matrix}\right.$ $(2)$

反作用轮2转轴 $z_{3}'$ 与摆杆坐标轴 $x_{1}'$ 平行反向，相对电机定子或摆杆的转动角用β表示。相应角速度与摆杆角速度之间的关系为：

$\left\{\begin{matrix} \! \! \! \! \! \! \! \! \! \! \! \! \omega_{x_{3}'}=\omega_{y_{1}'}\\ \! \! \! \! \! \! \! \! \! \! \! \! \omega_{y_{3}'}=\omega_{z_{1}'}\\ \omega_{z_{3}'}=\dot{\beta}-\omega_{x_{1}'}\\ \end{matrix}\right.$ $(3)$

2.系统的动力学建模

采用拉格朗日力学建立动力学方程组。系统拉格朗日函数

$L=T-V$ $(4)$

其中T是系统的总动能，V是系统的总重力势能(注：以下角速度符号统一使用摆杆角速度代替，坐标轴所带角标均省略，在Matlab程序中转动惯量的值相同的统统只用其中一个代替)。

$\begin{align*} &T=\frac{1}{2}m_{1}v_{1}^{2} +\frac{1}{2}m_{2}v_{2}^{2} +\frac{1}{2}m_{3}v_{3}^{2}+\frac{1}{2} \Omega_{1}J_{1}\Omega_{1}^{T}+ \frac{1}{2} \Omega_{2}J_{2}\Omega_{2}^{T}+\frac{1}{2} \Omega_{3}J_{3}\Omega_{3}^{T}\\& \; \;\;\! =\frac{1}{2} m_{1}[(l_{1}\omega_{x})^{2}+(l_{1}\omega_{y})^{2}]+\frac{1}{2} m_{2}[(l_{2}\omega_{x})^{2}+(l_{2}\omega_{y})^{2}]+\frac{1}{2}m_{3}[(l_{3}\omega_{x})^{2}+(l_{3}\omega_{y})^{2}] \\ &\; \; \; +\frac{1}{2}[I_{1z}\omega_{z}^{2}+I_{1x}\omega_{x}^{2}+I_{1y}\omega_{y}^{2}] +\frac{1}{2}[I_{2z}\(\dot{\alpha}-\omega_{y})^{2}+I_{2x}\omega_{x}^{2}+I_{2y}\omega_{z}^{2}] \\ &\; \; \; +\frac{1}{2}[I_{3z}\(\dot{\beta}-\omega_{x})^{2}+I_{3x}\omega_{y}^{2}+I_{3y}\omega_{z}^{2}] \end{align*}$ $(5)$

$V=(m_{1}l_{1}+m_{2}l_{2}+m_{3}l_{3})g\cos\theta$ $(6)$

系统拉格朗日函数的广义坐标对应系统所有运动部件的自由度，因此 $L$ 函数有五个广义坐标θ、ψ、φ、α、β。列出拉格朗日方程和广义坐标及其对应的广义力

$\frac{\mathrm{d} }{\mathrm{d} t}\left (\frac{\partial L}{\partial \dot{q_{i}}} \right )-\frac{\partial L}{\partial q_{i}}=F_{i} \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \begin{bmatrix} q\\ F \end{bmatrix} = \begin{bmatrix} \theta &\psi &\varphi &\alpha &\beta \\ 0& 0& 0& \tau_{1}&\tau_{2} \end{bmatrix}$ $(7)$

其中 $\tau1$ 、 $\tau2$ 是两轮的驱动力矩。将 $L$ 函数代入拉格朗日方程(7)，就可以得到系统的动力学方程组。本文采用MATLAB程序来进行这个过程，详见第3节。

由于涉及变量比较多，导致方程组表达式过于冗长、繁杂。为了便于分析，必须对其进行简化。如何简化才能对模型精度影响最小，保证可控性，又能使表达式更简洁？进行以下简单的定性分析:章动角θ和自转角ψ都是输出变量，不能省略。两轮转动角α和β是快速变化量，也不能省去。只有进动角φ是慢变量，对系统的动力学模型影响最小，且表达式中只有φ的一、二阶导（即进动角速度和加速度），因此省略后完全不用考虑进动角的变化。最终得到的方程组

由于省略了一个广义坐标，因此方程组中有一个方程是多余的。

采用直流电机驱动动量轮。取直流电机的其中一相进行分析，动力学简化模型

$\left\{\begin{matrix} u=L_{m}\frac{\mathrm{d} i}{\mathrm{d} t}+R_{m}i+K_{e}\omega\\ \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \tau=K_{t}i\\ \end{matrix}\right.$ $(9)$

其中 $u$ 和i分别是电机的实时驱动电压和电流， $L_{m}$ 是电枢自感系数， $R_{m}$ 是电枢电阻， $K_{e}$ 是反电势系数， $\omega\$ 是电机的输出转速， $k_{t}$ 是电机的输出转矩系数。由于自感系数 $L_{m}$ 较小，对驱动电压的影响微弱，因此略去自感项。

再将简化的方程(9)代入方程(8)的第四和五方程（用=替换≈），得到

$\left\{\begin{matrix} \ddot{\alpha}=\frac{K_{t}}{I_{2z}R_{m}}(u_{1}-K_{e}\dot{\alpha})-\ddot{\theta}\sin\psi -\dot{\psi}\dot{\theta}\cos\psi \\ \ddot{\beta}=\frac{K_{t}}{I_{3z}R_{m}}(u_{2}-K_{e}\dot{\beta})+\ddot{\theta}\cos\psi -\dot{\psi}\dot{\theta}\sin\psi \\ \end{matrix}\right.$ $(10)$

将此方程代入方程组(8)的第一和三方程（用=替换≈），第二个方程被略去，变形得到

选择略去(8)的第二个方程的原因会在后续的分析中给出。

3.代码说明

MATLAB的符号运算功能可以方便地进行表达式的联立、整合、变形，取代部分繁杂的笔算，让简单重复的步骤自动进行，减少工作量，降低错误率。以下是代码

%摆杆（包括电机的外壳和定子部分）为刚体1，两反作用轮（包括电机的转子部分）为刚体2、3
syms l1 l2 l3 m1 m2 m3 g                               %刚体的质心到地面质点间的距离、刚体质 
                                                       %                   量、重力加速度。
syms I1x I1y I1z I2x I2y I2z I3x I3y I3z               %刚体三个轴的转动惯量（坐标轴与惯性主 
                                                       %                        轴重合）。
syms theta(t) psi(t) phi(t) Dtheta(t) Dpsi(t) Dphi(t)  %摆杆的章动角、自转角、进动角以及各自 
                                                       %速度（用欧拉角来描述摆杆在静止坐标系 
                                                       %                      中的姿态）。
syms alpha(t) beta(t) Dalpha(t) Dbeta(t)               %反作用轮一、二自转角及各自速度。
syms omega_x omega_y omega_z                       %摆杆各向角速度（旋转轴为固连坐标系轴）。
syms D1_L1(t) D1_L2(t) D1_L3(t) D1_L4(t) D1_L5(t) D2_L1(t) D2_L2(t) D2_L3(t) D2_L4(t) D2_L5(t) 

%摆杆各向角速度与欧拉角的关系
omega_x=Dphi*sin(theta)*sin(psi)+Dtheta*cos(psi);
omega_y=Dphi*sin(theta)*cos(psi)-Dtheta*sin(psi);
omega_z=Dphi*cos(theta)+Dpsi;

%注：I1x=I1y、I2x=I2y=I3x=I3y、I2z=I3z 
T=1/2*m1*((l1*omega_x)^2+(l1*omega_y)^2)+1/2*m2*((l2*omega_x)^2+(l2*omega_y)^2)...     %动能     
 +1/2*m3*((l3*omega_x)^2+(l3*omega_y)^2)+1/2*(I1z*omega_z^2+I1x*omega_x^2+I1x*omega_y^2)...
 +1/2*(I2z*(omega_y-Dalpha)^2+I2x*omega_x^2+I2x*omega_z^2)+1/2*(I2z*(omega_x-Dbeta)^2+I2x*omega_y^2+I2x*omega_z^2);
U=(m1*l1+m2*l2+m3*l3)*g*cos(theta);                                                    %势能
L=T-U;                                                                                 %拉格朗日函数

%拉格朗日函数对五个广义坐标的一阶偏导
D1_L1(t)=functionalDerivative(L,theta);
D1_L2(t)=functionalDerivative(L,psi);
D1_L3(t)=functionalDerivative(L,phi);
D1_L4(t)=functionalDerivative(L,alpha);
D1_L5(t)=functionalDerivative(L,beta);
%拉格朗日函数对五个广义速度的一阶偏导
D2_L1(t)=functionalDerivative(L,Dtheta);
D2_L2(t)=functionalDerivative(L,Dpsi);
D2_L3(t)=functionalDerivative(L,Dphi);
D2_L4(t)=functionalDerivative(L,Dalpha);
D2_L5(t)=functionalDerivative(L,Dbeta);

%以下是不考虑摩擦力的拉格朗日方程
equation1=diff(D2_L1(t),t)-D1_L1(t)   % equation1=0   对章动角,广义力为零
equation2=diff(D2_L2(t),t)-D1_L2(t)   % equation2=0   对自传角,广义力为零
equation3=diff(D2_L3(t),t)-D1_L3(t)   % equation3=0   对进动角,广义力为零
equation4=diff(D2_L4(t),t)-D1_L4(t)   % equation4=torque1    对轮1转动角，扭矩1
equation5=diff(D2_L5(t),t)-D1_L5(t)   % equation5=torque2    对轮2转动角，扭矩2

%注:以上程序文件可命名为inverte_pendulum.m

将以上代码运行后就可以看到结果是相当冗长的。观察后发现有一些项没有自动合并，有一些没有化简，然后手动进行了化简合并。最后省略含有进动角 $\varphi$ 各阶时间导数因子的项后得到式(8)

%根据inverte_pendulum文件的展开结果，以状态变量的时间二阶导为未知数，求解这个代数线性方程组。
%注：还未做任何变量省略
syms A B C D E F G H I J K L M N O P Q R S T D2theta D2psi D2phi D2alpha D2beta
% y1=A*D2theta+ 0*D2psi+ 0*D2phi+ B*D2alpha+ C*D2beta==D;          这数
% y2=0*D2theta+ E*D2psi+ F*D2phi+ 0*D2alpha+ 0*D2beta==G;          是线
% y3=0*D2theta+ H*D2psi+ I*D2phi+ K*D2alpha+ L*D2beta==M;          一性
% y4=J*D2theta+ 0*D2psi+ N*D2phi+ O*D2alpha+ 0*D2beta==P;          组方
% y5=Q*D2theta+ 0*D2psi+ R*D2phi+ 0*D2alpha+ S*D2beta==T;          代程
 y1=A*D2theta  + B*D2alpha+ C*D2beta==D;
 y2=   E*D2psi+ F*D2phi  ==G;
 y3= H*D2psi+ I*D2phi+ K*D2alpha+ L*D2beta==M;
 y4=J*D2theta+  N*D2phi+ O*D2alpha ==P;
 y5=Q*D2theta+   R*D2phi+ S*D2beta==T;
[D2theta D2psi D2phi D2alpha D2beta] = solve(y1,y2,y3,y4,y5,D2theta,D2psi,D2phi,D2alpha,D2beta)

%注:以上程序文件可命名为dynamics_equation.m

%利用文件dynamics_equation.m的计算结果得到的状态变量的时间二阶导的表达式，
%将众系数代入，最终得到状态变量二阶导的表达式。

syms l1 l2 l3 m1 m2 m3 g I1x I1y I1z I2x I2y I2z I3x I3y I3z
syms theta psi phi Dtheta Dpsi Dphi alpha beta Dalpha Dbeta 
syms tau1 tau2
A= I1x+I2x+I2z+l1^2*m1+l2^2*m2+l3^2*m3;                   %系数使用文件invert_pendulum.m
B= I2z*sin(psi);                                          %的计算结果（还未省略任何变量）并 
                                                          %进行了手动化简合并。
C=- I2z*cos(psi);
D=-((2*I2x+I1z)*sin(theta)*Dphi*Dpsi + I2z*cos(psi)*Dalpha*Dpsi...
 - (m1*l1+m2*l2+m3*l3)*g*sin(theta)+ I2z*cos(psi)*cos(theta)*Dalpha*Dphi+ I2z*cos(theta)*sin(psi)*Dbeta*Dphi... 
 + 1/2*(I2x+I1z-I1x-I2z-(l1^2*m1+l2^2*m2+l3^2*m3))*sin(2*theta)*Dphi^2 + I2z*sin(psi)*Dbeta*Dpsi);
E=2*I2x + I1z;
F=(2*I2x + I1z)*cos(theta);
G=(I2z*cos(psi)*Dalpha+I2z*sin(psi)*Dbeta)*Dtheta+(2*I2x+I1z)*sin(theta)*Dphi*Dtheta...
  -(I2z*cos(psi)*Dbeta- I2z*sin(psi)*Dalpha)*sin(theta)*Dphi;
H=(2*I2x+ I1z)*cos(theta); I= (2*I2x+ I1z)*cos(theta)^2+(I1x+I2x+I2z+l1^2*m1+l2^2*m2+l2^2*m2)*sin(theta)^2;
K=- I2z*cos(psi)*sin(theta); L=- I2z*sin(psi)*sin(theta);
M=(2*I2x+I1z)*sin(theta)*Dpsi*Dtheta + (2*I2x+ I1z)*sin(2*theta)*Dphi*Dtheta...  
+ I2z*(cos(psi)*Dalpha+sin(psi)*Dbeta)*Dtheta- I2z*(sin(psi)*Dalpha-cos(psi)*Dbeta)*Dpsi...
- (I1x+I2x+I2z)*sin(2*theta)*Dphi*Dtheta- (l1^2*m1+l2^2*m2+l3^2*m3)*sin(2*theta)*Dphi*Dtheta;
J=I2z*(sin(psi));  N=-I2z*cos(psi)*sin(theta); O = I2z;
P= I2z*(- cos(psi)*Dtheta*Dpsi+ cos(psi)*cos(theta)*Dphi*Dtheta...
    -sin(psi)*sin(theta)*Dphi*Dpsi)+tau1;
Q=-I2z*cos(psi); R= -I2z*sin(psi)*sin(theta); S=I2z;
T= I2z*(- sin(psi)*Dtheta*Dpsi+ cos(psi)*sin(theta)*Dphi*Dpsi... 
 + cos(theta)*sin(psi)*Dphi*Dtheta)+tau2;

D2theta =(B*G*H*N*S - B*F*H*P*S + C*G*H*O*R + B*E*I*P*S - C*F*H*O*T + D*F*H*O*S + C*E*I*O*T + B*E*L*N*T - B*E*L*P*R - B*E*M*N*S - C*E*K*N*T + C*E*K*P*R - D*E*I*O*S + D*E*K*N*S - C*E*M*O*R + D*E*L*O*R)/(B*E*I*J*S - B*F*H*J*S - B*E*J*L*R + C*E*J*K*R + A*F*H*O*S - C*F*H*O*Q - A*E*I*O*S + C*E*I*O*Q + A*E*K*N*S + B*E*L*N*Q - C*E*K*N*Q + A*E*L*O*R);
D2psi =(B*G*I*J*S - B*G*J*L*R + C*G*J*K*R + B*F*J*L*T - B*F*J*M*S - C*F*J*K*T + D*F*J*K*S - A*G*I*O*S + C*G*I*O*Q + A*G*K*N*S + B*G*L*N*Q - C*G*K*N*Q - A*F*K*P*S - B*F*L*P*Q + C*F*K*P*Q + A*G*L*O*R - A*F*L*O*T + A*F*M*O*S - C*F*M*O*Q + D*F*L*O*Q)/(B*E*I*J*S - B*F*H*J*S - B*E*J*L*R + C*E*J*K*R + A*F*H*O*S - C*F*H*O*Q - A*E*I*O*S + C*E*I*O*Q + A*E*K*N*S + B*E*L*N*Q - C*E*K*N*Q + A*E*L*O*R);
D2phi =-(B*G*H*J*S + B*E*J*L*T - B*E*J*M*S - C*E*J*K*T + D*E*J*K*S - A*G*H*O*S + C*G*H*O*Q - A*E*K*P*S - B*E*L*P*Q + C*E*K*P*Q - A*E*L*O*T + A*E*M*O*S - C*E*M*O*Q + D*E*L*O*Q)/(B*E*I*J*S - B*F*H*J*S - B*E*J*L*R + C*E*J*K*R + A*F*H*O*S - C*F*H*O*Q - A*E*I*O*S + C*E*I*O*Q + A*E*K*N*S + B*E*L*N*Q - C*E*K*N*Q + A*E*L*O*R);
D2alpha =-(C*G*H*J*R - C*F*H*J*T + D*F*H*J*S + C*E*I*J*T - D*E*I*J*S + A*G*H*N*S - C*G*H*N*Q - C*E*J*M*R + D*E*J*L*R - A*F*H*P*S + C*F*H*P*Q + A*E*I*P*S - C*E*I*P*Q + A*E*L*N*T - A*E*L*P*R - A*E*M*N*S + C*E*M*N*Q - D*E*L*N*Q)/(B*E*I*J*S - B*F*H*J*S - B*E*J*L*R + C*E*J*K*R + A*F*H*O*S - C*F*H*O*Q - A*E*I*O*S + C*E*I*O*Q + A*E*K*N*S + B*E*L*N*Q - C*E*K*N*Q + A*E*L*O*R);
D2beta =(B*G*H*J*R - B*F*H*J*T + B*E*I*J*T - B*G*H*N*Q - B*E*J*M*R + D*E*J*K*R + B*F*H*P*Q - A*G*H*O*R - B*E*I*P*Q + A*F*H*O*T - D*F*H*O*Q - A*E*I*O*T + A*E*K*N*T - A*E*K*P*R + B*E*M*N*Q + D*E*I*O*Q - D*E*K*N*Q + A*E*M*O*R)/(B*E*I*J*S - B*F*H*J*S - B*E*J*L*R + C*E*J*K*R + A*F*H*O*S - C*F*H*O*Q - A*E*I*O*S + C*E*I*O*Q + A*E*K*N*S + B*E*L*N*Q - C*E*K*N*Q + A*E*L*O*R);
Dphi=0;      %省略进动角速度
D2theta=subs(D2theta)
D2psi=subs(D2psi)
D2alpha=subs(D2alpha)
D2beta=subs(D2beta)

运行以上程序在手动化简合并得到式(11)