未知環(huán)境中考慮姿態(tài)穩(wěn)定性的自主機(jī)器人路徑規(guī)劃

王東署，朱訓(xùn)林

(1.鄭州大學(xué) 電氣工程學(xué)院，河南 鄭州450001;2.鄭州大學(xué) 數(shù) 學(xué)系，河南 鄭 州450001)

0 引言

隨著自主移動機(jī)器人在人類生產(chǎn)生活中的應(yīng)用越來越廣泛，其工作環(huán)境變得越來越多樣化，更多地出現(xiàn)在室外未知的復(fù)雜環(huán)境中.這些未知復(fù)雜環(huán)境通常是人類難以達(dá)到的或非常危險(xiǎn)的區(qū)域，如星際探索、地下資源勘探以及地震、核事故或生化襲擊發(fā)生后的救災(zāi)場所.這些危險(xiǎn)區(qū)域促使機(jī)器人研究人員將研究重心轉(zhuǎn)向復(fù)雜環(huán)境中真實(shí)世界的挑戰(zhàn)，遙操作自主機(jī)器人在這些環(huán)境的路徑規(guī)劃技術(shù)就成為主要的研究熱點(diǎn)之一.

在很多未知復(fù)雜的應(yīng)用環(huán)境中，傳統(tǒng)的機(jī)器人路徑規(guī)劃方法是規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的、避開高梯度區(qū)域的無碰最短路徑［1］.然而經(jīng)驗(yàn)豐富的遙操作人員認(rèn)為這樣規(guī)劃出來的路徑并不合理，因?yàn)闄C(jī)器人在這種路徑上運(yùn)動時(shí)要不斷調(diào)整運(yùn)動方向，降低了運(yùn)動的安全性，增加了打滑或陷入某個陷阱的幾率，甚至可能造成側(cè)翻，最終導(dǎo)致機(jī)器人損壞和整個探索任務(wù)的失?。?］.

因此，路徑規(guī)劃中僅僅考慮無碰的安全最短路徑是不夠的，還應(yīng)考慮機(jī)器人自身運(yùn)動的平穩(wěn)性.國內(nèi)外諸多學(xué)者在此方面進(jìn)行了一定的探索［2-6］，如王鴻鵬等［2］以輪式滑動轉(zhuǎn)向移動機(jī)器人為研究對象，建立了運(yùn)動打滑模型，討論了打滑因素對移動機(jī)器人運(yùn)動平穩(wěn)性的影響.

在移動機(jī)器人路徑規(guī)劃的指標(biāo)函數(shù)中，Rowe等［7］提出了復(fù)雜環(huán)境中以能量損耗為評價(jià)指標(biāo)的路徑規(guī)劃算法，此后 Sun等［8］對其進(jìn)行了推廣，霍煒等［9］對Rowe等提出的方法進(jìn)一步推廣至平面區(qū)域中機(jī)器人的路徑規(guī)劃.此后，許多學(xué)者開始在路徑規(guī)劃中引入能量指標(biāo)，結(jié)合其他智能優(yōu)化算法，如人工勢場法［10］、遺傳算法［11］、蟻群-遺傳算法［12］等規(guī)劃出了合理的路徑.

筆者在前述研究的基礎(chǔ)上，針對自主移動機(jī)器人在室外未知復(fù)雜環(huán)境中的路徑規(guī)劃問題，提出了一種新的規(guī)劃思想:①該思想吸收了自主機(jī)器人遙操作人員的控制思想，在進(jìn)行路徑規(guī)劃時(shí)除考慮路徑因素外，還同時(shí)考慮了機(jī)器人側(cè)翻和打滑對規(guī)劃結(jié)果的影響;②遇到障礙物時(shí)，并非全部采取繞行的避障策略，在面對坡度較小的障礙物時(shí)，直接從障礙物上翻越過去，面對坡度較大的障礙物時(shí)才采取繞行的策略;③把備選路徑運(yùn)行時(shí)消耗的能量也作為評價(jià)路徑的性能指標(biāo)之一，規(guī)劃出最優(yōu)的無碰路徑.通過與傳統(tǒng)路徑規(guī)劃方法進(jìn)行比較，仿真驗(yàn)證了筆者方法的有效性.

