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

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