构建平面双连杆机械臂动态模型
1平面双连杆机械臂的分析
图1平面双连杆机械臂
平面双连杆机械臂如图1，图中θ1为关节1转角，θ2为关节2转角，l1为杆1的长度，l2为杆2的长度，r1为关节1到杆1质心的距离，r2为关节2到杆2质心的距离M1为负载质量。以
图中的O为原点的x0y0为基坐标。
2数学建模
21寻找动力学
末端坐标
xpll1cos1l2cos12ypll1si
1l2si
12
根据雅克比矩阵的形式
dxdx
Jdy1
2dy

12
对末端坐标进行微分得到末端速度方程
xpll11si
1l212si
12ypll11cos1l212cos12
其中11，22，将（3）、（4）两式联立整理成速度雅克比矩阵形式
J

l1s1l2s12

l1c1
l2c12
l2s12
l2c12

其中s1si
1，c1cos1，s2si
2，c2cos2，s12si
12，c12cos12。
1
f在机器人基础坐标系中的速度与各关节速度间的关系以及手部与外界接触力与对应各关节间的关系可
以利用雅克比矩阵来建立。对机械臂末端速度方程（3）、方程（4）进行求导得到末端加速度方程如
下
xpll1s1l2s121l2s122l1c1l2c1212l2c12222l212c12ypll1s1l2s121l2s122l1s1l2s1212l2s12222l212s12
其中11，22，上述推导的方程构成了进行动力学仿真的基础它们表明了有效负荷的加速度与
两节点处电动机的角速度和角加速度之间的关系。机械臂质心位置的加速度和关节处的变量之间关系方程如下
AAcc11
xy
r1s11r1c11

r1c1
21
r1s
1
21
AAcc22
xy
l1s1r2s121r2s122l1c1r2c121r2s122
l1c1l2c1212l1s1l2s1212
l2c1222l2c1222
2l21c2122l212s12
22构建拉格朗日模型221选定广义关节变量及广义力
选取笛卡尔坐标系。θ1为关节1转角，θ2为关节2转角，关节1和关节2相应的力矩是1和2。连杆1和连杆2的质量分别为m1和m2，l1为杆1的长度，l2为杆2的长度，质心分别为k1和k2r1为
关节1到杆1质心的距离，r2为关节2到杆2质心的距离。
因此，杆1质心k1的位置坐标为X1r1s1Y1r1c1
杆1质心k1速度的平方为X12Y12r112
杆1质心k2的位置坐标为X2l1s1r2s12Y2l1c1r2c12
杆1质心k1速度的平方为X2l1c11r2c1212Y2l1s11r2s1212X22Y22l1212r221222l1r21212c2
222系统动能
EkEkii12
Ek1

12
m1r1212
Ek2

12
m2l1212

12
m2r221
22
m2l2r212
12c2
223系统势能
EpEpii12
2
fEp1m1gr11c1Ep2m2gl11c1m2gr21c12
224拉格朗日函数
LEkEp

12
m
r2
11

m
l2
21
12

m2l1r212
12c2

12
m2r221
22

m1r1m2l1g1c1m2gr21c12
225系统动力学方程
根据拉格朗日方程式计算r