基于Bézier曲線的差速驅(qū)動機器人混合避障路徑規(guī)劃算法

甘 新 基

(北華大學 機械工程學院, 吉林 吉林 132021)

目前, 移動機器人廣泛應用于工業(yè)、 航空等領域, 其中差速驅(qū)動機器人可在道路及野外進行持續(xù)、 實時地自主運動, 是一種集環(huán)境感知、 動態(tài)決策與執(zhí)行等多功能于一體的智能化機器系統(tǒng)[1]. 在機器人工作中, 難免會遇到不同的障礙環(huán)境, 為順利完成作業(yè)內(nèi)容, 對避障路徑規(guī)劃的研究尤為重要. 避障路徑規(guī)劃是在有障礙物環(huán)境下, 為機器人探尋一條從初始點至目標點的安全路徑[2]. 根據(jù)對環(huán)境數(shù)據(jù)了解狀況的不同, 可使用相應的算法尋找最合適的路徑, 從而快速通過前方全部障礙物, 順利完成運動任務[3]. 目前關于機器人路徑規(guī)劃的方法已有許多: 許凱波等[4]提出了一種基于雙層蟻群算法和動態(tài)環(huán)境的規(guī)劃方法, 采用凸化處理柵格法建立環(huán)境模型, 加快了機器人路徑規(guī)劃速度, 并針對環(huán)境中不同動態(tài)障礙物的體積和速度, 提出3種避障策略, 完成路徑避障； 王雷等[5]在研究移動機器人避障時, 提出了一種改進的蟻群算法, 通過對信息素啟發(fā)因子及期望啟發(fā)因子實時調(diào)節(jié), 自適應改變揮發(fā)因素, 以免陷入局部最優(yōu), 實現(xiàn)了快速避障. 但上述兩種方法均存在路徑規(guī)劃耗時較長的缺點. 基于此, 本文提出一種基于Bézier曲線的差速驅(qū)動機器人混合避障路徑規(guī)劃算法. 通過分析差速驅(qū)動機器人運動規(guī)律, 了解機器人的運動原理及實時狀態(tài), 用Bézier曲線增強機器人路徑平滑性能, 并引入遺傳算法, 使機器人盡快脫離局部極小解, 以最快速率成功避障并到達目標點.

1 差速驅(qū)動機器人運動規(guī)律分析

本文以雙輪差速驅(qū)動機器人為例, 其模型如圖1所示. 如果機器人兩個驅(qū)動輪軸線的中心方位質(zhì)心為O、 坐標為(x0,y0)、 驅(qū)動輪半徑為R, 則雙輪差速驅(qū)動機器人運動方向可描述為

圖1 雙輪差速驅(qū)動機器人模型Fig.1 Two wheel differential drive robot model

按剛體力學原理[6], 將機器人運動學公式表示為

(2)

(3)

其中θ表示機器人運動方向與x軸的夾角,v表示質(zhì)心O位置的線速率,ω表示轉向角速率,l表示驅(qū)動輪之間的輪距,vl表示左驅(qū)動輪線速率,vr表示右驅(qū)動輪線速率.在車輪運動無滑動且保持純滾動狀態(tài)[7-8]下, 應符合下列收斂條件：

(4)

u=(v,ω),

(5)

(6)

s(q)=(-sinθ,cosθ,0),

(7)

(8)

則可得

(9)

(10)

雙輪差速驅(qū)動機器人在運動時, 可采用依次操控左右兩個驅(qū)動輪的線速率完成移動機器人的轉彎及其余非勻速運動[9], 所以假設采樣周期是T, 則將式(2)離散化后獲得的方程組為

(11)

2 基于Bézier曲線的機器人混合避障路徑規(guī)劃方法

Bézier曲線的定義依賴于某段曲線控制點的明確數(shù)量[10], (N+1)個頂點可描述成N次多項式的曲線.Bézier曲線內(nèi)每個點的參數(shù)方程表示為

(12)

其中：Pi為第i個頂點的坐標值； 基函數(shù)Bi(t)為Bernstein多項式, 表示為

(13)

P(t)=P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3.

(14)

