基于快速進行平方算法的巡檢機器人路徑規劃研究

武 剛,許家元,邸 迪

(沈陽大學信息工程學院,遼寧 沈陽110044)

0 引言

近年來,隨著產業智能化程度的不斷提高,各國對于智能化無人系統的研究在不斷深入,與其相關的機器人產業也迎來了發展的黃金時期[1]。巡檢機器人作為一種無需人工駕駛即可自主移動的機器人[2],可以在工廠和制造設施中執行巡檢任務,可以檢查生產線的設備是否運轉正常、是否存在故障,以及檢測可能的安全隱患[3]。

作為機器人產業的重要組成部分,巡檢機器人擁有復雜的自動引導系統,而路徑規劃部分是其自動引導系統中的關鍵。近年來,國內外學者在機器人的路徑規劃研究中采用了多種算法,包括Dijkstra算法[4]、A*算法[5]、RRT算法[6]、粒子群優化算法[7]、蟻群算法[8]、人工勢場法[9]和快速行進平方法[10-11]等。

快速行進平方法(fast marching square method,FMS)克服了人工勢場容易陷入局部最小值的問題,對比粒子群算法與蟻群算法,該算法所得的規劃路徑的一致性較好,相較于Dijkstra算法和A*算法等其他方法,快速行進平方法具有使用簡單、響應速度快等優點,近幾年被廣泛用于解決無人駕駛裝備的路徑規劃問題上。

然而,在將快速行進平方法應用到實際的場景時仍存在不足。具體而言,規劃路徑距障礙物越遠,路徑的安全性就越高,但長度也會相應增加,此時對于截止閾值參數Sat的調整沒有一個合適的評判標準,仍需要進一步的研究探討。

針對上述問題,本文設計了一種改進快速行進平方法,添加路徑評價函數,綜合考慮路徑的長度、機器人安全性與靈活性等因素,對截止閾值參數Sat進行量化調整,以產生適用于當前環境下的最優路徑。

1 基本原理

1.1 快速進行法

快速行進平方法是快速行進法(fast marching method,FMM)的一種變體。其在保持快速行進法的主要優點的同時,通過2次使用快速行進法來規劃路徑[12-13],彌補了快速行進法規劃路徑安全性不足的缺點。圖1顯示了這2種算法的不同之處。

圖1 快速行進法與快速行進平方法規劃的路徑差異

快速行進法是由Sethian提出用來對物理波界面Γ進行跟蹤建模的數值算法,是水平集方法的1個特例[14]。該方法已應用于不同的研究領域,包括計算機圖形學、醫學成像、計算流體動力學、圖像處理及軌跡計算等,可以迭代求解二維或多維界面的傳播問題。

其描述波面運動的Eikonal方程為

(1)

式中: (x,y)為該點的位置坐標;T(x,y)為波界面到達(x,y)所需的時間;F(x,y)為波在該位置的擴展速度。

通過離散化梯度T(x,y)能夠在空間中每點(x,y)迭代求解程函方程。將機器人所處的環境空間進行離散,通過式(2)和式(3)簡化所要求解的點(x,y)處的梯度。

T1=min(T(x-Δx,y),T(x+Δx,y))

(2)

T2=min(T(x,y-Δy),T(x,y+Δy))

(3)

式中:Δx、Δy為離散空間的x和y方向上的間距。

再將式(2)和式(3)代入程函方程并平方,得到離散化形式,即

(4)

從初始界面T(x,y)=0開始,進行迭代求解式(4),在后續的迭代過程使用前次迭代中已求解過的點來計算其臨近點的T值,直至所有點都被求解完成。

1.2 快速進行平方法

Garrido等在快速行進法的基礎上,提出了一種名為快速行進平方法的新算法,該算法有著較好的路徑平滑度與安全距離。

算法將獲取的地圖信息通過合適的分辨率轉化為二進制地圖,如圖2a所示。

圖2 快速行進平方法路徑規劃地圖與勢場

首先,將圖中所有障礙物柵格均被視為波源,波同時從障礙物邊界擴展,地圖中每個柵格的速度值都通過時間值來反饋,這樣就可以得到對應的速度勢場W(x),如圖2b所示。

其次,將機器人的目標點用作波源,從目標點開始擴展,直至到達起始點。利用上一步建立的速度勢場與傳播波速度可以得到時間勢場D(x),如圖2c所示。

在建立速度勢場的過程中,通過改變重要截止閾值參數Sat來調整速度值與時間值的映射關系,進而影響機器人運動范圍,此處Sat(x)的取值區間為wpi∈[0,Sat)→0。Sat值越大,障礙物的邊界范圍越大,實現了機器人在路徑規劃的過程中與障礙物時刻保持安全距離的目的。

2 改進快速行進平方法

為了更直觀地理解不同的截止閾值參數Sat對實際規劃路徑的影響,如圖3所示,本文任意選取2個不同的Sat,通過對不同Sat值下規劃的路徑與速度勢場W(x)比較可見,當Sat值調整的過小時,所規劃的路徑離障礙物距離較小,增加了機器人行駛的危險性,而Sat值調整的過大又會使所規劃的路徑較長,降低了機器人行駛的效率。

