武 剛,許家元,邸 迪

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

0 引言

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

作為機(jī)器人產(chǎn)業(yè)的重要組成部分,巡檢機(jī)器人擁有復(fù)雜的自動引導(dǎo)系統(tǒng),而路徑規(guī)劃部分是其自動引導(dǎo)系統(tǒng)中的關(guān)鍵。近年來,國內(nèi)外學(xué)者在機(jī)器人的路徑規(guī)劃研究中采用了多種算法,包括Dijkstra算法[4]、A*算法[5]、RRT算法[6]、粒子群優(yōu)化算法[7]、蟻群算法[8]、人工勢場法[9]和快速行進(jìn)平方法[10-11]等。

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

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

針對上述問題,本文設(shè)計了一種改進(jìn)快速行進(jìn)平方法,添加路徑評價函數(shù),綜合考慮路徑的長度、機(jī)器人安全性與靈活性等因素,對截止閾值參數(shù)Sat進(jìn)行量化調(diào)整,以產(chǎn)生適用于當(dāng)前環(huán)境下的最優(yōu)路徑。

1 基本原理

1.1 快速進(jìn)行法

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

圖1 快速行進(jìn)法與快速行進(jìn)平方法規(guī)劃的路徑差異

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

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

(1)

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

通過離散化梯度T(x,y)能夠在空間中每點(x,y)迭代求解程函方程。將機(jī)器人所處的環(huán)境空間進(jìn)行離散,通過式(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開始,進(jìn)行迭代求解式(4),在后續(xù)的迭代過程使用前次迭代中已求解過的點來計算其臨近點的T值,直至所有點都被求解完成。

1.2 快速進(jìn)行平方法

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

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

圖2 快速行進(jìn)平方法路徑規(guī)劃地圖與勢場

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

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

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

2 改進(jìn)快速行進(jìn)平方法

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

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

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

F(path)=length(path)

(5)

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

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

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

總體路徑評價函數(shù)表達(dá)式為

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

(6)

機(jī)器人行駛距離L′反映了機(jī)器人在路徑上移動的總距離,體現(xiàn)了路徑的長度,其表達(dá)式為

(7)

式中:m為在規(guī)劃路徑上航路點的數(shù)量。

行駛角度變化量P′通過航路點轉(zhuǎn)向角的總和,度量了機(jī)器人沿路徑行駛過程中方向的變化程度,其表達(dá)式為

(8)

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

(9)

式中:(xobs,yobs)為環(huán)境中障礙物離散點坐標(biāo)。

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

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

3 仿真及分析

3.1 仿真模型參數(shù)設(shè)置

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

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

圖4 仿真場景

其次,對路徑進(jìn)行跟蹤仿真,所使用的機(jī)器人底盤結(jié)構(gòu)如圖5所示,呈菱形結(jié)構(gòu),左右兩側(cè)為控制機(jī)器人運動狀態(tài)的驅(qū)動輪,前后為保持機(jī)器人平衡的萬向支撐輪,通過對左右兩輪差速驅(qū)動完成前進(jìn)、后退、轉(zhuǎn)彎等移動操作[15]。圖中,X-Y為環(huán)境坐標(biāo)系,XR-YR為底盤坐標(biāo)系,以質(zhì)心C為底盤坐標(biāo)系的原點,θ為2個坐標(biāo)系之間的夾角。

圖5 機(jī)器人底盤結(jié)構(gòu)

假設(shè)環(huán)境坐標(biāo)系中機(jī)器人的位姿矢量為ξi=(x,y,θ)T,底盤坐標(biāo)系中機(jī)器人的位姿矢量為ξR=(xR,yR,θR)T,二者通過正交旋轉(zhuǎn)矩陣R(θ)來反映映射關(guān)系。

(10)

機(jī)器人在行駛過程中符合運動學(xué)規(guī)律,因此滿足

vL=ωLr,vR=ωRr

(11)

(12)

式中:vL為左側(cè)驅(qū)動輪的線速度;vR為右側(cè)驅(qū)動輪的線速度;ωL為左側(cè)驅(qū)動輪的角速度;ωR為右側(cè)驅(qū)動輪角速度;r為驅(qū)動輪半徑;l為驅(qū)動輪輪距;ω為機(jī)器人角速度;v為機(jī)器人線速度。

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

表1 機(jī)器人參數(shù)

3.2 路徑規(guī)劃仿真

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

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

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

圖7 不同Sat值下的規(guī)劃路徑

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

圖8 不同Sat值下的規(guī)劃路徑參數(shù)

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

表2 不同Sat 值下規(guī)劃所得路徑的評價函數(shù)值

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

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

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

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

圖9 原算法與改進(jìn)算法規(guī)劃路徑對比

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

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

圖10 原算法與改進(jìn)算法機(jī)器人速度與航向角偏差

實驗結(jié)果表明,本文算法規(guī)劃出的路徑在平滑度方面表現(xiàn)更優(yōu)。與原算法相比,本文提出的算法規(guī)劃出的路徑航向角偏差更加小,速度變化更加平緩,這有利于機(jī)器人的平穩(wěn)行駛。

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

4 結(jié)束語

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