當t在區(qū)間(0,1)內(nèi)變動時, 會產(chǎn)生三次Bézier曲線. Bézier曲線性質(zhì)為： 曲線的初始點與終點和特征多邊形的起點與終點重疊[11]； 一階持續(xù)的Bézier曲線可通過較低次的Bézier曲線相連組成[12-14], 如圖2所示.

圖2 Bézier曲線路徑示意圖Fig.2 Schematic diagram of Bézier curve path

由圖2可見, 該曲線由兩端三次Bézier曲線構成, 其中： 一段曲線包含P0,P1,P2,P3四個點,P0為起點,P3為終點,P1和P2為控制點； 另一段曲線包含Q0,Q1,Q2,Q3四個點,Q0為起點,Q3為終點,Q1和Q2為控制點.為確保相連后的曲線為一階持續(xù)關系, 需滿足下列條件：

P3-P2=Q1-Q0,P3=Q0.

(15)

使用n條三次Bézier曲線描述路徑狀態(tài), 因為軌線初始點與終點已知[15], 且符合一階持續(xù)條件, 因此使用(4×n)個參量進行路徑規(guī)劃.由式(14),(15)可得粒子參數(shù)路徑內(nèi)每個點的公式：

(16)

其中P0為初始點,P1為終點,n為路徑的三次Bézier曲線個數(shù).當t在區(qū)間(0,1)內(nèi)變化時, 會產(chǎn)生第i段的三次Bézier曲線.最終產(chǎn)生的n段三次Bézier曲線即組成了全部路徑曲線, 從而增強機器人路徑平滑性能.

在Bézier曲線基礎上, 引入遺傳算法進一步提高差速驅(qū)動機器人混合避障路徑規(guī)劃的可靠性[16]. 遺傳算法是一種基于自然選擇與遺傳的全局優(yōu)化方法, 該方法通過選擇、 遺傳操作挑選算子, 對參數(shù)編碼字符串實施遺傳操作, 各字符串均對應一個可行解. 該遺傳操作是關于多個可行解構成的群體實施的, 因此在進化時能并行地對解空間不同范圍進行搜索, 令搜索結果接近于全局最佳解, 不會陷入局部極小解[17].

在遺傳算法中, 針對固定的適應度函數(shù), 在線計算時長取決于編碼長度與搜索空間大小. 所以, 本文使用簡化編碼長度的方法, 即將路徑的二維編碼化簡成一維編碼[18], 編碼技術如圖3所示. 圖3中, 初始點就是差速驅(qū)動機器人目前的方位, 目標點是機器視覺預瞄區(qū)域中的局部路徑目標點, 規(guī)劃的路徑就是初始點與目標點之間, 即圖內(nèi)的路邊約束范圍. 在地面坐標系XOY內(nèi), 路徑點序列坐標是二維坐標, 為減少編碼長度, 對坐標系實施轉換.設新坐標為xoy, 其中x軸是初始點與目標點相連的線段, 將x軸均勻劃分為x1,x2,…,xn, 則優(yōu)化的路徑點即化簡為一維y坐標編碼優(yōu)化問題.使用浮點數(shù)編碼方式, 其編碼格式如圖4所示.

圖3 路徑編碼方法Fig.3 Path coding method

圖4 路徑編碼方式Fig.4 Path coding mode

適應度函數(shù)是影響遺傳算法收斂性與平穩(wěn)性的關鍵因素, 本文基于Bézier曲線, 對遺傳算法混合避障路徑規(guī)劃的要求為： 路徑在路邊約束內(nèi)可以動態(tài)避障及路徑最短[19]. 所以在構造適應度函數(shù)中, 應盡可能使適應度函數(shù)的項數(shù)較少, 同時將路徑規(guī)劃的3個要求融入遺傳優(yōu)化算法中.

使用圖3的折線模式即能求解出每個折線的方程表達式. 路邊收斂制約了解空間的區(qū)域范圍, 即每個yi值僅能在路邊收斂區(qū)域中進行取值, 每個點的yi值取值范圍計算方法為： 首先推算每個xi方位和x軸垂直的各直線與路兩旁折線交叉的兩個y坐標值, 再依次向路中心進行收縮.收縮值以機器人遠離路邊的安全距離為準, 安全距離要高于機器人的最大半徑.若yi的取值范圍是(yi1,yi2), 則路邊收斂適應度函數(shù)為

(17)

