本文所提出的机械手轨迹规划方案为在关节空间内进行，其具体思路为： 1. 利用几何关系，寻各个插值点的坐标，并求机械手手爪在各个插值点处的运动速度与加速度，确定机械手端部在各个插值点处的位姿； 2. 利用机器人学的相关知识（主要是逆运动学分析与逆雅可比矩阵计算），将上面所求的机械手手爪的位姿、速度与加速度转化为各关节的位姿、速度与加速度；
3. 将上面所求的各关节运动参数进行样条曲线拟合插值。
【注】设定插补周期为s t ，对点位运动设定其运动时间为0t ，对直线运动和圆弧运动设定其运动速度为v 。 (一) 点位运动
1. 设定起始点()000Z Y X 与终止点()e e e Z Y X ；
2. 分别对起始点与终止点进行逆运动学计算得0i θ及ie θ；)6,5,4,3,2,1(=i
3. 分别对i θ进行线性插值000i i ie i t t θθ
θθ+-=。)6,5,4,3,2,1(=i
(二) 直线运动
1. 设定起始点()000Z Y X 与终止点()e e e Z Y X ；
2. 计算直线长度L ，机械手手爪在各点处的运动速度()T v ω
，插补步长d ，插补总步数N 及插补运动中各坐标轴的增量X ∆、Y ∆和Z ∆：
①202020)()()(Z Z Y Y X X L e e e -+-+-=；
②()z y x e e e
v v v L Z Z L
Y Y L X X v v =⎪⎭
⎫ ⎝⎛---=00
0 ，
故()()000z y x v v v v =ω
； ③s vt d =；
④⎥⎦⎤
⎢⎣⎡+=1d L N ；
⑤N X X X e 0-=∆、N Y Y Y e 0-=∆和N
Z Z Z e 0
-=∆。
3. 由此可以求得各插值点的坐标值：X X X i i ∆+=+1；Y Y Y i i ∆+=+1及Z Z Z i i ∆+=+1。
4. 分别对各插值点进行逆运动学计算，并求其对应的逆雅可比矩阵，得()
ij
ij ij
θθθ
。()1,,2,1,06,5,4,3,2,1-==N j i 5. 对所得的关节变量进行三次多项式插值：在())1(+j i ij θθ之间()1,,2,1,06,5,4,3,2,1-==N j i ，令0
θθ=ij
，f
j i θθ=+)
1(，0θθ =ij ，f
j i θθ =+)1(，
θθ =ij ，
f
j i θθ =+)1(；令
5
544332210t a t a t a t a t a a +++++=θ，则
453423215432t a t a t a t a a ++++=θ 且35
2432201262t a t a t a a +++=θ
故由⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧======f s
f s f s
t t t θθθθθθθθθθθθ )()0()()0()()0(00
0可以解得⎪⎪⎪⎪⎪⎪⎩
⎪⎪⎪⎪⎪
⎪⎨⎧
--+--=-+++-=--+--====320005
3200043
200030
201002)()66()(122)23()1614()(302)3()128()(202s s
f s f f s s
f s f f s s f s f f t t t a t t t a t t t a a a a θθθθθθθθθθθθθθθθθθθθθ 。 由此逐段插值得到的样条曲线必光滑。 (三) 圆弧运动
1. 设定起始点()000Z Y X O ，中间点()p p
p Z Y X P 及终点()e e
e Z Y X E ；
2. 首先计算出O ，P ，E 三点所确定圆弧的半径和圆心坐标：
(1)半径R
① 先计算出OPE ∆的三边长，202020)()()(Z Z Y Y X X a p p p -+-+-=，
202020)()()(Z Z Y Y X X b e e e -+-+-=，222)()()(p e p e p e Z Z Y Y X X c -+-+-=。
② 令)(2
1
c b a p ++=。 ③ )
)()((4c p b p a p p abc
R ---=
。
(2)圆心R O
据几何关系可知，点R O 满足两个条件：其一，R O 点位于O ，P ，E 三点所确定的平面上；其二，R O 点到O ，P ，E 三点的距离相等且皆为R 。根据解析几何的知识，我们可以求得O ，P ，E 三点所确定的平面π的方程为0)()()(030201=-+-+-Z Z A Y Y A X X A ，其中
有
001Z Z Y Y Z Z Y Y A e e p p ----=
；
002Z Z X X Z Z X X A e e p p -----
=；
00
03Y Y X X Y Y X X A e e p p ----=
故()R R
R R Z Y X O 满足方程组
⎪⎩
⎪⎨⎧=-+-+--+-+-=-+-+--+-+-=-+-+-0)()()()()()()()()()()()()()()(030201222202020222202020Z Z A Y Y A X X A Z Z Y Y X X Z Z Y Y X X Z Z Y Y X X Z Z Y Y X X R R R e R e R e R R R R p R p R p R R R R 化简得
⎪⎩
⎪
⎨⎧=++=-+-+-=-+-+-332120001000)()()()()()(h Z A Y A X A h Z Z Z Y Y Y X X X h Z Z Z Y Y Y X X X R R R R e R e R e R p R p R p ，
其中⎪⎪⎪
⎩
⎪⎪⎪⎨⎧++=++-++=++-++=0
3020132
020*******
020202221)]()[(21)]()[(21Z A Y A X A h Z Y X Z Y X h Z Y X Z Y X h e e e p p p 。
令⎪⎪⎪⎭
⎫ ⎝⎛------=321000000A A A Z Z Y Y X X Z Z Y Y X X D e e e p p p ，⎪⎪⎪
⎭⎫ ⎝⎛=321h h h H 。分析知3)(=D r ，
方程组有唯一解，故()H D Z Y X T R R R 1-=。
3. 建立插补直角坐标系，并确定其与基础坐标系之间的坐标变换关系： 以R O 为原点，以OP 方向为R X 轴，以OE OP ⨯方向为R Z 轴，建立插补直角坐标系R R R R Z Y X O ，而基础坐标系为0000Z Y X O 。下面求两坐标系间的变换关系。
据前文可知()00
0Z Z Y Y X X OP p p p ---=，而()32
1
A A A OE OP =⨯。
而0X 正方向为()001，0Z 正方向为()100。是以若R Z 轴正向与0Z 轴正向之间的夹角为α，则有2
3
2
22
11
cos A A A A ++=
α且),0(πα∈；若R X 轴正向与0
X 轴正向之间的夹角为θ，则有2
02
02
00
)
()()(cos Z Z Y Y X X X X p p p p -+-+--=
θ且),0(πθ∈。
∴由R R R R Z Y X O 到0000Z Y X O 的坐标变换矩阵为
⎪⎪⎪⎪⎪⎭
⎫
⎝
⎛--==10
00cos sin 0sin cos cos cos sin sin sin cos sin cos ),(),(),,(R R R R R R R Z Y X X Rot Z Rot Z Y X Tran T αααθαθθ
αθαθθ
αθ⎪⎪⎭⎫ ⎝⎛=10P R 。故易知由0000Z Y X O 到R R R R Z Y X O 的坐标变换矩阵为⎪⎪⎭
⎫ ⎝⎛-=-101
P R R T T
T R 。 4. 分别将起始点齐次坐标()T
Z Y X O 1000，中间点齐次坐标()
T p p p Z Y X P 1及终点齐次坐标()T
e e
e
Z Y X E 1左乘1-R T ，将其变换为坐标系R R R R Z Y X O 中的
坐标值，分别为起始点()T Y X O 1000'''，中间点(
)T
p
p
Y X P 10'''及终点
()T e e Y X E 10'''。将其简写为()T Y X O 00'''，()T p p Y X P '''及()T
e
e Y X E '''。 5. 计算总圆心角φ，插补步长θ∆，插补总步数N 及在坐标系R R R R Z Y X O 中
观察所得的机械手手爪在各点处的运动速度：
在此坐标环境下，圆弧圆心位于坐标原点，而圆弧半径在前文已经求出，故
①R
Y Y X X p p 2)()(arcsin
22020
1'-'+'-'=φ，R
Y Y X X p e p e 2)
()(arcsin
222
2'-'+'-'=φ从而有21φφφ+=；
②R
vt
s =∆θ；
③⎥⎦
⎤
⎢⎣⎡+∆=1θφN ； 根据θθθ∆+=+i i 1可判断是否到插补终点——若φθ≤+1i ，则继续插补下去；1+i θ﹥φ，则修正最后一步步长为i θφθ-='∆。
④令000arct an X Y ''
=θ，则有()()y R i x R i i i i v v v v ''=++-=')cos()sin(00θθθθ。在
基础坐标系中观察，有()zi yi xi i ωωωω=，其中ωω2
3
2
22
11
A A A A xi ++=
、
ωω2
3
2
22
12
A A A A yi ++=
、ωω2
3
2
22
13
A A A A zi ++=
，
而R
v
=ω。),,2,1,0(N i = 6. 计算各插值点的坐标值：
易知()11++''i i Y X 为()i i
Y X ''绕R Z 轴旋转θ∆角度得到的，即有⎪⎪⎭⎫ ⎝⎛''⎪⎪⎭⎫
⎝⎛∆∆∆-∆=⎪⎪⎭⎫ ⎝⎛''++i i i i Y X Y X θθ
θθ
cos sin sin cos 11。由此可得⎪⎩
⎪
⎨⎧∆+=∆'+∆'='∆'-∆'='+++θθθθθθ
θi i i i i i i i Y X Y Y X X 111cos sin sin cos 。 7. 分别将()T
i i Y X 10''与()T yRi
xRi v v 10''都左乘坐标变换矩阵R T ，将其变换为基础坐标系中的位置与速度，分别为()T
i i
i Z Y X 1与()T
zi yi xi v v v 1，
)
,,2,1,0(N i =。
从而(
)()T zi yi xi zi
yi xi
T
i
i v v v
v ωωωω=，),,2,1,0(N i =。
8. 分别对各插值点进行逆运动学计算，并求其对应的逆雅可比矩阵，得()
ij
ij
ij
θθ
θ
。()N j i ,,2,1,06,5,4,3,2,1 == 9. 对所得的关节变量进行三次多项式插值：在())1(+j i ij θθ之间()1,,2,1,06,5,4,3,2,1-==N j i ，令0
θθ=ij
，f
j i θθ=+)
1(，0θθ =ij ，f
j i θθ =+)1(，
θθ =ij ，
f
j i θθ =+)1(；令
5
544332210t a t a t a t a t a a +++++=θ，则
453423215432t a t a t a t a a ++++=θ 且35
2432201262t a t a t a a +++=θ 故由⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧======f s
f s f s
t t t θθθθθθθθθθθθ )()0()()0()()0(00
0可以解得⎪⎪⎪⎪⎪⎪⎩
⎪⎪⎪⎪
⎪
⎪⎨⎧
-
-+--=-+++-=--+--====3200053200043
200030201002)()66()(122)23()1614()(302)3()128()(202s s
f s f f s s f s f f s s f s f f t t t a t t t a t t t a a a a θθθθθθθθθθθθθθθθθθθθθ 。 由此逐段插值得到的样条曲线必光滑。
【注】关于轨迹规划插值运算中各插值点处加速度计算方法的一点说明： 为了使机械手手爪端部在工作过程中能够保持平稳，免受冲击，故要求机械手手爪端部在工作过程中做匀速运动，对直线运动而言为匀速运动，而对圆弧运
动而言为匀速圆周运动，故有以下结论，
1. 对于直线运动而言，机械手手爪端部的线加速度与角加速度均为0；
2. 对于圆弧运动而言，机械手手爪端部的角加速度为0，故其切向加速度
亦必为0，而其法向加速度可按照其线速度的求法进行求解，即令000arctan X Y ''
=θ，
则有
()()
n
y
R
i n x
R i i i n ni
a a a a ''=+-+-=')sin()cos(00θθθθ。再分别将
()T
n
y
R i n x
R i a a 10''
都左乘坐标变换矩阵R T ，
将其变换为基础坐标系中的加速度，分别为(
)T
nzi nyi
nxi
a a a 1，),,2,1,0(N i =。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议