融合TangentBug與Dubins曲線的智能輪式車輛局部路徑規劃算法*

張旭東，徐福康，鄒 淵，郭寧遠，張 宇

（1.北京理工大學，北京 100081；2.陸軍裝甲兵學院，北京 100072）

前言

隨著汽車智能化的發展，自動駕駛技術成為智能車輛領域研究的重要方向，而路徑規劃是其關鍵技術之一。智能車輛的路徑規劃是指在一定環境模型的基礎上，給定起點與目標點后，按照性能指標規劃出一條無碰撞、能安全到達目標點的有效路徑［1］。

目前路徑規劃按不同算法可分為圖搜索類算法［2-3］、采樣類算法［4-5］、人工勢場類算法［6-8］和智能仿生類算法［9-10］等。圖搜索和采樣類算法最為常見，運算速度較快，但需要路徑平滑處理以適應車輛的運動特性，并常用于已知全局地圖的情況。人工勢場類算法對障礙物的適應性較好，但容易陷入局部最優解。智能仿生類算法參考了生物的行為模式和特性，相比其他算法可以得出更優的結果，其中Lumelsky等［11］參考爬蟲（Bug）的行為模式，針對僅能獲取局部地圖和定位信息的情況，提出了具有完備性的Bug1和Bug2算法。

Bug1和Bug2面向可全向運動的機器人，解決其在二維平面上的局部路徑規劃問題，要求機器人裝有接觸式傳感器以探測周圍障礙物，并有精確的實時定位。Bug1和Bug2均由朝目標移動和環繞障礙物兩種狀態組成，但Bug2在簡單場景下規劃路徑的總長度可能更短，而在復雜場景時Bug1更為穩健。Sankaranarayanan等［12］修改了Bug2中環繞障礙物的規則，提出了在某些場景下規劃路徑總長度更短的Alg1算法。Kamon等［13］使用測距傳感器解決了Bug類算法必須接觸障礙物的問題，并提出了基于局部切線圖的TangentBug算法，其規劃路徑的總長度隨測距傳感器探測距離的增加而減小。Kim等［14］在TangentBug的基礎上考慮了采用Ackermann轉向的車輛存在最小轉向半徑的運動約束，其規劃路徑為預瞄算法生成的圓弧而非直線段，但路徑的曲率變化較大，不利于路徑跟隨控制。Xu等［15］在小尺度未知環境范圍內將Dubins曲線和Bug類算法思想結合，但由于中間位姿與目標點的位姿相關，其仿真結果中的規劃路徑出現了扭結和圓環。為保證規劃路徑的安全性，文獻［14］和文獻［15］中基于局部切線圖對障礙物邊界點進行擴展的方法，在實際應用中易受傳感器精度的制約和環境的干擾，擴展距離往往過于保守。此外，上述算法均要求被控對象有精確的實時定位，而算法的收斂性易受定位誤差的影響。

針對現有研究存在的不足，本文中面向采用Ackermann轉向的智能輪式車輛，提出了一種融合TangentBug與Dubins曲線的局部路徑規劃算法。首先通過采樣法構建規劃參考點集合，然后在TangentBug算法框架下生成Dubins曲線作為規劃路徑，旨在滿足車輛最小轉向半徑和目標點的航向要求，并加入了碰撞檢測和考慮定位誤差的狀態轉換規則，最后通過實車實驗，驗證了本文所提出算法的可行性。

1 車輛運動學建模

對于采用Ackermann轉向的車輛，其運動學模型如圖1所示［16］。在慣性坐標系OXY下，車輛后軸中心的坐標為（Xr，Yr），車輛的橫擺角為Ψ，點C為車輛在轉向過程中的瞬時轉動中心，車輛后軸中心速度為v，前輪偏角為δ，車輛軸距為lw，車輛后輪的轉向半徑為r。以車輛坐標和橫擺角為狀態量q=［Xr，Yr，Ψ］T，以車速和橫擺角速度為控制量u=［v，ω］T，可得車輛的運動學模型如式（1）所示。

圖1 車輛運動學模型

轉向半徑與前輪偏角的關系如式（2）所示。當前輪偏角為最大值δmax時，有最小轉向半徑rmin。

2 局部路徑規劃算法設計

