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

用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程 一、平面二连杆机器人手臂运动学 平面二连杆机械手臂如图1所示，连杆1长度l1，连杆2长度l2。建立如图1所示的坐标系，其中，(x0,y0)为基础坐标系，固定在基座上，(x1,y1)、(x2,y2)为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。 图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程 连杆2末段与中线交点处一点P在基础坐标系中的位置坐标： xp?l1cos?1?l2cos(?1??2) yp?l1sin?1?l2sin(?1??2) 2、用D-H方法建立运动学方程 （1） 假定z0、z1、z2垂直于纸面向里。从(x0,y0,z0)到(x1,y1,z1)的齐次旋转变换矩阵为： ?cos?1?sin?10?1T??0??0?sin?1cos0?? （2） 0??1? 从(x1,y1,z1)到(x2,y2,z2)的齐次旋转变换矩阵为： ?cos?2?sin?21?2T??0??0?sin?2cos?2000010l1?0?? （3） 0??1? 从(x0,y0,z0)到(x2,y2,z2)的齐次旋转变换矩阵为： 1 ?cos?1?sin?100??cos?2?sin?2?sin???sin?cos?00cos?2112001???T?T?T??212?0010??00???001??00?0 ?cos(?1??2)?sin(?1??2)0l1cos?1??sin(???)cos(???)0lsin?? 121211????0010???0001??0l1?00??10??01? （4） 那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为： ?cos(?1??2)?sin(?1??2)0l1cos?1??l2??sin(???)cos(???)0lsin???0? 121211???002P?2T?P????0010??0?????0001???1? （5） ?l1cos?1?l2cos(?1??2)??xp??lsin??lsin(???)??y? 1212???1??p????zp?0????1???1? 即， xp?l1cos?1?l2cos(?1??2) yp?l1sin?1?l2sin(?1??2) （6） 与用简单的平面几何关系建立运动学方程（1）相同。 建立以上运动学方程后，若已知个连杆的关节角?1、?2，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。 3、平面二连杆机器人手臂逆运动学 建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂二连杆的关节角?1、?2，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置(xp,yp)求相应关节????1、?2的过程。推倒如下。 （1）问题 xp?l1cos?1?l2cos(?1??2) yp?l1sin?1?l2sin(?1??2) 已知末端位置坐标(xp,yp)，求关节角?1、?2。 （2）求?1 2 由（6）式得到： 2 （7） (xp?l1cos?1)2?(yp?l1sin?1)2?l2 整理得到： 222 x2p?yp?l1?l2?2l1(xpcos?1?ypsin?1) （8） 令 xp?tg?p? sin?p （9） yp cos?p 由（8）式得到： x2?y2?l2?l22l1xppp12 ? cos?(cos?1cos?p?sin?1sin?p) px2222 2l1xpp?yp?l1?l2?cos?cos(?1??p) p 由此可解