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

JojenZz發表於2018-02-10
機器人

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
\begin{eqnarray*} \left\{ \begin{array}{l} \theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5\\ \dot{\theta}(t)=a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4\\ \ddot{\theta}(t)=2a_2+6a_3t+12a_4t^2+20a_5t^3 \end{array}\right. \end{eqnarray*}

約束條件如下，相對三次多項式插值，增加了對起止點角速度的約束（為簡便計算設t0
t_0
為0）
{θ(t0)=θ0=a0θ(tf)=θf=a0+a1tf+a2tf2+a3tf3+a4tf4+a5tf5θ˙(t0)=θ˙0=a1θ˙(tf)=θ˙f=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4θ¨(t0)=θ¨0=2a2θ¨(tf)=θ¨f=2a2+6a3tf+12a4tf2+20a5tf3
\begin{eqnarray*} \left\{ \begin{array}{l} \theta(t_0)=\theta_0=a_0\\ \theta(t_f)=\theta_f=a_0+a_1t_f+a_2t_f^2+a_3t_f^3+a_4t_f^4+a_5t_f^5\\ \dot{\theta}(t_0)=\dot{\theta}_0=a_1\\ \dot{\theta}(t_f)=\dot{\theta}_f=a_1+2a_2t_f+3a_3t_f^2+4a_4t_f^3+5a_5t_f^4\\ \ddot{\theta}(t_0)=\ddot{\theta}_0=2a_2\\ \ddot{\theta}(t_f)=\ddot{\theta}_f=2a_2+6a_3t_f+12a_4t_f^2+20a_5t_f^3 \end{array}\right. \end{eqnarray*}

求解得
{a0=θ0a1=θ˙0a2=θ¨02a3=20θf−20θ0−(8θ˙f+12θ˙0)tf−(3θ¨0−θ¨f)tf22tf3a4=30θ0−30θf+(14θ˙f+16θ˙0)tf+(3θ¨0−2θ¨f)tf22tf4a5=12θf−12θ0−(6θ˙f+6θ˙0)tf−(θ¨0−θ¨f)tf22tf5
\begin{eqnarray*} \left\{ \begin{array}{l} a_0=\theta_0\\ a_1=\dot{\theta}_0\\ a_2=\frac{\ddot{\theta}_0}{2}\\ a_3=\frac{20\theta_f-20\theta_0-(8\dot\theta_f+12\dot\theta_0)t_f-(3\ddot\theta_0-\ddot\theta_f)t_f^2}{2t_f^3}\\ a_4=\frac{30\theta_0-30\theta_f+(14\dot\theta_f+16\dot\theta_0)t_f+(3\ddot\theta_0-2\ddot\theta_f)t_f^2}{2t_f^4}\\ a_5=\frac{12\theta_f-12\theta_0-(6\dot\theta_f+6\dot\theta_0)t_f-(\ddot\theta_0-\ddot\theta_f)t_f^2}{2t_f^5}\\ \end{array}\right. \end{eqnarray*}

3.matlab程式碼實現
序號 位移 速度 加速度 時間
1 0 0 0 0
2 50 10 20 3
3 150 20 30 6
4 100 -15 -20 12
5 0 0 0 14 
clear;
clc;
q_array=[0,50,150,100,40];%指定起止位置
t_array=[0,3,6,12,14];%指定起止時間
v_array=[0,10,20,-15,0];%指定起止速度
a_array=[0,20,30,-20,0];%指定起止加速度
t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[a_array(1)];%初始狀態
for i=1:1:length(q_array)-1;%每一段規劃的時間
     T=t_array(i+1)-t_array(i)
     a0=q_array(i);
     a1=v_array(i);
     a2=a_array(i)/2;
     a3=(20*q_array(i+1)-20*q_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);
     a4=(30*q_array(i)-30*q_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);
     a5=(12*q_array(i+1)-12*q_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%計算五次多項式係數 
     ti=t_array(i):0.001:t_array(i+1);
     qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3+a4*(ti-t_array(i)).^4+a5*(ti-t_array(i)).^5;
     vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2+4*a4*(ti-t_array(i)).^3+5*a5*(ti-t_array(i)).^4;
     ai=2*a2+6*a3*(ti-t_array(i))+12*a4*(ti-t_array(i)).^2+20*a5*(ti-t_array(i)).^3;
     t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];
end
subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');hold on;plot(t_array,q_array,'o','color','g'),grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');hold on;plot(t_array,v_array,'*','color','y'),grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');hold on;plot(t_array,a_array,'^','color','r'),grid on;

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

相關文章