Frenet 坐標系及凸近似避障原理的無人車局部路徑規劃

袁 春，龔 城，何成誠，李 楊，郭宗環

(1.重慶理工大學 車輛工程學院，重慶 400054;2.重慶金康賽力斯新能源汽車設計院有限公司科技中心，重慶 401133)

自動駕駛是當前汽車技術開發的熱點，而路徑規劃是實現自動駕駛非常重要的環節［1］。自動駕駛中的路徑規劃分為兩類，即基于高精度地圖信息下的全局路徑規劃(又稱靜態規劃)和基于自車周圍環境變化的局部路徑規劃(也稱動態規劃)［2］。全局路徑規劃主要為局部路徑規劃提供導向和約束，使車輛能夠沿著導航所引導的期望路徑行駛［3-4］。局部路徑規劃指的是車輛在行駛過程中實時感知周圍的環境信息變化，從而使車輛實時調整行駛路徑順利跟蹤期望的路徑。局部路徑規劃主要有使用車輛自身安裝的各種傳感器(主要有激光雷達、毫米波雷達、攝像頭等)獲取到周圍環境變化的信息，以及全球定位系統GNSS 獲取當前車輛位置信息而實現無碰撞的避障操作，局部路徑規劃對實時性要求較高，在局部路徑規劃的過程中不僅要考慮到行駛路徑最短、達到時間較少、駕駛車輛最安全的原則，還要考慮自身車輛約束問題、動態變化的環境所帶來的約束問題、以及與時間相關的約束條件等［5］，全局路徑規劃與局部路徑規劃有許多相通的地方［6］。目前常用的全局路徑規劃算法主要有基于搜索的方法(Dijkstra算法、A*［7］、D*等)、基于優化的算法(最優控制、凸優化［8-10］)、基于采樣方法(確定性采樣、隨機采樣)、智能仿生算法(遺傳算法、蟻群算法、粒子群算法)等。局部路徑規劃的算法主要有人工勢場法［11］、動態窗口法、強化學習法、模擬退火法、模糊控制法、神經網絡法、基于行為的路徑規劃算法、凸近似算法、柵格法以及MPC［12］等算法。為了解結構化道路上的局部路徑規劃問題，本文引入了Frenet 坐標系，在此坐標系下可以得到自車與障礙物車在期望路徑上的橫向偏差，更加符合車輛行駛的實際情況。

本文基于Frenet 坐標系和凸近似避障原理［13-15］，提出了一種適應實際道路環境的局部路徑規劃算法。首先，在Frenet 坐標系下建立柵格點，并將此柵格點轉化到全局坐標系下，再通過全局坐標系得到車輛坐標系下的柵格點坐標，然后，建立自車的運動學和動力學模型，在笛卡爾坐標系和Frenet 坐標系下考慮車輛自身極限轉向動態機械結構以及轉角變化率等進一步縮小柵格點區域，同時基于安全距離、道路對中、路徑最短等代價函數得到自車安全行駛的最優路徑。最后，在金康賽力斯SF5 型新能源汽車平臺上搭建Labview +Matlab/Simulink 構架的無人車實驗平臺，在校園道路上進行算法的驗證，結果表明:該算法能夠順利的避開障礙物并跟蹤上期望路徑。

1 坐標系與改進的柵格法

在全局笛卡爾坐標系下，當無人駕駛車行駛在道路曲率較大的道路時，會出現找不到前視點，而使無人駕駛車輛偏離期望軌跡的情況，所以需要知道無人駕駛車在局部環境中所處的具體位置，以及篩選后的從柵格點到期望路徑投影點處的距離信息。這對后續代價函數的設計至關重要，在此情況下需要引入Frenet 坐標系與改進的柵格法。

1.1 Frenet 坐標系

圖1 為無人車在笛卡爾坐標系與Frenet 坐標系中的位置圖，設車在笛卡爾坐標系下的坐標為(x，y)，車輛位置(x，y)向道路參考線作投影，F 為此投影點，F 和車輛位置(x，y)之間的距離就是橫向位移d，從全局坐標系下的起始點到投影F的曲線距離是縱向位置s，用(s，d)描述Frenet 坐標系下無人駕駛車的坐標點位置，并構建(x，y)，Tref?(s，d)。式Tref為參考線，通常是結構化道路車道的中心線，可以通過全球定位系統GNSS 事先采集路徑點(Waypoint)信息，也可以根據高精度地圖得到道路中心線的位置信息。

