基于余弦曲線的機器人移動路徑自主規(guī)劃

李峰，金永喆，朱文慧，李自建

（1.國立群山大學(xué) 機械工程學(xué)院，韓國 群山 54150；2.鄭州經(jīng)貿(mào)學(xué)院 智慧制造學(xué)院，鄭州 451191）

0 引言

余弦曲線是一種三角函數(shù)曲線，曲線如湖面水波，連續(xù)平滑過渡，且可無限延伸。該曲線應(yīng)用比較廣泛，可用來做信號的模擬、模仿物體的簡諧運動等[1]。目前機器人技術(shù)發(fā)展日新月異，機器人在社會上的應(yīng)用也越來越廣泛。機器人路徑自主規(guī)劃是機器人研究的一個重點，它是保證機器人從所在位置到達目標(biāo)位置并完成任務(wù)所必須解決的問題。

雖然現(xiàn)在的路徑規(guī)劃算法很多，如蟻群算法、神經(jīng)網(wǎng)絡(luò)算法、人工勢場法、A*算法等，都能夠較好地完成機器人的路徑規(guī)劃，但這些算法內(nèi)部參數(shù)繁多，且對不同的問題參數(shù)設(shè)置較多，改變較大，大多不能作為獨立的可重復(fù)的子路徑單元重復(fù)使用。它們的智能程度雖高，但作為可重復(fù)執(zhí)行的子路徑單元，就會使計算量增大，計算繁瑣且規(guī)律性欠佳，不易掌握。而子路徑單元如果僅為直線移動，則機器人在規(guī)避危險、穿越障礙等特殊情況下困難重重。為此，中國海洋大學(xué)的宋大雷等[2]提出了橢圓曲線路徑規(guī)劃算法；華北電力大學(xué)計算機科學(xué)與技術(shù)學(xué)院的劉春陽等[3]提出了Bezier曲線模型；李峰等[4]提出了可以用彈道曲線來作為機器人航道曲線；許哲、朱海洋、王慶誠等[5]提出了S型速度曲線路徑平滑過渡算法。另外，陳丹[6]、張玲[7]、范程華等[8]、李亮玉等[9]、姚學(xué)峰等[10]也分別用B樣條曲線、三次樣條曲線、NURBS曲線等設(shè)計了機器人的路徑軌跡算法。以上算法雖可以完成機器人點對點的路徑規(guī)劃，但由于曲線本身的特點，大多不可在保持形狀的前提下無限延伸，導(dǎo)致兩點距離過大時機器人點對點規(guī)劃路徑被過度放大，機器人從一點到另一點無效路徑過長，路徑規(guī)劃效果較差，為此需要尋找一種既可以曲線平滑點對點的子路徑規(guī)劃方法。機器人通過路徑規(guī)劃，既可以到達目標(biāo)位置，又可以應(yīng)對突發(fā)情況，規(guī)避危險，而且路徑相對變化較小。

正常情況下，機器人路徑單元規(guī)劃大多為直線，即從A點到B點路徑為一條直線段。但在特殊情況下，需要規(guī)避風(fēng)險，穿越障礙物時就不能完成任務(wù)了。任意兩點之間余弦曲線連接的難點不僅在于余弦曲線的周期性波動和峰值變化，還要保證曲線不同方向的延伸，本文利用MATLAB軟件進行參數(shù)設(shè)定、方程求解、路徑規(guī)劃、曲線繪制，將機器人移動路徑設(shè)計為余弦曲線，機器人沿著余弦曲線一步一步地移動，從所在位置沿著余弦曲線軌跡到達目標(biāo)位置，完成預(yù)定任務(wù)。余弦曲線的連續(xù)、平滑且無窮延伸的特性不僅適合于機器人的行走，而且可保證機器人能夠從初始位置到達目標(biāo)位置，完成設(shè)定任務(wù)。

1 余弦曲線模型

1.1 余弦曲線變換模型

如圖1 所示，機器人余弦曲線變換模型中，curve1為初始位置余弦曲線，curve2為余弦曲線平移后的曲線，curve3為curve2 旋轉(zhuǎn)后的曲線。

圖1 余弦曲線變換模型

平面內(nèi)任意兩點之間確定余弦曲線的過程較為繁瑣，可通過將curve1平移為curve2，然后curve2繞初始點旋轉(zhuǎn)某一角度的方法得到curve3，讓curve3通過初始、終點坐標(biāo)位置，整個過程如圖1所示。從圖1中可以看出，curve1、curve2、curve3均為余弦曲線，且連續(xù)光滑，因只取曲線的一部分，因此未做無限延伸處理。如圖2所示，在機器人的余弦曲線路徑規(guī)劃出后，機器人可以從初始位置到達目標(biāo)位置，一步一步地按照規(guī)劃路徑行進。行進過程中按照規(guī)劃路徑位姿，即可到達目標(biāo)位置。余弦方程的簡潔特點既保證了計算量的降低，又保證了路徑的連續(xù)、平滑、可行。

