移動機器人低能耗最優(yōu)路徑規(guī)劃方法

張浩杰 蘇治寶 HERNANDEZ D E 蘇 波

(1.中國北方車輛研究所兵器地面無人平臺研發(fā)中心， 北京 100072； 2.德國移動技術有限公司， 伯恩 53757)

0 引言

移動機器人集成了傳感器、定位與深度學習、高精度地圖、路徑規(guī)劃、障礙物檢測與避障、機械控制、能耗與熱管理、系統(tǒng)集成與優(yōu)化等多項關鍵技術和基礎理論，在軍用和民用領域具有巨大的發(fā)展前景。全電驅(qū)動機器人具有零排放、噪聲小、高能源利用率和控制簡單準確等優(yōu)勢[1]，越來越多的研究機構(gòu)將其應用于智能工廠[2]、物流配送[3]、安防、精準農(nóng)業(yè)[4]和軍事巡邏偵察[5]等領域。隨著機器人自主能力的不斷提升，它們勢必在室外環(huán)境中執(zhí)行更多的任務。然而，由于受容量及重量的限制，機器人單次攜帶的能源是有限的，如電池或者燃料等，有時甚至很難返航進行能源補給，如在外太空執(zhí)行任務的星球探測車。因此，能源因素限制了機器人連續(xù)執(zhí)行任務的持久性。為了使機器人在能量有限或者補給不足的情況下更高效地執(zhí)行更多的任務，降低其運動過程中的能耗變得尤為重要，并逐漸成為研究熱點。

基于能耗最優(yōu)的路徑規(guī)劃方法最早應用于無人飛行器[6]及無人潛水器[7-8]，由于這類平臺在運動過程中受風流或水流干擾較大，能耗最優(yōu)的路徑規(guī)劃方法可以有效地借助風流及水流的運動[9]，降低機器人在運動過程中的能耗。隨著對移動機器人在室外越野環(huán)境執(zhí)行任務的需求增加，移動機器人的能耗最優(yōu)路徑規(guī)劃方法也逐漸成為研究熱點。BRODERICK等[10]采用油-電混合驅(qū)動系統(tǒng)可以降低部分能耗，然而一旦驅(qū)動系統(tǒng)確定，在機器人運動過程中只有通過路徑規(guī)劃、速度規(guī)劃降低能耗。JONES等[11]提出了一種在不確定性、強擾動環(huán)境下的能耗最優(yōu)路徑規(guī)劃框架，在該框架下采用迭代優(yōu)化算法進行路徑規(guī)劃。該優(yōu)化算法在路徑通行消耗估價中加入了能耗，改進隨機軌跡優(yōu)化算法(Stochastic trajectory optimization for motion planning，STOMP)[12]為能耗最優(yōu)隨機軌跡優(yōu)化算法(Energy-efficient stochastic trajectory optimization，EESTO)，通過不斷地迭代優(yōu)化獲得能耗最優(yōu)的路徑。當不確定性環(huán)境擾動引起環(huán)境信息更新后，該方法將根據(jù)設計的不同路徑對比準則選擇機器人在當前環(huán)境狀態(tài)下的能耗最優(yōu)路徑。與文獻[11]中的方法不同，文獻[13-14]將能耗最優(yōu)的路徑規(guī)劃過程分為兩層進行，首先，通過傳統(tǒng)的路徑規(guī)劃方法生成一系列路徑，如啟發(fā)式搜索算法[15-16]、RRT算法[17-18]等，其次，在路徑優(yōu)化過程中按照設計的能耗模型估算執(zhí)行這些路徑時的能耗，最終獲取能耗最優(yōu)路徑。這類方法的不同之處在于所采用的能耗模型，MEI等[13]采用六次多項式曲線建立電機的能耗模型，通過線性轉(zhuǎn)換描述平臺速度與驅(qū)動電機速度之間的關系，進而推算平臺的能耗，在能耗計算過程中考慮了轉(zhuǎn)彎、加速、減速等運動。同樣，也可以通過平臺執(zhí)行路徑時與地面的摩擦力計算能耗[14]，這種能耗模型僅考慮了機械運動能耗，而未涉及發(fā)熱損失能耗。

