基于最優控制策略的復雜環境移動機器人軌跡規劃

張泮虹 倪 濤 趙亞輝 翟海陽 趙澤仁

(燕山大學車輛與能源學院， 秦皇島 066000)

0 引言

自主移動機器人主要包括前端感知、中間規劃、終端控制3部分[1-3]。規劃層主要是為移動機器人計算出一條由起點到終點的無障礙的可行路徑[4]，是影響移動機器人移動行為質量的最直接因素。

目前，路徑規劃方法主要包括圖搜索、勢場、隨機采樣、曲線插值、機器學習以及優化方法[5-6]。

圖搜索的方法是對環境進行網格化建模，然后在圖中搜索符合任務要求的路徑。在對建模要求精度不高的條件下，基于A*的圖搜索[7-8]能夠快速生成質量較高的移動路徑，對于建模精度要求較高的運動規劃任務需要對圖搜索獲得的粗糙路徑進行精細化處理之后才能用于底層的控制環節[9]。

勢場方法[10]是對環境中的障礙物和非障礙物建模為斥力場和引力場，通過對勢場函數進行改進[11-12]可適應不同的應用環境。基于勢場的算法是[13-15]通過修改傳統人工勢場法斥力場函數獲得平順且安全的無碰撞路徑，但是易陷入局部最優[16]。

隨機采樣的方法是在環境中隨機生成一系列采樣點，然后篩選出滿足任務要求的序列作為路徑。相比于圖搜索，隨機采樣可以處理較高維度的運動規劃問題，具有概率完備性。主要包括概率路標算法(PRM)[17]和快速搜索隨機樹算法(RRT)[18]，其中RRT由于其規劃出的路徑可行性強而應用廣泛。盡管隨機采樣的方法具有概率完備性，但是無法處理復雜的約束條件，其生成運動狀態具有盲目性，導致最終解的不穩定，且解通常不是最優的[19]。

曲線插值[20]的方法是通過預設的路徑點擬合生成連續性、平滑性較好的路徑。常見的曲線插值方法包括杜賓曲線法、Reeds-Sheep曲線法、多項式樣條曲線擬合法，等等。該方法生成的路徑一般具有可跟蹤的性質，因此通常是配合其他規劃方法對軌跡進行平滑處理或生成初始路徑，比如王明強等[21]用三次樣條曲線獲取道路基準線。

機器學習的方法是以作業需求、機器人的始末狀態、環境設置等信息作為輸入量，通過增強學習模型[22]、卷積神經網絡模型[23]、隱式馬爾可夫模型[24]、高斯混合模型[25]等進行學習，輸出移動機器人的行駛路徑/軌跡，然而機器學習需要大量的訓練樣本，并需要提前進行離線學習，不適合解決未知環境下實時性的路徑規劃問題[26]。

基于優化的方法是在動態系統模型的基礎上加入約束條件以及任務需求，構成了最優控制問題，以最優控制問題的形式描述移動機器人的規劃任務，具有直觀、準確、客觀的特點，這是前述幾種方法所不具備的。目前計算最優控制方法已應用于車輛泊車[27]、避障[28]等結構化環境的駕駛情景中，基于最優控制形式建立的模型中一般包含復雜約束條件，一些研究將這些復雜的約束條件進行簡化并利用現代智能優化算法(如粒子群[29]、隨機分形搜索[30]等)進行求解。從目前的研究來看，基于最優控制思想去解決軌跡生成問題主要有以下難點：用于描述車輛運動學特性的方程是非凸的；用于限制車輛和障礙物碰撞的幾何約束是高度非凸且不可微；避碰約束的維度和障礙物的密度是密切相關的，障礙物較多環境下會導致規劃時間急劇增加；規劃過程會產生大量的局部最優。因此，在復雜環境下的軌跡規劃仍是一個需要研究的問題。

本文在上述研究的基礎上，提出一種基于最優控制的復雜環境下移動機器人的軌跡規劃方法，首先針對優化求解問題建立運動學模型、幾何模型、變量極值點約束以及避障模型等約束，通過對控制變量進行拉格朗日離散化，針對離散化階段的約束失效進行等距時間離散并引入懲罰函數，最后通過隨機分形搜索算法對所構建的優化問題進行求解。

1 優化約束模型

1.1 移動機器人的數學模型

由于機器人在復雜路面上的行駛速度較小，輪胎的側滑影響較小，因此移動機器人的運動學模型是基于自行車模型構建的。如圖1所示，其數學表達式為

圖1 移動機器人運動學模型Fig.1 Kinematic model of mobile robot

(1)

式中t——時間

tf——移動機器人整個運動過程總耗時

(x(t),y(t))——移動機器人后輪軸線中點坐標，表示機器人實時位置

