能耗最優(yōu)下輪式移動機器人作業(yè)路徑規(guī)劃的最優(yōu)節(jié)點A＊算法

江磊，賈文友，劉莉，梁利東，魏文濤

（安徽工程大學 機械工程學院，安徽蕪湖 241000）

隨著科技的發(fā)展，機器人的應(yīng)用也越來越普遍，其中，輪式機器人[1-3]是目前科學技術(shù)發(fā)展最活躍的領(lǐng)域之一[4]，國內(nèi)關(guān)于輪式移動機器人路徑規(guī)劃方法的文獻目前仍處于增長的狀態(tài)[5]。任建華等分別從傳統(tǒng)路徑規(guī)劃和現(xiàn)代路徑規(guī)劃兩個方面對路徑規(guī)劃方法展開了研究，以A*算法和蟻群算法為例進行了仿真實驗[6]；關(guān)泉珍等提出采用高精度地圖構(gòu)建技術(shù)還原路況信息，結(jié)合A*算法，在標記為有障礙的柵格模型中，為機器人尋找一條恰當?shù)膹钠鹗键c到目標點的運動路徑[7]，但是從仿真結(jié)果看出，該路徑可進行進一步的優(yōu)化；康靖等提出了一種改進的蟻群算法，為輪式機器人在復(fù)雜地形中探索并規(guī)劃出一條最省能量的行走路線[8]；劉貴杰等通過建立AUV 運動的速度模型和能耗模型獲得AUV 規(guī)劃路徑的總能耗，并采用蟻群算法，對降低水下機器人能耗、提高續(xù)航能力有一定的優(yōu)勢[9]；張浩杰等兼顧考慮機器人對降低能耗和路徑規(guī)劃效率兩方面的需求，提出了一種基于改進AD*算法的移動機器人低能耗最優(yōu)路徑規(guī)劃方法[10]；Zhang 等針對差分轉(zhuǎn)向，阿克曼轉(zhuǎn)向，滑移轉(zhuǎn)向和全向轉(zhuǎn)向輪式機器人的節(jié)能運動規(guī)劃問題進行了綜述研究[11]；Krimsky 等介紹了能量回收執(zhí)行器，使用一系列彈簧和離合器來捕獲并帶回與電動機并聯(lián)的彈性勢能，以達到減少能量消耗的目的，然而僅在特定的情況下才可節(jié)能[12]；Datouo 等將新的代價函數(shù)和啟發(fā)式函數(shù)集成到傳統(tǒng)的A*算法中，考慮了地面摩擦、轉(zhuǎn)彎、速度及功率造成的能耗，但是并未考慮到勢能變化造成的能耗[13]；Mauricio 等提出了一種兩輪差速驅(qū)動移動機器人的功率模型，考慮了機器人及其電機的動力學參數(shù)，預(yù)測了不同加速度和不同有效載荷下的運動軌跡的能量消耗[14]。綜上，基于電機效率、地面摩擦、地面起伏、速度變化、轉(zhuǎn)彎等多因素，構(gòu)建能耗模型，利用改變搜索方式，提出能耗最優(yōu)下輪式移動機器人避障路徑規(guī)劃的最優(yōu)節(jié)點A*算法（簡稱ONA*），實現(xiàn)輪式移動機器人在單位能耗最優(yōu)約束下以更少的時間規(guī)劃出理想作業(yè)路徑。

1 A＊算法簡介

A*算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，是一種典型的啟發(fā)式搜索算法，其對每一個搜索的節(jié)點進行評估，得到最好的節(jié)點，再從這個節(jié)點進行搜索，直至搜索到目標。對于評估節(jié)點n的估價函數(shù)f（n）為

式中：f（n）為從起始節(jié)點到n節(jié)點的實際代價；g（n）為從n節(jié)點到目標節(jié)點的預(yù)估代價。

A*算法在搜索過程中，擁有open 表和close 表，其中open 表中存儲待被搜索的節(jié)點，close 表中存儲已被搜索過的節(jié)點。g（n）表示搜索的廣度的優(yōu)先趨勢，計算當前點周圍相鄰的各個n節(jié)點（共8 個）的實際代價，在每次計算之后記錄n節(jié)點的父節(jié)點；h（n）是在對一個節(jié)點估價時的約束條件，如果約束條件越多，則排除的節(jié)點越多，估價函數(shù)越好，如果h（n）精確地等于從n移動到目標的代價，則A*將會僅僅尋找最佳路徑而不擴展別的任何節(jié)點，則運行最快；f（n）表示評估節(jié)點n的重要程度，在open 表中所有已評估過的節(jié)點中，最優(yōu)的節(jié)點將被下次搜索選中作為當前點，然后從當前點繼續(xù)搜索，直至目標節(jié)點。在搜索到目標點后，回溯查找其父節(jié)點、父節(jié)點的父節(jié)點，直至起點，即為最優(yōu)路線。

