金 梅 李清天 張立國

(燕山大學電氣工程學院 秦皇島 066000)

0 引言

車場日是陸軍部隊一項經常性的車輛技術狀況管理制度,其目的在于恢復和保持車輛、器材的良好狀態(tài),以保證完成各項執(zhí)勤備戰(zhàn)任務的需要。由于陸軍在組織實施車場日過程中,存在方法不當、質量不高、人員重視程度低等問題,巡檢機器人被逐漸引入進來。巡檢機器人可以通過搭載設備對戰(zhàn)士進行有效監(jiān)管、指導幫扶及自動化檢查評比,實現高效、高質、自動化,是傳統(tǒng)攝像監(jiān)控設備所不能及的,其中路徑規(guī)劃是巡檢機器人有效作業(yè)的重要前提[1-2]。

目前根據機器人的作業(yè)范圍,可以將路徑規(guī)劃分為靜態(tài)全局環(huán)境的全局路徑規(guī)劃和基于局部障礙的局部路徑規(guī)劃方法。常見的全局路徑規(guī)劃算法有基于節(jié)點的Dijkstra、A?算法,基于采樣的快速搜索隨機樹方法和基于啟發(fā)式的遺傳算法、蜂群算法等。局部路規(guī)劃算法主要有人工勢場法、動態(tài)窗口法等[3-5]。

A?算法在機器人的路徑規(guī)劃問題中應用相當廣泛,受其節(jié)點搜索策略的影響,存在得到路徑非最優(yōu)、規(guī)劃的路徑拐點較多等問題。文獻[6]通過簡化路徑坐標點和計算拐點處旋轉方向和角度對A?進行了兩方面的改進,簡化了路徑,提高了機器人行進的準確性,但未解決規(guī)劃路線拐點較多的問題且運行時間增加實時性不高。文獻[7]提出了跳點搜索算法,通過在拓展節(jié)點的過程中篩選出有代表性的跳點,只針對跳點拓展,從而減小了計算量,提高了速度,但存在內存消耗大,計算花費時間較長的問題。文獻[8]通過加入預處理的方法對其進行了改進,解決了實時性的問題,但拐點較多的問題依然沒有解決。文獻[9]提出了將搜索鄰域拓展到24 鄰域的方法,使得軌跡更加平滑,但所選鄰域過多,導致計算耗時較大,且不具有局部避障的能力。文獻[10]結合曼哈頓距離和歐幾里得距離提出一種新的啟發(fā)函數,改進了關鍵點選取策略,并且融合了A?和動態(tài)窗口法,使得算法同時具備局部避障能力。但上述傳統(tǒng)的路徑規(guī)劃算法中,最終規(guī)劃的路徑在直角邊處往往距離障礙過近,使得機器人存在極大的與障礙物發(fā)生碰撞的危險,特別是在軍隊車場環(huán)境下,存在較多直角邊,可能導致巡檢機器人損壞或破壞軍隊戰(zhàn)車等負面影響,造成不必要的損失,影響作業(yè)效率。

針對上述問題,本文使用危險指數來表示潛在碰撞程度,并基于此重新定義啟發(fā)函數從而提出一種安全A?算法,解決了傳統(tǒng)A?算法在直角邊處與障礙物存在較高碰撞風險的問題,提高了導航的安全性。同時將其搜索鄰域擴展并去除同方向冗余子節(jié)點,由傳統(tǒng)的8 鄰域搜索優(yōu)化到16 鄰域搜索,提高了搜索效率,減少轉折點個數,提高了巡檢機器人工作效率。最后將改進的A?算法和動態(tài)窗口法結合,使得算法能同時具備全局路徑規(guī)劃和局部的動態(tài)避障能力,符合軍隊車場巡檢躲避臨時擺放的雜物和正在作業(yè)的戰(zhàn)士的要求。

1 軍隊車場環(huán)境建模

