基于Lattice算法的多智能車編隊運動規劃方法

王 偉，常彥文，余 濤，雷 琦，邢曉龍

(1.北京石油化工學院信息工程學院，北京 102617；2.北京星航機電裝備有限公司，北京 100074)

0 引言

近年來，自動駕駛技術不斷取得新的進展,多車協同編隊技術也引起了機器人研究界廣泛關注。多車協同系統可以確保實現一個共同的任務目標,被廣泛應用在搜索、運輸等領域[1]。相比單車而言,多車不僅需要滿足編隊條件,還需要滿足運動過程中導航和避障等其他約束條件，運動規劃在導航和避障過程中扮演著重要角色。目前常見的運動規劃算法分為三類：一是基于搜索的路徑規劃A*[2]和動態窗口法(DWA)[3]算法，通過對A*算法改進，不僅消除A*算法的冗余點，還融合了Lattice算法[4]，能夠滿足車輛的非完整性約束，通過對DWA進行改進，修正速度窗口和三個現有評價函數,使多車系統在未知環境下保持隊形[5]；二是基于采樣的路徑規劃RRT*[6]算法,針對復雜環境提出改進RRT的多機器人編隊規劃算法，通過在規劃過程中加入多機器人之間位置約束,保證了多機器人在編隊規劃過程中隊形保持不變[7]；三是基于智能算法的路徑規劃遺傳算法[8]和蟻群算法[9],通過構建機器人運動行為模型和協同行為模型,引入多蟻群系統改進信息素更新機制和狀態轉移方程,改進了蟻群算法,使多機器人在三維環境中協同行進[10]。這些算法的共同點在于在一定的約束條件下,選出車輛(機器人)從當前點到目標點的最優路徑，這些方法在軌跡規劃中得到了廣泛的應用。但是這些方法也存在固有的缺點，例如：生成的軌跡形狀有時候無法處理復雜的障礙約束；由于該方法沒有考慮軌跡的最終狀態約束[11]，在非可執行軌跡的生成和評估中消耗了大量的計算資源,而且多車編隊的運動規劃還需要考慮各車輛的非完整性約束和位置約束。如何在結構化場景中動態地規劃出符合車輛行駛的運動軌跡并且保證車輛的協同行駛是亟待解決的問題。

狀態空間采樣具有充分利用參考路徑的幾何約束和生成路徑的空間分布一致性好的優點[12]，吸引了大量學者的研究[13-14]。本文針對車輛非完整約束底盤，提出一種基于改進的Lattice算法的多車運動規劃方法。

1 多車編隊方法

本文在考慮多車編隊隊形的同時加入位置約束并且設計跟隨、導航避障和等待的行為策略。跟隨者能夠根據感知信息動態變換跟隨方式,并且兼顧編隊隊形保持和避障。

1.1 多車編隊行為設計

本文針對多車在結構化環境編隊行駛及避障,提出多車保持領航-跟隨的編隊模型。設計了領航者的導航、避障和等待行為;增添了跟隨者的跟隨、自主導航避障行為,并且通過位置約束調節跟隨者的線速度和角速度。當跟隨者感知到障礙物且編隊模型無法規避時,選擇自主導航模式；未感知到障礙物體時則繼續選擇編隊模型位置約束保持隊形,這使得編隊避障更加靈活。多車編隊行為框圖如圖1所示。

圖1 多車編隊行為框圖Fig.1 Multi-vehicle formation behavior diagram

1.2 編隊模型建立

本文以仿真環境中三臺車保持三角陣型為例, 首先定義單車模型的非完整性約束模型，然后定義多車編隊模型,最后將改進運動規劃算法加入模型中，在Gazebo仿真環境下進行驗證。

1.2.1單車輛模型

無人車模型屬于典型的受非完整性約束的動力模型，其控制裝置包括加減速和左右轉向兩個控制量，采用阿克曼轉向結構。本文則利用車輛動力學模型對提出的改進算法進行驗證，狀態變量為(x,y,θ,v,φ),運動約束方程如式(1)所示，無人車運動模型如圖2所示。