2 輪式移動機器人能耗模型構(gòu)建

輪式移動機器人在作業(yè)過程中，能耗受約于多因素，主要包括電機效率損失的能量（記為Ep）、地面起伏時的勢能（記為Eh）、滾動摩擦阻力消耗的能量（記為Ef）、速度變化和轉(zhuǎn)彎消耗的能量（記為Ek）等方面。本文采用兩輪驅(qū)動輪式機器人，圖1 是機器人勻速轉(zhuǎn)彎時的示意圖。

圖1 輪式機器人轉(zhuǎn)彎示意圖

式中：r為機器人驅(qū)動輪半徑；B為機器人左右兩側(cè)輪距。

2.1 電機效率損失的能量

電機對外做功時，會有轉(zhuǎn)化效率問題，其對外輸出并不能百分百轉(zhuǎn)化出去，效率而損失的能量，計算公式為

式中：η為電機效率；P為電機功率；Scn為從n節(jié)點的父節(jié)點到n節(jié)點之間的路程；vn為機器人在n節(jié)點平移的速度；vn?1為機器人在n節(jié)點的父節(jié)點平移的速度。

2.2 地面起伏時的勢能

輪式移動機器人運動時，地面是非平整的，會有爬坡和下坡。下坡時，小型電機不能很好的將能量回收，而是以熱量的形式耗散掉，故不考慮下坡時的能量回收。爬坡時，能耗為

式中：m為輪式移動機器人的質(zhì)量；g為重力加速度；Δh為當前點到下個點的高度差。

2.3 滾動摩擦阻力消耗的能量

輪式機器人在向目標移動時，克服輪胎與地面的摩擦阻力需要消耗能量，則

式中：μ為滾動摩擦因數(shù)；θ為坡道與水平面之間的夾角。

2.4 速度變化和轉(zhuǎn)彎消耗的能量

加速時，需要電機對外做功；減速時，能量轉(zhuǎn)化為熱能消耗掉，轉(zhuǎn)彎同樣要消耗能量，計算公式為

式中：I為轉(zhuǎn)動慣量；ω為機器人轉(zhuǎn)彎時的角速度。

根據(jù)式（3）～ 式（6），輪式移動機器人在運動過程中的總能耗記為E，構(gòu)建能耗模型為

3 輪式移動機器人能耗最優(yōu)約束下A＊算法的改進

3.1 節(jié)點的估價函數(shù)推導(dǎo)

基于A*算法的能耗最優(yōu)路徑規(guī)劃的路徑搜索過程中，節(jié)點的評估函數(shù)f（n）是關(guān)鍵。根據(jù)輪式移動機器人在運動過程中的總能耗式（6），其為節(jié)點的實際代價，即

對于n(x1,y1,z1)節(jié)點和目標節(jié)點ngoal(x2,y2,z2)，兩者之間的預(yù)估距離采用對角線距離，即S（n），則：

式中Sdiagnol（n）和Sstraight（n）分別表示沿著對角線移動的距離和曼哈頓距離；非爬山路況中，因路線往豎直方向變動的趨勢很小甚至沒有，故Sdiagnol（n）和Sstraight（n）不計入豎直方向上的差，式（9）和（10）分別簡化為：

則采用A*算法時，在能耗最優(yōu)路徑規(guī)劃下的預(yù)估代價h（n）為

綜上，能耗最優(yōu)下輪式移動機器人路徑規(guī)劃的最優(yōu)節(jié)點 A*算法，由式（1）得能耗最優(yōu)策略下節(jié)點的估價函數(shù)f（n）為

3.2 能耗最優(yōu)下輪式移動機器人作業(yè)路徑規(guī)劃的最優(yōu)節(jié)點A＊算法（ONA＊）

在搜索過程中，當前節(jié)點放入close 表中，并把當前節(jié)點的8 個鄰節(jié)點中未被放入open 表中可通行的節(jié)點放入open 表中；在下次循環(huán)中，再在open 表中搜索到f（n）值最小的節(jié)點作為當前節(jié)點，再從該節(jié)點進行搜索。當輪式機器人作業(yè)的地圖較大，從搜索過程需要花費較長時間。

