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

一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图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 的齐次旋转变换矩阵为:⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 00sin cos 111101θθθθT (2) 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为:⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 0sin cos 2212212θθθθl T (3) 从222(,,)x y z 到333(,,)x y z 的齐次旋转变换矩阵为:3323312cos sin 0sin cos 000010001l T θθθθ-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦(3) 从),,(000z y x 到333(,,)x y z 的齐次旋转变换矩阵为:11221332112233001231231231231121cos sin 00cos sin 0cos sin 0sin cos 00sin cos 00sin cos 00001000100010000100010001cos()sin()0cos cos(l l T T T T l l θθθθθθθθθθθθθθθθθθθθ---⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=⋅⋅=⋅⋅⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦++-+++=212312311212)sin()cos()0sin sin()00100001l l θθθθθθθθθθ+⎡⎤⎢⎥++++++⎢⎥⎢⎥⎢⎥⎣⎦(4)那么,连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为:12312311212312312311212003311212312311212cos()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 θθθθθθθθθθθθ=++++=++++上面的运动学方程两边对时间求导,得到下面的速度表达式:111212123123123111212122123123sin sin()()sin()()cos cos()()cos()()p p dx l l l dt dy l l l dtθθθθθθθθθθθθθθθθθθθθθθθθ=-⋅-+⋅+-++⋅++=⋅++⋅++++⋅++&&&&&&&&&&&&(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)四、平面二连杆机器人手臂的轨迹规划轨迹规划就就是已知起点与终点的位置速度加速度等参数确定中间点的相应参数的过程。