圖2 無人車運動模型Fig.2 Motion model of unmanned vehicle

(1)

以小車的車身中點坐標(x,y)表示車輛在地圖中的參考位置，θ表示小車的前進方向，v表示小車前進速度(單位：m/s)，φ為轉向角度，S為前輪軸到后輪軸之間的距離，小車的控制變量u1、u2分別為小車的加速度(單位：m/s2)和車輪轉向速度(單位：rad/s)。由于小車是阿克曼轉向模型，所以約束小車轉向角度|φ|≤φmax，控制輸入|u1|≤u1max，|u2|≤u2max。

1.2.2編隊運動模型

以下針對有限信息設計距離-角度的編隊運動模型，在xOy中描述無人車位姿,設(x,y)是無人車的位置坐標,θ為其方向角。給定一組無人車{F1,F2,F3,…}，F1是領航者，其坐標位置向量為 [x1y1θ1]T,Fj是跟隨者(j∈[2,n])，Fj坐標位置向量為[xjyjθj]T。無人車的輸入是其線速度和角速度，即[viωi](i=1,2,…)。以其中兩輛車為例，領航-跟隨的編隊關系如圖3所示。

圖3 領航-跟隨編隊坐標關系Fig.3 Coordinate relationship of leader-follower formation

根據圖3，ρ為領航者質心到跟隨者質心之間的距離，利用歐式距離來計算：

(2)

α為領航者和跟隨者方向角差值，α=|θ1-θ2|。β為跟隨者y軸到領航無人車質心的視角,β=360°-γ,令ε=β+α，則領航-跟隨編隊運動系統模型為

(3)

(4)

1.3 編隊變換控制調節

跟隨者自主導航避障之后，繼續變換編隊控制，使用比例-積分方式調節方向和角度偏差，進而快速恢復隊形。其角速度、線速度計算公式為

(5)

(6)

式中，(dxdy)為自定義跟隨者和領航者保持的x和y方向的距離，kx、kz為調節變量。

2 Lattice運動規劃算法

針對Lattice規劃算法全局路徑擬合計算量大的問題，提出將矢量地圖法融合進Lattice算法中,并且優化車輛的碰撞形狀,將車輛矩形的碰撞形狀近似成一組半徑相同的圓,進而改進Lattice算法的代價函數。改進Lattice算法在進行采樣軌跡規劃時,需要將笛卡爾坐標系和Frenet[15]坐標系相互轉化，以降低軌跡擬合的計算量[16]。

2.1 Lattice算法分析

Lattice規劃算法,主要由狀態采樣、軌跡生成、碰撞檢測和評價函數組成。

圖4 基于Frenet坐標的狀態采樣Fig.4 State sampling based on Frenet coordinates

2)將t1起始狀態、t2末狀態作多項式擬合，縱向擬合多項式函數s(t)、橫向擬合五次多項式函數d(s)為

s(t)=c1t5+c2t4+c3t3+c4t2+c5t+c6，

(7)

d(s)=k1s5+k2s4+k3s3+k4s2+k5s+k6。

(8)

3)將擬合橫向和縱向軌跡進行合成，計算t*時刻的縱向偏移量s*=s(t*)和橫向偏移量d*=d(s*)，再通過全局參考線還原一個二維平面內的軌跡點Pt*=(x*y*t*)T，Pt*從t*獲得一系列的軌跡點P1，P2，P3，…,最終形成一系列完整的軌跡[4]。

2.2 Lattice算法改進

Lattice規劃算法需要有一條粗略的全局路徑作為先驗信息，但是并不要求參考路徑是無碰撞的或曲率連續的，為了平滑地跟蹤參考路徑，并對周圍環境的實時感知進行響應，提出一種改進的Lattice算法。

2.2.1Lattice全局軌跡的改進

Lattice算法中首先將車輛的初始狀態和末狀態轉換至Frenet坐標下，然后通過五次多項式擬合形成橫向軌跡和縱向軌跡，最后進行二維合成形成全局軌跡。但是對于延伸數公里的結構化道路地圖，使用多項式擬合法生成全局軌跡，計算量將會成指數增長。本文提出使用輸入矢量圖的方法，使用矢量圖可以更輕松快速地進行全局軌跡輸出，其流程圖如圖5所示。矢量映射組件如表1所示。

