基于轉向運動幾何的自動駕駛避障路徑算法

藤瑞品

（長沙民政職業技術學院，湖南 長沙 410004）

一、前言

自動駕駛汽車的避障路徑算法有幾何軌線算法[1-4]、人工勢場法[5-6]、模型預測算法[7-8]、遺傳算法[9]、模糊算法[10]等。傳統的避障路徑規劃方法是首先根據障礙物模型和分布情況，采用一定的算法計算出一條最優的避障路徑，再對計算出的避障路徑進行幾何光滑擬合處理并作為汽車的實際行駛目標路徑，最后采用軌跡跟蹤控制技術控制汽車的實際行駛軌跡符合所規劃擬合的目標路徑[11]。

實際上，駕駛員駕駛汽車進行避障操作時，第一反應并不會像目前的避障算法一樣進行避障路徑規劃、路徑平滑、軌跡跟蹤等步驟，而是在發現障礙物時，會直接根據之前的駕駛經驗本能地做出反應，操作方向盤做出左轉、右轉、保持等動作，控制汽車快速實現避障行駛。所以駕駛員的避障駕駛決策行為就是一種經驗加上本能的操作過程，駕駛員只需本能地控制方向盤做出簡單的轉向操作，即可實現汽車的避障駕駛功能。

因此參考駕駛員的駕駛決策行為模式，在設計自動駕駛避障算法時，將汽車的轉向操作與汽車的行駛軌跡進行對應，就能將汽車的避障控制直接轉化為轉向參數的控制，這樣計算得到的避障路徑符合汽車的實際行駛狀況，而無需對避障路徑的計算結果進行額外的擬合和光滑處理。又由于避障路徑和轉向控制參數對應，因此直接采用對應的轉向控制參數即可控制汽車實現避障行駛，而無需另外采用軌跡跟蹤控制技術。

二、避障行駛軌跡的計算

（一）汽車的避障行駛軌跡計算

設汽車的軸距為L，轉向輪轉角為β，汽車的轉彎半徑為R，建立汽車的單軌運動學模型[12]，轉向幾何關系如圖1 所示。則根據汽車的轉向阿克曼幾何關系可得：

設汽車轉向時汽車的轉向角速度為ω（定義順時針轉動為負，逆時針轉動為正），行駛車速為v，如圖1所示，則ω的計算公式如下：

圖1 轉向幾何關系

首先汽車在第一階段即避障階段行駛，假設這時汽車的轉向輪逆時針轉動避障，設汽車在避障行駛時的轉向輪轉動角速度為ψ（定義順時針轉動為負，逆時針轉動為正），轉向操作時間為t，則t與β之間的關系為：

設汽車行駛過程中的航向角為θ，則在t時刻，θ的計算方法為：

設汽車行駛軌跡的橫坐標為x，縱坐標為y，汽車的行駛速度為v,對于汽車的行駛軌跡在點（x,y）時，其航向角為θ，在行駛時間為dt時，汽車行駛的距離為ds，如圖2所示。則由此可得：

圖2 行駛軌跡坐標

設汽車避障階段的初始航向角θ0=0，行駛時間為t1，由此可得汽車在避障階段的行駛軌跡曲線方程，其中x坐標的計算過程如下：

y坐標的計算過程如下：

即汽車避障行駛階段的行駛軌跡曲線方程為：

式（5）中，x(t)、y(t)為行駛軌跡曲線上各點在t時刻的坐標，t∈[0,t1]。

汽車完成避障階段行駛后進入回正行駛階段，這時汽車的初始航向角為θ1，轉向輪以角速度-ψ反方向轉動。

根據式（4），初始航向角θ1的計算如下：

在t時刻，汽車轉向輪轉向角β的計算如下式：

由式（2）可得汽車轉向行駛時的轉動角速度ω的計算如下：

則汽車在回正階段行駛過程中在t時刻的航向角θ的計算如下：

設汽車在避障階段結束時軌跡點的坐標為(x1，y1)，由此可得汽車在回正階段的行駛軌跡曲線方程，其中x坐標的計算過程如下：

y坐標的計算過程如下：

即汽車回正行駛階段的行駛軌跡曲線方程為：

式（8）中，x(t)、y(t)為行駛軌跡曲線上各點在t時刻的坐標，t∈[0,t2]，t2為回正階段的行駛時間。

（二）避障路徑曲線

圖3 為根據2.1 中算法計算得到的典型避障路徑曲線。曲線由兩部分組成：避障部分OAB 和回位部分BCD。

圖3 典型避障路徑曲線

曲線OAB 為避障部分，該部分路徑的目的是實現汽車的避障行駛并完成回正行駛。避障部分的路徑曲線又分為兩段：避障行駛段OA 和回正行駛段OB。在避障行駛段OA 部分，汽車轉向輪首先以角速度ψ逆時針轉動，汽車執行避障行駛動作，行駛至A 點后，汽車轉向輪向以角速度-ψ順時針方向轉動，汽車執行回正行駛動作，直到B 點，這時汽車的航向角為0 度，汽車實現回正行駛，從而完成避障部分的行駛路徑。

