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

（安徽工程大學(xué) 機(jī)械工程學(xué)院，安徽蕪湖 241000）

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

1 A＊算法簡(jiǎn)介

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

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

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

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

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

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

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

2.1 電機(jī)效率損失的能量

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

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

2.2 地面起伏時(shí)的勢(shì)能

輪式移動(dòng)機(jī)器人運(yùn)動(dòng)時(shí)，地面是非平整的，會(huì)有爬坡和下坡。下坡時(shí)，小型電機(jī)不能很好的將能量回收，而是以熱量的形式耗散掉，故不考慮下坡時(shí)的能量回收。爬坡時(shí)，能耗為

式中：m為輪式移動(dòng)機(jī)器人的質(zhì)量；g為重力加速度；Δh為當(dāng)前點(diǎn)到下個(gè)點(diǎn)的高度差。

2.3 滾動(dòng)摩擦阻力消耗的能量

輪式機(jī)器人在向目標(biāo)移動(dòng)時(shí)，克服輪胎與地面的摩擦阻力需要消耗能量，則

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

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

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

式中：I為轉(zhuǎn)動(dòng)慣量；ω為機(jī)器人轉(zhuǎn)彎時(shí)的角速度。

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

3 輪式移動(dòng)機(jī)器人能耗最優(yōu)約束下A＊算法的改進(jìn)

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

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

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

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

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

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

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

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

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

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

圖2 ONA*算法流程圖

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

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

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

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

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

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

步驟7 若當(dāng)前節(jié)點(diǎn)n是目標(biāo)節(jié)點(diǎn)（則尋路成功）或open 表已為空表（則尋路失?。惴ńY(jié)束，否則繼續(xù)往下執(zhí)行。

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

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

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

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

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

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

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

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

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

輪式移動(dòng)機(jī)器人在平地上時(shí)，單個(gè)輪子所受到的轉(zhuǎn)矩為

輪式移動(dòng)機(jī)器人在爬坡時(shí)，單個(gè)輪子所受到的轉(zhuǎn)矩為

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

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

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

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

4.2 ONA＊算法實(shí)驗(yàn)結(jié)果分析

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

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

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

表3 三者的實(shí)驗(yàn)結(jié)果對(duì)比

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

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

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

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

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

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

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

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

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

5 結(jié)論

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