其中i表示路徑內(nèi)的全部點數(shù).由式(17)可知, 若每個路徑點在路邊安全距離內(nèi), 則其適應度為1, 否則為0.

混合避障是較重要的收斂條件.加入障礙物數(shù)量、 位置與速度信息可通過機器視覺與激光雷達確認.在局部路徑規(guī)劃中, 設機器人以當前的速度行走, 每個障礙物也用當前測量的速度進行勻速直線運動[20].此外, 路徑跟蹤控制會自行操控機器人行走速度的改變, 在進行路徑規(guī)劃時, 不需考慮機器人與障礙物行走速度的改變.混合避障的基礎條件是： 關于某個路徑, 構成路徑的點與障礙物間的最小距離一定大于機器人和障礙物的半徑總和.

若差速驅(qū)動機器人從當前點P0至Pi(xi,yi)所需時間是ti, 從Pi-1(xi-1,yi-1)到Pi(xi,yi)所需時間是Ti-1, 則可得

(18)

其中V表示差速驅(qū)動機器人的當前速率.如果差速驅(qū)動機器人位于ti時段, 第k個障礙物的方位是Obk(xbk(ti),ybk(ti)), 則

(19)

其中(xbk(t0),ybk(t0))為第k個障礙物的初始坐標,Vkx和Vky分別為第k個障礙物的當前速度Vk在xoy坐標系內(nèi)的分量.假設在ti時段, 路徑點Pi(xi,yi)與第k個障礙物的距離為dik：

(20)

則關于隨機一條路徑, 路徑內(nèi)的點與障礙物的最小距離為

Dmin=min{dik}.

(21)

繼而得到混合避障的適應度函數(shù)為

(22)

由式(22)可知, 路徑每個點與每個障礙物的最小距離高于機器人與障礙物的安全半徑總和, 機器人就會安全規(guī)避障礙物, 因此適應度是1, 反之是0.將路徑最短適應度函數(shù)記為

(23)

最終獲得綜合適應度函數(shù)為

fit=fit1×fit2×fit3.

(24)

綜合適應度函數(shù)將3個收斂條件進行有機結合, 使計算更便捷, 同時可防止發(fā)生3項加權求和引起的優(yōu)化不穩(wěn)定問題, 最后使機器人能盡快地脫離局部極小值, 并成功繞過障礙物到達目標點.

3 仿真實驗

在MATLAB 2018 b軟件中進行仿真實驗, 實驗設備處理器為Intel(R)CoreTMi5-4590 CPU@3.30 GHz, 服務器內(nèi)存大小為32 GB, 系統(tǒng)類型為Windows 10, 64位處理器. 設置避障柵格地圖場景大小為20×20, 并在地圖中加入障礙物, 驗證本文方法的避障性能. 設機器人的最大線速度為2 m/s, 最大角速度為20°/s. 將基于雙層蟻群算法和動態(tài)環(huán)境的機器人路徑規(guī)劃方法[4]與改進蟻群算法在移動機器人避障中的應用[5]這兩種方法作為對照組. 實驗分為避障路徑規(guī)劃效果、 相對步數(shù)對比、 避障用時對比三部分, 以驗證本文方法避障路徑規(guī)劃的優(yōu)勢.

3.1 避障路徑規(guī)劃效果對比

為驗證本文方法避障路徑規(guī)劃的效果, 在仿真平臺中模擬障礙物, 設差速驅(qū)動機器人運行初始階段在環(huán)境內(nèi)沒有任何數(shù)據(jù)信息, 其使用傳感器即能監(jiān)測到自身周邊4 m區(qū)域內(nèi)的障礙物, 機器人從初始點(0,0)運動至目標點(16,16), 其引力系數(shù)為15, 斥力系數(shù)為3, 障礙物影響距離是5 m, 步長為0.08, 實驗結果如圖5所示.

圖5 不同方法混合避障路徑規(guī)劃對比Fig.5 Comparison of mixed obstacle avoidance path planning of different methods