本文提出一種基于改進實時動態(tài)(Anytime Dynamic A*， AD*)算法的移動機器人低能耗最優(yōu)路徑規(guī)劃方法。該方法利用建立的機器人運動學模型和能耗模型，采用基于采樣的模型預測算法生成優(yōu)化軌跡簇，將執(zhí)行路徑時的距離成本消耗和能耗融進優(yōu)化軌跡簇。依據(jù)優(yōu)化軌跡簇中的節(jié)點連接關系，改進啟發(fā)式搜索算法，以實現(xiàn)在路徑規(guī)劃過程中獲得機器人的能耗最優(yōu)路徑。

1 機器人動力學模型及能耗模型

1.1 機器人動力學模型

機器人的動力學模型能夠更好地表達機器人的控制輸入、運動速度、加速度、機器人與地面的作用力等特征。尤其當機器人工作在室外環(huán)境時，在有起伏坡度的地形條件下，通過動力學模型能夠更加真實準確地估算機器人跟蹤路徑的能耗。定義機器人坐標系xoy和大地坐標系XOY，在機器人坐標系中，x軸的正方向指向機器人的前方，y軸正方向指向機器人的左方，z軸正方向指向機器人的上方，而大地坐標系為固定坐標系。對于差速轉(zhuǎn)向的機器人而言，其相對機器人坐標系的平移速度和角速度由兩側(cè)電動機的驅(qū)動旋轉(zhuǎn)產(chǎn)生。圖1所示是機器人以恒定的平移速度和角速度繞瞬時轉(zhuǎn)動中心運動示意圖。

圖1 機器人繞轉(zhuǎn)動中心運動示意圖Fig.1 Sketch of robot performing a circular turn

在圖1中，以(vx,ωz)表示機器人在機器人坐標系中沿x軸的平移速度和繞z軸的角速度，則其在運動過程中的狀態(tài)轉(zhuǎn)移方程為

(1)

式中r——機器人驅(qū)動輪半徑，mm

B——機器人寬度，mm

ωl、ωr——左、右兩側(cè)驅(qū)動輪的角速度，rad/s

對于給定的采樣控制量(vx,ωz)，對式(1)求解，可得兩側(cè)驅(qū)動輪的轉(zhuǎn)動角速度ω為

(2)

參考MEI等[13]對機器人在起伏路面運動時的非線性動力學約束的研究成果，將機器人的動力學模型以車輪狀態(tài)表示為

(3)

其中

τ=[τlτr]T

式中M——質(zhì)量矩陣

G(q)——克服的重力

q——左、右驅(qū)動輪的角位移矢量

τ——左、右驅(qū)動輪的電動機輸出扭矩

1.2 機器人能耗模型

機器人在運動過程中的能耗包含機械能量消耗和電動機的電阻線圈發(fā)熱能耗，本文對這兩部分能耗進行分析，從而建立機器人的能耗模型。將機器人左右兩側(cè)的驅(qū)動系統(tǒng)簡化為圖2所示的等效電路，圖中Ul和Ur是左、右兩側(cè)電動機驅(qū)動器的輸出電壓，il和ir是左、右等效電路中的電流。

圖2 機器人電動機驅(qū)動系統(tǒng)等效電路Fig.2 Circuit diagram for motor driving system of robot

在圖2中，等效電路包括電池、電動機驅(qū)動器和電動機線圈電阻Re。電動機和電動機驅(qū)動器之間的關系如圖3所示，電動機驅(qū)動器采用電流調(diào)速的方式使左右車輪達到期望速度。當電動機驅(qū)動器向電動機提供電流驅(qū)動時，電動機轉(zhuǎn)動并消耗能量。相反地，當外在扭矩驅(qū)動電動機轉(zhuǎn)動時，電動機產(chǎn)生反電動勢，儲存能量。

