使用matlab推导两连杆系统动力学方程

第一步物理建模

image.png

第二步设未知数

代码如下

clc
clear
close all

syms Theta_1(t) Theta_2(t) Alpha_1 Alpha_2 Omega_1(t) Omega_2(t)   
syms d_1 d_2 T_1 T_2 m_1 m_2 g
syms t 
syms A B C D T

使用 syms 函数定义符号变量来进行公式推导

第一行角度，角速度，角加速度设为时间的函数，这样在后面推导时就可以对时间求导了

第二行为一些常量，如连杆长度，质量，重力加速度等，还有作用在两连杆上的广义力

第三行为时间

第四行的变量为一些系数矩阵，最后的将结果写为矩阵形式时用到

第三步列写拉格朗日函数

代码如下

x_1 = d_1*sin(Theta_1);
y_1 = d_1*cos(Theta_1);
x_2 = x_1 + d_2*sin(Theta_1+Theta_2);
y_2 = y_1 + d_2*cos(Theta_1+Theta_2);

V_1 = sqrt(diff(x_1,t)^2+diff(y_1,t)^2);
V_2 = sqrt(diff(x_2,t)^2+diff(y_2,t)^2);
K_1 = 1/2*m_1*V_1^2;
K_2 = 1/2*m_2*V_2^2;
K = K_1+K_2;

P_1 = -g*m_1*y_1;
P_2 = -g*m_2*y_2;
P = P_1+P_2;

L = K-P;

L = subs(L,diff(Theta_1,t),Omega_1(t));
L = subs(L,diff(Theta_2,t),Omega_2(t));
L = simplify(L)

首先使用速度分解法求出合速度，在求出系统总的动能函数

在列写势能时，由于将零势能点定义在坐标原点，所以势能前面加一负号

进而求出Language函数，使用subs函数把角度对时间的一阶导数转化为角速度

最后，使用simplify函数化简结果

结果如下

$\frac{{d_{1}}^2\,(m_{1}+m_{2})\,{\omega _{1}}^2}{2} +\frac{{d_{2}}^2\,m_{2}\,({\omega _{2}}^2+{\omega _{1}}^2+2\omega _{1}*\omega _{2})}{2}$

$+d_{2}\,g\,m_{2}\,\cos\left(\theta _{1}+\theta _{2}\right) +d_{1}\,g\,(m_{1}+m_{2})\,\cos\theta _{1} +d_{1}\,d_{2}\,m_{2}\,\cos\theta _{2}\,({\omega _{1}}^2+\omega _{1}\,\omega _{2})$

第四步列些动力学方程

L_omega_1=functionalDerivative(L,Omega_1(t));
L_omega_1_t = diff(L_omega_1,t);
L_omega_1_t = subs(L_omega_1_t,diff(Theta_1,t),Omega_1(t));
L_omega_1_t = subs(L_omega_1_t,diff(Theta_2,t),Omega_2(t));
L_omega_1_t = subs(L_omega_1_t,diff(Omega_1,t),Alpha_1);
L_omega_1_t = subs(L_omega_1_t,diff(Omega_2,t),Alpha_2);
L_omega_1_t = simplify(L_omega_1_t,'steps',50);
L_omega_1_t = collect(L_omega_1_t,[Alpha_1 Alpha_2 Omega_1(t) Omega_2(t)])


L_omega_2=functionalDerivative(L,Omega_2(t));
L_omega_2_t = diff(L_omega_2,t);
L_omega_2_t = subs(L_omega_2_t,diff(Theta_1,t),Omega_1(t));
L_omega_2_t = subs(L_omega_2_t,diff(Theta_2,t),Omega_2(t));
L_omega_2_t = subs(L_omega_2_t,diff(Omega_1,t),Alpha_1);
L_omega_2_t = subs(L_omega_2_t,diff(Omega_2,t),Alpha_2);
L_omega_2_t = simplify(L_omega_2_t,'steps',50);
L_omega_2_t = collect(L_omega_2_t,[Alpha_1 Alpha_2 Omega_1(t) Omega_2(t)])

L_1= functionalDerivative(L,Theta_1);
L_2= functionalDerivative(L,Theta_2);


right_1 = simplify(L_omega_1_t - L_1);
right_1 = collect(right_1,[Alpha_1 Alpha_2 Omega_1(t) Omega_2(t)])

right_2 = simplify(L_omega_2_t - L_2);
right_2 = collect(right_2,[Alpha_1 Alpha_2 Omega_1(t) Omega_2(t)])

符号说明

$L\_omega\_1 = \frac{dL}{d\omega_1}$
$L\_omega\_2 = \frac{dL}{d\omega_2}$

