二连杆动力学建模

仿真环境：matlab
内容涉及：拉格朗日方程，动力学建模。
实现效果：将关节视为轴承连接，无动能损耗，连杆摆动过程中，动能与势能相互转换，但是连杆的总能量恒定，如下图红线所示，黑线和蓝线分别代表动能和势能。

（一）欧拉-拉格朗日方程推导

根据牛顿第二定律：

定义：

那么牛顿第二定律可以表示为：

L称为拉格朗日算子，上式称为欧拉-拉格朗日方程。
将上述用广义坐标q=(q1,q2,q3,…qn)的形式表示,n为系统的自由度数目。

是与广义坐标qk相关的广义力。
（二）二连杆动能势能计算
2.1 速度传递（以转动关节为例）

其中vk表示k杆中任意位置pk处的速度,wk表示k杆的角速度。（从中我们可以发现，线速度表示的某点的属性，而角速度在同一杆件上处处相同，是杆件坐标系的属性。）

使用以上公式可以计算杆件上任意一点的角速度和线速度，首先应表达出杆件重心和杆件末端的坐标，坐标的表示可参照上期内容。
两个关节的绝对角速度表示如下：
aw1=[w1_x,w1_y,w1_z]’;
aw2=[w2_x,w2_y,w2_z]’;
以矢量的形式表示：
w0_1=aw1.x1;
w1_2=aw2.x2;
速度传递：
w1=w0_1;
w2=w1+(R0_1w1_2);
v_a=v0;
v_e=v_a+w1xr_ae;
v_b=v_a+w1xr_ab;
v_c=v_b+w2xr_bc;
v_f=v_b+w2xr_bf;
2.2动能计算
K=平动动能+转动动能
平动动能=0.5m1v_ev_e+0.5m2v_fv_f;
转动动能=0.5w1’I1_ww1+0.5w2’I2_ww2;
其中，I1_w，I2_w表示的是杆件的惯性张量。
I1_w=R0_1I1R0_1’;
I2_w=R0_2I2R0_2’;
I1，I2表示初始姿态（基态）的惯性张量，常将容易计算的位置当做初始状态。
2.3势能计算
P=m1gdot(p_e,z0)+m2g*dot(p_f,z0));
dot表示在世界坐标系z0方向（重力方向）上的投影。
（三）状态更新
问题描述：输入关节力矩为零，求解关节变量角度和角速度。
问题转换：即求解以下微分方程：

（四）仿真

关注公众号获取更多内容