圖3 機器人速度閉環(huán)控制系統(tǒng)原理框圖Fig.3 Closed-loop control system for robot

從圖2及圖3可知，機器人的能耗主要包括電動機輸出扭矩驅(qū)動機器人運動的能耗及電動機線圈的發(fā)熱能耗，因此，它的能耗模型可以表示為

P=ωτ+I2Re

(4)

式中P——機器人能耗

I——電動機線圈電流

以扭矩τ表示電動機的驅(qū)動電流

(5)

式中KT——扭矩常量系數(shù)g——傳動比

η——電動機的傳動效率

假設左、右車輪的能耗效率分別為ηl和ηr，則左、右車輪的能耗模型可分別表示為

(6)

(7)

將式(5)代入式(6)、(7)，可將左、右車輪的能耗Pl和Pr表示為扭矩函數(shù)，即

(8)

(9)

式(8)、(9)中τl和τr可由式(3)計算獲得。

因此，根據(jù)式(8)、(9)可知，機器人在運動過程中的總能耗E(t)可以表示為

(10)

2 基于運動學約束和能耗模型的優(yōu)化軌跡簇

2.1 機器人運動學約束分析

給定起始狀態(tài)下，機器人的運動學約束能夠?qū)蛇_到的下一個位置狀態(tài)進行限制。機器人在運動過程中既可平移，又可轉(zhuǎn)動，因此，它共有6個自由度，分別以位置量(x,y,z)和轉(zhuǎn)動量(φ,ψ,θ)表示，其中φ、ψ、θ分別為側(cè)傾角、俯仰角、橫擺角。如果以z=[xyz]T和ψ=[φψθ]T表示機器人在大地坐標系中的位移矢量和角度矢量，根據(jù)歐拉變換關系，可得機器人的運動學模型為

(11)

(12)

式中，[vxvyvz]T是機器人相對于機器人坐標系的線速度，而[ωxωyωz]T是機器人相對于機器人坐標系的角速度，c表示cos函數(shù)，s表示sin函數(shù)，t表示tan函數(shù)。

給定機器人的起始狀態(tài)，將采樣控制量作用于起始狀態(tài)，則根據(jù)式(11)、(12)表示的機器人運動學模型可以估計它的下一時刻可達到的狀態(tài)。在本文中，為了優(yōu)化計算，不考慮機器人的俯仰和側(cè)傾，即φ=ψ=0。以Xinput=[vxvyvzωz]T作為機器人運動學模型的輸入量，而Xout=[xyzθ]T作為輸出狀態(tài)量，則式(11)、(12)可合并為

(13)

因此，給定機器人的輸入量，即可通過式(13)獲得機器人在控制周期內(nèi)的可達到的狀態(tài)量。

2.2 SBMPO算法預測生成優(yōu)化軌跡簇

SBMPO算法融合了基于采樣的路徑規(guī)劃算法和模型預測控制算法思想生成軌跡[20]，以避免基于采樣的路徑規(guī)劃算法的計算復雜等瓶頸問題。該算法集成在模型預測控制算法框架中，因此，對于給定的起始狀態(tài)xcurrent和目標狀態(tài)xgoal，SBMPO算法對起始狀態(tài)執(zhí)行控制，并重復優(yōu)化計算兩個狀態(tài)之間的最優(yōu)路徑，直至到達目標狀態(tài)xgoal。在算法實施過程中，SBMPO算法生成一系列滿足輸入約束的控制狀態(tài)空間采樣，將采樣控制量作用于系統(tǒng)模型，以生成與當前節(jié)點狀態(tài)相關聯(lián)的鄰近節(jié)點。按照距離最短、時間最快或者能耗最少等期望目標，評估計算生成的鄰近節(jié)點與當前節(jié)點之間通行消耗值。通過將采樣控制量連續(xù)作用于通行消耗值最低的節(jié)點，直至目標狀態(tài)xgoal被擴展搜索，從而生成從起始狀態(tài)到目標狀態(tài)的優(yōu)化軌跡。