v(t)——機器人后輪(點P)實時線速度

θ(t)——機器人實時方向角

φ(t)——機器人前輪實時轉向角

L——機器人前后輪之間的縱向距離

此外，為了保證后續規劃路徑在規避障礙物時更加有效，本文在運動學建模時加入了M、N兩個量，分別表示移動機器人后懸長度和前懸長度，更加符合實際情況。

從模型的數學表達式可以看出，如果機器人的實時速度v(t)和方向角θ(t)已知，那么其位置P(x(t),y(t))和前輪的實時轉向角φ(t)均可以通過數學積分來推導得出。因此將u=(v(t),θ(t))作為控制向量，χ=(x(t),y(t),φ(t))作為狀態向量。

此外，由圖1所示的幾何關系，可以得出移動機器人的幾何模型，即4個邊角點的坐標，以便于后續避障模型的搭建。

(2)

式中B——前后輪之間的橫向距離的一半

在移動機器人的路徑規劃中，最重要的原則是保證機器人避開環境中的所有障礙物；其次，也將移動機器人本身硬件所具有的最大速度和轉向角加入優化條件中；此外，對移動機器人終點狀態也做了相應的約束。

1.2 邊界約束

在移動機器人的整個運動過程中，其線性速度不宜過高，轉向角也存在極大值，即

(3)

1.3 環境約束

環境約束主要包括移動機器人的始末狀態約束以及保證機器人在移動過程中不與環境中的障礙物發生碰撞。本文將障礙物建模為矩形，通過不規律的放置來使環境的復雜程度加大。

假設Q(X,Y)表示移動機器人的某一邊角點，如圖2所示。A、B、C、D分別表示矩形障礙物的4個邊角點，設矩形的幾何尺寸為m×n，其中，lAQ=(X-m,Y-n)，lAD=(-m,0)，lAB=(0,-n)。

圖2 避障模型構建Fig.2 Obstacle avoidance model construction

若點Q在障礙物矩形區域內，即說明0

(4)

本文所述算法對移動機器人的終點狀態做了相應的約束，在實際應用中，根據移動機器人不同的任務需求其終點狀態也可以做相應的改變。本文定義機器人的終點是以v(tf)=0的狀態進入到預先定義好的矩形框區域內。由于凸集特性，本文所定義的終端條件等效于移動機器人的4個角點位于終端矩形框內部的約束。移動機器人終端約束類似于矩形障礙物約束，此處不再贅述。

2 優化求解策略

本文將路徑規劃問題歸結為一個最優控制問題，主要包括離散化和目標函數設計兩部分。

2.1 離散化階段

將[0,tf]區間平均分成Nf份，每一個子區間為[ti-1,ti](i=1,2,…,Nf)。如圖3所示，在每個子區間內設置K+1個插值點{zi0,zi1,…,ziK}，并通過拉格朗日多項式來描述控制變量，以v(t)為例，即

圖3 拉格朗日離散化Fig.3 Lagrange discretization

(5)

其中，τ∈[0,1]，τi表示高斯點，當K確定時可以離線計算。因此當插值點足夠多時，就能很好地表示控制變量v(t)和φ(t)。此外，為了保證子區間之間變量的連續性，需要保證前后區間端點處的插值點相等，即

(6)

即

ziK=z(i+1)0

(7)

在用插值點描述控制變量之后，移動機器人的狀態變量就可以通過數值積分的方式進行計算，如圖4所示，在數值積分之前，把前述的分段拉格朗日多項式等距時間均分成Nsp(足夠大)份，即v(t)可以表示為{v0,v1,…,vNsp-1}。

圖4 等距時間離散化Fig.4 Isometric time discretization

2.2 目標函數設計

上一階段僅僅是將連續變量通過拉格朗日插值的形式進行離散化，而第1節所述的約束條件也僅僅是針對插值點，但是插值點之間的時刻并沒有被約束，這將引起如下問題：如圖5所示，即便插值點1、2、3均滿足控制變量約束條件，但由于控制變量的連續性，也仍然會存在某些不符合約束條件的時刻。此外，如圖6所示，盡管在離散化情況下機器人的運動軌跡滿足了障礙物約束，但在連續時間內，圖示箭頭所指機器人運動的下一時刻將會和障礙物發生碰撞，從而導致任務失敗。

圖5 連續變量越界Fig.5 Continuous variable out of bounds

圖6 連續變量避障失效示意圖Fig.6 Continuous variable obstacle avoidance failure

針對如圖5所示的問題，在數值積分之前， 將重新均分的各個離散化點，即集合{v0,v1,…,vNsp-1}中的每個元素都進行邊界約束檢查，若vi超出邊界線，則令vi=vmax或者令vi=vmin，將有效減少控制變量超出邊界的情況。