圖2 余弦曲線運動模型

1.2 余弦曲線路徑規(guī)劃算法設(shè)計

在確定好余弦曲線規(guī)劃方法后，對余弦曲線進行詳細(xì)分析，研究曲線參數(shù)，假定初始坐標(biāo)為（x1，y1），目標(biāo)坐標(biāo)為（x2，y2），求解其他相關(guān)核心參數(shù)，以便確定曲線，求得最終規(guī)劃路徑余弦曲線。

余弦曲線的基本方程為

為方便計算機分析計算，將方程參數(shù)化，可得：

即

曲線可以先平移、后旋轉(zhuǎn)，直至通過已知點（x1，y1）、（x2，y2），而曲線平移、旋轉(zhuǎn)后的參數(shù)方程矩陣為

即：

但由于方程中x、y、θ、t、m、n未知數(shù)為5個，而已知條件只有兩點坐標(biāo)，所以未知參數(shù)在現(xiàn)有條件下不可解，需要創(chuàng)造新條件。

為此，令過（x1，y1）、（x2，y2）兩點直線與X軸夾角為基本余弦曲線旋轉(zhuǎn)角度，則有

為了使得曲線既通過（x1，y1）、（x2，y2）兩點，又能夠圓滑連接，令（x1，y1）為基本余弦曲線旋轉(zhuǎn)、平移后初始位置，而基本余弦曲線旋轉(zhuǎn)平移后可從（x1，y1）開始無限延伸，（x1，y1）為余弦曲線第一個（-π/2，π/2）區(qū)間，則易得：

設(shè)定參數(shù)t區(qū)間為（-0.5π，zπ-0.5π），其中：

經(jīng)過前面分析計算，分別求出θ、t、m、n，利用式（5）可得經(jīng)過任意（x1，y1）、（x2，y2）兩點余弦曲線。

1.3 機器人余弦曲線路徑規(guī)劃

要將余弦曲線應(yīng)用于機器人路徑規(guī)劃，還要針對機器人的運動特點進行分析。針對機器人運動特點，在機器人行進過程中還要實時調(diào)整機器人的姿態(tài)ρi：

式中：ρi為機器人在（xi，yi）點的方向矢量，ui、vi分別為x、y方向的單位方向矢量，li為各段距離。

如圖3所示，根據(jù)式（5），機器人可從A點一步一步地沿坐標(biāo)位置逐步調(diào)整姿態(tài)，依次以直線段方式經(jīng)過C、D等各點，到達目標(biāo)位置B。即將從A到B的余弦曲線首先離散化設(shè)計，分為若干段（如CD段），然后完成各段，各段綜合逼近余弦曲線路徑即可完成路徑規(guī)劃。

圖3 機器人余弦線路徑位姿規(guī)劃模型

2 試驗參數(shù)

為了驗證路徑規(guī)劃方法的正確性、合理性和有效性，利用MATLAB進行試驗分析。MATLAB是一款進行數(shù)據(jù)分析與圖像處理的軟件，可以進行計算機視覺、信號處理及機器人研究、控制分析，是美國研發(fā)的一款具有強大功能的數(shù)學(xué)工具[11]。試驗參數(shù)如表1所示。

表1 余弦曲線路徑規(guī)劃初始參數(shù) m

根據(jù)表1中試驗參數(shù)，輸入機器人余弦曲線路徑初始位置和目標(biāo)位置坐標(biāo)，確定差分離散區(qū)間后，根據(jù)式（1）～式（8），依次生成曲線，并產(chǎn)生中間差分點坐標(biāo)。

3 仿真試驗

圖4～圖9分別為表1中曲線1～曲線6，6條曲線過渡平滑，曲線1和曲線4、曲線6由于兩點距離較近，為小于1個周期的余弦曲線，曲線3、曲線5稍遠(yuǎn)，為1個周期左右的余弦曲線，曲線2因初始坐標(biāo)位置和目標(biāo)坐標(biāo)位置較遠(yuǎn)，為多個周期余弦曲線，雖然較遠(yuǎn)，但從圖5明顯可以看出，其路徑始終圍繞初始坐標(biāo)位置到目標(biāo)坐標(biāo)位置的直線移動，達到預(yù)期目標(biāo)。

圖4 曲線1

圖1～圖6中根據(jù)離散化設(shè)計，從初始坐標(biāo)到目標(biāo)坐標(biāo)已離散成了若干份，可根據(jù)機器人單信號行走距離確定該離散區(qū)間。從圖4～圖9可以看出，本文設(shè)計的余弦曲線路徑規(guī)劃可以實現(xiàn)任意兩點的機器人移動路徑規(guī)劃，根據(jù)本文設(shè)計方法，在輸入首尾坐標(biāo)及差分區(qū)間后，機器人余弦曲線路徑將自動規(guī)劃生成。

