用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
一、平面二连杆机器人手臂运动学
平面二连杆机械手臂如图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）问题
已知末端位置坐标),(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）式得到：
)cos(cos 211222122p 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 221221212212222p 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）
二、平面二连杆机器人手臂的速度雅可比矩阵
速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。
上面的运动学方程两边对时间求导，得到下面的速度表达式：