針對圖6所示的問題，本文在目標函數設計中加入了懲罰函數，對于機器人運動過程中避障，基于式(4)，得到障礙物的懲罰值

Qc=min((lABlAB-lAQlAB),lAQlAB,
(lADlAD-lAQlAD),lAQlAD)

(8)

式(8)保證了點Q是以最小的代價位于障礙物外的，對于移動機器人終端約束也可以用類似的思想，以保證最終機器人位于終點所定義的矩形框內。

因此，最終懲罰項

(9)

為了將求解軌跡的可行解與不可行解區分開，本文引入一個足夠大的定值N來保證可行解的懲罰值絕對小于不可行解的懲罰值。顯然，若路徑能保證無障礙和滿足最終狀態，那么其懲罰值為0。

3 優化求解器

根據前文所述，將離散化的插值點和移動機器人的運動時間tf作為決策變量。然后，通過積分得到狀態變量，檢查其邊界約束，并將障礙物懲罰函數加入優化準則中。考慮到元啟發算法具有全局優化的能力，本文采用了隨機分形搜索算法[31]作為整個優化過程的求解器。

分形指的是具有自相似性的現象、圖像或者物理過程等，隨機分形表現在結構或復雜度上具有統計意義上的自相似性，一般是通過萊維飛行、高斯游走等隨機規則來產生。隨機分形主要包括擴散和更新兩個過程，擴散過程是基于粒子的當前位置，因此有利于尋找全局最優解。

首先是種群的初始化，在這個過程中，基于約束條件隨機初始化種群中的每一個個體，第j個個體的初始化方程為

Pj=LB+ε(UB-LB)

(10)

其中，LB和UB分別是求解問題向量的上下邊界，本文指的是控制變量轉向角和速度的上下限值，ε是在區間[0,1]上服從均勻分布的隨機數。

初始化種群之后，計算個體的適應度函數以獲得最佳個體BP，然后所有個體都圍繞當前的位置游走以搜索環境空間，隨機搜索算法采用高斯游走作為唯一的游走方式，即

(11)

式中Pi——第i個個體的位置

ε、ε′——區間[0,1]上服從均勻分布隨機數

μBP、μP——高斯函數均值

σ——高斯參數標準差

μBP=|BP|，μP=|Pi|，高斯參數中的標準差為

(12)

第1次更新過程，首先根據個體的適應度進行排序，然后賦予每個個體性能級別，個體適應度為

(13)

式中N——種群的個體數量

rank(Pi)——個體Pi在種群中的排序

之后判斷是否滿足Pai<ε，若滿足則更新個體Pi的第j個分量，否則保持不變，更新公式為

P′i(j)=Pr(j)-ε(Pt(j)-Pi(j))

(14)

式中P′i——個體Pi更新后的位置

Pr、Pt——種群中隨機選擇的個體

第2次更新過程主要是根據個體的分量進行，對于更新后的P′i，判斷是否滿足P′ai<ε，若滿足則更新P′i的當前位置，否則保持不變，更新公式為

(15)

式中P′r、P′t——第1次更新后種群中選擇的個體

若P″i對應的適應度優于P′i對應的適應度，則用P″i更新替換掉P′i。

4 仿真實驗

4.1 實驗設置

仿真實驗在Matlab 2019b中展開，計算機環境是Windows10,處理器Intel(R) Core(TM) i7-10870H CPU @ 2.20 GHz 2.21 GHz，RAM 16 GB，本文所述算法中提到的一些參數設置如表1所示。為了驗證本文所述算法的有效性，鑒于現在大多數的規劃算法均是將機器人建模為質點，與真實情況相差較大，在本仿真實驗中，引入了“足跡”的概念，即表示各個時刻移動機器人在地面上的投影，以足跡和障礙物不發生交集來作為避障有效的判定，如1.3節所述。將建好的模型經拉格朗日離散化處理，然后代入SFS算法中進行迭代求解，最終得到最優解。

表1 基于優化算法的參數Tab.1 Parameter based on optimization algorithm

4.2 障礙物較少場景驗證

為驗證本文所述算法的有效性，場景1隨意設置10個矩形障礙物，用戶可根據環境或者任務不同隨意指定起點，本文取x1(0)=-40 m,y1(0)=15 m，終點設置在以(0,0)為中心，長為2.5 m、寬為1.5 m的終端盒子內。初始方位角為0°，初始轉向角為0°，初始速度為0。從圖7a可以看出，所規劃的軌跡可以實現準確避障，即說明優化模型中的避障模型是有效的，此外終端約束也滿足，圖7b中的速度最大值控制在3.0 m/s，前輪轉角控制在0.714 rad內，即邊界約束有效，同時可以看出移動機器人的足跡和障礙物矩形的交集為空，因此針對離散化導致的避障失效問題也得到了有效解決。