圖6 曲線3

圖7 曲線4

圖8 曲線5

圖9 曲線6

從表2參數(shù)可以看出，曲線2兩點距離最長，單位置相對誤差最小，曲線4兩點距離最小，單位置相對誤差并非最大，但在離散區(qū)間同為0.5的曲線內(nèi)為相對誤差最大。同樣地，曲線5比曲線6距離遠(yuǎn)，但相對誤差小。分析原因為：使用該路徑規(guī)劃方法時，在離散區(qū)間相同的情況下，由于余弦曲線的連續(xù)重復(fù)循環(huán)性，曲線整體變化規(guī)律不因曲線的長短而變化，因此曲線越長，相對誤差越小，而這也是在機器人控制過程中期望的。

表2 余弦曲線路徑規(guī)劃試驗結(jié)果m

4 機器人余弦曲線移動軌跡生成試驗

4.1 試驗平臺

試驗所用機器人為九自由度人形機器人（如圖10）。九自由度人形機器人是一種比較特殊的機器人，它可以多種方式模仿人類運行。人形機器人憑借9個自由度可以向各個方向移動，比傳統(tǒng)輪式機器人設(shè)計更能夠自然地在環(huán)境中移動。試驗平臺為課題研發(fā)的人形機器人北斗導(dǎo)航試驗系統(tǒng)，該系統(tǒng)主要由機器人余弦曲線運動軌跡規(guī)劃模塊、人形機器人基本運動軌跡生成模塊及無線控制模塊組成[12]。試驗場景如圖11所示。

圖10 試驗用機器人

圖11 試驗現(xiàn)場

4.2 試驗參數(shù)

為了對機器人余弦曲線路徑運動規(guī)劃方法進行合理性驗證，本課題組做了機器人余弦曲線路徑運動控制試驗。試驗主要參數(shù)如表3所示。從表3中可以看出，系統(tǒng)主要參數(shù)包括初始、目標(biāo)坐標(biāo)和九自由度人形機器人基本試驗運動參數(shù)。

表3 機器人余弦曲線路徑運動控制主要試驗參數(shù)

4.3 試驗結(jié)果及分析

圖12中兩條路徑分別為機器人余弦曲線運動控制試驗規(guī)劃路徑及試驗路徑。從圖12中可以看出，人形機器人余弦曲線規(guī)劃和試驗路徑接近，這說明人形機器人余弦曲線規(guī)劃算法可以達到對人形機器人進行余弦曲線控制的目的。從試驗路徑看出，雖然人形機器人在初期位移變化較大，但由于余弦曲線路徑規(guī)劃算法的設(shè)計，人形機器人逐漸逼近規(guī)劃路徑，實現(xiàn)了算法的預(yù)期目的。在整個人形機器人運動過程中，在機器人姿態(tài)變化較大處，位移較大，這是因為人形機器人本身每控制信號旋轉(zhuǎn)角度和每控制信號移動位移有一定的調(diào)整度，另外，由于試驗地面光滑程度的不同，導(dǎo)致與控制規(guī)劃路徑有一定的精度誤差。從整體上看，余弦曲線的機器人移動路徑自主規(guī)劃算法實現(xiàn)了預(yù)期目的，達到了對人形機器人進行余弦曲線運動控制的目的。

圖12 機器人余弦曲線運動控制試驗規(guī)劃路徑及試驗路徑

5 結(jié)論

人形機器人運動控制是今后機器人技術(shù)發(fā)展的主要研究方向。為提高機器人路徑規(guī)劃自主程度，針對機器人遠(yuǎn)距離運動時避障和規(guī)避危險問題，提出利用余弦曲線來進行機器人運動路徑規(guī)劃的方法。通過仿真與試驗研究，可以得出以下結(jié)論：1）通過分析，建立了余弦曲線變換模型和余弦曲線運動模型，進行了余弦曲線路徑規(guī)劃算法設(shè)計和機器人余弦曲線路徑規(guī)劃；2）通過不同坐標(biāo)位置、參數(shù)的Matlab仿真試驗表明，本文建立的數(shù)學(xué)模型是正確的，提出的機器人余弦曲線移動路徑規(guī)劃方法是合理的；3）在提出的余弦曲線移動路徑自主規(guī)劃方法時，在差分區(qū)間相同的情況下，其設(shè)計的運動精度將隨著兩點距離的增大而提高；4）利用該余弦曲線移動路徑規(guī)劃方法可節(jié)省規(guī)劃時間，提高規(guī)劃效率，為后續(xù)機器人技術(shù)的發(fā)展打下良好基礎(chǔ)。