由圖5可見, 在相同避障環(huán)境下, 文獻[4]和文獻[5]方法的避障規(guī)劃路徑較本文方法長, 這是因為本文方法在Bézier曲線基礎上引入了遺傳算法, 使避障路徑更平滑, 所以探尋到了全局最優(yōu)路徑. 保證機器人能在最短時間內(nèi)順利抵達目標點, 證明本文方法具有較高的魯棒性與實用性. 因為本文方法在利用Bézier曲線優(yōu)化的基礎上, 構建了路邊約束、 動態(tài)避障及最短路徑的混合函數(shù), 使機器人可以較快地脫離局部極小解, 因此實現(xiàn)了較短路徑的規(guī)劃.

3.2 相對步數(shù)對比

為進一步驗證本文方法在避障方面的優(yōu)越性, 基于上述實驗環(huán)境對比3種方法在相同避障步長下的步數(shù), 步數(shù)越少說明避障效果越好. 避障步長由仿真平臺計算得出, 實驗結果列于表1.

表1 不同方法的相對步數(shù)對比

由表1可見, 在相同步長的情況下, 本文方法步數(shù)明顯小于其他兩種方法, 表明本文方法將路徑規(guī)劃問題轉換為產(chǎn)生Bézier曲線有限點方位優(yōu)化問題, 提升了機器人的運動平滑性, 可有效解決不同環(huán)境下的機器人路徑規(guī)劃極小值問題, 從而提高了混合避障路徑規(guī)劃效率.

3.3 避障用時對比

下面以避障用時作為實驗指標, 驗證本文方法的避障效率. 在上述實驗環(huán)境的基礎上, 以3.1中的初始方位與目標作為起點與終點. 為保證計算避障的可靠性, 進行40次實驗, 計算應用3種方法進行避障的用時. 用時時間越短, 說明避障效率越高, 實驗結果如圖6所示. 由圖6可見, 在40次實驗過程中, 文獻[4]方法的避障用時最長, 為8.4～11.8 min, 避障用時波動較大； 文獻[5]方法的避障用時為7～9.2 min； 而本文方法避障用時較穩(wěn)定, 始終維持在4.9～5.3 min, 低于對比方法, 說明本文方法的避障效率較高. 這主要是因為本文方法利用遺傳算法將二維路徑編碼簡化為一維編碼問題, 提高了脫離局部極小解的速度, 縮短了避障時間.

圖6 不同方法的避障用時結果Fig.6 Results of obstacle avoidance time of different methods

3.4 算法收斂性對比

為進一步驗證本文方法在路徑規(guī)劃方面的優(yōu)勢, 對比3種方法搜索到最優(yōu)路徑的迭代次數(shù), 實驗結果如圖7所示. 由圖7可見, 本文方法搜索的最優(yōu)路徑短于文獻[4]與文獻[5]方法. 經(jīng)計算, 本文方法迭代至40步時路徑長度不再下降, 規(guī)劃的路徑長度為30.19 m； 文獻[4]方法迭代至50步時路徑長度不再下降, 搜索到的最優(yōu)路徑長度為41.03 m； 文獻[5]方法迭代至20步時緩慢下降, 直到57步時路徑長度才趨于穩(wěn)定, 搜索到的路徑長度與文獻[4]方法接近, 為40.02 m. 上述分析表明, 本文方法尋優(yōu)的速度優(yōu)于對比方法, 這是因為本文方法混合了路邊約束、 動態(tài)避障需求及最短路徑需求, 構建的適應度函數(shù)兼顧了算法的收斂性與尋優(yōu)速度, 因此本文方法的路徑規(guī)劃效果較好.

圖7 不同方法的迭代收斂性Fig.7 Iterative convergence of different methods

綜上所述, 針對差速驅(qū)動機器人避障路徑規(guī)劃效率低、 精度不高等問題, 本文提出了一種基于Bézier曲線的差速驅(qū)動機器人混合避障路徑規(guī)劃方法. 先通過分析差速驅(qū)動機器人運動規(guī)律, 用Bézier曲線提高機器人運動平滑性, 再代入遺傳算法將動態(tài)避障需求及最短路徑需求混合為適應度函數(shù), 完成高效率混合避障路徑規(guī)劃目標. 仿真對比實驗結果表明： 該方法規(guī)劃后的避障路徑較對比方法路徑短, 且避障用時最長為5.3 min； 該方法迭代至40步時搜索到的最優(yōu)路徑長度為30.19 m, 優(yōu)于其他兩種對比方法, 避障路徑規(guī)劃效果較理想.