基于轉(zhuǎn)向運(yùn)動(dòng)幾何的自動(dòng)駕駛避障路徑算法

藤瑞品

（長沙民政職業(yè)技術(shù)學(xué)院，湖南 長沙 410004）

一、前言

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

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

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

二、避障行駛軌跡的計(jì)算

（一）汽車的避障行駛軌跡計(jì)算

設(shè)汽車的軸距為L，轉(zhuǎn)向輪轉(zhuǎn)角為β，汽車的轉(zhuǎn)彎半徑為R，建立汽車的單軌運(yùn)動(dòng)學(xué)模型[12]，轉(zhuǎn)向幾何關(guān)系如圖1 所示。則根據(jù)汽車的轉(zhuǎn)向阿克曼幾何關(guān)系可得：

設(shè)汽車轉(zhuǎn)向時(shí)汽車的轉(zhuǎn)向角速度為ω（定義順時(shí)針轉(zhuǎn)動(dòng)為負(fù)，逆時(shí)針轉(zhuǎn)動(dòng)為正），行駛車速為v，如圖1所示，則ω的計(jì)算公式如下：

圖1 轉(zhuǎn)向幾何關(guān)系

首先汽車在第一階段即避障階段行駛，假設(shè)這時(shí)汽車的轉(zhuǎn)向輪逆時(shí)針轉(zhuǎn)動(dòng)避障，設(shè)汽車在避障行駛時(shí)的轉(zhuǎn)向輪轉(zhuǎn)動(dòng)角速度為ψ（定義順時(shí)針轉(zhuǎn)動(dòng)為負(fù)，逆時(shí)針轉(zhuǎn)動(dòng)為正），轉(zhuǎn)向操作時(shí)間為t，則t與β之間的關(guān)系為：

設(shè)汽車行駛過程中的航向角為θ，則在t時(shí)刻，θ的計(jì)算方法為：

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

圖2 行駛軌跡坐標(biāo)

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

y坐標(biāo)的計(jì)算過程如下：

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

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

汽車完成避障階段行駛后進(jìn)入回正行駛階段，這時(shí)汽車的初始航向角為θ1，轉(zhuǎn)向輪以角速度-ψ反方向轉(zhuǎn)動(dòng)。

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

在t時(shí)刻，汽車轉(zhuǎn)向輪轉(zhuǎn)向角β的計(jì)算如下式：

由式（2）可得汽車轉(zhuǎn)向行駛時(shí)的轉(zhuǎn)動(dòng)角速度ω的計(jì)算如下：

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

設(shè)汽車在避障階段結(jié)束時(shí)軌跡點(diǎn)的坐標(biāo)為(x1，y1)，由此可得汽車在回正階段的行駛軌跡曲線方程，其中x坐標(biāo)的計(jì)算過程如下：

y坐標(biāo)的計(jì)算過程如下：

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

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

（二）避障路徑曲線

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

圖3 典型避障路徑曲線

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

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

三、避障路徑規(guī)劃及控制算法

（一）避障控制步驟

基于上述避障路徑算法，實(shí)現(xiàn)汽車的避障行駛控制的步驟如下：

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

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

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

加速度a的計(jì)算公式為：

根據(jù)式（1）、式（2）可得：

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

根據(jù)式（7）和式（8）可得|θ2|的計(jì)算公式為：

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

表1 部分行駛軌跡曲線庫

（續(xù)表）

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

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

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

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

4.最優(yōu)汽車行駛軌跡曲線的選擇和計(jì)算

根據(jù)避障約束條件、汽車的避障行駛方向和汽車的行駛車速v 在已經(jīng)建立的行駛軌跡曲線庫中選擇和計(jì)算最優(yōu)的避障曲線，并驗(yàn)證避障約束條件。

5.確定汽車的避障行駛控制參數(shù)

根據(jù)選擇計(jì)算得到的行駛軌跡曲線，與曲線對應(yīng)的轉(zhuǎn)向輪轉(zhuǎn)向角速度ψ、轉(zhuǎn)向方向以及對應(yīng)的行駛時(shí)間t1和t2即為汽車的轉(zhuǎn)向避障操作參數(shù)。

