移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃平滑性研究

PRUDNIKAU MIKALAI

摘 要：針對(duì)機(jī)器人操縱裝置或機(jī)器人路徑的平滑性研究，對(duì)軌跡節(jié)點(diǎn)的平滑離散是很有必要，此外，還建立了滿足標(biāo)準(zhǔn)平滑度的軌跡路徑。平滑起點(diǎn)和機(jī)器人的最終位置必須要精確定義，所以經(jīng)典最小二乘法是不適用的。為了解決這個(gè)問題，提出了一種改進(jìn)的最小二乘法。該方法要求首先建立一個(gè)平滑的運(yùn)動(dòng)軌跡，同時(shí)使其起點(diǎn)和終點(diǎn)保持一致，而且減小了機(jī)器人的驅(qū)動(dòng)單元的動(dòng)態(tài)負(fù)載和計(jì)算時(shí)間。

關(guān)鍵詞：路徑規(guī)劃 機(jī)器人 最小二乘法

中圖分類號(hào)：TP391.9 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1672-3791（2015）04（c）-0218-02

目前用于創(chuàng)建一個(gè)平滑軌跡的方法比較多，例如：采用運(yùn)動(dòng)規(guī)劃的采集點(diǎn)，采用分段線性化的方法解決軌跡平滑性問題，該方法是用兩點(diǎn)之間的直線替代光滑曲線上兩點(diǎn)間的實(shí)際曲線，但這些算法往往沒有考慮障礙物等實(shí)際情況，而且在狹窄的空間中應(yīng)用不是很好[1-2]。其他的方法便是使用數(shù)值計(jì)算的方法[3-5]，其中樣條函數(shù)是較為經(jīng)典的方法[6-8]。樣條函數(shù)可以很好地解決平面和空間路徑的任務(wù)，該算法廣泛應(yīng)用于陸地、海水和空中運(yùn)行的無人操作機(jī)器人等方面。GilimyanovR.F提出了遞歸的方法[9]。在他的工作基礎(chǔ)上，科技工作者開發(fā)了一個(gè)工作窗口，可以實(shí)時(shí)顯示、存儲(chǔ)有約束的長(zhǎng)軌跡平滑曲率。

在出現(xiàn)大量實(shí)驗(yàn)點(diǎn)時(shí)，因?yàn)椴逯岛瘮?shù)的不穩(wěn)定性以及給定點(diǎn)的波動(dòng)性太大，所以利用多項(xiàng)式插值計(jì)算平滑度是沒有意義的。常用的方法（例如樣條法）也不能給出較好的結(jié)果。因此，基于多項(xiàng)式的離散特定函數(shù)對(duì)軌跡平滑性研究有很大的幫助，其系數(shù)是通過平均積分點(diǎn)集的平滑函數(shù)的最小偏差來確定的，該方法稱為最小二乘法。

該文提出了一種改進(jìn)的最小二乘法。改進(jìn)點(diǎn)主要是需要保持軌跡的起點(diǎn)和終點(diǎn)不變。

1 運(yùn)動(dòng)學(xué)分析

沿著離散點(diǎn)和在特定障礙物環(huán)境中，機(jī)器人的運(yùn)動(dòng)方向會(huì)發(fā)生較大的變化。因此，機(jī)器人的動(dòng)態(tài)負(fù)載增加較快，這樣就加快機(jī)械磨損速度，增加了能量消耗。這個(gè)問題的解決方案包括平滑離散軌跡規(guī)劃，以此來減少?gòu)某跏嘉恢玫侥繕?biāo)位置運(yùn)動(dòng)過程中引起機(jī)器人的動(dòng)態(tài)負(fù)載。

構(gòu)建軌跡必須滿足系統(tǒng)的約束，因?yàn)檩喪綑C(jī)器人不能在橫向上運(yùn)動(dòng)或打滑，再加上車輪的旋轉(zhuǎn)角度和速度都是受限的。運(yùn)動(dòng)軌跡曲率不應(yīng)超過其最大值Kmax=1/Rmin，相應(yīng)地，車輪的最小轉(zhuǎn)彎半徑為Rmin。

機(jī)器人的運(yùn)動(dòng)學(xué)模型為：

（1）

其中：

