平面双连杆机械臂动态模型

1 构建平面双连杆机械臂动态模型 1. 平面双连杆机械臂的分析 图1 平面双连杆机械臂 平面双连杆机械臂如图1，图中θ1 为关节 1 转角，θ2 为关节 2 转角，l1 为杆1 的长度，l2为杆2 的长度，r1 为关节 1 到杆1 质心的距离，r2 为关节 2 到杆2 质心的距离,M1 为负载质量。以图中的O 为原点的00yx为基坐标。 2. 数学建模 2.1 寻找动力学 末端坐标 )cos(cos21211llxpl )sin(sin21211llypl 根据雅克比矩阵的形式 2121dydydxdxJ 对末端坐标进行微分得到末端速度方程 )sin()(sin21212111llxpl )cos()(cos21212111llypl 其中11，22，将（3）、（4）两式联立整理成速度雅克比矩阵形式 1221221112212211clclclslslslJ 其中1s =1sin  ，1c =1cos ，22sin s，22cos c，12s=)sin(21 ，12c =)cos(21 。2 在机器人基础坐标系中的速度与各关节速度间的关系以及手部与外界接触力与对应各关节间的关系可以利用雅克比矩阵来建立。对机械臂末端速度方程（3） 、方程（4） 进行求导得到末端加速度方程如下 1221222122211221121221122112)()(clclclclslslslxpl 1221222122211221121221122112)()(slslslslslslslypl 其中1 =1 ，2 =2 ，上述推导的方程构成了进行动力学仿真的基础,它们表明了有效负荷的加速度与 两节点处电动机的角速度和角加速度之间的关系。机械臂质心位置的加速度和关节处的变量之间关系方程如下 2111111,2111111,11srcrAcrsrAycxc 122122212221122112122112211,122122212221122112122112211,2)()(2)()(22slclslslsrcrclAclclclclsrsrslAycxc 2.2构建拉格朗日模型 2.2.1选定广义关节变量及广义力 选取笛卡尔坐标系。θ1 为关节1 转角，θ2 为关节2 转角，关节1和关节2相应的力矩是1 和2 。连杆1和连杆2的质量分别为1m 和2m ，l1 为杆1 的长度，l2为杆2 的长度，质心分别为1k 和2k r1 为关节1 到杆1 质心的距离，r2 为关节2 到杆2 质心的距离。 因此，杆1质心1k 的位置坐标为 111...