在路徑規(guī)劃問題中,首先需要建立環(huán)境地圖。依據軍隊車場地形規(guī)整(見圖1)、直角邊較多的特點,這里選擇使用簡單有效且直觀的柵格法來建立環(huán)境地圖[11]。將巡檢機器人所在環(huán)境用大小相同的正方形柵格來表示,被填充的柵格表示障礙物,未被填充的表示可行進區(qū)域。根據軍隊車場地圖建模如圖2 所示,其中戰(zhàn)車及其停車基座、墻壁、柱子、雜物均已填充表示為固定障礙物(圖中1 柵格寬度的條形障礙物表示車庫的墻壁和柱子,4×9 的矩形障礙物塊表示停放的8 輛戰(zhàn)車及其停車基座),設定單位柵格邊長為1 m,在此基礎上進行路徑規(guī)劃。

圖1 軍隊車場地形

圖2 中,白色柵格表示自由區(qū)域,黑色柵格表示障礙區(qū)域,每個柵格都有唯一的序號和對應的位置坐標。柵格序號和對應坐標之間的轉換關系如下[12]。

圖2 移動機器人柵格環(huán)境圖

其中,(xi,yi)表示第i個柵格的位置坐標;i是柵格的序號;mod( )是冗余運算符;ceil()是正無窮大的舍入運算符;Nx和Ny分別是行和列方向的柵格數。

2 所提路徑規(guī)劃方法

2.1 全局安全路徑規(guī)劃

2.1.1 路徑規(guī)劃階段的危險指數設計

在移動機器人同障礙物共存的軍隊停車場環(huán)境中,保障移動機器人運行的安全性對提升機器人的導航效率有著至關重要的意義。因此,必須對環(huán)境中機器人運行的安全性進行量化估計,以改善移動機器人在環(huán)境中的安全自主導航性能。危險指數(danger index,DI)可以用于評估機器人運行過程中與障礙物存在的潛在碰撞程度[13]。該指標可以量化移動機器人同障礙物之間的安全距離與碰撞程度,用于提升移動機器人運行過程中的安全自主性。

在機器人路徑規(guī)劃的階段,機器人同障礙物之間的距離是影響機器人安全性的重要因素[14]。如果障礙物同機器人的距離過小,會導致移動機器人直接和障礙物相撞,反之控制好這個距離可以改善機器人的運行安全性(如圖3 所示),避免機器人與障礙物相碰。將障礙物和移動機器人之間的危險指數定義如下。

其中,δ為距離比例因子,具體計算為δ=,Dmax為障礙物對移動機器人安全影響的最大距離,Dmin為機器人與障礙物之間接觸的最小距離,dk為機器人當前位置為障礙物之間的距離(如圖3 所示)。

圖3 基于危險指數的移動機器人安全域示例

2.1.2 基于危險指數的安全A?算法

在移動機器人路徑規(guī)劃領域中,Dijkstra 算法是最早被提出用于最優(yōu)路徑的搜索,該算法具有良好的性能[15]。與Dijkstra 算法相比,A?算法的提出極大提升了路徑的搜索效率,通過提出啟發(fā)式路徑搜索策略,引入了評價函數用來指導節(jié)點的擴展與搜索,提升了算法收斂速度與機器人路徑規(guī)劃的效率。該算法在盡可能保證最優(yōu)路徑的同時,可以減少搜索時間,提高路徑的搜索效率。根據定義的代價條件,A?算法確定從給定起始節(jié)點到目標節(jié)點的最小代價路徑。A?算法在起始節(jié)點上開展路徑搜索,從起始節(jié)點到當前節(jié)點k的路徑成本可以根據行進距離G(k)來計算,即:

其中,k表示要搜索的當前節(jié)點,sk-1,k表示2 個連續(xù)節(jié)點k-1 和k之間的距離。

A?算法最小成本路徑是由參與節(jié)點產生的路徑成本決定的,而剩余路徑的最小成本是通過啟發(fā)式函數H(k)來評估。由此可知,A?算法是以最短的距離和最小的完成時間代價完成最優(yōu)路徑選擇。A?算法的評估函數定義如下[16]。

其中H(k)=sk,goal,sk,goal表示節(jié)點k和目標節(jié)點之間的距離。