本文算法與自動駕駛系統結合的框架如圖2所示。本文算法對應“局部路徑規劃”模塊，其輸入為環境感知模塊的輸出信息，包括障礙物信息和車輛及目標位姿，其輸出為參考路徑，并作為運動控制模塊的輸入。局部路徑規劃模塊包含環境建模和決策規劃兩層，環境建模層用于接收障礙物信息以構建規劃參考點集合和局部地圖，之后決策規劃層利用上述信息結合車輛自身和目標的位姿生成參考路徑。

圖2 自動駕駛系統框架圖

2.1 基于采樣的規劃參考點集合構建

假設在二維平面R2上有一點P，其狀態q=［X，Y，Ψ］T，以此構建如圖3所示的車輛坐標系oxy，其中點P與點o重合。對于可以在任意方向上獲取障礙物距離信息的測距傳感器，在狀態q和方位角φ上獲得的最近障礙物距離ρ（q，φ）可用式（3）表示。

圖3 車輛坐標系與障礙物信息

式中:d（a，b）為點a與點b在二維平面上的歐氏距離；λ為比例系數，取值范圍在［0，∞］，λ[cosφ，sinφ]T表示模長為λ、輻角為φ的向量；WOi表示第i個障礙物。

在實際應用中，由于測距傳感器存在角度分辨率φr，故方位角φ是離散的而非連續的，離散的角度用φn表示，此外還受最大探測距離ρmax的限制。因此重新定義測距傳感器在不同方向上獲取的距離ρr（q，φn），如式（4）所示。

式中atan2（a，b）表示點a相對于點b的方位角。

對于沒有障礙物遮擋，位于測距傳感器探測邊界的點b，定義其構成的集合B為

在障礙物的邊界附近，距離ρr（q，φn）會發生較大的變化，因此通過對sn相鄰點的距離進行分析，可得到不同類型的障礙物邊界點。對于障礙物邊界點的旋轉采樣，可分為順時針和逆時針采樣兩種情況。如圖4所示，點O1為順時針采樣的邊界點，點O2為逆時針采樣的邊界點。對于集合S中的任意一點sn，令sn-1，sn-2，...，sn-m表示該點順時針方向上相鄰的m個點，令sn+1，sn+2，...，sn+m表示該點逆時針方向上相鄰的m個點。設用于障礙物邊界點判斷的距離閾值為de，Ecw為順時針采樣的邊界點集合，Eccw為逆時針采樣的邊界點集合，那么Ecw和Eccw可分別用式（7）和式（8）表示。

圖4 障礙物邊界點的旋轉采樣

式中:ksafe為安全系數；rsafe為安全距離。

然后確定旋轉角度的上限θsample，這里令θsample=ksampleθsafe，ksample為采樣比例系數。采樣的步長取為傳感器的角度分辨率φr，那么逆時針旋轉采樣點的集合E′ccw為

同理可得順時針旋轉采樣點的集合E′cw為

在本文的規劃算法中，作為規劃參考的點有3種:一是不受障礙物遮擋，位于測距傳感器探測邊界上的點；二是基于障礙物邊界點旋轉采樣后的點；三是目標點T。對于集合T′，若T位于傳感器探測范圍內，則T′=｛T｝，否則T′=?。當考慮全部方向時，設規劃參考點的集合為F，則

若僅考慮自身前方信息，設規劃參考點的集合為F′，則

2.2 沿規劃路徑的碰撞檢測

本文算法通過沿規劃出的Dubins曲線進行嚴格的碰撞檢測以保證規劃路徑的安全性。對存在最小轉向半徑且不能后退的車輛，在朝各個方向運動時的最短路徑可用Dubins曲線表示［17］。與圓弧路徑相比，Dubins曲線在滿足目標點的航向要求的情況下，使規劃路徑最短。

如圖5所示，采用覆蓋圓將車輛包圍，其圓心分別位于車輛中心，前軸中心和后軸中心，覆蓋圓的半徑為rsafe。若任意覆蓋圓內存在障礙物，則認為存在碰撞的風險。從Dubins曲線起點S開始，沿曲線以固定步長選取車輛位置參考點，該參考點與車輛后軸中心點重合，參考點處切向量的方向為車輛的航向。若從規劃出的Dubins曲線的起點到終點均通過了碰撞檢測，那么認為該規劃路徑是安全的，否則認為有碰撞的風險。

圖5 沿規劃路徑進行碰撞檢測

2.3 決策規劃算法

