移動機器人低能耗最優路徑規劃方法

張浩杰 蘇治寶 HERNANDEZ D E 蘇 波

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

0 引言

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

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

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

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

1.1 機器人動力學模型

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

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

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

(1)

式中r——機器人驅動輪半徑，mm

B——機器人寬度，mm

ωl、ωr——左、右兩側驅動輪的角速度，rad/s

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

(2)

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

(3)

其中

τ=[τlτr]T

式中M——質量矩陣

G(q)——克服的重力

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

τ——左、右驅動輪的電動機輸出扭矩

1.2 機器人能耗模型

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

圖2 機器人電動機驅動系統等效電路Fig.2 Circuit diagram for motor driving system of robot

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

圖3 機器人速度閉環控制系統原理框圖Fig.3 Closed-loop control system for robot

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

P=ωτ+I2Re

(4)

式中P——機器人能耗

I——電動機線圈電流

以扭矩τ表示電動機的驅動電流

(5)

式中KT——扭矩常量系數g——傳動比

η——電動機的傳動效率

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

(6)

(7)

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

(8)

(9)

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

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

(10)

2 基于運動學約束和能耗模型的優化軌跡簇

2.1 機器人運動學約束分析

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

(11)

(12)

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

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

(13)

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

2.2 SBMPO算法預測生成優化軌跡簇

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

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

whilei=1 &&i<=Ndo

xi←start

repeat

SBMPO(xi,xo(t),B)

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

until 生成B條有效軌跡

i++

end while

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

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

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

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

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

(14)

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

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

μ——地面摩擦因數

g——重力加速度

m——機器人質量，kg

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

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

3 AD*算法與低能耗的最優路徑規劃

3.1 AD*算法

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

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

(15)

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

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

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

3.2 節點評估函數

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

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

(16)

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

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

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

(17)

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

(18)

3.3 基于AD*算法與低能耗模型的最優路徑規劃算法

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

(1)xcurrent←start。

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

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

(4) repeat。

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

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

g(x)=rhs(x)。

(7) 將x放入CLOSED表中。

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

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

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

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

(12)如果環境地圖變化

對所有關聯節點，更新其通行消耗成本。

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

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

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

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

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

4 仿真實驗

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

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

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

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

4.1 仿真場景設計

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

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

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

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

4.2 結果分析

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

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

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

表3 仿真實驗結果Tab.3 Simulation results

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

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

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

5 結束語

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