使用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函数化简结果

结果如下

第四步 列些动力学方程

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)])

符号说明

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];

各系数矩阵含义如下

首先令全部为零，解得D，然后把D减掉

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

令解得A第一列，令解得A第二列

令解得B第一列，令解得B第二列

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

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

最终结果为