-機(jī)器人的基準(zhǔn)點(diǎn)在坐標(biāo)系xOy中的坐標(biāo)；

-機(jī)器人的速度矢量；

-機(jī)器人的速度矢量與x軸正向之間的角度；

-分別是左、右輪的角速度；

-車輪的半徑。

輸入的數(shù)據(jù)是一個(gè)在三維空間中的初始軌跡的n個(gè)節(jié)點(diǎn)的序列。

（2）

其中，-軌跡點(diǎn)的坐標(biāo)；

-節(jié)點(diǎn)數(shù)。

用一個(gè)序列替換（2）的點(diǎn)序列，進(jìn)行平滑軌跡，

（3）

其中，-平滑軌跡點(diǎn)的坐標(biāo)。

平滑后軌跡的起點(diǎn)和終點(diǎn)保持不變，即滿足條件：

（4）

此外，平滑的軌跡要盡可能接近原來的軌跡路徑，同時(shí)要保證避開障礙物。

2 改進(jìn)的最小二乘法

根據(jù)最小二乘法[10]，為了得到平滑路徑，可以通過多項(xiàng)式Fm（i）得到：

（5）

其中，m-多項(xiàng)式的階數(shù)；

K-多項(xiàng)式系數(shù)的個(gè)數(shù)；

ak-多項(xiàng)式的系數(shù)，可從線性代數(shù)方程組系統(tǒng)（SLAE）得到：

（6）

其中，j-系統(tǒng)方程的數(shù)目。

這里和下面的公式中，坐標(biāo)Y和Z是相同的。

將式（4）代入式（5）中可以得出：

（7）

根據(jù)條件（7），方程式（6）可寫成：

（8）

為了滿足方程式（4），必須要根據(jù)方程（7）替換（8）中的任意兩個(gè)方程。例如，更換方程式（7）中的第M和M-1個(gè)方程，SLAE的形式為：

（9）

SLAE的矩陣形式為：

（10）

其中：—矩陣SLAE；

—自由變量；

—多項(xiàng)式方程（5）中的系數(shù)變量。

偏離初始軌跡的最大允許偏離度不應(yīng)超過，本標(biāo)準(zhǔn)可用于多項(xiàng)式要求度的自動(dòng)選擇。

通過生成和求解系統(tǒng)方程來尋找X，Y和Z的軌跡分量：

（11）

由上述公式可知平滑性軌跡偏離初始軌跡的最大偏差量如下：

1.平滑軌跡被分割成一序列在時(shí)間上均勻地分布的節(jié)點(diǎn)，節(jié)點(diǎn)數(shù)目與初始軌跡節(jié)點(diǎn)數(shù)目相等。

2.搜尋初始軌跡和平滑軌跡節(jié)點(diǎn)之間的最大距離。

該算法的結(jié)果較好，因?yàn)楦鱾€(gè)節(jié)點(diǎn)之間的距離小于初始軌跡節(jié)點(diǎn)到平滑軌跡節(jié)點(diǎn)之間的距離。此外，尋找空間上兩點(diǎn)之間的距離要比從初始軌跡節(jié)點(diǎn)到平滑軌跡節(jié)點(diǎn)的距離更容易、更快，而且這種替代使最大允許偏差的標(biāo)準(zhǔn)更為規(guī)范，并減少了搜索時(shí)間。

3 仿真結(jié)果

通過MATLAB程序進(jìn)行以上算法的數(shù)值仿真，在實(shí)驗(yàn)中，假設(shè)（機(jī)器人）初始軌跡由虛線表示，其坐標(biāo)值在表1中給出。在有障礙物的環(huán)境下運(yùn)動(dòng)時(shí)，將會(huì)導(dǎo)致其在運(yùn)動(dòng)方向上發(fā)生較大變化，進(jìn)而使機(jī)器人驅(qū)動(dòng)單元?jiǎng)討B(tài)負(fù)載過大。

從圖2可以看出平滑計(jì)算需要較少的時(shí)間；從圖3可以看出，改進(jìn)的方法要求平滑前后軌跡的起點(diǎn)和終點(diǎn)保持不變，且平滑性較好。