采用SBMPO算法對機器人控制狀態(tài)空間進行采樣，將采樣控制量作用于機器人的運動學模型，以生成在一定仿真時間內(nèi)的預測優(yōu)化軌跡簇。以xi=(0,0,0,θi)表示機器人在起始位置的離散初始狀態(tài)，i=1,2,…,N，位置為坐標原點，其中，N為機器人在大地坐標系中橫擺角等額離散的數(shù)目。在機器人的控制狀態(tài)空間u，采用擬隨機采樣生成控制量，將控制量作用于式(13)表示的機器人運動學模型，根據(jù)它的初始狀態(tài)量xi生成在仿真時間[t1,t2]內(nèi)的輸出狀態(tài)xo(t)，其算法流程如下：

whilei=1 &&i<=Ndo

xi←start

repeat

SBMPO(xi,xo(t),B)

如果xo(t)有效，則存儲

until 生成B條有效軌跡

i++

end while

對于給定的機器人初始狀態(tài)xi，采用SBMPO算法生成輸出狀態(tài)，如果輸出狀態(tài)有效，則存儲，直至對每一個初始狀態(tài)生成B條有效軌跡。對于每一個給定的起始狀態(tài)xi，這些輸出狀態(tài)序列組成了與初始狀態(tài)關聯(lián)的軌跡T(xi)={xo(t1),xo(t1+Δt),…,xo(t2)}，其中xi=xo(t1)，Δt為時間步長。

對于采樣生成的軌跡T(xi)，根據(jù)感知模塊提供的環(huán)境地圖的柵格分辨率優(yōu)化軌跡，將其末端狀態(tài)xo(t2)優(yōu)化至柵格中心，這些與初始狀態(tài)關聯(lián)的軌跡簇，組成了用于能耗最優(yōu)規(guī)劃的優(yōu)化軌跡簇。圖4所示是機器人在初始狀態(tài)xi=(0,0,0,0)時采用SBMPO算法生成的優(yōu)化軌跡簇示例。在該優(yōu)化軌跡簇生成中，設定的地圖柵格分辨率為0.2 m，采用的控制量采樣空間為(vx,vy,vz)∈[0,0.5]和ωz∈[0,π/8]。從圖4中可以看出，基于SBMPO算法生成的優(yōu)化軌跡簇包含7條有效軌跡，分別以不同的顏色顯示，這些軌跡是與機器人起始狀態(tài)相關聯(lián)的可到達的最優(yōu)路徑，既有在XOY平面的平移，也有沿Z軸方向的梯度運動位移。

圖4 采樣空間生成的軌跡簇Fig.4 Trajectory cluster generated by sampling

對于軌跡簇中的每一條局部軌跡T(xi)，其通行成本消耗主要包括距離成本消耗和能量成本消耗兩部分，即軌跡起始狀態(tài)和終止狀態(tài)之間的消耗c(xo(t1),xo(t2))。距離成本消耗可以通過機器人的初始狀態(tài)和末端狀態(tài)之間的歐幾里得距離計算[21]，而能量成本消耗依據(jù)生成該局部軌跡時的采樣控制量，按照式(2)推算左、右驅(qū)動輪的角速度ωl和ωr。將機器人的左、右驅(qū)動輪的角速度代入式(8)、(9)計算左、右驅(qū)動輪的能耗，而在采樣控制周期內(nèi)的總能耗按式(10)計算獲得。因此，軌跡簇中每條軌跡的總通行成本消耗為距離成本消耗和能耗成本消耗之和，即

c(xo(t1),xo(t2))=α‖xo(t1),xo(t2)‖+

(14)