$L\_omega\_1\_t = \frac{d}{dt}\frac{dL}{d\omega_1}$
$L\_omega\_2\_t = \frac{d}{dt}\frac{dL}{d\omega_2}$

$L\_1 = \frac{dL}{d\theta_1}$
$L\_2 = \frac{dL}{d\theta_2}$

$right\_1 就是连杆一方程的右侧$
$right\_2 就是连杆二方程的右侧$

Note.使用 functionalDerivative函数来实现Language函数对广义坐标的一阶导数求导，即对角速度求导，如果直接使用diff函数就会出现以下错误

错误使用 sym/diff (line 26)
Arguments, except for the first, must not be symbolic functions.

第五步改写为矩阵形式

vars =[Alpha_1 Alpha_2 Omega_1(t) Omega_2(t)];
value =[0 0 0 0];
D(1,1) = subs(right_1,vars,value);
D(2,2) = subs(right_2,vars,value);
right_1 = right_1-D(1);
right_2 = right_2-D(2);


syms n
n =eye(4);
value =n(1,:);
A(1,1) = subs(right_1,vars,value);
A(2,1) = subs(right_2,vars,value);
value =n(2,:);
A(1,2) = subs(right_1,vars,value);
A(2,2) = subs(right_2,vars,value);

value =n(3,:);
B(1,1) = subs(right_1,vars,value);
B(2,1) = subs(right_2,vars,value);
value =n(4,:);
B(1,2) = subs(right_1,vars,value);
B(2,2) = subs(right_2,vars,value);

syms c1 c2
temp =solve( A*[Alpha_1;Alpha_2]+B*[Omega_1(t)^2;Omega_2(t)^2]+[c1 0;0 c2]*[Omega_1(t)*Omega_2(t);Omega_1(t)*Omega_2(t)] - [right_1;right_2],[c1,c2]);
C =[temp.c1 0;0 temp.c2];

T =[T_1;T_2];

各系数矩阵含义如下

$T= A \begin{bmatrix}\alpha_1 \\ \alpha_2\end{bmatrix} + B \begin{bmatrix}\omega_1^2 \\ \omega_2^2\end{bmatrix}+ C \begin{bmatrix}\omega_1\omega_2 \\ \omega_2\omega_1\end{bmatrix}+ D$

首先令 $\alpha_1\,,\alpha_2\,,\omega_1\,,\omega_2$ 全部为零，解得D，然后把D减掉

定义一个 $4*4$ 的单位阵，让以上四个变量只有一个是1，其余三个为零

令 $\alpha_1 = 1$ 解得A第一列，令 $\alpha_2 = 1$ 解得A第二列

令 $\omega_1 = 1$ 解得B第一列，令 $\omega_2 = 1$ 解得B第二列

观察C为一个对角阵，设两个未知参数 $c_1,c_2$ ,使用solve函数把C矩阵解出

最后方程另一边T就为广义力的向量

最终结果为
$\begin{bmatrix}T_1 \\ T_2\end{bmatrix} = \begin{bmatrix} {d_{1}}^2\,(m_{1}+m_{2})+{d_{2}}^2\,m_{2}+2\,d_{1}\,d_{2}\,m_{2}\,\cos\theta_{2} & {d_{2}}^2m_2+d_{1}d_2m_2\,\cos\theta_{2} \\{d_{2}}^2m_2+d_{1}d_2m_2\,\cos\theta_{2}&{d_{2}}^2\,m_{2}\end{bmatrix} \begin{bmatrix}\alpha_1 \\\alpha_1\end{bmatrix} \\+ \begin{bmatrix}0& -d_{1}\,d_{2}\,m_{2}\,\sin\theta_{2} \\d_{1}\,d_{2}\,m_{2}\,\sin\theta_{2}& 0\end{bmatrix} \begin{bmatrix}\omega_1^2 \\ \omega_2^2\end{bmatrix} +\begin{bmatrix} -2\,d_{1}\,d_{2}\,m_{2}\,\sin\theta_{2} & 0 \\ 0& 0 \end{bmatrix} \begin{bmatrix}\omega_1\omega_2 \\ \omega_2\omega_1\end{bmatrix} \\ +\begin{bmatrix}d_{1}\,g\,({m_{1}+m_{2}})\,\sin\theta_{1}+d_{2}\,g\,m_{2}\,\sin\left(\theta_{1}+\theta_{2}\right) \\ d_{2}\,g\,m_{2}\,\sin\left(\theta_{1}+\theta_{2}\right)\end{bmatrix}$