程序會(huì)自動(dòng)計(jì)算得到，當(dāng)M=6時(shí)的多項(xiàng)式的平滑度最高，進(jìn)而進(jìn)行軌跡規(guī)劃。同時(shí)使得從給定的點(diǎn)（xi，yi）到（xi，y（n）（xi））的距離的平方和最小。

為了確保多項(xiàng)式是獨(dú)一無二的，所以多項(xiàng)式的次數(shù)必須小于給定點(diǎn)的數(shù)量。例如，如果有三個(gè)點(diǎn)，多項(xiàng)式可以是零階或一級(jí)（直線）或拋物線來近似運(yùn)算。此外，拋物線將完全通過已知的三點(diǎn)，即距離的平方和等于零，因?yàn)槎味囗?xiàng)式的三個(gè)系數(shù)可以通過給定的三個(gè)點(diǎn)唯一確定。這并不是數(shù)據(jù)近似求解，而是數(shù)據(jù)插值。

用Matlab進(jìn)行數(shù)據(jù)擬合，在使用最小二乘法進(jìn)行擬合時(shí)，經(jīng)常使用“polyfit（）”函數(shù)。根據(jù)給定的點(diǎn)找出被刪除的多項(xiàng)式曲線圖離給定的距離是多少，在數(shù)據(jù)擬合處理時(shí)，“polyfit（）”函數(shù)需要兩個(gè)輸出參數(shù)，分別與多項(xiàng)式系數(shù)，近似值相關(guān)。

可以從多項(xiàng)式P的系數(shù)可得到，用R，Df和normr表示的S也可以得到。normr是擬合誤差：

（12）

Df：表示了點(diǎn)的數(shù)目和多項(xiàng)式系數(shù)數(shù)目之間的差值。

增加多項(xiàng)式的階數(shù)可以減小誤差，但近似值的準(zhǔn)確度并不會(huì)一直提高。

例如，對(duì)于所給定的數(shù)據(jù)，六階多項(xiàng)式會(huì)產(chǎn)生誤差normr=0，1442。

此外，將沿原軌跡走過的路徑長(zhǎng)度與沿著平滑的軌跡走過的路徑長(zhǎng)度進(jìn)行比較，發(fā)現(xiàn)路徑長(zhǎng)度可減少15%，從而減少運(yùn)動(dòng)時(shí)間，節(jié)約能源。因此，沿著平滑的軌跡移動(dòng)所需要的能量（由實(shí)線表示）要比沿初始軌跡移動(dòng)的能量少得多。

4 結(jié)語

針對(duì)于平滑離散軌跡提出的改進(jìn)最小二乘法，該方法是在MATLAB中實(shí)現(xiàn)的。具有以下優(yōu)點(diǎn)：

1.構(gòu)建平滑運(yùn)動(dòng)路徑，消耗能量較少，運(yùn)動(dòng)路徑的所消耗時(shí)間減少。

2.顯著降低在機(jī)器人作業(yè)時(shí)的動(dòng)態(tài)負(fù)載。

在程序運(yùn)行過程中可知，當(dāng)M=6時(shí)的多項(xiàng)式的平滑度最高，進(jìn)而進(jìn)行軌跡規(guī)劃，六階多項(xiàng)式產(chǎn)生的擬合誤差為normr=0.1442，并縮短路徑長(zhǎng)度15%，這極大地縮短了運(yùn)動(dòng)時(shí)間，減少了能量消耗。

與經(jīng)典的最小二乘法相比，該算法增加了初始軌跡和平滑軌跡二者之間的重合度，節(jié)省計(jì)算機(jī)資源。為機(jī)器人機(jī)械手的工作路徑平滑性研究和軌跡規(guī)劃提供了依據(jù)。

參考文獻(xiàn)

[1] Hauser，K.&V.Ng-Thow-Hing.Fast smoothing of manipulator trajectories using optimal bounded-acceleration shortcuts [J]. In Proceedings of International Conference on Robotics and Automation，2010，12（2）：774-786.

[2] Yang， K. & S. Sukkarieh.3d smooth path planning for a uav in cluttered natural environment [J].In Proceedings of International Conference on Intelligent robots and systems，2008（2）：794-800.

[3] Ratliff， N.，Zucker M. & S. Srinivasa. Chomp：Gradient optimization techniques for efficient motion planning [J].In Proceedings of International Conference on Robotics and Automation，2009（5）：472-476.