式中α——機器人在平坦路面移動歐幾里得距離1 m時的能耗

‖xo(t1),xo(t2)‖——節(jié)點xo(t1)與xo(t2)之間的歐幾里得距離

μ——地面摩擦因數(shù)

g——重力加速度

m——機器人質(zhì)量，kg

xo,z(t1)、xo,z(t2)——節(jié)點xo(t1)與xo(t2)在z軸方向的位移

通過上述采樣、預測、優(yōu)化軌跡過程，這些軌跡簇組成了節(jié)點與節(jié)點間的連接有向圖，這些節(jié)點間的通行成本消耗可以按照距離成本消耗和能耗成本兩部分加權(quán)計算獲得，見式(14)。

3 AD*算法與低能耗的最優(yōu)路徑規(guī)劃

3.1 AD*算法

AD*算法是基于傳統(tǒng)A*算法的改進算法，通常簡稱為AD*算法，主要用于解決環(huán)境信息改變及時間約束下的最優(yōu)路徑規(guī)劃問題。與A*算法不同，它在估價函數(shù)中對啟發(fā)值增加了一個比例因子ζ(ζ≥1)，其修正后的估價函數(shù)可以表示為

f(x)=g(x)+ζh(x)

(15)

式中g(x)——從起始節(jié)點到當前節(jié)點x的真實通行成本

h(x)——從當前節(jié)點x到目標節(jié)點的估算通行成本，也稱為啟發(fā)值

AD*算法在進行路徑搜索過程中共維持了OPEN表、CLOSED表和INCONS表3個表。OPEN表中存儲待被搜索的節(jié)點，CLOSED表中存儲已經(jīng)被搜索過的節(jié)點，而INCONS表中存儲由于比例因子ζ改變或節(jié)點之間平移消耗改變后導致出現(xiàn)狀態(tài)不一致的節(jié)點(v≠g)。AD*算法搜索節(jié)點的先后順序由節(jié)點的f值決定，通過將搜索過程中出現(xiàn)的狀態(tài)不一致的節(jié)點重新移至OPEN表中, 實現(xiàn)算法的實時性和增量性，關于該算法的理論分析參見文獻[22]。

3.2 節(jié)點評估函數(shù)

在采用低能耗最優(yōu)路徑規(guī)劃方法進行路徑搜索過程中，對于每一個搜索節(jié)點x，生成x的節(jié)點集合稱為x的父節(jié)點集P(x)，而由x生成的節(jié)點集合稱為x的子節(jié)點集S(x)。以x′∈P(x)為到達x節(jié)點的最優(yōu)父節(jié)點，則節(jié)點x評估函數(shù)中的g(x)項可表示為

g(x)=g(x′)+c(x′,x)

(16)

式中，c(x′,x)為節(jié)點x′與節(jié)點x之間的真實通行成本消耗，即這兩個節(jié)點之間的優(yōu)化軌跡成本消耗，可按式(14)計算。

節(jié)點x的估價函數(shù)中的h(x)為節(jié)點x到目標節(jié)點xgoal的估算消耗成本，該估算消耗成本為兩個節(jié)點之間的歐幾里得距離成本消耗和機器人克服重力的能耗[23]之和，即

h(x)=α‖x,xgoal‖+μmg(xz-xgoal,z)

(17)

將式(16)、(17)代入式(15)，可得節(jié)點x的評估函數(shù)為

(18)

3.3 基于AD*算法與低能耗模型的最優(yōu)路徑規(guī)劃算法

本文在研究過程中采用AD*算法思想，沿用其在路徑搜索過程中管理節(jié)點的OPEN表、CLOSED表和INCONS表，設計基于AD*算法和低能耗模型的最優(yōu)路徑規(guī)劃算法(稱為最優(yōu)路徑規(guī)劃改進AD*算法)，其算法流程如下：

(1)xcurrent←start。

