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

一、平面二连杆机器人手臂运动学 平面二连杆机械手臂如图1所示，连杆1长度，连杆2长度，连杆3长度为。建立如图1所示的坐标系，其中，为基础坐标系，固定在基座上，、、为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。关节角顺时针为负逆时针为正。 图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程 连杆2末段与中线交点处一点P在基础坐标系中的位置坐标： （1） 2、用D-H方法建立运动学方程 假定、、垂直于纸面向外。从到的齐次旋转变换矩阵为： （2） 从到的齐次旋转变换矩阵为： （3） 从到的齐次旋转变换矩阵为： （3） 从到的齐次旋转变换矩阵为： （4） 那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为： （5） 即， （6） 结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。 补充：正解用于仿真，逆解用于控制 建立以上运动学方程后，若已知个连杆的关节角，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。 3、平面二连杆机器人手臂逆运动学 二、平面二连杆机器人手臂的速度雅可比矩阵 速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。 上面的运动学方程两边对时间求导，得到下面的速度表达式： （17） 把上式写成如下的矩阵形式： （18） 令上式中的末端位置速度矢量， 关节角速度矢量， 矩阵 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可以写成： 速度雅可比矩阵可以进一步写成： （19） 其中， （20） 由此可知雅可比矩阵的定义： （21） 三、平面二连杆机器人手臂的动力学方程 推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是和。 1、求两连杆的拉格朗日函数 （1）求系统总动能 连杆1的动能为： （21） 求连杆2质心D处的线速度：对连杆2质心位置求导得到其线速度。连杆2质心位置为： （22） 连杆2质心速度为： （23） （24） 连杆2的动能： (25) 系统总动能： （26） （2）求系统总势能 系统总势能为： （27） （3）求拉格朗日函数 （28） （4）列写动力学方程 按照拉格朗日方程，对应关节1、2的驱动力矩分别为： （29） （30） 同理： （31） 联合（30）、（31）式，将动力学方程写成如下矩阵形式：