圖1 Frenet 坐標系中無人車的位置示意圖

圖2 是Frenet 坐標系、笛卡爾坐標系以及車輛坐標系三者之間的相互轉化，大地坐標系是XOY，車輛坐標系是xoy，Frenet 坐標系是(s，d)。圖中參考線即是期望路徑。車輛在大地坐標系下的位置M(x，y)。車輛在期望路徑上的投影點是F。圖中的網格就是在車輛坐標系下搜索的柵格點。后續需要將柵格點的坐標轉換到Frenet 坐標系上。然后在Frenet 坐標系下，經規劃得到橫向、縱向運動軌跡后，需要將其重新映射到大地坐標系下，以供控制模塊調用。

圖2 結構化道路下的局部柵格點示意圖

首先將車輛坐標系與大地坐標系捆綁看成一個整體，之間的轉換不存在道路的變形。車輛坐標系下的柵格點坐標為SG(x，y)，車輛在大地坐標系下的坐標為DD(xche，yche)，參考線的坐標是(x1，y1)且每個Waypoint 的路徑信息S 是可以算出來的。柵格點在大地坐標系下的坐標是(xD，yD)。其中:

式中:θ 為自車的航向角，然后將柵格點在大地坐標系下的坐標映射到期望路徑上，得到每一個柵格點投影點p(xty，yty)。算出每個柵格點與對應的投影點之間的距離:

投影點的計算，首先取大地坐標系下柵格點坐標，判斷此坐標點在期望路徑的局部位置，從車的當前位置n 開始得到:

其中xf＜0，且xf·xr＜0 得到一個近似點，之后取此點前一系列點進行距離計算最終得到所需投影點。其中θ＜0 且yD＞y1，當θ≤0 且yD＜y1，d 取負號，否則取正號。最后算出每個柵格點以及自車對應的Frenet 坐標(s，d)。

1.2 改進的柵格法

傳統的柵格法，主要是進行地圖的構建，但是這樣會增加計算量而達不到實時性的要求。目前使用較多的是在局部環境下構建基于車輛坐標系的局部柵格點，描述車輛前方的可行駛區域［17］。

此種柵格也存在問題，當道路曲率非常大的時候，柵格點大部分會在道路外面，如圖3 所示，所以會使算法找不到前視點而出現求解錯誤，最終無法跟蹤到期望路徑。

圖3 Frenet 坐標系下的柵格點示意圖

改進的自適應柵格法:根據前面的Frenet 坐標系，首先在道路上建立柵格點

a 和b 為離期望路徑上的一系列等距間隔的點。得到每個柵格點的S_L 坐標點:

式中:sqw為柵格點的S 坐標信息;Lqw為L 信息值;nout為車當前位置的S 信息值。

然后把Frenet 坐標系下的柵格點轉換到大地坐標系下，此時，將Frenet 坐標系與大地坐標系捆綁成一個整體，之間的轉化不存在道路的變形。最后將大地坐標系下的柵格點轉到車輛坐標系下，這樣就得到了自適應的柵格點，很好地避免了由于道路曲率過大而找不到前視點使程序出錯的問題。最終開發算法進行仿真得到自適應網格，如圖4 所示。可以看出所有的柵格點都是隨道路的曲率變化而變化，在急轉彎時柵格點也都在道路中，有效避免了大曲率路上找不到前視點的情況。

圖4 道路自適應柵格點示意圖

2 凸近似的局部避障原理

2.1 基于凸近似的路徑規劃算法研究

凸近似避障的原理其實就是多個凸多邊形構成的車輛安全行駛區域。

1)為了深入理解凸近似原理，引入向量組內積與向量組內積不等式:

式中:θ 為2 個向量的夾角。a、b、c、d 分別為a=。

圖5 中直線L 垂直于OD，所以此直線將平面區域分為2 個部分，其中點xA在左半部分，xc在右半部分。向量OxA在d 上的投影必然小于向量即b 向量在d 上的投影，向量在d 投影必然大于向量即b 向量在d 上的投影。所以:

圖5 向量內積大小與向量終點位置關系示意圖

左邊的區域可以表示為:

右邊的區域可以表示為:

2)多個向量內積構成凸多邊形區域

根據前面的式(7)(8)。設p0，q1，q2，…，qM∈R2，p0≠qi，i=1，2，3，…，M 可以得到凸多邊形區域P:

其中:

當i=1，2，…，m 時p0存在于凸多邊形p 中，qi不在凸多邊形中。最后得到的凸多邊形就是無人駕駛車輛的可行駛區域。當i=1，i=2，圖6、7還未形成封閉的凸多邊形p，但是已經將p0所在的區域縮小了。圖8 是最后形成的凸多邊形p 的形狀。

圖6 i=1 時的凸多邊形p 位置示意圖

圖7 i=2 時的凸多邊形p 位置示意圖

圖8 i=M 時的凸多邊形p 位置示意圖

基于以上的理論公式，后續考慮到障礙物以及自動駕駛車輛的外形，進行了算法的改進，將Q1、Q2、Q3、Q4表示為障礙物形成的凸多邊形，P 為車輛的可行駛區域。還需要預留一個障礙物與車的安全距離。該避障原理充分考慮了自車外形尺寸，以及周邊障礙物的外形尺寸，可以獲得無人駕駛車可行駛的一片安全空間區域。最后考慮自車與障礙物車之間的安全距離，進行理論公式的推導得到優化后的凸近似避障的公式如下所示。

在實際行車時需要考慮障礙物車外形尺寸，所以實驗時在公式里面加了一個障礙物車外形膨脹系數(如圖9 所示)，得到行車較安全的距離，使自車與障礙物車始終保持一定的安全距離。

圖9 障礙物凸多邊形與車輛行駛區域

3 車輛建模與代價函數

3.1 車輛模型

在進行路徑規劃的時候，需要建立合適的車輛運動學以及動力學的模型，充分考慮車輛運行的實際情況。

圖10 為車輛模型，OXY 構成大地坐標系，ovxvyv構成車輛坐標系，φ 為車輛的航向角，δf為前輪轉角，L 為車的軸距，a/b 為前/后軸距離質心的距離，R 為車輛轉彎半徑，vf和vr為車輛前軸和后軸的速度，vlg和vla為質心的縱向和側向的速度。根據上圖的幾何關系然后在對車輛運動學進行建模的時候，使用理想化假設:①認為車輛在平坦的道路上行駛，忽視路面不平造成車輛垂直方向的運動;②將左右車輪合并為一個車輪，忽略左右輪載荷轉移的問題;③忽略車輛運動中橫縱向的空氣阻力以及前后載荷轉移問題;④考慮車輛輪胎的側偏特性，忽略輪胎的橫縱向的耦合關系;⑤假設轉向車輪為剛性轉動，即忽略轉向系統的傳動問題;⑥假設車輛只做水平運動，垂向速度、側傾角、俯仰角均為零;⑦假設車輛轉向輪無轉向偏差。

圖10 車輛模型示意圖

基于上面的假設之后就可以建立平面車輛簡化的動力學模型，車輛只有橫向運動、橫擺運動、縱向運動，最后推導公式得到車輛的運動學以及動力學模型［18］。此處所用到的理論公式是將車輛運動學與動力學相結合，更加符合車輛的實際情況，但是有選擇性做了一些理想假設，最終建立符合車輛行駛真實條件的車輛模型。

3.2 代價函數

在上面建立柵格點之后，先根據車輛的方向盤最大轉角進行第一次約束篩選，之后用阿克爾曼轉向模型得到車輛前輪轉角:

式中:l 為SF5 車距，x 為車輛到要跟蹤點的橫向偏差，L 為此時的前視距離。金康SF5 前輪轉角最大為32°，然后對轉角進行約束。根據模型預測控制原理的基本內容，主要考慮到車上乘客的舒適性以及合理的交通規則和車輛運動學以及動力學。交通規則:道路對中、逐次變道、偏離期望路徑等。舒適性:前輪轉角變化率、側向加速度。

1)左側優先變道:

k=0，1，2…，N-1，klp為權重系數。

2)逐次變道:

kc為權重系數，ML為實驗所在車道寬度。

3)側向加速度代價函數:

式中:kay為權重系數。考慮到車輛的舒適性以及側向加速度加一個限制條件:

4)方向盤轉角變化率的代價函數:

在無人駕駛汽車局部路徑規劃時候，為了使跟蹤的軌跡平滑，需要考慮方向盤前輪轉角變化率，防止程序在計算時出現方向盤轉角變化率過大而導致實驗時方向盤劇烈抖動，車輛失控的現象。根據廠商得到的資料，后面實驗車方向盤與前輪轉角的傳動比為15.8。