為此，提出一種新的搜索最優(yōu)點方式，在把當前節(jié)點的8 個鄰節(jié)點中未被放入open 表中的可通行節(jié)點放入open 表后，再將其放入每次尋找新的當前節(jié)點后都會被清空的cacu 表中，同時把已在open 表中，但g（n）值更低的節(jié)點也放入cacu 表中，再從cacu 表中找到f（n）值最小的節(jié)點作為預(yù)選節(jié)點np。判斷該預(yù)選節(jié)點是否在障礙范圍內(nèi)，若在，則找出障礙范圍內(nèi)f（n）值最小的節(jié)點與np的f（n）值相比，較小者作為當前節(jié)點；若不在，預(yù)選節(jié)點np直接作為當前節(jié)點，從而避免局部最優(yōu)的情況出現(xiàn)。若cacu 表為空，再在open 表中找f值最小的節(jié)點作為當前節(jié)點。在此情形下，大多數(shù)是最多會從8 個點中找到最優(yōu)節(jié)點，而不是從open 表中找到最優(yōu)節(jié)點，從而提高算法的運行速度。

綜上，ONA*算法流程圖如圖2 所示。

圖2 ONA*算法流程圖

步驟1 如果是首次循環(huán)，則起點作為當前節(jié)點n，跳轉(zhuǎn)至步驟五，否則繼續(xù)往下執(zhí)行。

步驟2 如果cacu 表是空表，從open 表找到離目標點最近的f（n）值最小的節(jié)點作為當前節(jié)點n，跳轉(zhuǎn)至步驟5，否則繼續(xù)往下執(zhí)行。

步驟3 找到cacu 表中f（n）值最小的節(jié)點，將該最小節(jié)點作為預(yù)選節(jié)點np。

步驟4 判斷預(yù)選節(jié)點np是否在障礙范圍內(nèi)，若在，找出障礙范圍內(nèi)f（n）值最小的節(jié)點n' p，與np的f（n）值比較，若小于np的f值，將n' p作為當前節(jié)點n，否則將np作為當前節(jié)點n；若不在，則np作為當前節(jié)點n。

步驟5 將當前節(jié)點n放 入close 表中，之后的循環(huán)中，該節(jié)點將不會再被當作當前節(jié)點。

步驟6 設(shè)置空表cacu 表。

步驟7 若當前節(jié)點n是目標節(jié)點（則尋路成功）或open 表已為空表（則尋路失敗），算法結(jié)束，否則繼續(xù)往下執(zhí)行。

步驟8 搜尋點n的8 個鄰節(jié)點，如有不可越過的節(jié)點，放入close 表中。

步驟9 如果某鄰節(jié)點，既沒有在open 表，也不在close 表中，將其放入open 表，計算出該鄰節(jié)點的f（n）值，并設(shè)父節(jié)點為n。如果已在open 表中，判斷其g（n）值是否更小，若是，則用更小的g（n）值替換原來的g（n）值，同時在預(yù)估代價值h（n）前引入比例因子α，重新計算f（n）值，并設(shè)父節(jié)點為n，否則查看其它鄰節(jié)點，將f（n）值更新的鄰節(jié)點放入cacu 表中。

步驟10 所有鄰節(jié)點查看完畢后，跳回步驟2。

4 仿真實驗驗證及應(yīng)用案例

4.1 輪式移動機器人相關(guān)參數(shù)確定及作業(yè)環(huán)境地圖搭建

為了驗證上述提出ONA*算法的有效性，采用MATLAB R2016a 進行尋路的仿真驗證，仿真所用計算機為Lenovo R7000，CPU 型號為AMD Ryzen 7 4800 H with Radon Graphics，主頻為2.90 GHz。輪式移動機器人的相關(guān)參數(shù)如表1 所示,仿真實驗關(guān)鍵參數(shù)如表2 所示，其余未給出具體數(shù)值的參數(shù)，在移動機器人移動過程中會有變化，需由已給出的參數(shù)實時計算。

表1 輪式移動機器人相關(guān)參數(shù)

表2 仿真實驗關(guān)鍵參數(shù)

當再次計算某個節(jié)點時，預(yù)估代價值h（n）前引入比例因子α確定，經(jīng)實驗驗證，取值α 較小時，仿真速度慢；α較大時，仿真誤差大。本驗證實驗取α=10。

輪式移動機器人在平地上時，單個輪子所受到的轉(zhuǎn)矩為

輪式移動機器人在爬坡時，單個輪子所受到的轉(zhuǎn)矩為

