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

1 一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1 所示， 连杆 1 长度1l ，连杆 2 长度2l。建立如图 1 所示的坐标系，其中，),(00 yx为基础坐标系，固定在基座上，),(11 yx、),(22 yx为连体坐标系，分别固结在连杆1 和连杆 2 上并随它们一起运动。关节角顺时针为负逆时针为正。图 1 平面双连杆机器人示意图1、用简单的平面几何关系建立运动学方程连杆 2 末段与中线交点处一点P 在基础坐标系中的位置坐标：)sin(sin)cos(cos2121121211llyllxpp（1）2、用 D-H 方法建立运动学方程假定0z 、1z 、2z 垂直于纸面向里。 从),,(000zyx到),,(111zyx的齐次旋转变换矩阵为：1000010000cossin00sincos111101T（2）从),,(111zyx到),,(222zyx的齐次旋转变换矩阵为：1000010000cossin0sincos2212212lT（3）从),,(000zyx到),,(222zyx的齐次旋转变换矩阵为：1 2 0x0y1x1y2x2y12P A B C D 2 10000100sin0)cos()sin(cos0)sin()cos(1000010000cossin0sincos1000010000cossin00sincos112121112121221221111120102lllTTT（4）那么，连杆2 末段与中线交点处一点P 在基础坐标系中的位置矢量为：110)sin(sin)cos(cos10010000100sin0)cos()sin(cos0)sin()cos(212112121121121211121212020pppzyxlllllllPTP（5）即，)sin(sin)cos(cos2121121211llyllxpp（6）与用简单的平面几何关系建立运动学方程（1）相同。建立以上运动学方程后，若已知个连杆的关节角21、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂二连杆的关节角21、，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置),(pp yx求相应关节角21、的过程。推倒如下。（1）问题)sin(sin)cos(cos2121121211llyllxpp已知末端位置坐标),(pp yx，求关节角21、。（2）求13 由（ 6）式得到：22211211)sin()cos(llylxpp（7）整理得到：)sincos(2111222122ppppyxlllyx（8）令ppppptgyxcossin（9）由（ 8）式得到：)sinsincos(coscos2111222122ppppppxlllyx)cos(cos211222122pppppxlllyx（10）由此可解出1 。ppppppxyarctgxlllyxcos2arccos12221221（11）（3）求2由（ 6）式得到：2122122212)]sin([)]cos([llylxpp（12）整理得到：)]sin()c...