傳統(tǒng)A?算法以8 鄰域算法搜索節(jié)點策略為基礎,導致在這種搜索策略下,規(guī)劃出的路徑存在路徑冗余的節(jié)點和拐點[17]。機器人在運行過程中需要不斷地轉向,直接導致機器人的運行效率低。其搜索模式如圖4 所示。因此為了改進搜索方向,本文提出基于16 鄰域的搜索節(jié)點策略來改進路徑存在冗余拐點的不足,改進策略如圖5 所示。該方法有助于去除同方向的多余子節(jié)點,突破傳統(tǒng)A?算法節(jié)點移動方向的局限性,提高A?算法的搜索效率。

圖4 8 鄰域搜索方式

圖5 16 鄰域搜索方式

基于前述危險指數的定義,為保障移動機器人運行的安全性,重新定義如下啟發(fā)式函數H(k)。

于是得到改進后的A?算法評價函數如下。

基于危險指數的安全A?算法的流程圖如圖6所示。

圖6 安全A?算法流程圖

2.2 動態(tài)窗口法

動態(tài)窗口方法(dynamic window approach,DWA)是機器人局部路徑規(guī)劃中主流的方法,它可以跟蹤機器人基于全局規(guī)劃生成的全局路徑,同時結合實時避障原理,實現機器人的自主安全導航[18]。

2.2.1 機器人運動模型

首先需要建立移動機器人的運動模型,基于運動模型才能進一步探索機器人運行的軌跡特點,依據非完整約束的通用模型建立機器人運動模型如圖7所示。在本文中假設機器人的移動軌跡是一段圓弧,那么可以通過微分的原理將路徑軌跡劃分為許多段小圓弧,然后以一對運動指令(vi,wi)來代表一小段的圓弧軌跡。對于一個移動機器人而言,其運動學模型可以描述如下[10]。

圖7 機器人運動模型

其中,[xt+dt,yt+dt,θt+dt]T表示機器人在時間t+dt時在世界坐標系中的位置,[xt,yt,θt]T表示機器人在時間t時在世界坐標系中的位置,[dx,dy,dθ]T表示機器人在時間dt時的位置。底盤坐標系中的理想變化量,[dx,dy,dθ]T=[vxdt,vydt,wdt]T,vx、vy為機器人在底盤坐標系中沿x和y軸的線速度,w表示移動機器人的角速度。

2.2.2 速度采樣

根據上文描述的機器人運動模型,通過約束機器人的速度,可以有效采集到機器人的速度,并推算出機器人在該采集速度下的運行軌跡。機器人的搜索空間受到機器人的運動學約束Vk、動力學約束Vb和障礙物約束Vo的限制[17]。機器人運動學約束了機器人采樣速度和角速度的上下界。機器人動力學約束是基于電機性能對于機器人運行速度的限制,因受到電機轉矩的限制,機器人實際能夠到達的速度被限制在一定的范圍之內。為保障一定機器人的安全性,障礙物約束作用是避免移動機器人同障礙物之間的碰撞,當機器人檢測到障礙物時,需要立即減速,從而達到有效避障的目的。

相關約束描述如下。

其中,vmin和vmax代表機器人速度的上限和下限,wmin和wmax表示機器人角速度的上限和下限,vc是小車的當前線速度,wc是當前角速度,分別對應線加速度的上限和下限,分別對應角加速度的上限和下限,dmin表示機器人在下一時刻與障礙物的距離。

2.2.3 DWA 評價函數設計

通過設計path(v,ω)評價子函數,可以提高局部路徑描述和全局路徑描述的融合權重(見圖8)。設計公式為

圖8 path(v,ω)評價函數示例

其中(x1,y1)為DWA 模擬出的局部路徑末端坐標,(x2,y2)為全局路徑規(guī)劃節(jié)點坐標。

通過機器人在運行過程中需要不斷進行速度采樣工作,可以確定機器人的速度搜索空間,篩選有效的采樣速度簇就可以有效推算出機器人的多條路徑軌跡。然而,對于眾多的軌跡需要采用一個評價準則來選擇最優(yōu)軌跡,于是本文設計評價函數為

其中,heading(v,ω)用于測量朝向目標的進度,當機器人直接移動到目標時,該進度最大;dist(v,ω)表示到靜態(tài)障礙物或臨時障礙物的最近距離;vel(v,ω)代表前進速度;path(v,ω)子函數為提高局部路徑和全局路徑的融合權重。ε、τ、π和γ是權重,σ是4 個評價函數的歸一化參數。

