一種基于回旋曲線的局部路徑規劃方法*

蘇治寶，趙熙俊，朱 森，張浩杰

（中國北方車輛研究所，北京 100072）

0 引言

對于地面機器人而言，路徑規劃是基于一定的優化準則，在有障礙的環境中生成從指定起始位置到達目標位置無碰撞運動軌跡的過程。根據規劃范圍不同，路徑規劃分為全局路徑規劃和局部路徑規劃。其中，全局路徑規劃是主要基于先驗信息（例如地圖），為地面機器人找到到達目標位置最佳路徑的過程，而局部路徑規劃則根據基于平臺即刻所采集的環境感知傳感器數據建立的地圖，規劃當前的可執行路徑。

目前常用的局部路徑規劃方法有最短路徑法、向量場直方圖（VFH）法[1]、曲率速度法（CVM）[2]、勢場法[3]、基于回旋曲線的方法[4]等。其中，Dijkstra和A*算法是典型的最短路徑方法，這類方法將地圖轉化成代價圖進行搜索，獲得最短路徑，但獲得的路徑沒有考慮平臺的運動約束，無法保證其可執行性，且計算時間相對較長，只適用于低速平臺。向量場直方圖法以射線為基本路徑模型，根據各個方向上存在障礙的概率確定行駛方向，這種方法不適用于阿克曼轉向平臺，且規劃距離短，容易陷入局部極小。曲率速度法將障礙地圖轉化為3D（旋轉速度、平移速度和到障礙的曲線距離）配置空間，并利用曲率將旋轉速度和平移速度關聯起來，按照一定的目標函數獲得具有適當大小曲率的軌跡，這種基于曲率的方法只適合于低速導航。目前使用較多的還有基于特定數學模型（例如多項式函數）的路徑規劃方法[5]。這類方法中，路徑用特定數學模型表示，以運動學或動力學約束為邊界條件，確定模型參數。預先生成一簇備選軌跡，根據地圖可通行狀況和某種優化準則，選擇可執行路徑。

本文在參考相關文獻基礎上，針對某輪式阿克曼轉向機器人平臺，研究了基于回旋曲線路徑模型的局部路徑規劃方法，介紹了該方法的基本實現過程，對關鍵步驟進行了說明，并利用仿真系統驗證了這種方法，最后給出了研究結論。

1 基本方法介紹

基于回旋曲線路徑模型的局部路徑規劃算法由兩部分組成：離線部分和在線部分。

其中，離線部分在機器人運行之前完成，主要通過軌跡空間與柵格單元映射關系建立搜索圖，基本過程如下：

1）根據機器人阿克曼轉向約束，在控制量空間計算所有可能的回旋曲線軌跡，建立軌跡空間；

2）根據預瞄距離，在機器人前方區域離散生成一系列節點，利用通過節點的各軌跡之間的連接關系，確定節點之間的連通關系；

3）獲得并保存局部地圖各單元與各軌跡索引之間的映射關系，即通過地圖某單元有哪些軌跡，某條軌跡上有哪些地圖單元，建立搜索圖。

在線部分是在機器人運行時完成，主要根據搜索圖和環境模型按照某優化準則采用A*算法搜索獲取最優路徑，基本過程如下：

1）根據軌跡與地圖單元的映射關系，結合當前地圖的障礙分布情況，在軌跡空間里排除與障礙物發生碰撞的軌跡，獲得可行軌跡空間；

2）在可行軌跡空間構建的搜索圖中，采用A*算法尋找連通機器人當前位置與當前目標節點位置之間的最優路徑。

2 離線算法

2.1 基于節點構建回旋曲線軌跡空間

2.1.1 節點位置的確定

如圖1所示，以地面機器人當前位置為圓心，在距離機器人平臺為10 m和20 m的弧線上各選擇46個節點，節點位置與該節點相對于平臺的方位角有關。選取節點位置的原則是“中間密，兩邊疏”，其原因是：中間對應平臺希望行駛的路徑方向，“中間密”可以保證規劃的路徑避讓障礙物之后盡可能回到希望行駛的方向上；“兩邊疏”考慮到平臺很少行駛到兩邊區域，只有當中間區域被障礙物全部占據沒有可行路徑的情況下平臺才向兩邊行駛。

圖1 節點分布示意圖

2.1.2 生成各層軌跡

圖2 車輛運動模型

對于阿克曼轉向的輪式平臺而言，假定Δt時間間隔內前輪偏角變化率和速度為定值，那么該段行駛軌跡為回旋曲線，平臺運動模型為

其中，（x，y）為平臺位置坐標，θ為航向，δ為前輪偏角，vf為行駛速度，Δt為時間步長。

在離線算法中，計算回旋曲線軌跡的參數包括平臺行駛速度、當前前輪偏角、前輪偏角變化率、計算步長、回旋曲線段數（即軌跡有幾段組成）、計算時間等。從理論上，只要對參數的離散化程度足夠密，所產生的回旋曲線就是阿克曼轉向平臺實際可能行駛的所有軌跡。然而由于時間消耗約束，必須對各參數進行合理的離散化處理。

1）行駛速度

在0到12 m/s車速范圍內，以步長2 m/s離散化車速，因此，速度數量為6。

2）前輪偏角

最大前輪偏角取30°。離散化時，前輪偏角間隔取1.5°，則共有41個前輪偏角。0～40的序號對應于-30°～30°。

3）前輪偏角變化率

前輪偏角變化率，取決于自動轉向執行機構及其控制系統。前輪偏角變化率取7個值，即-30°/s～30°/s，步長為 10°/s。

4）計算步長

計算步長與速度有關，計算步長=0.2/速度。

5）回旋曲線段數

