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

一、平面二连杆机器人手臂运动学
平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l 。

建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。

关节角顺时针为负逆时针为正。

图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程
连杆2末段与中线交点处一点P 在基础坐标系中的位置坐标：
)
sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （1）
2、用D-H 方法建立运动学方程
假定0z 、1z 、2z 垂直于纸面向里。

从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为：
⎥⎥
⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡-=100
010000cos sin 00sin cos 1
111
01
θθ
θθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：
⎥⎥
⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡-=100
010000cos sin 0sin cos 2
212212
θθ
θθl T （3） 从),,(000z y x 到),,(222z y x 的齐次旋转变换矩阵为：
⎥
⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡+++-+=⎥⎥
⎥⎥⎦
⎤
⎢
⎢⎢
⎢⎣⎡-⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=⋅=10000100sin 0)cos()sin(cos 0)sin()cos(
1000010
000cos sin 0sin cos 1000
010000cos sin 00sin cos 1121211121212212
2111
1120102θθθθθθθθθθθθθθθθ
θθl l l T T T （4）
那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为：
⎥
⎥⎥
⎥
⎦
⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡++++=⎥
⎥
⎥
⎥⎦
⎤
⎢⎢⎢⎢⎣⎡⋅⎥⎥⎥⎥⎦⎤⎢
⎢⎢
⎢⎣⎡+++-+=⋅=110)sin(sin )cos(
cos 10010000100sin 0)cos()sin(cos 0)sin()cos(
212112121121121211121212
020p p p z y x l l l l l l l P T P θθθθθθθθθθθθθθθθ （5）
即，
)
sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （6）
与用简单的平面几何关系建立运动学方程（1）相同。

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

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

逆运动学可以用于对机械臂关节角和末端位置的控制。