最后動態(tài)窗口法通過選擇最小的G(v,ω)值下的路徑軌跡作為機器人的最優(yōu)路徑。

2.3 基于安全A?算法與動態(tài)窗口法的融合路徑規(guī)劃方法

動態(tài)窗口法具備良好的避障性能,然而在機器人的運行過程中,機器人只能采集到部分局部環(huán)境信息,這容易引導機器人陷入局部最優(yōu)解[16]。由于采集信息的不完備性,本文將基于安全A?算法搜索的全局路徑信息作為DWA 的先驗路徑信息,設計融合函數對全局路徑信息和DWA 局部軌跡信息進行實時約束,可以有效解決移動機器人陷入局部最優(yōu)解的問題,從而提升移動機器人的導航性能。因此,實現安全A?算法與動態(tài)窗口法的結合,可以有效獲得全局最優(yōu)的路徑軌跡,同時可以保障移動機器人實時避障的能力。具體融合算法的實現如圖9所示。

具體實現步驟為:首先,依據前文搭建的環(huán)境模型,建立機器人運動的柵格環(huán)境地圖;其次,給定起始點和終點,采用安全A?算法搜索有效的全局安全路徑,保存機器人的路徑信息Path_list;然后,讀取安全路徑信息Path_list,通過檢測環(huán)境中出現的臨時障礙物,以安全A?算法的有效路徑節(jié)點為DWA 的子目標點進行實時路徑規(guī)劃;最后,通過篩選最小的G(v,ω)值,規(guī)劃出最優(yōu)的安全路徑。

3 仿真與實驗

本文以Matlab 為仿真平臺,依據文獻[18-20],通過合理的參數設置可以提升移動機器人的導航性能。移動機器人的仿真參數設置如下:移動機器人的最大速度為0.6 m/s,移動機器人的最大角速度設置為3.6 rad/s,根據已有文獻的經驗數據設置移動機器人的數據采樣周期為0.1 s,動態(tài)窗口法的評價函數的加權系數ε=0.1、τ=0.2、π=0.4 和γ=0.3。

3.1 仿真驗證與結果分析

3.1.1 驗證安全A?算法

首先驗證所提的基于危險指數的安全A?算法。通過設置安全A?算法、傳統(tǒng)A?算法以及改進A?算法[20],對比并驗證所提安全A?算法的有效性及可行性。依據前文搭建的軍隊裝甲車停車場的環(huán)境模型,設置合理的移動機器人仿真配置環(huán)境。

定期檢查保養(yǎng)車輛、維護車場設施、整理車場內務,離不開移動機器人的廣泛運用,在任務期間對整個車場開展巡視工作可以有效幫助戰(zhàn)士開展工作。據此,本文采用多起點與多終點的仿真設置,通過依次連接環(huán)境中多個起始點和終點,方便移動機器人巡視環(huán)境中的每一個位置。仿真結果如圖10、圖11和圖12 所示,分別為原始A?算法、改進A?方法以及本文方法的路徑生成示意圖。仿真輸出結果如表1所示。此外,本文還依據軍隊車場直角邊多的特點搭建簡單環(huán)境驗證所提方法的有效性,避免偶然性,仿真結果如圖13 所示,表2 對應為輸出結果。

圖10 原始A?算法

圖11 改進A?算法

圖12 本文方法

圖13 簡單環(huán)境仿真結果

由實驗得到的路徑可以看出,原始A?算法在車輛后方(圖10 中序號①處)及墻壁(圖10 中序號②)規(guī)劃的路徑較差,出現了較多的拐點,并且其路徑在轉彎處距離直角邊過近,極易發(fā)生碰撞;由圖11可以看出,改進的A?算法拐點個數有所減少但碰撞風險依然存在;由圖12 可見,本文所提的安全A?算法轉折點最少,且在直角邊處保持了安全的距離,較好地解決了碰撞的問題。

