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

...一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度1l，连杆2长度2l，连杆3长度为3l。建立如图1所示的坐标系，其中，),(00yx为基础坐标系，固定在基座上，),(11yx、),(22yx、33(,)xy为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。关节角顺时针为负逆时针为正。1图1平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P在基础坐标系中的位置坐标：112123123112123123coscos()+cos()sinsin()+sin()ppxlllylll（1）2、用D-H方法建立运动学方程假定0z、1z、2z垂直于纸面向外。从),,(000zyx到),,(111zyx的齐次旋转变换矩阵为：120x0y1x1y2x2y12PABCD33y3x...1000010000cossin00sincos111101T（2）从),,(111zyx到),,(222zyx的齐次旋转变换矩阵为：1000010000cossin0sincos2212212lT（3）从222(,,)xyz到333(,,)xyz的齐次旋转变换矩阵为：3323312cossin0sincos0000100001lT（3）从),,(000zyx到333(,,)xyz的齐次旋转变换矩阵为：11221332112233001231231231231121cossin00cossin0cossin0sincos00sincos00sincos00001000100010000100010001cos()sin()0coscos(llTTTTll212312311212)sin()cos()0sinsin()00100001ll（4）那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为：12312311212312312311212003311212312311212cos()sin()0coscos()sin()cos()0sinsin()00010000011coscos()cos()sinsin()lllllPTPlllll3123sin()011pppxlyz（5）即，112123123112123123coscos()+cos()sinsin()+sin()ppxlllylll（6）结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。...补充：正解用于仿真，逆解用于控制建立以上运动学方程后，若已知个连杆的关节角123、、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。112123123112123123coscos()+cos()sinsin()+sin()ppxlllylll上面的运动学方程两边对时间求导，得到下面的速度表达式：111212123123123111212122123123sinsin()()sin()()coscos()()cos()()ppdxllldtdyllldt（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）其中，...)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（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...22221221221222212222122122222112121sin21sin)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）四、平面二连杆机器人手臂的轨迹规划...轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。如图所示，一两自由度机械手，已知两连杆起点和终点的关节角，确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹规划的缺点。三次多项式轨迹规划举例：要求一个５轴机器人的第一关节在５秒之内从初始角３０度运动到终端角７５度，用三次多项式计算在第１、２、３、４秒时的关节角。...五次多项式轨迹规划...抛物线过渡的线性运动轨迹规划（略）具有中间点以及用抛物线过渡的线性运动轨迹规划（略）高次多项式运动轨迹规划（略）直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。这个过程可以归纳为以下计算循环：（a）将时间增加一个增量；（b）利用所选择的轨迹函数计算出手的位姿；（c）利用逆运动学方程计算相应的关节变量；（d）将关节变量信息送给控制器；（e）返回到循环的开始。.........五