对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置
),(p p y x 求相应关节角21θθ、的过程。

推倒如下。

（1）问题
)
sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p
已知末端位置坐标),(p p y x ，求关节角21θθ、。

（2）求1θ
由（6）式得到：
2
2
211211)sin ()cos (l l y l x p p =-+-θθ （7） 整理得到：
)sin cos (21112
22122θθp p p p y x l l l y x +=-++ （8）
令
p
p p p
p tg y x θθθcos sin =
= （9）
由（8）式得到：
)sin sin cos (cos cos 211122
2122p p p p p p x l l l y x θθθθθ+=
-++
)cos(cos 2112
22122p p
p p p x l l l y x θθθ-=-++ （10）
由此可解出1θ。

p p p p p p x y arctg x l l l y x +⎥⎥⎦
⎤⎢⎢⎣⎡-++=θθcos 2arccos 12
2
21221 （11）
（3）求2θ 由（6）式得到：
2122122212)]sin([)]cos([l l y l x p p =+-++-θθθθ （12）
整理得到：
)]sin()cos(
[2212122
12222θθθθ+++=-++p p p p y x l l l y x （13） 令
p
p p p
p tg y x θθθcos sin =
= （14）
由（14）式得到：
)
cos(cos 2]
sin )sin(cos )[cos(cos 22122121221
2222p p
p p p p p p p x l x l l l y x θθθθθθθθθθθ-+=
+++=
-++ （15）
由此可解出2θ。

122
1
22222cos 2arccos θθθ-+⎥⎥⎦
⎤⎢⎢⎣⎡-++=p p p p p p x y arctg x l l l y x （16）
二、平面二连杆机器人手臂的速度雅可比矩阵
速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。

现已二连杆平面机器人为例推导速度雅可比矩阵。

)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p
上面的运动学方程两边对时间求导，得到下面的速度表达式：
)()cos(cos )()sin(sin 2
12121112
1212111θθθθθθθθθθθθ +⋅++⋅=+⋅+-⋅-=l l dt
dy l l dt dx p p （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 2121221211
21221211θθθθθθθθθθθθJ l l l l l l =⎥⎦⎤
⎢⎣⎡++++-+--
),(21θθJ 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。

（18）式可
以写成：
Θ⋅= ),(2
1θθJ X 速度雅可比矩阵可以进一步写成：
⎥⎦
⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡++++-+--=2221121121221211
2122121121)cos()cos(cos )sin()sin(sin ),(J J J
l l l l l l J J θθθθθθθθθθθθ （19）
其中，
)
cos()cos(cos )
sin()sin(sin 2122
222121112121221221211111θθθθθθθθθθθθθθ+=∂∂=
++=∂∂=+-=∂∂=+--=∂∂=l y J l l y J l x J l l x J p p p p （20）
由此可知雅可比矩阵的定义：
⎥⎥⎥
⎥⎦
⎤
⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂=⎥⎦⎤⎢
⎣⎡=2121
2221
1211
21),(θθθθθθp p p p
J y y x x J J J J （21） 三、平面二连杆机器人手臂的动力学方程
推倒动力学方程的方法很多，各有优缺点。

拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。

下面推导图1所示的平面双连杆机器人的动力学方程。

图1中所示连杆均为均质杆，其转动惯量分别是1I 和2I 。

1、求两连杆的拉格朗日函数 （1）求系统总动能
连杆1的动能为：
21211212112116
1)31(212
1θθθ l m l m I K A ===
（21）
求连杆2质心D 处的线速度：对连杆2质心位置求导得到其线速度。

连杆2质心位置
为：
)
sin(2
1
sin )
cos(2
1
cos 2121121211θθθθθθ++=++=l l y l l x D D （22）
连杆2质心速度为：
)()cos(2
1cos )()sin(2
1sin 212121112
1212111θθθθθθθθθθθθ +⋅++⋅=+⋅+-⋅-=l l Y l l x D D （23）
21221222222212212221222)cos 2
1(41)cos 41(θθθθθθ l l l l l l l l y x V D D D +++++=+=
（24）
连杆2的动能：
2
122122222222212212
2212212212222222122122212221222222212)cos 32(2161)cos 31(21])cos 21(41)cos 41[(21))(121(2121)(21θθθθθθθθθθθθθθθθ l l l m l m l l l l m l l l l l l l l m l m V m I K D D +++++=+++++++=++=
(25) 系统总动能：
212212222222222122122222112122
122122222222212212
22122
1)cos 2131(61)cos 21616121()cos 32(2161)cos 31(21θθθθθθθθθθθθ l l m l m l m l l m l m l m l m l l l m l m l l l l m K K K ++++++=+++++=
+= （26） （2）求系统总势能 系统总势能为：
))sin(2
1
sin (sin 21212112111θθθθ+++=l l g m gl m P （27）
（3）求拉格朗日函数
)]sin(2
1
sin [sin 21)cos 2131(61)cos 21616121(21211211121221222222222212212222211212θθθθθθθθθθ++--++++++=-=l l g m gl m l l m l m l m l l m l m l m l m P
K L （28） （4）列写动力学方程
按照拉格朗日方程，对应关节1、2的驱动力矩分别为：
22
2111θθτθθτ∂∂-∂∂∂∂=
∂∂-∂∂∂∂=
L L t L
L t （29）
22212222122122222112121
)cos 2131()cos 3131(θθθθθ l l m l m l l m l m l m l m L +++++=∂∂
22
221221221222212222122122222112121
sin 2
1sin )cos 2131()cos 3131(θθθθθθθθθθ l l m l l m l l m l m l l m l m l m l m L t --+++++=∂∂∂∂ )cos(2
1cos )21(212211211θθθθ+-+-=∂∂gl m gl m m L )cos(2
1cos )21(sin 21sin )cos 2
131()cos 3131(212211212222122122122
2212222122122222112121θθθθθθθθθθθθτ++
++--+++++=gl m gl m m l l m l l m l l m l m l l m l m l m l m （30）
同理：
1221222222222)cos 2131(3
1θθθθ l l m l m l m L ++=∂∂ 2
122122222122122222
sin 2131)cos 2131(θθθθθθθ l l m l m l l m l m L t -++=∂∂∂∂ )cos(2
1sin 21sin 212
1222122122122122θθθθθθθθ+---=∂∂gl m l l m l l m L )cos(2
1sin 2
13
1)cos 2
13
1
(212
22122122222122122222θθθθθθθτ+++++=gl m l l m l m l l m l m （31）
联合（30）、（31）式，将动力学方程写成如下矩阵形式：
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡+++++⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡-+⎥⎦⎤⎢⎣⎡⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡-+⎥⎦⎤⎢⎣⎡⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡+++++=⎥⎦⎤⎢⎣⎡)
cos(21)cos(21cos )21(000sin 0sin 21
sin 21031cos 2131cos 2
131cos 3131212221221121212122122221221222122122
2221222222122
22221222221121221θθθθθθθθθθθθθθθθθθθττgl m gl m gl m m l l m l l m l l m l m l l m l m l l m l m l l m l m l m l m （32）
四、平面二连杆机器人手臂的轨迹规划
轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。

轨迹规划是机器人完成规定任务所必需的。

它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。

关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。

如图所示，一两自由度机械手，已知两连杆起点和终点的关节角，确定中间位置的关节角。

（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹规划的缺点。

三次多项式轨迹规划
举例：要求一个５轴机器人的第一关节在５秒之内从初始角３０度运动到终端角７５度，用三次多项式计算在第１、２、３、４秒时的关节角。

五次多项式轨迹规划
抛物线过渡的线性运动轨迹规划（略）
具有中间点以及用抛物线过渡的线性运动轨迹规划（略）
高次多项式运动轨迹规划（略）
直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。

这个过程可以归纳为以下计算循环：
（a）将时间增加一个增量；
（b）利用所选择的轨迹函数计算出手的位姿；
（c）利用逆运动学方程计算相应的关节变量；
（d）将关节变量信息送给控制器；
（e）返回到循环的开始。

五、二连杆机器人的控制。