(2)g(xcurrent)=rhs(xcurrent)=∞;g(xgoal)=∞;rhs(xgoal)=0;ζ=ζ0。

(3) 將xcurrent和xgoal放入OPEN表中。

(4) repeat。

(5) 從OPEN表中按照評估函數(shù)移出估值最小的節(jié)點x。

(6) 如果g(x)>rhs(x)

g(x)=rhs(x)。

(7) 將x放入CLOSED表中。

(8) 根據(jù)軌跡簇找到x的子節(jié)點集S(x)，并更新子節(jié)點的通行消耗成本(距離和能耗)。

(9) 否則g(x)=∞。

(10)根據(jù)軌跡簇找到x的子節(jié)點集S(x)，并更新子節(jié)點的通行消耗成本(距離和能耗)。

(11)untilxgoal被搜索，生成能耗最優(yōu)路徑。

(12)如果環(huán)境地圖變化

對所有關聯(lián)節(jié)點，更新其通行消耗成本。

(13)對所有關聯(lián)節(jié)點，按照g(xi)和rhs(xi)關系更新xi的搜索表所屬關系。

(14)將INCONS表中節(jié)點移至OPEN表中。

(15)重復步驟(2)～(9)，生成環(huán)境變化后的能耗最優(yōu)路徑。

所提出的基于AD*算法和能耗模型的最優(yōu)路徑規(guī)劃方法與傳統(tǒng)AD*路徑規(guī)劃方法的不同之處在于其在搜索過程中使用的節(jié)點關聯(lián)關系及節(jié)點通行成本評估函數(shù)。采用該算法進行路徑搜索過程中，從機器人的起始狀態(tài)開始，根據(jù)式(18)節(jié)點評估值的計算方法，不斷將估價值最小的節(jié)點進行搜索，按照軌跡簇找到其相關的子節(jié)點，并將子節(jié)點不斷放進OPEN表或INCONS表進行搜索，直至搜索到目標節(jié)點，路徑搜索終止，從而得到了從起始狀態(tài)到目標狀態(tài)的能耗最優(yōu)路徑。

該方法在路徑規(guī)劃過程中，充分考慮了每個節(jié)點與父節(jié)點及子節(jié)點之間的距離成本消耗和能耗成本，因此，搜索獲得的路徑為能耗最優(yōu)的路徑。

4 仿真實驗

圖5 機器人實驗平臺Fig.5 Experimental platform of robot

為了驗證所提出的最優(yōu)路徑規(guī)劃方法的有效性，本文根據(jù)機器人實驗平臺的性能參數(shù)，利用機器人操作系統(tǒng)(Robot operating system，ROS)中的仿真軟件Gazebo生成模擬的移動實驗平臺，并隨機搭建了20個室外場景進行仿真驗證。實驗平臺為四輪驅(qū)動機器人，如圖5所示。實驗平臺的硬件主要是由工控機、UTM-30LX-EW型激光雷達、CCD攝像頭和遙控手柄組成。機器人的工作模式分為遙控模式和自主模式兩種，在遙控模式下，操作者可以通過遙控手柄控制機器人運動，而在自主模式下，機器人需要根據(jù)探測到的環(huán)境信息進行自主運動。

機器人實驗平臺的主要性能參數(shù)如表1所示，在Gazebo中按照表1的參數(shù)模擬機器人平臺進行仿真測試。

表1 機器人平臺性能參數(shù)Tab.1 Main specifications of robot

4.1 仿真場景設計

在仿真測試中，通過仿真軟件Gazebo搭建了20多個室外測試環(huán)境，如圖6所示，尺寸約為60 m×60 m。仿真測試環(huán)境中根據(jù)表1的機器人可通行性能設定了一定的起伏及坡度。

圖6 仿真測試場景示例Fig.6 Example of scenario for simulation