1 路徑規(guī)劃方法

設(shè)自主機(jī)器人運(yùn)行在室外未知的復(fù)雜環(huán)境中，利用三維激光傳感器獲得周圍環(huán)境的信息.路徑規(guī)劃方法由以下3步組成.

(1)梯度計(jì)算.利用3維激光傳感器采集環(huán)境信息，作為數(shù)字高程圖的信息來源.數(shù)字高程圖中的某個網(wǎng)格用Gheight(xn，yn)表示，xn和yn是決定網(wǎng)格在x-y坐標(biāo)平面中位置的參數(shù)，Gheight(xn，yn)表示網(wǎng)格所包含區(qū)域的最大高度，具體含義如圖1所示.經(jīng)過對采集到的數(shù)據(jù)進(jìn)行插值處理后，可以得到數(shù)字高程圖，用Matlab軟件仿真后，可以得到如圖2所示的虛擬數(shù)字高程圖.

圖1 數(shù)字高程圖的定義Fig.1 Definition of digital heightmap

圖2 虛擬的數(shù)字高程圖Fig.2 Virtual digital heightmap

在得到區(qū)域環(huán)境的數(shù)據(jù)信息圖后，對得到的數(shù)據(jù)組做進(jìn)一步的處理，對每個網(wǎng)格中的所有網(wǎng)格的高程值進(jìn)行重新賦值計(jì)算，采取的方法是:分別計(jì)算當(dāng)前網(wǎng)格的高程值和與其相鄰的其他網(wǎng)格的高程值之差的絕對值的最大值賦值給當(dāng)前網(wǎng)格，如圖3所示，對于中間的網(wǎng)格，如網(wǎng)格B，會有8個相鄰的網(wǎng)格，而對于邊界上的非頂點(diǎn)網(wǎng)格(如網(wǎng)格A3)有5個相鄰網(wǎng)格，4個頂點(diǎn)的網(wǎng)格(如網(wǎng)格A)則只有3個相鄰的網(wǎng)格.按照這種操作對每個網(wǎng)格進(jìn)行相應(yīng)的賦值計(jì)算，把數(shù)字高程圖上的每個網(wǎng)格的梯度全部計(jì)算一遍，這樣就形成了一個新的基于網(wǎng)格的地圖，即數(shù)字梯度圖，圖2中的數(shù)字高程圖所對應(yīng)的數(shù)字梯度圖如圖4所示，其上的每個網(wǎng)格用Ggrad(xn，yn)表示.

圖3 數(shù)據(jù)轉(zhuǎn)換的三種情況Fig.3 Three cases of data transform

(2)節(jié)點(diǎn)探測.所謂節(jié)點(diǎn)是指在復(fù)雜環(huán)境中人為定義的一個特殊區(qū)域，在這個區(qū)域中機(jī)器人很容易就可以翻越過去，并可以在該區(qū)域做無側(cè)翻的轉(zhuǎn)動操作.

圖4 數(shù)字梯度圖示意圖Fig.4 Digital gradientm ap

在小節(jié)(1)中已經(jīng)建立了DGM圖，按照一定的評價(jià)規(guī)則從中選出環(huán)境中的平坦區(qū)域，這里設(shè)定一個閾值Hthreshold用來探測機(jī)器人是否位于平坦區(qū)域.如果DGM圖中的某一個網(wǎng)格的梯度值Ggrad(xn，yn)≥ Hthreshold的值， 那 么該 網(wǎng) 格Ggrad(xn，yn)就被認(rèn)為位于平坦區(qū)域.相應(yīng)的把平坦區(qū)域的中心叫做節(jié)點(diǎn)，并用它來表征平坦區(qū)域，如圖5所示.按照這樣的甄選規(guī)則對整個環(huán)境區(qū)域進(jìn)行遴選，整個區(qū)域環(huán)境中就會有很多個節(jié)點(diǎn).在探測出環(huán)境區(qū)域里的節(jié)點(diǎn)后，根據(jù)機(jī)器人的攀爬能力，設(shè)定一個坡度作為機(jī)器人能否翻越障礙物的閾值，這里閾值取30°.每兩個節(jié)點(diǎn)之間會定義一條邊界(路徑)，機(jī)器人選擇的路徑就是由邊界和節(jié)點(diǎn)組成的線路.