選擇最多5段回旋曲線來描述平臺前方20 m內的行駛軌跡。

6）計算時間

每段回旋曲線段的計算時間最多為40個周期，計算結束條件為是否滿足軌跡長度及航向約束。整條回旋曲線軌跡的計算是否結束取決于軌跡總長和段數。

圖3 layer02、layer01、layer12 軌跡

軌跡的生成采用深度優先搜索算法的思路，即由頂節點開始計算，獲得一條子軌跡，再以子軌跡末端為起點繼續計算，直到達到滿足計算結束條件。所獲得的所有軌跡都起始于機器人當前位置，止于第2層節點，因此，稱為layer02軌跡。

根據軌跡是否通過第1層節點和第2層節點，從layer02軌跡中獲取起始于機器人當前位置、止于第1層節點的軌跡集合layer01，獲取起始于第1層節點、止于第2層節點的軌跡集合layer12。

2.1.3 生成不同速度和初始前輪偏角下的軌跡空間

為了提高搜索效率，需要將所生成的軌跡空間按照不同速度和初始前輪偏角進行組織和保存，這通過按照速度和初始前輪偏角從layer01和layer12軌跡中進行索引而離線獲取。圖4為不同速度下前輪偏角為6°時的軌跡空間示例。

圖4 不同速度下的軌跡空間示例

2.2 生成地圖單元-軌跡映射表

假設環境地圖采用占有柵格形式表示。為了判斷軌跡上是否有障礙，需要確定軌跡所通過的柵格單元索引。利用幾何計算方法，確定軌跡上離散點位于哪一個柵格單元。需要說明的是，為了確保平臺行駛安全，不同的車速下規劃的路徑有一定的寬度，速度越快，路徑越寬，在確定軌跡所通過柵格單元時要考慮路徑寬度。軌跡通過哪些柵格的結果按文件的形式保存。

另外，為了減小搜索空間，需要知道每個柵格單元有哪些軌跡通過的信息，這通過反轉軌跡通過哪些柵格單元而得到，將這些信息以文件形式保存。

3 在線算法

圖5 局部路徑規劃流程圖

如圖5所示，在線算法實現局部路徑規劃的基本流程為：

1）初始化搜索圖，即根據所定義的節點初始化A*算法使用的搜索圖；

2）加載離線階段已創建的單元-軌跡映射表；

3）從上層規劃器獲取全局路徑，從感知模塊獲取局部地圖，以及機器人當前位姿；

4）由航路點管理模塊確定當前規劃周期的目標點；

5）根據當前目標點，依據一定準則（例如與目標點的距離、相對角度最小等）確定A*算法的當前目標節點；

6）利用環境感知模塊提供的局部圖地圖，更新搜索圖中各節點的通過成本；

7）利用單元-軌跡映射表和局部地圖，排除不能通行的軌跡，獲得可行軌跡空間：

8）在可行軌跡空間中利用A*算法搜索可到達目標節點的最優軌跡，最優準則包括軌跡速度、軌跡長度等。

4 仿真實驗

為了測試規劃算法的規劃能力和運行速度，設計了多種場景，開展了仿真實驗。仿真條件如下：

1）平臺參數：輪距3.2 m，車長4.8 m，車寬2.4 m，初始前輪偏角為零；

2）局部地圖尺寸：120×100，分辨率：0.25 m；

3）運算平臺：64位臺式計算機，Intel（R）Core（TM）i7-6700K CPU@4.00 GHz，16 G 內存，Ubuntu14.04操作系統。

4.1 靜態仿真

機器人平臺靜止，前輪偏角為零，以第2層右側某節點為目標點，在圖中用圓圈所示。圖6為4種場景下的仿真實驗結果，所規劃軌跡的速度分別為：12 m/s、6 m/s、4 m/s、2 m/s，所規劃的速度越快，路徑越寬；規劃耗時分別為 33 ms、116 ms、145 ms、186 ms。

圖6 不同場景下局部路徑規劃結果

圖7 無法規劃出路徑的場景示例

在場景4的基礎上，再增加一個障礙，如圖7所示，規劃器則無法規劃出路徑。其原因在于，對規劃空間采取了離散化處理，其完備性會受到影響。

4.2 動態仿真

在起伏越野環境中，布置若干靜態障礙，并給定全局路徑，動態仿真機器人從起點到終點的自主導航過程，測試所提出局部路徑規劃方法的性能。

仿真結果如下頁圖8所示。其中，圖8（a）為仿真場景，圖8（b）為給定的全局路徑和機器人實際行駛路徑，圖8（c）為每次局部路徑規劃的耗時，圖8（d）為機器人行駛速度曲線。在自主導航過程中，機器人可以較合理地選擇局部路徑，每次規劃平均耗時37.383 8 ms，平均行駛速度6.842 7 m/s。

5 結論

圖8 動態仿真結果

本文研究了一種基于回旋曲線路徑模型的局部路徑規劃方法，在離線計算軌跡空間時，由于考慮了平臺的運動學特性，因此，所規劃的路徑具有良好的可執行性；在離線生成地圖單元-軌跡映射表時，考慮了平臺運行速度，提高了平臺行駛安全性。但受回旋曲線模型的約束，所提出的局部路徑規劃方法只適用于采用阿克曼轉向的輪式平臺；在根據平臺參數預先計算軌跡空間時，對參數進行了離散化處理，影響了這種規劃方法的完備性。在未來研究工作中，需要關注兩點：一是如何優化組合計算軌跡空間時所用的參數，降低對規劃完備性的影響，確保在環境復雜但有路徑可行的情況下，規劃方法確實能規劃出軌跡；而是研究規劃完備性與規劃耗時之間的關系，確保同時滿足規劃方法實用性要求與平臺運行實時性需求。