在這些模擬測試環(huán)境中，機器人平臺通過攜帶的多種激光傳感器和立體視覺傳感器完成對模擬環(huán)境的探測，并生成三維柵格地圖，如圖7所示。柵格分辨率設定為20 cm×20 cm×20 cm，該部分工作由環(huán)境感知模塊完成，而所輸出的三維柵格地圖是本文研究工作的基礎。

圖7 三維柵格地圖Fig.7 3D grid map

4.2 結(jié)果分析

分別采用AD*算法在圖7所示的三維柵格地圖上進行距離成本最優(yōu)路徑規(guī)劃和利用本文所提出的規(guī)劃方法進行能耗最優(yōu)路徑規(guī)劃，以進行規(guī)劃結(jié)果的對比分析。整個仿真計算在裝有Ubuntu 16.04系統(tǒng)的計算機上運行，該計算機的配置為Intel(R) Core(TM) i7-4790 CPU，主頻為3.60 GHz。在仿真實驗過程中，所采用的用于計算能耗的關鍵參數(shù)如表2所示。

表2 仿真實驗關鍵參數(shù)Tab.2 Key parameters for simulation

對于給定的起始狀態(tài)和目標狀態(tài)，分別采用AD*算法進行距離成本最優(yōu)路徑規(guī)劃和本文所提出的能耗最優(yōu)規(guī)劃方法進行路徑規(guī)劃。根據(jù)這兩種方法獲得路徑上的節(jié)點連接關系，計算路徑上節(jié)點之間的歐幾里得距離之和，即路徑長度。同時，按照式(14)計算路徑節(jié)點之間的能耗之和，即從起始狀態(tài)到目標狀態(tài)的能量消耗之和。仿真實驗結(jié)果的統(tǒng)計分析見表3，分別對這兩種方法規(guī)劃生成路徑的長度和能量消耗進行對比分析。從表3可以看出，本文所提出的移動機器人能耗最優(yōu)規(guī)劃方法與距離最優(yōu)規(guī)劃方法相比，在整個路徑規(guī)劃過程所需要的規(guī)劃時間基本一樣，規(guī)劃路徑的長度增加了3.7%，而能量消耗約降低了16.1%。

表3 仿真實驗結(jié)果Tab.3 Simulation results

圖8所示是移動機器人能耗最優(yōu)規(guī)劃方法及距離最優(yōu)規(guī)劃方法規(guī)劃的路徑在三維柵格地圖中的表示，其中紫色為距離最優(yōu)路徑，而綠色為能耗最優(yōu)路徑，其相對應在仿真環(huán)境中的顯示如圖9所示。從圖8、9也可看出，這兩條路徑基本接近，而路徑能耗則降低了約16.1%。

圖8 三維柵格地圖中的能耗最優(yōu)及距離最優(yōu)路徑Fig.8 Energy optimal and distance optimal paths in 3D grid map

圖9 仿真場景中的能耗最優(yōu)及距離最優(yōu)路徑Fig.9 Energy optimal and distance optimal paths in simulation scenario

5 結(jié)束語

針對機器人在室外環(huán)境執(zhí)行任務時每次供給的能源(電池/燃料)有限，有時甚至很難實現(xiàn)補給等問題，本文提出了一種基于AD*算法的低能耗移動機器人最優(yōu)路徑規(guī)劃方法。該方法通過構(gòu)建機器人的動力學模型及其在運動過程中的能耗模型，實現(xiàn)對路徑的能耗估計。結(jié)合機器人的運動學模型，采用基于采樣的模型預測算法生成優(yōu)化軌跡簇，這些軌跡簇融合了距離成本消耗和能耗模型估算的能耗成本。改進AD*算法，將路徑能耗融入路徑搜索算法的節(jié)點評估函數(shù)，根據(jù)軌跡簇中的節(jié)點連接關系和三維環(huán)境地圖進行在線規(guī)劃，從而獲得能耗最優(yōu)路徑。最后，通過仿真測試證實了所提出的規(guī)劃方法可以有效地降低能耗，提高執(zhí)行任務的效率。