机械手臂的运动学公式推导1. 仿人机器人手臂模型● 仿人机器人的手臂有6个自由度，肩部（shoulder ）3个，肘部（elbow ）2个，腕部（wrist ）1个，如图1所示。

● 机器人手臂的几何尺寸（mm ）：上臂长度：216 小臂长度：173.5● 关节的运动范围（右手）：如表1所示。

表1 关节运动范围⑴ 参考坐标系为了对仿人机器人进行控制，同时也便于描述机器人的动作状态，必须建立适当的初始坐标系。

我们设定机器人手臂的初始姿态：大臂从肩垂直向下，小臂向前平伸，与大臂成90。

参考坐标系（实验室坐标系）的设定以机器人本身的初始位置与实验室坐标系相一致的原则设定，如图2所示。

X 轴：以机器人初始（状态）位置的右侧方向作为实验室坐标系的X 轴； y 轴：设定y 轴使其为右手系坐标系，即正前方为y 轴正向。

Z 轴：以机器人初始（状态）位置的上方向作为实验室坐标系的Z 轴；按D-H 坐标建立的方法，各个关节的轴线与各关节坐标系的Z 轴共线.(2) 关节坐标系各关节坐标系的建立如图3所示。

12 3 4 5 6图1 手臂模型 X 5 XOZ Y图2 参考坐标系X 3 O 3 Z 3Y 3Z 4 O 4Y 4X 4O 5Z 5Y 5Z 6 O 6Y 6 X 6 X 1 O 1 Z 1Y 1 Y 2 O 2 X 2Z 2shoulder 1、2、3elbow wrist 4、5 6 图3 关节坐标系（3）连杆参数连杆参数列表如表2所示。

表2 连杆参数连杆之间的齐次变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---=----------100001111111111i i i i i i i i c d c s c s s s d s c c c s a s c T i i i i i i i ii i i αααααααα从而可以确定：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-=100001000000111101c s s c T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=100000010000222212c s s c T ⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=10000100003303323c s l s c T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=100000010000444434c s s c T⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=100000100005515545c s l s c T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=10000010000666656c s s c TT T T T T T T 56453423120106= =[ (((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*cos(t5)-(-cos(t1)*c os(t2)*sin(t3)+sin(t1)*cos(t3))*sin(t5))*cos(t6)-(-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*sin(t6),-(((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*cos(t5)-(-cos(t1)*cos (t2)*sin(t3)+sin(t1)*cos(t3))*sin(t5))*sin(t6)-(-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*cos(t6),-((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*sin(t5)-(-cos(t1)*cos(t2)*sin(t3)+sin(t1)*cos(t3))*cos(t5), (-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*l1-cos(t1)*sin(t2)*l0] [ (((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*cos(t5)-(-sin(t1)*co s(t2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*cos(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+s in(t1)*sin(t2)*cos(t4))*sin(t6),-(((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*cos(t5)-(-sin(t1)*cos(t 2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*sin(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(t1)*sin(t2)*cos(t4))*cos(t6), -((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*sin(t5)-(-sin(t1)*cos(t2)*sin(t3)-cos(t1)*cos(t3))*cos(t5), (-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(t1)*sin(t2)*cos(t4))*l1-sin(t1)*sin(t2)*l0] [((-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*cos(t5)-sin(t2)*sin(t3)*sin(t5))*cos(t6)-(sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*sin(t6), -((-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*cos(t5)-sin(t2)*sin(t3)*sin(t5))*sin(t6)-(sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*cos(t6), -(-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*sin(t5)-sin(t2)*sin(t3)*cos(t5), (sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*l1-cos(t2)*l0] [0,0,0,1]⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-==-1000010000cos sin 00sin cos 111110110θθθθTT ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-100000100cos 0sin 0sin 0cos 222211221θθθθTT ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡----==-10000100cos 0sin 0sin 0cos 0333312332l TT θθθθ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-1000100cos 0sin 0sin 0cos 444413443θθθθTT⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡----==-10000100cos 0sin 0sin 0cos 1555514554l TT θθθθ ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-1000100cos 0sin 0sin 0cos 666615665θθθθT T 以“6”为参考，1、2、3三个关节交点“0”的位置由4、5、6三个关节决定，因此有26363266P T P =其中，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=1266z y x p p p P 已知；263P 为23T 的第4列，即⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=1000263l P T T T T 43546563= p=[]Tl s s l c l c c s c s l s l c s s c c P T 1)()(054160464561604645626363-+--+-=1222266266+++=z y x T p p p P P120142120+-+=l l c l l p P T a l l p p p l l c zy x =---+=1222212042令2tan 4θ=u ，则a u u c =+-=22411，有aau +-±=11 a a +-=11arctan24θ或aa+-+-=11arctan 24πθ（4） θ4的范围为 –150----30 由054l s s p z -=得： 405s l p s z-=其中04≠s令u =2tan5θ，则2512sin u u+=θ，即40212s l p uu z-=+ )11arctan(211122420405242040402-±-=-±-==++s l p p s l s l p p s l u u p sl u zz zz zθ)11arctan(2)11arctan(224204052420405-++-=-+-=s l p p s l s l p p s l zz zz πθθ （5）θ5的范围为 –180----180 160460456l s l c s l c c c p x +-= 则有：0)22()(0450042045=-+-++l c c p u l l c u l c c p x x)(2))((4)22()22(0450450452004004l c c p l c c p l c c p l l c l l c u x x x +-+--±--=因此6θ有两个值： 04522425424200046)12()(arctan2l c c p p c c c c l l l c x x+-++-+--=θ或4522425424200046)12()(arctan 2l c c p p c c c c l l l c x x+-++-+-+-=πθ二、手腕方位的反解321θθθ、、决定手腕的方位。

T T T p a o n p a o n p a o n z z z z y y y y x x x x 10213210=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡ 或⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡------+---+=10000022121323132131321323132131321102132l c s s s c s s c c s c s cs s c c c s s c c c s s s c c c T T T3tan θ-=xy a a 当0sin 2≠θ时),(2tan 3x y a a a -=θ1tan θ-=zzn o ),(2tan 1z z n o a -=θ12sin tan θθz za o =)sin ,(2tan 12θθz z a o a =。