其中:kSW為權重系數，δf為前輪轉角。

最終得到總的代價函數:

4 仿真分析與實車實驗

4.1 仿真分析

利用Matlab/Simulink 建立了二自由度車輛模型，在Carsim 中搭建車輛仿真模型。利用Carsim+Simulink 聯合仿真的方法對算法進行驗證。為了得出算法對期望路徑的跟蹤效果，事先通過華測P2 采集實際道路的經緯度、高度以及航向角，用Labview 編寫程序對數據進行采集和處理，選取的路線是金康SF5 在最大轉角情況下采集的大曲率閉環以及Uturn 道路的Waypoint 點，然后將路線的經緯度轉換為大地坐標系下的相對坐標。設置車輛速度為18 km/h 進行仿真，觀察車輛對路徑的跟蹤能力。

由圖11 仿真結果可以看出，在閉環大曲率道路上車輛能夠跟蹤上期望路徑，并且可以繞開直線上的障礙物。

圖11 閉環路線仿真跟蹤與避障結果示意圖

然后進行Uturn 的路線跟蹤仿真。圖12 是自車跟蹤期望路徑效果，圖13、14 分別表示跟蹤期望路徑時自車的航向角和前輪轉角。

圖12 Uturn 路徑跟蹤效果示意圖

圖13 車輛航向角曲線

圖14 車輛方向盤轉角曲線

由以上仿真數據可以看出，所設計的控制算法能夠安全的跟蹤到Uturn 道路，且滿足車輛跟蹤的魯棒性與穩定性要求。

Uturn 路徑上存在障礙物的仿真:將障礙物設置在離道路起始點12 m 的位置，如圖15 所示。

圖15 Uturn 避障路徑跟蹤效果示意圖

圖16、17 分別表示避障時自車的航向角和方向盤轉角。同樣根據仿真結果可以看出，在有靜態障礙物的時候所設計的算法能夠成功的避開障礙物，而且能夠重新快速安全的回到期望路徑上面來。

圖16 避障時車輛航向角曲線

圖17 避障時車輛方向盤轉角曲線

4.2 實車實驗

在仿真通過后，為了進一步驗證該算法的可靠性，選取校園的開放路段進行彎道避障以及路徑跟隨實驗，由于受車輛底層控制以及硬件的限制，所選取的為小曲率路段。慣性導航設備使用華測P2，障礙物識別使用ZED 雙目相機。所有設備如圖18 所示。車輛控制使用周立功CAN 卡和研華AIIS-3410U 工控機。實車試驗車型為金康賽力斯SF5 型電動汽車。

圖18 實驗用設備實物圖

根據廠家提供的報文信息，開發Labview 底層算法，結合周立功CAN 卡，篩選出報文信息的幀ID，對車進行實時控制。車輛底層控制邏輯如圖19 所示。此次實驗車輛速度是通過Labview 給的一個定值。

圖19 Labview 底層轉向控制邏輯框圖

實驗道路為一條小曲率的開放道路，將障礙物車停放在離道路起點56 m 的位置處，障礙物車為圖20 中的白色轎車，當自車行駛到3 s 之后提前做出預判，迅速向左進行變道，當自車超過障礙物時候緩慢控制方向盤使自車回到期望路徑上。最終的跟蹤效果如圖21 所示，圖中的矩形為實際車輛位置，紅色線是車輛實際行駛路線。

圖20 障礙物實車測試環境圖

圖21 避障和路徑跟蹤效果曲線

最終程序輸出的方向盤轉角與車輛實際方向盤轉角跟蹤效果如圖22 所示，藍色的線為程序輸出方向盤轉角，紅色線為車輛實際方向盤轉角，可以看出整體的方向盤跟蹤效果達到了預期的期望。但還是有一定的延遲，達不到完美的跟蹤效果，這與底層的控制器以及車輛的響應有關。

圖22 方向盤轉角跟蹤效果曲線

5 結論

本文將Frenet 坐標系與柵格法以及凸近似避障原理相結合，提出了一種改進的道路自適應規劃算法，并將道路對中、逐次變道、偏離期望路徑和前輪轉角變化率等條件融入到算法中。然后搭建基于Labview/Simulink 構架的自動駕駛系統，實現Labview 與Simulink 以及車的通訊。最后通過仿真和實驗證明改進的道路自適應規劃算法能夠有效的避開障礙物得到平滑的規劃軌跡，且能夠跟蹤路徑。