曲線BCD 為回位部分，該部分路徑的目的是實現汽車的回位行駛，即回到避障前的行駛路徑方向上并完成回正行駛。回位部分的路徑曲線也分為兩段：回位行駛段BC 和回正行駛段CD。在回位行駛段BC 部分，汽車轉向輪繼續以角速度-ψ順時針方向轉動，汽車執行回位行駛動作，行駛至C 點后，汽車轉向輪以角速度ψ逆時針方向轉動，汽車執行回正行駛動作，直到D 點，這時汽車的航向角為0 度，汽車實現回正行駛，從而完成回位部分的行駛路徑，這時汽車的行駛路徑方向與避障行駛前相同。

三、避障路徑規劃及控制算法

（一）避障控制步驟

基于上述避障路徑算法，實現汽車的避障行駛控制的步驟如下：

1.建立汽車的行駛軌跡曲線庫

根據式（5）、式（8）的汽車行駛軌跡曲線算法，計算不同車速v、不同轉向輪轉向角速度ψ、不同避障行駛時間t1和t2、不同汽車軸距參數L下汽車的行駛軌跡，建立汽車的行駛軌跡曲線庫。

為了保證汽車避障行駛時的側向穩定性和平順性，在建立汽車行駛軌跡曲線庫時，要對避障行駛過程中的側向加速度a的絕對值進行限制，這里設定為：|a|≤4m/s2。

加速度a的計算公式為：

根據式（1）、式（2）可得：

同時對汽車完成回正行駛后（圖3 中的B 點和D點）的航向角絕對值|θ2|的大小進行約束，這里設定的約束條件為：|θ2|≤1°。

根據式（7）和式（8）可得|θ2|的計算公式為：

表1 為部分行駛曲線庫，表示軸距L為2.5m，行駛車速v 為5m/s 的汽車在不同轉向參數下對應的行駛軌跡曲線的相應參數。其中|a1|max表示在避障階段的最大側向加速度絕對值，|a2|max表示在回正階段的最大側向加速度絕對值，xBO表示軌跡曲線終點與起點的x 坐標差，yBO表示軌跡曲線終點與起點的y坐標差。

表1 部分行駛軌跡曲線庫

（續表）

2.確定汽車避障的約束條件

確定汽車在避障過程中與障礙物、道路邊界等影響汽車行駛軌跡的各因素之間的約束控制條件。

3.確定汽車的避障行駛方向

汽車在避障行駛時有左右兩個行駛方向可選擇，通過計算障礙物與道路邊界之間的避障空間，來確定汽車的避障行駛方向是向左或者向右。當某個行駛方向的避障空間滿足汽車避障的行駛要求時，則選擇該方向作為汽車的避障方向，如兩個方向的避障空間都滿足避障行駛要求，則選擇總避障行駛路徑短的方向作為避障行駛方向。

4.最優汽車行駛軌跡曲線的選擇和計算

根據避障約束條件、汽車的避障行駛方向和汽車的行駛車速v 在已經建立的行駛軌跡曲線庫中選擇和計算最優的避障曲線，并驗證避障約束條件。

5.確定汽車的避障行駛控制參數

根據選擇計算得到的行駛軌跡曲線，與曲線對應的轉向輪轉向角速度ψ、轉向方向以及對應的行駛時間t1和t2即為汽車的轉向避障操作參數。

（二）障礙物的避障計算

障礙物的避障工況如圖4 所示，設長度L為4m、寬度W為1.8m 的汽車以5m/s 的速度在寬度為6m 的道路上行駛（設汽車的軸距為2.5m），行駛過程中需避開前方障礙物，相關尺寸如圖4中所標注。

圖4 單個障礙物避障工況圖

實現避障行駛過程如下：

步驟1：設定避障約束條件

本例中，設定避障約束條件為：①汽車避障行駛過程中不與障礙物發生接觸；②汽車行駛過程中與障礙物的距離d盡可能小，但不小于0.5m；③汽車行駛過程中與道路邊界的距離D不小于0.2m。

步驟2：確定汽車的避障行駛方向

首先根據圖4 計算汽車在障礙物兩側的避障空間。設汽車向右行駛時與障礙物之間避障空間為WR（圖中為下方），向左行駛時與障礙物之間的避障空間為WL（圖中為上方），根據圖4中的數據可得：

WR=600；WL=6000-600-600=4800

根據預先設定的避障約束條件，汽車要實現避障行駛，避障空間需滿足以下條件：

WL（WR）-W≥D+d=500+200=700

由于汽車的車寬W為1800mm，所以WR不滿足汽車的避障空間要求。

而對于WL，由于：WL-W=4800-1800=3000＞700，所以向左（上）行駛的避障空間WL滿足汽車的避障行駛要求。因此汽車選擇向左行駛進行避障。

步驟3：選擇計算最優行駛軌跡曲線

避障基準點的確定：選擇汽車右邊（下方）最前點即圖4 中的a 點作為避障基準點，以a 點行駛軌跡作為基準避障路徑曲線。