本文的局部路徑規劃算法基于以下假設:車輛和障礙物均位于二維平面上；障礙物的數量和周長有限；車輛配有測距傳感器，可比較準確地獲取探測范圍內各個方向上障礙物的距離；車輛配有定位系統，可比較準確地定位；目標的位姿已知；車輛只能向前移動；車輛采用Ackermann轉向。

本文的局部路徑規劃以TangentBug算法為基本框架，同樣由朝目標移動和環繞障礙物兩種狀態組成，以保證本文算法的完備性。但本文算法在生成參考路徑時借助規劃參考點集合，生成了Dubins曲線以代替TangentBug中的直線并考慮了其安全性，在處理狀態轉換規則和判斷目標點是否可到達時考慮了定位系統的誤差，且無須提前規定環繞障礙物的方向。局部路徑規劃算法的偽代碼如表1所示，其中:unreachable為判斷目標點是否可以到達的旗標，初始化時令unreachable=false；ld為生成的Dubins曲線長度，dt為距離閾值，若規劃參考點為目標點且ld＜dt，即認為車輛到達目標點附近，dt的取值可根據車輛的幾何尺寸或實際需要設定；d′（P，T）用于記錄車輛與目標點的距離。

表1 局部路徑規劃算法

2.3.1 朝目標移動狀態

朝目標移動狀態算法的偽代碼如表2所示。其中state為狀態旗標，若state=“motion toward the target”，車輛則處于朝目標移動的狀態，state初始化為此值。若state=“boundary following”，車輛則處于環繞障礙物的狀態。

表2 朝目標移動狀態算法

在朝目標移動狀態時，設目標點T在車輛坐標系下為點t，集合F中任意元素fi的代價cfi為車輛自身到該元素的距離加該元素到目標點的歐式距離，即

按代價由小到大的順序，依次生成Dubins曲線并進行碰撞檢測，一旦通過碰撞檢測，則以此Dubins曲線作為局部參考路徑，否則認為沒有安全的路徑并停車。如圖6所示，在車輛坐標系oxy下，Dubins曲線的起點在原點o處，其狀態可表示為:qo=［0，0，0］T。對于集合F中的元素fi，其在車輛坐標系下的坐標為（xfi，yfi），方位角φfi=atan2（fi，o），故Dubins曲線的終點狀態為:qfi=［xfi，yfi，φfi］T。

圖6 生成Dubins曲線

當目標點進入傳感器探測范圍后，首先嘗試滿足目標點處的航向角要求。設目標點T在車輛坐標系下的狀態為qt=［xt，yt，φt］T，那么首先嘗試以qo為起點，qt為終點生成Dubins曲線，若成功，則以此為參考路徑。否則，計算此時車輛坐標系下的目標點t的方位角φt′=atan2（t，o），并嘗試以qt′=［xt，yt，φt′］T為終點生成Dubins曲線。

對于假設車輛可全向移動的Bug算法，在朝目標移動的狀態時，一旦出現距離d（P，T）增大的情況，則認為存在局部最近點Mi，此時車輛的位置記作SWi，見圖7。但對于存在最小轉向半徑的車輛，即使不受障礙物的影響，也會因為自身狀態等原因遠離目標點，如圖8所示。此外，由于定位系統存在誤差，即使車輛實際沒有遠離目標，也會出現距離d（P，T）增大的情況。而根據文獻［13］可知:在TangentBug算法中，距離d（P，T）在朝目標移動狀態下是非增的。因此，對于在某一時刻i的距離d′i（P，T），在朝目標移動狀態時，除按目標點航向規劃成功外，其取值由式（16）和式（17）確定，此時d′i（P，T）為當前時刻i之前到目標點距離的最小值。而其他情況下均為d′i（P，T）=di（P，T）。

圖7 車輛軌跡示意圖

圖8 車輛因最小轉向半徑而遠離目標

設一距離閾值dm，若在某一時刻i，有式（18）成立，那么認為存在局部最近點Mi，隨即切換為環繞障礙物狀態。當在任何情況下均令d′i(P，T)=di（P，T），且dm=0時，對應定位系統無誤差的理想情況。dm應結合定位系統的誤差取一較小的正值。

2.3.2 環繞障礙物狀態

環繞障礙物狀態算法的偽代碼如表3所示。當處于環繞障礙物狀態時，首先更新局部最近點Mi。設點Mi到目標點的距離為dmin，在初始化時令dmin=∞。若有式（19）成立，則更新dmin，并記錄局部最近點Mi和車輛此時的位置SWi。

