六軸機器人軌迹規劃之五次多項式插值

1.軌迹規劃的定義

軌迹規劃(trajectory planning)是運動規劃(motion planning)研究的主要内容。運動規劃指的是運動插補，在起始點和終止點之間插入中間點序列，實作沿着軌迹的平穩運動。運動控制包含路徑規劃(path planning)和軌迹規劃，路徑規劃是規劃位置，在起終點之間經過的路徑點，軌迹規劃是規劃時間，将路徑點與時間相對應。

對于我們的六軸機器人而言軌迹規劃可以分為：關節空間軌迹規劃和笛卡爾空間軌迹規劃。關節空間軌迹規劃是把機器人的關節變量變換成跟時間的函數，然後對角速度和角加速度進行限制。笛卡爾空間軌迹規劃是把機器人末端在笛卡爾空間的位移、速度和加速度變換成跟時間的函數關系。

五次多項式插值能夠解決三次多項式插值的角速度變化不平滑且加速度存在跳變的情況。

2.數學基礎

五次多項式插值，角位移、角速度和角加速度的函數表達式為：

⎧⎩⎨⎪⎪⎪⎪θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5θ˙(t)=a1+2a2t+3a3t2+4a4t3+5a5t4θ¨(t)=2a2+6a3t+12a4t2+20a5t3 { θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 θ ˙ ( t ) = a 1 + 2 a 2 t + 3 a 3 t 2 + 4 a 4 t 3 + 5 a 5 t 4 θ ¨ ( t ) = 2 a 2 + 6 a 3 t + 12 a 4 t 2 + 20 a 5 t 3

限制條件如下，相對三次多項式插值，增加了對起止點角速度的限制（為簡便計算設 t0 t 0 為0）

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪θ(t0)=θ0=a0θ(tf)=θf=a0+a1tf+a2t2f+a3t3f+a4t4f+a5t5fθ˙(t0)=θ˙0=a1θ˙(tf)=θ˙f=a1+2a2tf+3a3t2f+4a4t3f+5a5t4fθ¨(t0)=θ¨0=2a2θ¨(tf)=θ¨f=2a2+6a3tf+12a4t2f+20a5t3f { θ ( t 0 ) = θ 0 = a 0 θ ( t f ) = θ f = a 0 + a 1 t f + a 2 t f 2 + a 3 t f 3 + a 4 t f 4 + a 5 t f 5 θ ˙ ( t 0 ) = θ ˙ 0 = a 1 θ ˙ ( t f ) = θ ˙ f = a 1 + 2 a 2 t f + 3 a 3 t f 2 + 4 a 4 t f 3 + 5 a 5 t f 4 θ ¨ ( t 0 ) = θ ¨ 0 = 2 a 2 θ ¨ ( t f ) = θ ¨ f = 2 a 2 + 6 a 3 t f + 12 a 4 t f 2 + 20 a 5 t f 3

求解得

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪a0=θ0a1=θ˙0a2=θ¨02a3=20θf−20θ0−(8θ˙f+12θ˙0)tf−(3θ¨0−θ¨f)t2f2t3fa4=30θ0−30θf+(14θ˙f+16θ˙0)tf+(3θ¨0−2θ¨f)t2f2t4fa5=12θf−12θ0−(6θ˙f+6θ˙0)tf−(θ¨0−θ¨f)t2f2t5f { a 0 = θ 0 a 1 = θ ˙ 0 a 2 = θ ¨ 0 2 a 3 = 20 θ f − 20 θ 0 − ( 8 θ ˙ f + 12 θ ˙ 0 ) t f − ( 3 θ ¨ 0 − θ ¨ f ) t f 2 2 t f 3 a 4 = 30 θ 0 − 30 θ f + ( 14 θ ˙ f + 16 θ ˙ 0 ) t f + ( 3 θ ¨ 0 − 2 θ ¨ f ) t f 2 2 t f 4 a 5 = 12 θ f − 12 θ 0 − ( 6 θ ˙ f + 6 θ ˙ 0 ) t f − ( θ ¨ 0 − θ ¨ f ) t f 2 2 t f 5

3.matlab代碼實作

序号	位移	速度	加速度	時間
1
2	50	10	20	3
3	150	20	30	6
4	100	-15	-20	12
5	14

clear;
clc;
q_array=[,,,,];%指定起止位置
t_array=[,,,,];%指定起止時間
v_array=[,,,-,];%指定起止速度
a_array=[,,,-,];%指定起止加速度
t=[t_array()];q=[q_array()];v=[v_array()];a=[a_array()];%初始狀态
for i=::length(q_array)-;%每一段規劃的時間
     T=t_array(i+)-t_array(i)
     a0=q_array(i);
     a1=v_array(i);
     a2=a_array(i)/;
     a3=(*q_array(i+)-*q_array(i)-(*v_array(i+)+*v_array(i))*T-(*a_array(i)-a_array(i+))*T^)/(*T^);
     a4=(*q_array(i)-*q_array(i+)+(*v_array(i+)+*v_array(i))*T+(*a_array(i)-*a_array(i+))*T^)/(*T^);
     a5=(*q_array(i+)-*q_array(i)-(*v_array(i+)+*v_array(i))*T-(a_array(i)-a_array(i+))*T^)/(*T^);%計算五次多項式系數 
     ti=t_array(i)::t_array(i+);
     qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^+a3*(ti-t_array(i)).^+a4*(ti-t_array(i)).^+a5*(ti-t_array(i)).^;
     vi=a1+*a2*(ti-t_array(i))+*a3*(ti-t_array(i)).^+*a4*(ti-t_array(i)).^+*a5*(ti-t_array(i)).^;
     ai=*a2+*a3*(ti-t_array(i))+*a4*(ti-t_array(i)).^+*a5*(ti-t_array(i)).^;
     t=[t,ti(:end)];q=[q,qi(:end)];v=[v,vi(:end)];a=[a,ai(:end)];
end
subplot(,,),plot(t,q,'r'),xlabel('t'),ylabel('position');hold on;plot(t_array,q_array,'o','color','g'),grid on;
subplot(,,),plot(t,v,'b'),xlabel('t'),ylabel('velocity');hold on;plot(t_array,v_array,'*','color','y'),grid on;
subplot(,,),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');hold on;plot(t_array,a_array,'^','color','r'),grid on;
           

PS:相對于三次多項式插值，加速度也是平滑的曲線，并沒有出現跳變的情況。然而在機器人系統中，單純的多項式規劃有一個非常嚴重的問題：沒有勻速段，無法根據期望速度提供勻速控制，而在大部分機器人應用中，對加工的速度控制都是有要求的。另一個問題就是，次數越高的多項式，加速過程越慢，整個運動過程中的平均速度越小，影響效率。下次将會為大家介紹能限制速度的三段s曲線加減速直線插值方式。