（二）障礙物的避障計(jì)算

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

圖4 單個(gè)障礙物避障工況圖

實(shí)現(xiàn)避障行駛過程如下：

步驟1：設(shè)定避障約束條件

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

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

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

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

根據(jù)預(yù)先設(shè)定的避障約束條件，汽車要實(shí)現(xiàn)避障行駛，避障空間需滿足以下條件：

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

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

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

步驟3：選擇計(jì)算最優(yōu)行駛軌跡曲線

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

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

根據(jù)約束條件，由圖4、圖5可知：

圖5 a點(diǎn)避障路徑初選條件

所以選擇行駛軌跡曲線的約束條件為：yBO≥1.2m且盡可能小，又已知車輛行駛速度為5m/s，根據(jù)上述條件在表1 的行駛路徑曲線庫里選擇，結(jié)果可以得到當(dāng)ψ=0.63rad/s時(shí)避障曲線為最優(yōu)，這時(shí)yBO=1.23m，對應(yīng)的xBO=7.8m，對應(yīng)的轉(zhuǎn)向操作時(shí)間和順序?yàn)椋孩倌鏁r(shí)針方向轉(zhuǎn)向0.47s；②順時(shí)針方向轉(zhuǎn)向1.12s。

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

圖6 避障段行駛過程

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

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

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

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

圖7 汽車的駛離行駛

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

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

完成駛離段行駛后，汽車進(jìn)入回位行駛。回位段的車速不變，轉(zhuǎn)向操作參數(shù)按照與避障段大小相同、方向相反的原則初步確定，即ψ=-0.63rad/s，轉(zhuǎn)向操作時(shí)間和順序?yàn)椋孩夙槙r(shí)針方向轉(zhuǎn)向0.47s；②逆時(shí)針方向轉(zhuǎn)向1.12s。

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

圖8 汽車a點(diǎn)的避障行駛軌跡

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

計(jì)算結(jié)果滿足預(yù)設(shè)約束條件d≥0.5m、D≥0.2m 的要求。

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

步驟4：汽車實(shí)現(xiàn)避障行駛

汽車按照上述計(jì)算得到的轉(zhuǎn)向避障操作參數(shù)操縱汽車，實(shí)現(xiàn)汽車的避障行駛，即：

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

②操作轉(zhuǎn)向輪以角速度ψ=0.63rad/s 逆時(shí)針方向轉(zhuǎn)動(dòng)，操作時(shí)間為0.47s。

③操作轉(zhuǎn)向輪以角速度ψ=0.63rad/s 順時(shí)針方向轉(zhuǎn)動(dòng)，操作時(shí)間為1.12s。

④轉(zhuǎn)向輪快速回正，以回正狀態(tài)直線行駛1.2s。

⑤操作轉(zhuǎn)向輪以角速度ψ=0.63rad/s 順時(shí)針方向轉(zhuǎn)動(dòng)，操作時(shí)間為0.47s。

⑥操作轉(zhuǎn)向輪以角速度ψ=0.63rad/s 逆時(shí)針方向轉(zhuǎn)動(dòng)，操作時(shí)間為1.12s。

⑦快速回正轉(zhuǎn)向輪，實(shí)現(xiàn)避障行駛。

四、結(jié)論

（1）提出了一種自動(dòng)駕駛避障路徑算法，算法步驟為：根據(jù)避障工況確定避障約束條件，根據(jù)避障約束條件在預(yù)設(shè)的避障路徑曲線數(shù)據(jù)庫中選擇最優(yōu)的避障路徑曲線，數(shù)據(jù)庫中的避障路徑曲線具有對應(yīng)的避障駕駛操作參數(shù)；根據(jù)所選最優(yōu)避障路徑曲線所對應(yīng)的避障駕駛操作參數(shù)，操縱汽車實(shí)現(xiàn)避障行駛。

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

（3）以汽車對單個(gè)障礙物工況的避障行駛為例，計(jì)算了汽車的避障路徑曲線，驗(yàn)證了算法的可行性。