表3 環繞障礙物狀態算法

設一集合L，有

若?li∈L，使得從qo到qli=［xli，yli，atan2（li，o）］T生成的Dubins曲線通過碰撞檢測，令dleave=d（li，t），那么有式（21）成立，即在集合F中存在一元素比局部最近點到目標點的距離更小，且生成的Dubins曲線通過碰撞檢測。隨后車輛切換為朝目標移動狀態。記此時在慣性坐標系下的車輛坐標為點Li，li的坐標為LRi。在環繞障礙物狀態下，dleave需要實時更新，在初始化時dleave=d（S，T）。

環繞障礙物的方向由狀態轉換時記錄的目標方位確定，若此時atan2（t，o）＜0，則環繞方向為順時針，否則為逆時針。以逆時針為例，如圖7所示，車輛從狀態轉換點SWi處開始逆時針環繞障礙物，直到點Li處駛離障礙物。為防止在環繞障礙物狀態下出現規劃路徑的局部振蕩，以僅考慮自身前方信息的集合F′作為規劃參考點集合。逆時針環繞時，按方位角由大到小的順序對集合F′中的元素進行排序，而順時針時，按由小到大的順序排序。依次生成Dubins曲線并進行碰撞檢測，一旦通過則以此作為參考路徑，否則認為沒有安全的路徑并停車。

對于環繞障礙物一周的情況，由以下規則判斷。

（1）若存在一車輛位置o，滿足d（swi，o）＞dsw且|atan2（swi，o）|＞π/2，則認為車輛駛離狀態轉換點SWi，其中點swi為點SWi在車輛坐標系下的位置，dsw為距離閾值，可根據定位系統的誤差取一較小的正值。

（2）在滿足條件（1）的情況下，若存在一車輛位置o，滿足d（swi，o）＜dsw且|atan2（swi，o）|＜π/2，則認為車輛再次回到點SWi。

當條件（1）和（2）均滿足時，認為車輛繞障礙物一周回到了狀態轉換點SWi，即得出目標不可到達的結論并停車。

2.3.3 算法收斂性證明

車輛存在最小轉向半徑的運動約束不會影響本文算法的收斂性。如圖8所示，若目標點位于測距傳感器探測范圍內，則車輛保持朝目標移動狀態前進至目標點。若目標點位于測距傳感器探測范圍外，由于受最小轉向半徑的運動約束，車輛以朝目標移動狀態遠離目標，當滿足式（18）后，切換為環繞障礙物狀態。若在測距傳感器探測范圍內不存在障礙物，則距離dmin=∞；或雖然存在障礙物但未阻礙車輛運動，此時存在使規劃路徑通過碰撞檢測的點LR，隨后式（21）均成立。因此車輛的狀態將在朝目標移動和環繞障礙物之間切換，直到車輛在朝目標移動的狀態下不再滿足式（18）。而在文獻［13］中已經證明上述兩種狀態的組合不會導致TangentBug發散，故在有限次狀態轉換后車輛將保持在朝目標移動的狀態到達目標點。

3 實車實驗

3.1 實驗平臺搭建

為驗證本文算法在真實環境中的表現，通過如圖9所示的北京理工大學智能輪式車輛平臺進行了實驗。該平臺為線控電動底盤，上位機可通過以太網轉CAN設備（CANet）對底盤進行控制。傳感器包括作為測距傳感器的激光雷達和毫米波雷達，作為定位系統的慣性組合導航系統。軟件算法在上位機中運行，程序指令通過CANet下發到CAN總線上，從而控制驅動電機和轉向、制動執行器等設備，以實現車輛控制。

圖9 實驗車輛及關鍵零部件

實驗的軟件平臺為Ubuntu 16.04和ROSKinet?ic，編程語言為C++。局部路徑規劃程序接收來自環境感知程序發布的障礙物信息和定位信息，并發布一條參考路徑給運動控制程序。本文橫向運動控制采用了預瞄控制算法，縱向運動控制采用了PID算法。本文局部路徑規劃算法主要參數的取值見表4。

表4 算法主要參數取值

3.2 實車實驗驗證

實驗環境為工業園區，障礙物主要包括建筑物、花壇、貨物和其他停放的車輛等。通過環境建模得到的規劃參考點集合和局部柵格地圖如圖10所示，其中黑色的點為障礙物邊界；藍色的箭頭為規劃參考點集合，外圍圓環為測距傳感器探測邊界上的點，其余為障礙物邊界旋轉采樣后的點；紅色的Dubins曲線為生成的參考路徑。