圖5 機(jī)器人運(yùn)動節(jié)點(diǎn)和路徑簡圖Fig.5 Nodes and edges graphics

(3)路徑評估.路徑規(guī)劃時(shí)機(jī)器人沿著這些邊線運(yùn)動，且應(yīng)使機(jī)器人盡可能多地通過這些水平區(qū)域向目標(biāo)點(diǎn)移動.利用狀態(tài)空間的盲目搜索中的一般圖搜索方法規(guī)劃從起始點(diǎn)經(jīng)其他節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)的備選路徑.

定義評估函數(shù)來對這些備選路徑進(jìn)行評估，定義如下

式中:Δroll(°)表示機(jī)器人與水平面間的橫滾角;Δyaw(°)表示路徑上每個位置處的側(cè)擺角;l(m)表示路徑上每部分的長度;kpos和kroll是系數(shù);評估函數(shù)中第1項(xiàng)kpos·Δroll*Δyaw表示控制機(jī)器人打滑的代價(jià);第2項(xiàng)kroll*Δroll表示控制機(jī)器人側(cè)翻的代價(jià);第3項(xiàng)l表示路徑冗余的代價(jià);第4項(xiàng)表示機(jī)器人運(yùn)動中沿某條路徑運(yùn)動時(shí)所消耗的能量.

2 能量計(jì)算的說明

能量的計(jì)算原理如下:設(shè)移動機(jī)器人運(yùn)行環(huán)境的地形圖已知，且由一系列的平面三角形組成.移動機(jī)器人的能量消耗可以定義為機(jī)器人在各三角形表面上運(yùn)動時(shí)摩擦力與重力消耗能量之和，同時(shí)引入各向異性來解決當(dāng)?shù)匦伪砻孢^于陡峭而導(dǎo)致機(jī)器人側(cè)翻或者動力不足的問題.如圖6所示，機(jī)器人由從P1點(diǎn)沿直線運(yùn)動到P2點(diǎn)所消耗的能量E可由下式給出

式中:m為機(jī)器人的質(zhì)量;μ為該平面的摩擦系數(shù)，α和β分別為機(jī)器人所在的平面以及機(jī)器人的運(yùn)動方向與水平面的夾角.

假設(shè)機(jī)器人勻速運(yùn)動，轉(zhuǎn)彎時(shí)無能量損耗.如圖7所示，機(jī)器人運(yùn)行過程中有3個禁止區(qū)域(用角度表示).第1個是禁止通行區(qū)域(用角度α1表示)，表示上坡時(shí)坡度太陡機(jī)器人上不去的區(qū)域.另外兩個禁止區(qū)域是側(cè)翻區(qū)域(用角度α2表示)，表示機(jī)器人重心的投影點(diǎn)超出了支撐點(diǎn)構(gòu)成的凸多邊形而發(fā)生側(cè)翻的區(qū)域.除了這3個禁止區(qū)域外，另一個特殊的區(qū)域是制動區(qū)域(用角度α3表示)，表示機(jī)器人下坡獲得能量和加速度的區(qū)域.圖7中的常規(guī)區(qū)域是機(jī)器人可以自由通行的區(qū)域.

圖6 能量消耗示意圖Fig.6 Energy consum ing graphics

圖7 四個區(qū)域示意圖Fig.7 Four districts graphics

在制動區(qū)域，當(dāng)機(jī)器人沿一定的運(yùn)動方向β下坡時(shí)，有μcosα+sinβ＜0，使機(jī)器人獲得能量進(jìn)而加速運(yùn)動.所以，下坡時(shí)為使機(jī)器人保持勻速運(yùn)動，應(yīng)施加一個制動力 -mg(μcosα+sinβ)來避免機(jī)器人加速運(yùn)動.制動區(qū)域α3的兩個臨界制動角度β0可通過下式計(jì)算得到

當(dāng)機(jī)器人在這兩個臨界制動角所組成的制動區(qū)域中運(yùn)動時(shí)，下坡時(shí)獲得的能量被用于制動，機(jī)器人在此范圍內(nèi)運(yùn)動時(shí)不消耗能量.所以前述公式(2)表示的能量計(jì)算公式只在機(jī)器人運(yùn)行于常規(guī)區(qū)域中才使用.