由式（16）和式（17）可推出輪式移動機器人在平地上和在爬坡時的功率關(guān)系為

在轉(zhuǎn)向時，電機需要消耗額外的功率，參考康亞彪[15]對野外無人車轉(zhuǎn)向的研究，轉(zhuǎn)向時機器人單個輪子的功率表示為

輪式移動機器人作業(yè)環(huán)境地圖平面尺寸為192 × 192，具有凹凸不平整特性，不可越過的區(qū)域設(shè)為紅色；設(shè)置起點坐標為（3，3，0.2），終點坐標為（183，168，0.2），MATLAB 生成的具體地圖如圖3 所示。

圖3 尺寸為192 × 192 的輪式移動機器人作業(yè)環(huán)境地圖

4.2 ONA＊算法實驗結(jié)果分析

ONA*算法以能耗最優(yōu)為目標，距離是其中一項重要參數(shù)，為此對比能耗最優(yōu)約束下A*算法（A*algorithm with optimal energy，OEA*）和距離成本最優(yōu)約束下A*算法（A* algorithm with optimal distance，ODA*），采用相同的輪式移動機器人的相關(guān)參數(shù)和仿真實驗參數(shù)在MATLAB 中進行規(guī)劃尋路，仿真結(jié)果分別如圖4～圖6 所示。

圖4 ONA*算法下輪式移動機器人作業(yè)規(guī)劃尋路仿真結(jié)果

根據(jù)圖4～圖6 的仿真結(jié)果，從路徑長度、能量消耗、單位距離能耗和算法尋路時間等進行對比分析，具體結(jié)果如表3 所示。

表3 三者的實驗結(jié)果對比

對比ONA*算法和OEA*算法，由圖4、圖5 和表3 可知，ONA*算法和OEA*算法所尋路徑長度基本一致，但ONA*算法所尋路徑轉(zhuǎn)彎次數(shù)有所減少，且單位距離能耗降低了近6.4%，算法尋路時間降低了86.9%；對比ONA*算法和ODA*算法，由圖5、圖6 和表3 可知，ONA*算法能避開ODA*算法不必要的爬坡，ONA*算法中單位距離能耗降低了17.5%，尋路時間也降低了30.4%?？梢?，ONA*算法具有良好綜合效果。

圖5 OEA*算法下輪式移動機器人作業(yè)規(guī)劃尋路仿真結(jié)果

圖6 ODA*算法下輪式移動機器人作業(yè)規(guī)劃尋路仿真

4.3 ONA＊算法應(yīng)用案例

ONA*算法既能夠適應(yīng)于不同廣域的作業(yè)環(huán)境地圖案例，平面尺寸可從32 × 32 小地圖（如圖7 所示）到192 × 192 大地圖（如圖8 和圖9 所示）；又能機器人從作業(yè)環(huán)境地圖不同方向進行作業(yè)尋路案例，可從作業(yè)環(huán)境地圖的ONA*算法下輪式移動機器人在小地圖中作業(yè)避障尋路應(yīng)用尋路（如圖7 所示）或從作業(yè)環(huán)境地圖的左下角到右上角尋路（如圖8 和圖9 所示）。

圖7 ONA*算法下輪式移動機器人在小地圖中作業(yè)避障尋路應(yīng)用

圖8 ONA*算法下輪式移動機器人起點在左上角的大地圖中作業(yè)避障尋路應(yīng)用

圖9 ONA*算法下輪式移動機器人起點在右上角的大地圖中作業(yè)避障尋路應(yīng)用

由圖7～圖9 可得出，ONA*算法在尋路過程中，能夠在多種避開不必要的爬坡同時，選擇一條理想的路，進一步驗證了ONA*算法有效性。

5 結(jié)論

因輪式移動機器人每次作業(yè)攜帶的能源有限，提出了能耗最優(yōu)下輪式移動機器人避障路徑規(guī)劃的最優(yōu)節(jié)點A*算法（ONA*），通過與能耗最優(yōu)約束下OEA*算法和距離成本最約束下ODA*實驗驗證對比，ONA*算法不論在單位距離能耗，還是在算法作業(yè)規(guī)劃尋路時間，都有較大縮短，綜合效果好；ONA*算法適用案例既不受約于作業(yè)環(huán)境地圖規(guī)模大小、障礙密集程度，又不受約于機器人作業(yè)方向，能為輪式移動機器人規(guī)劃出一條單位距離能耗優(yōu)的理想作業(yè)路徑。