圖10 實驗場景下算法實時運行結果

實驗測試場景有兩種，場景1只包含朝目標移動的狀態，場景2包含兩種狀態組合的情況，兩種測試場景均通過了測試。場景1中車輛軌跡長度為113.248 m，場景2的軌跡長度為140.492 m。實驗平臺使用的上位機的配置為CPU i5?3450，8G內存。規劃程序的控制周期固定在了0.05 s，每次循環實際計算時間如圖11所示，其中場景1計算時間平均值為0.004 8 s，最大值為0.030 3 s，場景2計算時間平均值為0.006 6 s，最大值為0.030 1 s，由此可得該算法可滿足實時運行的要求。

圖11 每次循環算法實際計算時間

通過記錄實驗數據，離線運行LeGO?LOAM［18］分別得到場景1、2的全局地圖和車輛軌跡如圖12和圖13所示。可以看出車輛在行駛的過程中避開了附近的障礙物，并在目標處按規定航向停止。在圖12中，車輛以朝目標移動的狀態避開了周圍的障礙物，最終順利到達目標點處。在圖13中，車輛由于存在最小轉向半徑，從起點S開始遠離目標點T，直到狀態轉換點SW1處，滿足式（18）所述條件，記錄下局部最近點M1后開始逆時針環繞障礙物。直到點L1處，由于存在點LR1比點M1離目標更近且生成的Dubins曲線通過了碰撞檢測，故結束環繞障礙物的狀態，并以朝目標移動的狀態前進直到目標點處。

圖12 場景1地圖和車輛軌跡

圖13 場景2地圖和車輛軌跡

為驗證本文算法對定位系統誤差的魯棒性，將式（17）改為:di′（P，T）=di（P，T），并令式（18）中的距離閾值dm=0，即不考慮定位系統的誤差。重復場景1下的實驗，得到的實驗結果如圖14所示。由于實際定位誤差導致車輛與目標點間的距離增加，使得車輛由朝目標移動狀態轉換為環繞障礙物狀態。由此可得，實際定位誤差會對Bug類算法的穩定性產生影響，通過式（17）和對式（18）中距離閾值dm合理地取值，可避免因定位誤差造成的狀態轉換問題，使得本文算法對定位誤差的魯棒性提高。

圖14 定位系統誤差對本文算法的影響

由于規劃路徑的曲率變化會直接影響運動控制程序的控制效果，故對本文算法生成的參考路徑的平均曲率與文獻［14］中用的圓弧作為參考路徑的算法進行了對比分析。在場景1下兩種算法生成的參考路徑的平均曲率變化如圖15所示，正值表示左轉，負值表示右轉。對兩組數據進行分析，分別得到統計特征值如表5所示，對于參考路徑平均曲率的標準差，融合圓弧曲線的算法為0.045 2，本文融合Dubins曲線的算法為0.025 3，提升了44%。對于參考路徑平均曲率的最大值，融合圓弧曲線的算法為0.128 4，本文融合Dubins曲線的算法為0.068 0，提升了47%。由此可得，本文算法更有利于路徑跟隨控制。

圖15 參考路徑平均曲率對比

表5 參考路徑平均曲率統計特征值

4 結論

本文中面向采用Ackermann轉向的智能輪式車輛，針對僅能獲取局部地圖和定位信息的情況，提出了一種融合TangentBug和Dubins曲線的局部路徑規劃算法。首先通過采樣的方法構建了規劃參考點集合，無須構建局部切線圖。然后以Dubins曲線作為規劃路徑，滿足了車輛最小轉向半徑的運動約束和目標點處的航向要求，并通過加入沿規劃路徑的碰撞檢測，提升了算法的安全性。結合TangentBug的證明結果，提出了適應定位系統誤差的狀態轉換規則，并證明了車輛最小轉向半徑的運動約束不會影響本文算法的收斂性。最后通過實車實驗證明了本文算法在真實環境下的可行性，結果表明:本文算法可規劃出安全的局部參考路徑，使車輛按規定位姿到達目標點，并可保證算法的實時性；本文提出的狀態轉換規則可有效避免定位誤差的影響，提高了本文算法對定位誤差的魯棒性；相對于使用圓弧曲線，本文通過融合Dubins曲線規劃出的路徑更有利于路徑跟隨控制。