圖5 生成全局軌跡流程圖Fig.5 Generate global trajectory flow chart

表1 矢量地圖元素信息Tab.1 Element information of vector map

2.2.2Lattice碰撞策略和評價函數改進

碰撞測試采用環境感知信息進行,可以表示為占用網格圖。為了降低碰撞測試的計算復雜度，本文將車輛的矩形形狀改用一組半徑相同的圓來近似。如圖6所示,為保證安全,障礙物與圓中心之間的所有距離都必須大于圓半徑。

圖6 車輛形狀圓形分解Fig.6 Circular decomposition of vehicle shape

在此車輛形狀的基礎上對Lattice設置如下5個評價函數，并對每個評價函數設置權重：

1)障礙物橫向距離ρa，計算障礙物各輪廓點到各軌跡的垂直距離ρa作為障礙物橫向距離代價因子，障礙物與軌跡重合則設置最大值；

2)障礙物縱向距離ρc，計算車輛沿每一條備選路徑方向上距障礙物輪廓點的縱向距離ρc，障礙物不在備選路徑上則設置為0；

3)車輛橫向偏移ρo，車輛距離中心軌跡的距離ρo，設置此代價因子主要是為了讓車輛盡可能遵循全局軌跡行駛；

4)橫向加速度ρm，此代價函數為了車輛平穩地變道，ρm值越小，變道越穩定；

5)向心加速度ρn，為了保證車輛能在轉彎或掉頭處減速慢行。在彎道處車速慢的軌跡其向心加速度就會相對較低，更容易被選出來。

最終評價函數總表達式為

(9)

式(9)中，Ci表示每一條候選路徑計算出的評價值(i=1，2，3，…),λ{a,c,o,m,n}代表權重系數，ρ{a,c,o,m,n}代表各項評價函數，i是候選軌跡索引，εi是被評估的每條候選者軌跡，最終選擇最小評價值軌跡。

2.2.3改進Lattice算法流程

改進的整體算法流程圖如圖7所示。首先通過環境感知信息獲取車輛的初始狀態,給定車輛一個目標點，使用矢量地圖法輸出全局參考路徑,然后截取車體全局路徑的部分距離做狀態采樣,將車輛目前行駛狀態和截取部分路徑末狀態做多項式擬合,形成橫向和縱向軌跡,接著給定t*時刻計算橫向和縱向偏移量，再通過矢量地圖法輸出的全局參考軌跡還原成二維平面的軌跡點，最終形成多個完整的局部軌跡。通過碰撞測試和設定的評價函數計算每條候選軌跡的評價值,選擇評價最低的作為最優軌跡。

圖7 整體改進算法流程圖Fig.7 Overall improved algorithm flow chart

3 實驗驗證

對改進Lattice算法在C++環境下進行實驗，為了評估改進方法，有效驗證算法的真實性，本文采用Gazebo仿真環境。

3.1 改進Lattice算法全局規劃實驗

在此針對改進Lattice算法全局規劃進行仿真，使用式(1)中無人車運動模型，約束無人車轉向角度|φmax|= 0.8 rad，角速度|ωmax|=0.8 rad/s，約束線速度vmax=8 m/s，加速度|u2max|=4 m/s2，設置車輛的初始位置為(25，10)，給定的終點為(32，65)。為了驗證矢量地圖法輸出的全局路徑是否符合車輛的運動學模型，在實驗中構建S型彎道的全局路徑。圖8為在Gazebo仿真環境中建立的環境模型下，矢量地圖法輸出的全局路徑與無人車運動模型行駛軌跡對比。矢量地圖輸入的S型全局路徑，在路徑上沒有障礙物時，車輛基本按照全局路徑行駛，圖中輸入的全局路徑和車輛行駛的軌跡路線基本一致。在非完整性約束下，從圖9、圖10兩圖可看出無人車行駛的線速度、角速度v≤vmax=8 m/s，|ω|≤ωmax=0.8 rad/s，均滿足運動學要求。因此矢量地圖法輸入的全局路徑符合車輛的運動學模型。

