六軸機器人軌跡規劃之五次多項式插值
1.軌跡規劃的定義
軌跡規劃(trajectory planning)是運動規劃(motion planning)研究的主要內容。運動規劃指的是運動插補,在起始點和終止點之間插入中間點序列,實現沿著軌跡的平穩運動。運動控制包含路徑規劃(path planning)和軌跡規劃,路徑規劃是規劃位置,在起終點之間經過的路徑點,軌跡規劃是規劃時間,將路徑點與時間相對應。
對於我們的六軸機器人而言軌跡規劃可以分為:關節空間軌跡規劃和笛卡爾空間軌跡規劃。關節空間軌跡規劃是把機器人的關節變數變換成跟時間的函式,然後對角速度和角加速度進行約束。笛卡爾空間軌跡規劃是把機器人末端在笛卡爾空間的位移、速度和加速度變換成跟時間的函式關係。
五次多項式插值能夠解決三次多項式插值的角速度變化不平滑且加速度存在跳變的情況。
2.數學基礎
五次多項式插值,角位移、角速度和角加速度的函式表示式為:
\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*}
約束條件如下,相對三次多項式插值,增加了對起止點角速度的約束(為簡便計算設
t_0
為0) \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*}
求解得
\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曲線加減速直線插值方式。
相關文章
- 機器人學之運動學筆記【7】—— 機械手臂軌跡規劃例項機器人筆記
- 軌跡規劃理解
- 【2018.04.19 ROS機器人作業系統】機器人控制:運動規劃、路徑規劃及軌跡規劃簡介之一ROS機器人作業系統
- Mapbox 輕鬆規劃起始點軌跡
- 2020研究生數模整理(2):plotly雙座標軸繪製多個軌跡
- 機器人路徑規劃 A*與D*機器人路徑規劃演算法詳述機器人演算法
- 畢設(5)—笛卡爾空間軌跡規劃(直線、圓弧)
- 多車軌道路徑規劃演算法設計演算法
- GPS軌跡記錄器:myTracks for macMac
- KITTI_00_SPTAM軌跡和KITTI_00_ORB軌跡ORB
- 自動生成相機標定軌跡
- 機器人移動的規劃和導航機器人
- 軌跡資料 (三)
- 軌跡聯邦用到的具體公式 + 軌跡模組的設計公式
- Leaflet 帶箭頭軌跡以及沿軌跡帶方向的動態marker
- 2.Vue插值表示式Vue
- 上線倒數計時《星之軌跡》x陳樂一合作企劃公開!
- 聊聊 RocketMQ 訊息軌跡MQ
- 智慧手環軌跡APIAPI
- 【Android 動畫】動畫詳解之插值器(二)Android動畫
- 前端筆記之JavaScript(六)讓人頭疼的正規表示式前端筆記JavaScript
- 《黎之軌跡》發售紀念訪談:《軌跡》系列的巨大分水嶺
- 【機器學習】多項式迴歸原理介紹機器學習
- 【機器學習】多項式迴歸python實現機器學習Python
- 【機器學習】多項式迴歸sklearn實現機器學習
- Swift 5 字串插值之美Swift字串
- Falcom 之軌跡:40 年不變的 JRPG 魂(上)
- Falcom 之軌跡:40 年不變的 JRPG 魂(下)
- 移動機器人運動規劃及運動模擬機器人
- vue.js插值與表示式Vue.js
- HybridDBforPostgreSQL軌跡相似(伴隨分析)SQL
- not noly go —— 執行軌跡[一]Go
- 動態規劃系列之六01揹包問題動態規劃
- 機器學習在高德地圖軌跡分類的探索和應用機器學習地圖
- 機器學習入門規劃機器學習
- 滲透測試學習之隱藏蹤跡與規避檢測六
- 【路徑規劃】基於蟻群的多無人機攻擊排程無人機
- Qt自定義動畫插值函式QT動畫函式
- 手擼機器學習演算法 - 多項式迴歸機器學習演算法