圖7 障礙物較少場景Fig.7 Scenarios with fewer obstacles

4.3 狹窄區域場景驗證

場景2設置了10個障礙物并設置了一個狹窄區域，起點設置為x2(0)=-50 m,y2(0)=5 m，終點設置在以(0,0)為中心，長為2.5 m、寬為1.5 m的終端盒子內。初始方位角為0°，初始轉向角為0°，初始速度為0。從圖8a可以看出，所規劃的軌跡可以實現準確避障，即說明優化模型中的避障模型是有效的，此外終端約束也嚴格滿足，圖8b中的速度最大值控制在3.0 m/s，前輪轉角控制在0.714 rad內，即邊界約束有效，同時可以看出移動機器人的足跡和障礙物矩形的交集為空，因此針對離散化導致的避障失效問題也得到了有效的解決，因此本文所述方法在狹窄區域內仍具有一定的穩定性。

圖8 狹窄區域場景Fig.8 Narrow area scenario

4.4 復雜場景驗證

場景3設置了30個障礙物矩形區域，起點設置為x3(0)=-40 m,y3(0)=0 m，終點設置在以(0,0)為中心，長為2.5 m、寬為1.5 m的終端盒子內。初始方位角為0°，初始轉向角為0°，初始速度為0。從圖9a可以看出，所規劃的軌跡可以實現準確避障，即說明優化模型中的避障模型是有效的，此外嚴格滿足終端約束，圖9b中的速度最大值控制在3.0 m/s，前輪轉角控制在0.714 rad內，即邊界約束有效，同時可以看出移動機器人的足跡和障礙物矩形的交集為空，因此針對離散化導致的避障失效問題也得到了有效的解決，在復雜環境下進一步證明了本文所述方法具有較強的魯棒性。

圖9 障礙物較多場景Fig.9 Scenarios with many obstacles

4.5 仿真實驗對比分析

采用Matlab隨機生成了10個場景，由于混合A*算法考慮了機器人的運動學模型，而且算法中也加入了非線性優化和非參數插值，因此將本文所述優化算法與混合A*算法作比較，結果如表2所示，成功率是指移動機器人足跡不與矩形障礙物產生交集，如圖10虛線圓處所示，計算第4障礙物和第6障礙物與混合A*算法所規劃軌跡的最近距離，分別為0.312 m和0.965 m，而本文設定的移動機器人的寬為2 m，長為4 m，也就說明移動機器人在實際移動中會與障礙物發生碰撞，導致避障失效，即混合A*算法在狹窄區域內并不有效，同時，基于優化算法所得到的軌跡較短，時間也相對較小，效率更高。這里的軌跡長度是對求得的離散軌跡點求和，即

圖10 本文算法與混合A*算法運行軌跡Fig.10 Proposed algorithm and hybrid A* algorithm running track

表2 本文算法與混合A*算法性能對比Tab.2 Performance comparison between proposed algorithm and hybrid A* algorithm

(16)

此外，與現有文獻中的基于優化的規劃算法比較，用于驗證本文所提算法的求解質量。選取文獻[32]中提到的優化算法作為基準，用于驗證其余算法的最優性損失，假設算法X以代價值cX取得最優解，文獻[33]算法以cbaseline為代價值取得最優解，那么算法X的最優性損失計算式為[34]

(17)

很明顯最優性損失越小，說明求解質量越高。通過計算，本文所述算法最優性損失為19.72%，文獻[32]算法為45.1%，文獻[33]算法為34.61%，因此本文所述算法在軌跡規劃的求解上質量更高，輸出的軌跡具有更好的性能。

此外，驗證SFS算法中迭代次數對于求解結果的影響。選擇場景1，對不同迭代次數下求得的目標函數值作比較，如表3所示，從表3可以看出，迭代次數在大于500以后，目標函數值的降低率趨緩，因此迭代次數并不是越大越好，選擇合適的迭代次數會增加求解效率。

表3 不同迭代次數SFS算法的目標值Tab.3 Target values of different iterations of SFS algorithm

5 結論

(1)將軌跡規劃轉化為最優控制問題，通過設置控制變量極值、障礙物避碰、起點終點狀態等優化限制條件，采用隨機分形搜索算法的元啟發策略進行求解，得到了移動機器人精確的移動軌跡，本文所述方法在復雜、狹窄區域均具有較好的表現。

(2)通過等距時間離散，解決了由離散化導致的離散點之間約束失效的問題，Matlab仿真實驗顯示移動機器人的線速度不超過最大速度3.0 m/s，轉向角不超過最大轉角0.714 rad，均滿足變量的邊界約束。