用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程

1一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度1l，连杆2长度2l。建立如图1所示的坐标系，其中，),(00yx为基础坐标系，固定在基座上，),(11yx、),(22yx为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标：)sin(sin)cos(cos2121121211llyllxpp（1）2、用D-H方法建立运动学方程假定0z、1z、2z垂直于纸面向里。从),,(000zyx到),,(111zyx的齐次旋转变换矩阵为：1000010000cossin00sincos111101T（2）从),,(111zyx到),,(222zyx的齐次旋转变换矩阵为：1000010000cossin0sincos2212212lT（3）从),,(000zyx到),,(222zyx的齐次旋转变换矩阵为：120x0y1x1y2x2y12PABCD210000100sin0)cos()sin(cos0)sin()cos(1000010000cossin0sincos1000010000cossin00sincos112121112121221221111120102lllTTT（4）那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为：110)sin(sin)cos(cos10010000100sin0)cos()sin(cos0)sin()cos(212112121121121211121212020pppzyxlllllllPTP（5）即，)sin(sin)cos(cos2121121211llyllxpp（6）与用简单的平面几何关系建立运动学方程（1）相同。建立以上运动学方程后，若已知个连杆的关节角21、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂二连杆的关节角21、，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置),(ppyx求相应关节角21、的过程。推倒如下。（1）问题)sin(sin)cos(cos2121121211llyllxpp已知末端位置坐标),(ppyx，求关节角21、。（2）求13由（6）式得到：22211211)sin()cos(llylxpp（7）整理得到：)sincos(2111222122ppppyxlllyx（8）令ppppptgyxcossin（9）由（8）式得到：)sinsincos(coscos2111222122ppppppxlllyx)cos(cos211222122pppppxlllyx（10）由此可解出1。ppppppxyarctgxlllyxcos2arccos12221221（11）（3）求2由（6）式得到：2122122212)]sin([)]cos([llylxpp（12）整理得到：)]sin()cos([221212212222ppppyxlllyx（13）令ppppptgyxcossin（14）由（14）式得到：)cos(cos2]sin)sin(cos)[cos(cos221221212212222pppppppppxlxlllyx（15）由此可解出2。4122122222cos2arccosppppppxyarctgxlllyx（16）二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。)sin(sin)cos(cos2121121211llyllxpp上面的运动学方程两边对时间求导，得到下面的速度表达式：)()cos(cos)()sin(sin2121211121212111lldtdylldtdxpp（17）把上式写成如下的矩阵形式：212122121121221211)cos()cos(cos)sin()sin(sinllllllyxpp（18）令上式中的末端位置速度矢量Xyxpp，关节角速度矢量21，矩阵),()cos()cos(cos)sin()sin(sin212122121121221211Jllllll),(21J就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可以写成：),(21JX速度雅可比矩阵可以进一步写成：22211211212212112122121121)cos()cos(cos)sin()sin(sin),(JJJllllllJJ（19）其中，5)cos()cos(cos)sin()sin(sin2122222121112121221221211111lyJllyJlxJllxJpppp（20）由此可知雅可比矩阵的定义：21212221121121),(ppppJyyxxJJJJ（21）三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是1I和2I。1、求两连杆的拉格朗日函数（1）求系统总动能连杆1的动能为：212112121121161)31(2121lmlmIKA（21）求连杆2质心D处的线速度：对连杆2质心位置求导得到其线速度。连杆2质心位置为：)sin(21sin)cos(21cos2121121211llyllxDD（22）连杆2质心速度为：)()cos(21cos)()sin(21sin2121211121212111llYllxDD（23）21221222222212212221222)cos21(41)cos41(llllllllyxVDDD6（24）连杆2的动能：21221222222222122122212212212222222122122212221222222212)cos32(2161)cos31(21])cos21(41)cos41[(21))(121(2121)(21lllmlmllllmllllllllmlmVmIKDD(25)系统总动能：212212222222222122122222112122122122222222212212221221)cos2131(61)cos21616121()cos32(2161)cos31(21llmlmlmllmlmlmlmlllmlmllllmKKK（26）（2）求系统总势能系统总势能为：))sin(21sin(sin21212112111llgmglmP（27）（3）求拉格朗日函数)]sin(21sin[sin21)cos2131(61)cos21616121(21211211121221222222222212212222211212llgmglmllmlmlmllmlmlmlmPKL（28）（4）列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为：222111LLtLLt（29）22212222122122222112121)cos2131()cos3131(llmlmllmlmlmlmL722221221221222212222122122222112121sin21sin)cos2131()cos3131(llmllmllmlmllmlmlmlmLt)cos(21cos)21(212211211glmglmmL)cos(21cos)21(sin21sin)cos2131()cos3131(2122112122221221221222212222122122222112121glmglmmllmllmllmlmllmlmlmlm（30）同理：1221222222222)cos2131(31llmlmlmL2122122222122122222sin2131)cos2131(llmlmllmlmLt)cos(21sin21sin2121222122122122122glmllmllmL)cos(21sin2131)cos2131(21222122122222122122222glmllmlmllmlm（31）联合（30）、（31）式，将动力学方程写成如下矩阵形式：)cos(21)cos(21cos)21(000sin0sin21sin21031cos2131cos2131cos3131212221221121212122122221221222122122222122222212222221222221121221glmglmglmmllmllmllmlmllmlmllmlmllmlmlmlm（32）四、平面二连杆机器人手臂的轨迹规划8轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。如图所示，一两自由度机械手，已知两连杆起点和终点的关节角，确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