表1 仿真結果表明,本文所提方法在處理軍隊裝甲車停車場的路徑規(guī)劃問題時具備良好的性質,即相對于其他兩種方法,安全A?算法可以獲得安全的路徑,且路徑轉折點更少,路徑長度更短。依據表2 結果,小環(huán)境中的安全A?算法具備路徑安全性,但是小環(huán)境下的仿真效果較其他方法優(yōu)勢不是特別突出。綜上,本文所提方法適用于大環(huán)境的軍隊裝甲車停車場的路徑規(guī)劃問題,且路徑具備良好的性質。

表1 A?、安全A?以及改進A?算法的仿真結果

表2 簡單環(huán)境仿真結果

3.1.2 驗證混合路徑規(guī)劃方法

通過在環(huán)境中設置臨時障礙物代表突然出現的人或其他障礙物,以驗證本文提出混合方法的有效性。依據前文設置的機器人仿真參數,對仿真環(huán)境進行合理設置。仿真結果如圖14 所示。輸出機器人的線速度、角速度以及角度曲線如圖15、圖16和圖17 所示。

圖14 混合方法仿真結果

圖15 移動機器人線速度

圖16 移動機器人角速度

圖17 移動機器人角度

混合方法的平滑路路徑長度為322.99 m,仿真時間為579.1 s。仿真結果表明,混合方法可以有效開展局部避障工作,且能夠避開突然出現的障礙物,具有較好的導航性能。

3.2 實物驗證與結果分析

本文使用樹莓派3b +、rplidar A1 單線激光雷達、STM32F103、24 V 直流霍爾編碼器減速電機自行搭建機器人操作系統(tǒng)(ROS)平臺對本文所提混合方法進行驗證。首先依據搭建的環(huán)境模型,設置固定障礙物;然后給機器人設置起始點和終點。在機器人運行過程中,突然人為加入臨時障礙物,測試機器人的局部避障性能。

圖18 所示為機器人實際環(huán)境下的運行示意圖,共有2 次路徑規(guī)劃實驗,分別對應圖18(a)、(b)、(c)和(d)、(e),其中前一次實驗加入了臨時障礙。從實驗結果可以看出,機器人能夠在保持安全距離的情況下有效避開固定的障礙物以及臨時加入的障礙物,并完成機器人路徑規(guī)劃任務。機器人狀態(tài)參數輸出結果如圖19 所示,分別輸出機器人的位置坐標、線速度隨時間的變化趨勢、角速度隨時間的變化趨勢以及角度隨時間的變化趨勢。本次實驗機器人運行50.1 s,運行距離為6.75 m。結果表明,機器人可有效避障并完成巡檢任務。

圖18 實物驗證結果與現場環(huán)境對應

圖19 機器人輸出結果

4 結論

本文就軍隊車場巡檢機器人工作情景,針對傳統(tǒng)路徑規(guī)劃算法所規(guī)劃路線在拐角處容易發(fā)生碰撞的問題,將危險指數融入啟發(fā)函數,提出一種安全A?算法,使得規(guī)劃的路徑在拐角處能與障礙保持一定距離,大幅降低碰撞的風險;由傳統(tǒng)8 搜索鄰域擴展并去除同方向冗余子節(jié)點,平滑了路徑,提高搜索效率;并將改進的A?算法所規(guī)劃的全局路徑返回與動態(tài)窗口法結合,使得巡檢機器人同時具備了局部動態(tài)避障能力。

本文在依據軍隊車場地形創(chuàng)建柵格地圖中,通過多起點多終點的仿真設置,模擬真實的巡檢情景,對傳統(tǒng)A?算法、改進A?算法和本文所提安全A?算法進行了對比驗證,用簡單地圖作為補充驗證,避免偶然性。實驗表明,本文所提算法能夠有效降低拐角處碰撞風險,且在運行時間、轉折點數、規(guī)劃路徑上有較明顯優(yōu)勢。在仿真地圖設置臨時障礙運行融合算法,證明了本文所提融合算法的可行性和有效性。使用ROS 小車平臺在自行搭建的真實物理環(huán)境下實驗,結果表明算法在真實環(huán)境下同樣具有安全路徑規(guī)劃和避開臨時障礙的能力,進一步驗證了本文算法的可行性和有效性。