3 路徑規(guī)劃算法實(shí)現(xiàn)

3.1 本算法的實(shí)現(xiàn)

在梯度計(jì)算階段，每個網(wǎng)格的大小設(shè)為1 m2，Lxn和Lyn均取100 m，這樣數(shù)字高度圖上的網(wǎng)格數(shù)為10 000個.路徑規(guī)劃階段，系數(shù)kpos=0.1，kroll=10.在仿真環(huán)境中，設(shè)定A、B、C、D和E5座小山坡，坡度分別為 10°，25°，20°，50°和 15°，其俯視圖如圖2所示，路徑規(guī)劃的起點(diǎn)設(shè)在節(jié)點(diǎn)A附近，目標(biāo)點(diǎn)設(shè)在節(jié)點(diǎn)E附近.

節(jié)點(diǎn)探測階段所產(chǎn)生的節(jié)點(diǎn)和邊線(紅色直線)如圖8所示.一共規(guī)劃出了5條路徑，分別記為 A-E、A-B-E、A-B-C-E-C-E、A-C-B-E.然后根據(jù)評估函數(shù)(1)，分別計(jì)算不同路徑的代價(jià)，結(jié)果如表1所示.運(yùn)動過程中設(shè)機(jī)器人勻速運(yùn)動，轉(zhuǎn)彎時(shí)消耗能量為零，參照公路的摩擦力系數(shù)，室外未知環(huán)境的摩擦力系數(shù)取0.4(大于一般公路的摩擦系數(shù))，表中所有數(shù)據(jù)都進(jìn)行了取整處理.把所獲得的5條路徑的總代價(jià)進(jìn)行比較，最后獲得的最優(yōu)路徑是A→B→E的路徑，即路徑2.在表1中，由于筆者所提方法采用“直線路徑”(俯視圖中的效果，實(shí)際運(yùn)行路徑為三維曲線)，這里吸收了自主機(jī)器人遙操作人員的控制思想，在實(shí)際工作環(huán)境中控制機(jī)器人沿“直線路徑”運(yùn)動，避免機(jī)器人運(yùn)動過程中運(yùn)動方向的頻繁轉(zhuǎn)換，可有效避免打滑因素對機(jī)器人運(yùn)行平穩(wěn)性的影響，所以表1中的前5條路徑的打滑代價(jià)均為0.

圖8 筆者算法獲得的路徑(俯視圖)Fig.8 Path Obtained by Proposed Method(Planform)

表1 不同路徑的代價(jià)比較Tab.1 Cost Comparison among Different Paths

3.2 傳統(tǒng)方法實(shí)現(xiàn)

傳統(tǒng)方法包含3個階段.①梯度計(jì)算，障礙物探測和路徑規(guī)劃.第1步的梯度計(jì)算和該方法完全相同.②障礙物探測，引入一個坡度閾值Hthreshold，把數(shù)字梯度圖上的網(wǎng)格劃分為自由空間的網(wǎng)格和有障礙物空間的網(wǎng)格.坡度值大于Hthreshold的網(wǎng)格被認(rèn)為是含有障礙物的網(wǎng)格.③路徑規(guī)劃，在自由空間中采用最短路徑搜索方法來規(guī)劃一條從起點(diǎn)A到終點(diǎn)E的最短路徑.利用該方法獲得的最短路徑如圖9所示(俯視圖).

圖9 傳統(tǒng)方法獲得的路徑(俯視圖)Fig.9 Path Obtained by Traditional Method(Planform)

3.3 討論

采用筆者所提方法獲得的最優(yōu)路徑是表1中的路徑2，雖然最短路徑是路徑5，但由于路徑2的能量消耗小于路徑5的對應(yīng)項(xiàng)，使得路徑2的總代價(jià)小于路徑5的總代價(jià)，故路徑2為規(guī)劃的最優(yōu)路徑.

