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

.

一、平面二连杆机器人手臂运动学

平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l ，连杆3长度为3l 。建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 、

33(,)x y 为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。关节角顺

时针为负逆时针为正。

1

θ

图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程

连杆2末段与中线交点处一点P 在基础坐标系中的位置坐标：

112123123112123123cos cos()+cos()sin sin()+sin()

p p x l l l y l l l θθθθθθθθθθθθ=++++=++++

（1）

2、用D-H 方法建立运动学方程

假定0z 、1z 、2z 垂直于纸面向外。从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为：

.

⎥⎥

⎥⎥⎦

⎤⎢⎢⎢⎢⎣⎡-=100

010000cos sin 00sin cos 1

11101θθ

θθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：

⎥⎥

⎥⎥⎦

⎤⎢⎢⎢⎢⎣⎡-=100

010000cos sin 0sin cos 2

212212θθ

θθl T （3） 从222(,,)x y z 到333(,,)x y z 的齐次旋转变换矩阵为：

33233

12cos sin 0sin cos 0000100

01l T θθθ

θ-⎡⎤⎢⎥⎢⎥=⎢⎥

⎢⎥⎣⎦

（3）

从),,(000z y x 到333(,,)x y z 的齐次旋转变换矩阵为：

1122

133

21122

33

001231

231231231121cos sin 00cos sin 0cos sin 0sin cos 00sin cos 00sin cos 000010001000

100

00100010001cos()sin()0cos cos(l l T T T T l l θθθθθθθθθθθθθθθθθθθθ---⎡⎤⎡⎤⎡⎤

⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥

=⋅⋅=⋅⋅⎢⎥⎢⎥⎢⎥⎢⎥⎢

⎥⎢

⎥⎣⎦⎣⎦⎣⎦

++-+++=2123123

11212)sin()cos()

0sin sin()0010

000

1l l θθθθθθθθθθ+⎡⎤⎢⎥++++++⎢⎥

⎢⎥⎢

⎥

⎣

⎦

（4）

那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为：

12312311212312312311212003

311212312311212cos()sin()0cos cos()sin()cos()0sin sin()00010000011cos cos()cos()sin sin()l l l l l P T P l l l l l θθθθθθθθθθθθθθθθθθθθθθθθθθθ++-++++⎡⎤⎡⎤⎢⎥⎢⎥

++++++⎢⎥⎢⎥=⋅=⋅

⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦

+++++++=3123sin()011p p p x l y z θθθ⎡⎤⎡⎤⎢⎥⎢⎥

+++⎢⎥⎢⎥

=⎢⎥⎢⎥⎢⎥⎢⎥

⎣⎦⎣⎦

（5） 即，

112123123112123123cos cos()+cos()sin sin()+sin()

p p x l l l y l l l θθθθθθθθθθθθ=++++=++++ （6）

结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。

补充：正解用于仿真，逆解用于控制

建立以上运动学方程后，若已知个连杆的关节角123θθθ、、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。 3、平面二连杆机器人手臂逆运动学

二、平面二连杆机器人手臂的速度雅可比矩阵

速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。

112123123112123123cos cos()+cos()sin sin()+sin()

p p x l l l y l l l θθθθθθθθθθθθ=++++=++++

上面的运动学方程两边对时间求导，得到下面的速度表达式：

111212123123123111212122123123

sin sin()()sin()()cos cos()()cos()()p p dx l l l dt dy l l l dt

θθθθθθθθθθθθθθθθθθθθθθθθ=-⋅-+⋅+-++⋅++=⋅++⋅++++⋅++&&&&&&&&&&&&

（17）

把上式写成如下的矩阵形式：

⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡++++-+--=⎥⎦⎤⎢⎣⎡21

21221211

21221211)cos()cos(cos )sin()sin(sin θθθθθθθθθθθθ&&&&l l l l l l y x p p （18） 令上式中的末端位置速度矢量X

y x p p &&&=⎥⎦⎤⎢⎣⎡， 关节角速度矢量Θ

=⎥⎦

⎤⎢⎣⎡&&&21θθ， 矩阵),()cos()cos(

cos )sin()sin(sin 212122121121221211θθθθθθθθθθθθJ l l l l l l =⎥⎦⎤

⎢

⎣⎡++++-+-- ),(21θθJ 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可

以写成：

Θ⋅=&&),(2

1θθJ X 速度雅可比矩阵可以进一步写成：

⎥⎦

⎤⎢⎣⎡=⎥⎦

⎤

⎢

⎣⎡++++-+--=22211211212212112122121121)cos()cos(cos )sin()sin(sin ),(J J J

l l l l l l J J θθθθθθθθθθθθ （19）

其中，