精心整理
一、平面二连杆机器人手臂运动学
平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l ，连杆3长度为3l 。
建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 、33(,)x y 为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。
关节角顺时针为负逆时针为正。
1
θ
图11112123123p p x y 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 的齐次旋转变换矩阵为：
33212cos sin 0l T θθ-⎡⎤⎢⎥=从(003T =003P =结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。
补充：正解用于仿真，逆解用于控制
建立以上运动学方程后，若已知个连杆的关节角123θθθ、、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。
3、平面二连杆机器人手臂逆运动学
二、平面二连杆机器人手臂的速度雅可比矩阵
速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。
现已二连杆平面机器人为例推导速度雅可比矩阵。
上面的运动学方程两边对时间求导，得到下面的速度表达式：
111212123123123111212122123123sin sin()()sin()()cos cos()()cos()()
p p dx l l l dt dy l l l dt
θθθθθθθθθθθθθθθθθθθθθθθθ=-⋅-+⋅+-++⋅++=⋅++⋅++++⋅++（17）
把上式写成如下的矩阵形式：
⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡++++-+--=⎥⎦⎤⎢⎣⎡21
2122121121221211)cos()cos(
cos )sin()sin(sin θθθθθθθθθθθθ l l l l l l y x p p （18） 令上式中的末端位置速度矢量X
x p =⎥⎤⎢⎡， 矩阵1
1l l ⎢⎣⎡-(1θJ
1(J θ2122
22211211θ∂J J J J 由此可知雅可比矩阵的定义：
⎥⎥⎥
⎥⎦
⎤
⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂=⎥⎦⎤⎢⎣⎡=21
21
2221
1211
21),(θθθθθθp p p p
J y y x x J J J
J （21） 三、平面二连杆机器人手臂的动力学方程
推倒动力学方程的方法很多，各有优缺点。
拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。
下面推导图1所示的平面双连杆机器人的动力学方程。
图1中所示连杆均为均质杆，其转动惯量分别是1I 和2I 。
1、求两连杆的拉格朗日函数 （1）求系统总动能
连杆1的动能为：
2121121211211
1)1(12
1θθθ l m l m I K A ===
（21）
y x D D Y x D D （122(21121(2121l m I K ===
212212222222222122122222112122
122122222222212212
22121)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 ++++++=+++++=
=（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的驱动力矩分别为：
211
1τθθτ∂-
∂∂∂∂=∂∂-∂∂∂∂=
L
L t L
L t （29）
(21τ-=l m ）
同理：
2τ=（32）
轨迹规以及 用三直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。
这个过程可以归纳为以下计算循环：
（a ）将时间增加一个增量；
（b ）利用所选择的轨迹函数计算出手的位姿； （c ）利用逆运动学方程计算相应的关节变量； （d ）将关节变量信息送给控制器； （e ）返回到循环的开始。
五、二连杆机器人的控制。