利用傳統(tǒng)方法獲得的最優(yōu)路徑的打滑和側(cè)翻代價(jià)都較大，雖然路徑6和路徑2相比，距離代價(jià)相差不大，但路徑6的打滑以及側(cè)翻代價(jià)之和遠(yuǎn)大于路徑2的側(cè)翻代價(jià)，消耗能量也大于路徑2的對應(yīng)項(xiàng)，這是由于在機(jī)器人運(yùn)動過程中，傳統(tǒng)方法總是迫使機(jī)器人不斷調(diào)整運(yùn)動方向，增加了打滑和側(cè)翻的風(fēng)險(xiǎn)，同時(shí)也增加了能量消耗，而這正是機(jī)器人實(shí)際運(yùn)動控制中應(yīng)盡量避免的.

4 結(jié)論

設(shè)計(jì)了針對非平坦復(fù)雜區(qū)域中自主機(jī)器人的路徑規(guī)劃，該方法與傳統(tǒng)方法的區(qū)別主要有兩點(diǎn):①除考慮路徑因素外，還考慮了打滑和側(cè)翻因素對規(guī)劃結(jié)果的影響，并考慮了路徑的能量損耗，使規(guī)劃的最優(yōu)路徑更安全、更合理;②在機(jī)器人遇到障礙物時(shí)，并非全部采用躲避繞行的策略，而是根據(jù)障礙物的坡度，確定機(jī)器人采用繞行或者直接從障礙物上面越過的原則，減少了運(yùn)行中頻繁更換運(yùn)動方向?qū)е碌拇蚧騻?cè)翻的風(fēng)險(xiǎn).把該方法規(guī)劃的最優(yōu)路徑與傳統(tǒng)方法規(guī)劃的路徑進(jìn)行了對比分析，仿真結(jié)果驗(yàn)證了筆者所提方法的有效性.

［1］ LIU Hui，NORBERT S，STEFFEN J，et al.A Floyd-Dijkstra Hybrid Application for Mobile Robot Path Planning in Life Science Automation［C］.8th IEEE International Conference on Automation Science and Engineering，Seoul，Korea，2012:279-284.

［2］ 王鴻鵬.復(fù)雜環(huán)境下輪式自主移動機(jī)器人定位與運(yùn)動控制研究［D］.天津:南開大學(xué)信息技術(shù)科學(xué)學(xué)院，2009.

［3］ MICHAEL B，BERND B，DIRK S.Towards autonomously traversing complex obstacleswithmobile robots with adjustable chassis［C］.2012 13th International Carpathian Control Conference(ICCC)，61-64.

［4］ SHEN C C，KE JH，CHENG H L.Trajectory planning for stair climbing in the leg-wheel hybrid mobile robot Quattroped［C］.2011 IEEE International Conference on Robotics and Automation，Shanghai，China，1229-1234.

［5］ AHMED E，ACHIMW.Lyapunov stability study for a special actuated holonomic wheeled mobile robot［C］.2011 IEEE International Conference on Systems，Man and Cybernetics(SMC)，1962-1967.

［6］ OHKI T，NAGATANI K，YOSHIDA K.Safety path planning formobile robot on rough terrain considering instability of attitudemaneuver［C］.2010 IEEE/SICE International Symposium on System Integration，55-60.

［7］ ROWE N C，ROSSR S.Optimal grid-free path planning across arbitrarily contoured terrain with anisotropic friction and gravity effects［J］.IEEE Transactions on Robotics and Automation，1990，6(5):540-553.

［8］ ZHENG Sun，JOHN H R.Finding energy minimizing paths on terrains［J］.IEEE Transactions on Robotics，2005，21(1):102-114.

［9］ 霍煒，劉大維，王江濤.基于局部最小能量的移動機(jī)器人路徑規(guī)劃［J］.青島理工大學(xué)學(xué)報(bào)，2008，29(4):99-104.

［10］ LIGuang-hui，ATSUSHIY，HAJIME A.An efficient improved artificial potential field based regression searchmethod for robot path planning［C］.Proceedings of 2012 IEEE International Conference on Mechatronics and Automation，1227-1232.

［11］ NAFISEH S.Mobile robot path planning by new structured multi-objective genetic algorithm［C］.2011 International Conference of Soft Computing and Pattern Recognition(SoCPaR)，79-83.

［12］ IMEN C，ANIS K.smartPATH:A Hybrid ACO-GA Algorithm for Robot Path Planning［C］.WCCI 2012 IEEEWorld Congress on Computational intelligence，1-8.