圖8 全局路徑規劃與車輛行駛軌跡對比圖Fig.8 Comparison of global path planning and vehicle trajectory

圖9 車輛S彎無障礙物線速度Fig.9 Vehicle S-bend line speed without obstacles

圖10 車輛S彎無障礙物角速度Fig.10 Vehicle S-bending angular velocity without obstacles

3.2 改進Lattice算法局部避障實驗

本節對Lattice算法的局部規劃改進進行仿真，在Gazebo仿真環境中對矢量地圖法生成的全局路徑上添加障礙物，測試改進前與改進后的軌跡生成,軌跡圖如圖11所示。在實驗中對線速度、轉向角度、控制變量進行約束，令vmax=6 m/s,φmax=0.8 rad, |u1max|=4 m/s2,u2max=0.6 rad/s。由圖11 可知，在車輛行駛路線上未遇到障礙物時，兩種算法生成的軌跡幾乎重合，在拐彎之后感知到前方有樹時，兩種算法開始規劃不同的軌跡，左邊軌跡為改進Lattice算法生成的軌跡，中間軌跡為Lattice算法生成的軌跡，兩者均達到規避障礙物的效果。在規避障礙物時由于改進的Lattice算法將車輛形狀分解成多個圓形，從而對候選軌跡的評價值計算更為靈活，避障效果更加安全。圖12、圖13為改進前后線速度和角速度對比。

圖11 算法改進前后軌跡對比Fig.11 Trajectory comparison before and after algorithm improvement

圖12 算法改進前后線速度對比Fig.12 Linear velocity comparison before and after algorithm improvement

圖13 算法改進前后角速度對比Fig.13 Comparison of angular velocity before and after algorithm improvement

根據對改進前和改進后的車輛線速度、角速度分析，改進后的Lattice算法不僅能夠靈活規避障礙物，而且因為改進的評價函數所計算出的避障軌跡距障礙物橫向距離更大，所以在行駛過程中降低對速度的控制，從而縮短車輛行駛的時間。

3.3 多車編隊避障仿真實驗

對編隊三角陣型加入非完整性約束(車輛位置、速度和轉角約束)，令領航者和跟隨者縱向距離保持5 m，橫向距離保持5 m，將基于改進的Lattice算法應用到多車編隊中。領航者和跟隨者的位置通過NDT正態變換對點云地圖進行匹配，可獲得每臺車在點云地圖中的位置。設定領航者的初始位置為(28，10)，跟隨者1的初始位置為(33，7)，跟隨者2的初始位置為(23，7)，對速度、轉向角度、控制變量進行約束，令vmax=6 m/s，φmax=0.8 rad，|u1max|=4 m/s2，|u2max|=0.6 rad/s，實驗結果如圖14所示。編隊模型中各車線速度和角速度如圖15、圖16所示。

圖14 多車編隊及避障軌跡圖Fig.14 Multi-vehicle formation and obstacle avoidance trajectory diagram

圖15 編隊模型中各車線速度Fig.15 Line speed in formation model

圖16 編隊模型中各車角速度Fig.16 Vehicle angular velocity in formation model

根據圖14可看出在編隊運動過程中，不僅能保持編隊隊形行駛，而且在行駛路徑上遇到障礙物時能夠動態規避障礙物。根據圖15、圖16可看出領航者前行時，跟隨者線速度和角速度通過多車模型實時調節，使其一直保持指定三角陣型行駛。

4 結論

本文提出將改進Lattice算法應用到多無人車編隊運動規劃中，該算法可在結構化道路環境下簡化領航者全局軌跡生成的復雜度，并提高車輛局部避障的精確性；另外在編隊運動過程中加入車輪轉向角度、角速度等非完整性約束以及跟隨者自主導航行為，設計隊形恢復調節函數，使各車能夠分布式避障后快速恢復隊形。仿真實驗證明，基于改進Lattice算法的多無人車編隊在按照規劃的全局軌跡行駛時，能夠根據周圍環境信息靈活避障和隊形保持。