首先以y向避障幅度yBO為控制參數進行a 點的避障路徑曲線的初選。根據預先設定的避障約束條件：汽車行駛過程中與障礙物的距離d盡可能小，但不小于0.5m，據此確定避障路徑曲線的初選約束條件為：在避障行駛段結束后，汽車a點（對應圖3 中軌跡的B點位置）距離障礙物上y坐標最大的點P 的距離yBP盡可能小但不小于0.5m，如圖5所示。

根據約束條件，由圖4、圖5可知：

圖5 a點避障路徑初選條件

所以選擇行駛軌跡曲線的約束條件為：yBO≥1.2m且盡可能小，又已知車輛行駛速度為5m/s，根據上述條件在表1 的行駛路徑曲線庫里選擇，結果可以得到當ψ=0.63rad/s時避障曲線為最優，這時yBO=1.23m，對應的xBO=7.8m，對應的轉向操作時間和順序為：①逆時針方向轉向0.47s；②順時針方向轉向1.12s。

接下來確定避障初始點O 點的x向位置。根據避障約束條件②，要求汽車行駛過程中與障礙物的距離d 不小于0.5m，因此初步設汽車完成避障段行駛時B點與障礙物最左點Q 在x方向的距離為0.5m，如圖6所示。由于xBO=7.8m，因此避障初始點O 的x向位置按下式確定：xQO=7.8+0.5=8.3m。

圖6 避障段行駛過程

由此得到汽車行駛路徑曲線避障段的操作參數為：v=5m/s，ψ=0.63rad/s，避障初始點O 的確定方法為：xQO=8.3m。

圖6 為按上述確定的參數操作汽車，汽車所執行的避障行駛過程。

完成避障段行駛后，由于汽車和障礙物都具有y向長度，因此不能立即執行回位操作，還需要繼續直線行駛一段距離，以駛離障礙物。汽車為了實現該段路徑的行駛，在完成避障段的行駛后，需快速回正轉向輪（不考慮回正時間對汽車航向角的影響），然后以回正狀態直線行駛。

駛離段的長度D按以下方法初步確定：汽車尾部d點與障礙物最后點s 的y向距離yds=0.5m，如圖7 所示。

圖7 汽車的駛離行駛

同時根據汽車長度尺寸、障礙物尺寸以及汽車避障完成時的位置數據，可計算汽車的駛離距離D為：

即汽車的駛離段長度為6m，由于汽車的行駛車速為5m/s，因此，汽車在駛離段的行駛時間為1.2s。

完成駛離段行駛后，汽車進入回位行駛。回位段的車速不變，轉向操作參數按照與避障段大小相同、方向相反的原則初步確定，即ψ=-0.63rad/s，轉向操作時間和順序為：①順時針方向轉向0.47s；②逆時針方向轉向1.12s。

按照上述分析計算方法即可得到汽車a 點的避障行駛軌跡曲線，如圖8所示。

圖8 汽車a點的避障行駛軌跡

汽車避障行駛過程中，與障礙物的最小距離dmin以及與道路邊界的最小距離Dmin的計算結果分別為：

計算結果滿足預設約束條件d≥0.5m、D≥0.2m 的要求。

如避障路徑曲線與障礙物以及道路邊界的最小距離不滿足預設約束條件，則增加yBO重新選擇避障路徑曲線，并重復步驟3直到最小距離滿足預設約束條件。

步驟4：汽車實現避障行駛

汽車按照上述計算得到的轉向避障操作參數操縱汽車，實現汽車的避障行駛，即：

①汽車的行駛車速不變，為5m/s。

②操作轉向輪以角速度ψ=0.63rad/s 逆時針方向轉動，操作時間為0.47s。

③操作轉向輪以角速度ψ=0.63rad/s 順時針方向轉動，操作時間為1.12s。

④轉向輪快速回正，以回正狀態直線行駛1.2s。

⑤操作轉向輪以角速度ψ=0.63rad/s 順時針方向轉動，操作時間為0.47s。

⑥操作轉向輪以角速度ψ=0.63rad/s 逆時針方向轉動，操作時間為1.12s。

⑦快速回正轉向輪，實現避障行駛。

四、結論

（1）提出了一種自動駕駛避障路徑算法，算法步驟為：根據避障工況確定避障約束條件，根據避障約束條件在預設的避障路徑曲線數據庫中選擇最優的避障路徑曲線，數據庫中的避障路徑曲線具有對應的避障駕駛操作參數；根據所選最優避障路徑曲線所對應的避障駕駛操作參數，操縱汽車實現避障行駛。

（2）基于轉向運動幾何建立了避障路徑曲線的計算模型，模型依據汽車的轉向角速度參數、行駛參數、幾何參數直接計算得到，因此更符合汽車的行駛實際，且其對應的控制參數可以直接操縱汽車實現避障操作，而無需像傳統的避障算法需要路徑規劃、路徑光滑擬合和軌跡跟蹤三個步驟，相對于傳統算法更簡便，更符合駕駛操作習慣和汽車行駛軌跡特征；計算避障路徑曲線時考慮了側向加速度約束條件，能夠保證汽車在避障行駛時的側向平順性能。

（3）以汽車對單個障礙物工況的避障行駛為例，計算了汽車的避障路徑曲線，驗證了算法的可行性。