一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图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 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 00sin cos 111101θθθθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 0sin cos 2212212θθθθl T （3） 从),,(000z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+++-+=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=⋅=10000100sin 0)cos()sin(cos 0)sin()cos(1000010000cos sin 0sin cos 1000010000cos sin 00sin cos 112121112121221221111120102θθθθθθθθθθθθθθθθθθl l l T T T （4）那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡++++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+++-+=⋅=110)sin(sin )cos(cos 10010000100sin 0)cos()sin(cos 0)sin()cos(212112121121121211121212020p 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）式得到：22211211)sin ()cos (l l y l x p p =-+-θθ （7） 整理得到：)sin cos (2111222122θθp p p p y x l l l y x +=-++ （8）令pp p pp tg y x θθθcos sin == （9）由（8）式得到：)sin sin cos (cos cos 2111222122p p p p p p x l l l y x θθθθθ+=-++)cos(cos 211222122p pp 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 12221221 （11）（3）求2θ 由（6）式得到：2122122212)]sin([)]cos([l l y l x p p =+-++-θθθθ （12）整理得到：)]sin()cos([221212212222θθθθ+++=-++p p p p y x l l l y x （13） 令pp p pp tg y x θθθcos sin == （14）由（14）式得到：)cos(cos 2]sin )sin(cos )[cos(cos 221221212212222p pp p p p p p p x l x l l l y x θθθθθθθθθθθ-+=+++=-++ （15）由此可解出2θ。

122122222cos 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 2121211121212111θθθθθθθθθθθθ +⋅++⋅=+⋅+-⋅-=l l dtdy l l dt dx p p （17）把上式写成如下的矩阵形式：⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡++++-+--=⎥⎦⎤⎢⎣⎡212122121121221211)cos()cos(cos )sin()sin(sin θθθθθθθθθθθθ l l l l l l y x p p （18） 令上式中的末端位置速度矢量Xy x p p =⎥⎦⎤⎢⎣⎡， 关节角速度矢量Θ=⎥⎦⎤⎢⎣⎡ 21θθ， 矩阵),()cos()cos(cos )sin()sin(sin 212122121121221211θθθθθθθθθθθθJ l l l l l l =⎥⎦⎤⎢⎣⎡++++-+--),(21θθJ 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。

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

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

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

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

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

连杆2质心位置为：)sin(21sin )cos(21cos 2121121211θθθθθθ++=++=l l y l l x D D （22）连杆2质心速度为：)()cos(21cos )()sin(21sin 2121211121212111θθθθθθθθθθθθ +⋅++⋅=+⋅+-⋅-=l l Y l l x D D （23）21221222222212212221222)cos 21(41)cos 41(θθθθθθ l l l l l l l l y x V D D D +++++=+=（24）连杆2的动能：21221222222222122122212212212222222122122212221222222212)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) 系统总动能：212212222222222122122222112122122122222222212212221221)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(21sin (sin 21212112111θθθθ+++=l l g m gl m P （27）（3）求拉格朗日函数)]sin(21sin [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 PK L （28） （4）列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为：222111θθτθθτ∂∂-∂∂∂∂=∂∂-∂∂∂∂=L L t LL t （29）22212222122122222112121)cos 2131()cos 3131(θθθθθ l l m l m l l m l m l m l m L +++++=∂∂22221221221222212222122122222112121sin 21sin )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(21cos )21(212211211θθθθ+-+-=∂∂gl m gl m m L )cos(21cos )21(sin 21sin )cos 2131()cos 3131(2122112122221221221222212222122122222112121θθθθθθθθθθθθτ++++--+++++=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(31θθθθ l l m l m l m L ++=∂∂ 2122122222122122222sin 2131)cos 2131(θθθθθθθ l l m l m l l m l m L t -++=∂∂∂∂ )cos(21sin 21sin 2121222122122122122θθθθθθθθ+---=∂∂gl m l l m l l m L )cos(21sin 2131)cos 2131(21222122122222122122222θθθθθθθτ+++++=gl m l l m l m l l m l m （31）联合（30）、（31）式，将动力学方程写成如下矩阵形式：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+++++⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡-+⎥⎦⎤⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-+⎥⎦⎤⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+++++=⎥⎦⎤⎢⎣⎡)cos(21)cos(21cos )21(000sin 0sin 21sin 21031cos 2131cos 2131cos 3131212221221121212122122221221222122122222122222212222221222221121221θθθθθθθθθθθθθθθθθθθττ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）四、平面二连杆机器人手臂的轨迹规划轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。