圖3 不同Sat值下的規劃路徑與速度勢場

在傳統的路徑規劃方法中,評價函數通常只考慮路徑的長度,即從起點到終點的直線距離。這種評價函數的設計主要基于最小化總移動距離的理念,以提高效率。傳統的評價函數可以表示為

F(path)=length(path)

(5)

式中:F(path) 為路徑的評價函數值;length(path) 為路徑的長度函數,它計算從起點到終點的直線距離。

這種評價函數在解決許多簡單的路徑規劃問題時表現良好,尤其在起點和終點接近直線,且不存在復雜障礙物的場景中。然而,在存在障礙物或起點和終點之間存在復雜路徑的情況下使用快速行進平方法,僅考慮路徑長度可能會導致規劃出的路徑不是最安全或最有效率的。

針對這一問題,設計出了一個適用于快速行進平方法的評價函數。該函數受機器人行駛距離、與障礙物的安全距離以及沿路徑行駛的角度變化量3個因素影響。通過評價函數來綜合評價所規劃的路徑是否適合機器人在當前環境下行駛,從而選擇出最優的截止閾值參數Sat進行路徑規劃,使得機器人能夠高效且安全地行駛。

總體路徑評價函數表達式為

E=k1(1-L′)+k2(1-P′)+k3D′

(6)

機器人行駛距離L′反映了機器人在路徑上移動的總距離,體現了路徑的長度,其表達式為

(7)

式中:m為在規劃路徑上航路點的數量。

行駛角度變化量P′通過航路點轉向角的總和,度量了機器人沿路徑行駛過程中方向的變化程度,其表達式為

(8)

障礙物的最短距離D′通過計算航路點與障礙物的離散點的最短距離,衡量了機器人與障礙物之間的安全距離,體現了路徑的可行性,其表達式為

(9)

式中:(xobs,yobs)為環境中障礙物離散點坐標。

這3個參數具有不同的量綱,如長度、角度和距離等,因此需要進行無量綱化處理,以便用于評價路徑。通過結合數量級分析將這3個參數進行歸一化處理,從而使它們的量綱相同。這樣,這3個參數就可以在路徑評價函數中平等考慮,以便機器人能夠更好地選擇最佳路徑。

在權重系數方面,k1、k2和k3之和為1,這是為了使得路徑評價函數能夠應用于不同類型的機器人。如對于小型機器人,由于其功耗相對較低,重點是規劃路徑要短,以滿足行駛效率;而對于大型機器人而言,其體積更大,操控能力更差,需要充分考慮規劃路徑與障礙物的最短距離和行駛角度變化等因素,以確保其安全行駛。因此,通過根據不同類型的機器人進行權重系數的調整,可以使路徑評價函數更加適用于各種場景,并有效幫助機器人選擇最優路徑。

3 仿真及分析

3.1 仿真模型參數設置

根據上文對快速行進平方法的改進,通過計算機進行仿真模擬,軟件仿真環境為MATLAB R2022b,硬件處理器配置為Intel(R) Core(TM) i7-8750H CPU 2.20 GHz,RAM配置為8.00 GB (7.88 GB 可用),Windows 11(64位操作系統)。此外,為了驗證規劃路徑的合理性,本文建立機器人運動學模型,并通過跟蹤控制驗證路徑是否符合機器人的運動學特性。

首先,本文所使用的仿真地圖如圖4所示,電子地圖文件中深色的區域為模擬巡檢環境的墻壁以及柱子等建筑實體,淺色區域巡檢環境的機器設備、安全門及堆疊的物品,白色區域為巡檢通行區域。通過使用MATLAB將電子地圖文件轉化為大小為418×720的二進制地圖文件,將圖中不可通行區域,在二進制地圖中顯示為黑色,可通行區域顯示為白色,起始點與目標點分別用叉號與星號表示。

圖4 仿真場景

其次,對路徑進行跟蹤仿真,所使用的機器人底盤結構如圖5所示,呈菱形結構,左右兩側為控制機器人運動狀態的驅動輪,前后為保持機器人平衡的萬向支撐輪,通過對左右兩輪差速驅動完成前進、后退、轉彎等移動操作[15]。圖中,X-Y為環境坐標系,XR-YR為底盤坐標系,以質心C為底盤坐標系的原點,θ為2個坐標系之間的夾角。

圖5 機器人底盤結構

假設環境坐標系中機器人的位姿矢量為ξi=(x,y,θ)T,底盤坐標系中機器人的位姿矢量為ξR=(xR,yR,θR)T,二者通過正交旋轉矩陣R(θ)來反映映射關系。

(10)

機器人在行駛過程中符合運動學規律,因此滿足

vL=ωLr,vR=ωRr

(11)

(12)

式中:vL為左側驅動輪的線速度;vR為右側驅動輪的線速度;ωL為左側驅動輪的角速度;ωR為右側驅動輪角速度;r為驅動輪半徑;l為驅動輪輪距;ω為機器人角速度;v為機器人線速度。

機器人任務的起點和目標點分別設置為(25,372)和(439,169)。在仿真中使用的機器人參數如表1所示。

表1 機器人參數

3.2 路徑規劃仿真

為了驗證所使用算法的有效性,在靜態環境下,本文采用不同的截止閾值參數Sat進行了仿真對比實驗。如圖6所示,對選取的Sat值為0.1、0.3時的速度勢場與時間勢場對比。當Sat值為0.1時,通過觀察可以發現,障礙物對周圍區域的影響較小,在規劃路徑時模擬機器人與障礙物的距離較遠,機器人的可行區域范圍較廣,所規劃路徑時將較為平直。當Sat值提高到0.3時,此時障礙物的邊界范圍因Sat值的增大而增大,模擬機器人與障礙物的距離較近,機器人的可行區域縮小,為了保證機器人行駛的安全性,規劃路徑時會比Sat值為0.1時更加曲折。

圖6 不同Sat值下的速度勢場與時間勢場

隨著Sat值的增加,W(x)的障礙物區域不斷增加,因此基于W(x)使用梯度下降法得到的路徑也會隨之變化。通過圖7,對模擬場景進行路徑規劃,可以更加直觀地發現,這些路徑與障礙物的最短距離逐漸增加,增強了路徑的安全性。

圖7 不同Sat值下的規劃路徑

由上述的仿真測試,如圖8所示,本文對不同Sat值下的規劃路徑參數進行對比分析。根據圖8c可知,使用快速行進平方法規劃路徑時,當Sat=0.1時,距離地圖區域的最短距離為0.84 m,而Sat=0.3時距離地圖區域的最短距離1.39 m,說明越大的Sat值距離障礙物距離越大。由于巡檢環境的工作區域常常在墻壁附近,這對于機器人行駛來說是不安全的。為了保證機器人的行駛安全性,應盡可能遠離障礙物,因此需要選擇較大的Sat值。但圖8b的結果顯示,隨著Sat值的增大,規劃路徑的長度從839.04 m增加到985.52 m,增長率為14.85%。規劃機器人行駛距離增加必然會導致規劃時間的增加,進而影響機器人的運輸效率與經濟性。為此在考慮機器人安全性的同時也需要考慮規劃路徑的長度。

圖8 不同Sat值下的規劃路徑參數

根據本文提出的路徑評價函數(式(6)),選取的多個Sat值進行仿真模擬,通過計算得到的評價函數結果如表2所示。

表2 不同Sat 值下規劃所得路徑的評價函數值

由于本文使用的巡檢機器人具有體型小、質量輕、靈活性好的特點,權重系數分配如下:k1=0.4、k2=0.3、k3=0.3。通過表2的對比,評價函數的最優值在Sat=0.15時取得,其值為0.492 6。

因此,在綜合考慮機器人行駛的安全性、經濟性和控制復雜性后,對于本文選取的地圖區域,Sat=0.15是最合適的閾值參數?？梢砸源薙at值下規劃所得的路徑作為機器人在本地圖區域從任務執行的起點到目標終點行駛的全局路徑。

為了進一步評估新算法的性能,在原有路徑規劃的基礎上引入了PID控制器,將其與原算法進行對比分析。在仿真實驗中,采用相同的實驗條件和參數設置,并對2種算法的路徑規劃結果進行比較。

首先,本文對比了2種算法在路徑長度與安全性方面的表現,如圖9所示。

圖9 原算法與改進算法規劃路徑對比

實驗結果表明,在保障巡檢車輛行駛安全性的同時,本文算法下,機器人能夠規劃出更短的路徑。與原算法相比,本文提出的算法規劃出的路徑長度降低了10.75%,與此同時,這表明本文算法在路徑規劃效率方面具有優勢。

此外,本文還對比了2種算法在平滑度與操作性方面的表現,如圖10所示。

圖10 原算法與改進算法機器人速度與航向角偏差

實驗結果表明,本文算法規劃出的路徑在平滑度方面表現更優。與原算法相比,本文提出的算法規劃出的路徑航向角偏差更加小,速度變化更加平緩,這有利于機器人的平穩行駛。

綜上所述,通過與原算法生成的機器人軌跡的對比分析發現,本文提出的新算法在路徑長度、安全性和平滑度方面均具有優勢。這些結果表明,本文的算法充分利用無人巡檢機器人的移動特征,具有重要的實際應用價值。

4 結束語

針對目前快速行進平方法在實際場景中,對截止閾值參數Sat值的選取沒有合適的評價尺度這一問題,本文提出了一種改進快速行進平方法。通過引入路徑評價函數,并對機器人行駛距離、機器人行駛角度變化量、與障礙物的距離進行無量綱化處理,以獲得當前環境下最優的Sat。經過仿真實驗的驗證,改進后的快速行進平方法可根據Sat值生成適用于不同的車型與環境特點的最優路徑。