背景

根據(jù)智能車決策規(guī)劃模型以及考慮周圍實(shí)時(shí)環(huán)境變化帶來(lái)的環(huán)境約束和自身?xiàng)l件約束，為車輛提供可行使域的過(guò)程就是路徑規(guī)劃，路徑規(guī)劃分為全局和局部規(guī)劃兩種。全局路徑規(guī)劃是在已知地圖信息的條件之下，同時(shí)利用環(huán)境的局部信息和車輛實(shí)時(shí)反饋信息，從而生成可行駛區(qū)域以及車輛的參考行駛路徑。但是對(duì)于全局路徑規(guī)劃而言未能考慮駕駛方向、道路曲率、速度以及環(huán)境中所出現(xiàn)的障礙物，同時(shí)智能車在真實(shí)環(huán)境下會(huì)出現(xiàn)不可預(yù)測(cè)的因素，因此必須考慮局部環(huán)境因素，故局部路徑規(guī)劃便應(yīng)運(yùn)而生。為了解決路徑規(guī)劃所帶來(lái)的問(wèn)題，現(xiàn)階段已經(jīng)有大量的方法被提出來(lái)。其中早期方法可分為三類：人工勢(shì)場(chǎng)法、單元分解法、基于路網(wǎng)的方法。人工勢(shì)場(chǎng)是通過(guò)模擬對(duì)目標(biāo)的吸引與排斥從而產(chǎn)生一個(gè)階梯狀的勢(shì)場(chǎng)，通過(guò)這個(gè)階梯狀的勢(shì)場(chǎng)，按照階梯勢(shì)場(chǎng)的梯度找出最佳的可行駛路徑。該方法被廣泛應(yīng)用于機(jī)器人的路徑規(guī)劃。單元分解法是將環(huán)境切割成凸的無(wú)障礙區(qū)域，這些凸區(qū)域被定義為單元。路網(wǎng)規(guī)劃與單元分解類似都是將環(huán)境離散，然后進(jìn)行圖搜索，從而找到一條無(wú)碰撞路徑。可視圖法是路網(wǎng)規(guī)劃的一種改進(jìn)方法，它是將兩個(gè)可見(jiàn)的點(diǎn)連接成一條直線，從而用搜索算法進(jìn)行尋找可行路徑。上述傳統(tǒng)的路徑規(guī)劃方法大部分不會(huì)考慮智能車運(yùn)動(dòng)學(xué)或者動(dòng)力學(xué)特性，規(guī)劃的路徑并不光滑，而且不考慮車輛的軌跡跟蹤控制約束?；诙囗?xiàng)式路徑規(guī)劃方法可以根據(jù)車輛初始狀態(tài)與目標(biāo)狀態(tài)對(duì)需要變道的智能車進(jìn)行變道過(guò)程中的路徑規(guī)劃，從而使得智能車在一定時(shí)間內(nèi)可以到達(dá)指定車道。其中在隨著多項(xiàng)式次數(shù)的增多，曲線擬合效果就越好，但是次數(shù)的增多會(huì)伴隨著參數(shù)求解過(guò)程復(fù)雜，一般采用五次多項(xiàng)式進(jìn)行車輛換道避障的規(guī)劃。其中五次多項(xiàng)式公式為：

You F等基于五次多項(xiàng)式構(gòu)造變道軌跡函數(shù)組，分別在橫向和縱向建立多項(xiàng)式，通過(guò)增加縱向自由度的形式解決變道超車縱向車速變化的問(wèn)題。Guoqing G等利用五次多項(xiàng)式記錄不同工況下的換道路徑數(shù)據(jù)，根據(jù)已有的數(shù)據(jù)利用遺傳算法和BP神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練從而得到最優(yōu)駕駛員模型?；诙囗?xiàng)式的路徑規(guī)劃在縱向和側(cè)向都可以達(dá)到期望效果。但在縱向車速變化情況下自適應(yīng)能力較差。B樣條是另外一種比較特別的多項(xiàng)式擬合形式。它利用加權(quán)點(diǎn)生成中間狀態(tài)來(lái)滿足約束條件，在進(jìn)行智能車輛路徑規(guī)劃過(guò)程中廣泛應(yīng)用。Guilin Z等在B三次樣條的基礎(chǔ)之上考慮了單輪模型并與電機(jī)動(dòng)力限制相結(jié)合，將其應(yīng)用在了單輪車上的路徑規(guī)劃。日本學(xué)者M(jìn)aekawa T等在給定障礙物位置和工作區(qū)間幾何條件的情況下利用B三次樣條進(jìn)行避障軌跡規(guī)劃。貝塞爾曲線以其線條光滑且曲率小被廣泛應(yīng)用。康奈爾大學(xué)在2005年DARPA挑戰(zhàn)賽上利用貝塞爾曲線生成符合車輛動(dòng)力學(xué)的路徑。Ma L等利用貝塞爾曲線完成了道路上的行為決策和在線軌跡規(guī)劃。Bae I等對(duì)三階和五階貝塞爾曲線進(jìn)行對(duì)比，得出滿足起始曲率為0且舒適性最優(yōu)的五階貝塞爾曲線，并根據(jù)五階貝塞爾曲線實(shí)時(shí)控制智能駕駛車輛的橫縱向行駛軌跡。

智能車避障算法設(shè)計(jì)

——引言

車輛換道避障的行為決策需要滿足行駛穩(wěn)定和安全舒適的駕駛員意圖，本文所設(shè)計(jì)的行為決策模型，主要結(jié)合車輛動(dòng)力學(xué)，設(shè)計(jì)了一個(gè)動(dòng)態(tài)風(fēng)險(xiǎn)評(píng)估模型保證車輛在換道避障過(guò)程中不會(huì)出現(xiàn)車輛失穩(wěn)和碰撞的情況。本文基于非解耦五次多項(xiàng)式進(jìn)行路徑規(guī)劃的設(shè)計(jì)，考慮安全性約束、邊界約束和非完整性約束多個(gè)約束條件。利用QP算法將原本路徑規(guī)劃過(guò)程中非凸問(wèn)題轉(zhuǎn)換為凸優(yōu)化問(wèn)題并求解出最優(yōu)路徑。

——車輛行為決策規(guī)劃

問(wèn)題描述

在車輛的實(shí)際避障過(guò)程中，決策規(guī)劃是自動(dòng)駕駛最重要的部分之一。規(guī)劃決策系統(tǒng)在處理完環(huán)境信息之后，基于車輛動(dòng)力學(xué)的同時(shí)結(jié)合駕駛需求進(jìn)行車輛行為的實(shí)時(shí)決策規(guī)劃，主要分為行為決策和運(yùn)動(dòng)規(guī)劃兩部分，行為決策在滿足交通規(guī)則、行駛安全等約束條件下生成駕駛意圖；而運(yùn)動(dòng)規(guī)劃主要是根據(jù)車輛狀態(tài)和已知的環(huán)境信息，在考慮多重動(dòng)態(tài)和多個(gè)約束條件下，確保車輛的安全性、舒適性和可靠性，實(shí)時(shí)獲取車輛的期望軌跡。

坐標(biāo)系轉(zhuǎn)換

在Frenet坐標(biāo)系下，非解耦五次多項(xiàng)式是弧長(zhǎng)

為自變量，橫向偏移量

為因變量的空間函數(shù)。在進(jìn)行路徑規(guī)劃過(guò)程中通常以路徑連續(xù)性、平滑性、安全性作為主要的評(píng)價(jià)指標(biāo)。如圖1所示，在Frenet坐標(biāo)系下建立切線向量和法線向量，將智能車的運(yùn)動(dòng)狀態(tài)時(shí)刻分成

，使得車輛在擬合路徑過(guò)程中，減少坐標(biāo)信息的處理。

圖1 Frenet坐標(biāo)系

假設(shè)笛卡爾坐標(biāo)系下車輛換道避障最優(yōu)規(guī)劃路徑位置、曲率和航向角為：

其中

為規(guī)劃路徑上的速度、航向角、曲率。上式中所得的關(guān)系變量是以時(shí)間t為自變量，轉(zhuǎn)換為曲線坐標(biāo)系以弧長(zhǎng)

為自變量：

上式中

為車道中心線弧長(zhǎng)并非局部路徑長(zhǎng)度。找出規(guī)劃路徑的曲率、速度、

之間關(guān)系，即為簡(jiǎn)化推導(dǎo)過(guò)程，如圖2所示，可得以下關(guān)系：

圖2 笛卡爾坐標(biāo)與曲線坐標(biāo)系的關(guān)系

其中在上圖中m點(diǎn)下笛卡爾坐標(biāo)系的曲率，以時(shí)間t為參數(shù)，可得：

由上式(2)可知，為求m點(diǎn)在笛卡爾坐標(biāo)系下曲率，需求m點(diǎn)在

方向上的加速度和速度：

將(3)和(4)代入(2)得：

上式是局部路徑關(guān)于時(shí)間t得一階和二階導(dǎo)，在Frenet坐標(biāo)系下關(guān)于中心線弧長(zhǎng)

的函數(shù)關(guān)系為：

在實(shí)際彎道行駛過(guò)程中，局部路徑的橫向偏移量

遠(yuǎn)小于車道中心線半徑，故實(shí)際應(yīng)用過(guò)程中，針對(duì)一個(gè)規(guī)劃窗內(nèi)的車輛動(dòng)力學(xué)運(yùn)動(dòng)狀態(tài)可忽略不計(jì)，即

行為決策模型設(shè)計(jì)

為了設(shè)計(jì)更為安全舒適的路徑，本章節(jié)的車輛行為決策以車輛在直道上換道避障為例(彎道情況與其類似)，在Frenet坐標(biāo)系下進(jìn)行行為決策規(guī)劃。假設(shè)跟隨車輛車寬為1.7m，設(shè)計(jì)的安全閾值為0.5m，即

。車輛實(shí)際行駛的縱向距離

和行駛縱向車速

實(shí)時(shí)計(jì)算

，當(dāng)

時(shí)，可進(jìn)行換道避障，否則車輛存在碰撞或橫向失穩(wěn)風(fēng)險(xiǎn)。行為決策如下圖3所示，詳細(xì)的決策模型可參考公眾號(hào)文章：優(yōu)化思想及KKT原理在換道決策中的應(yīng)用。

圖3 決策規(guī)劃動(dòng)作圖

——五次多項(xiàng)式路徑規(guī)劃

問(wèn)題描述

根據(jù)行為決策結(jié)果，車輛將進(jìn)行車道保持模式和換道避障模式的切換。其中兩種模式在行駛過(guò)程中目標(biāo)和參考線有所區(qū)別，本節(jié)主要以換道模式下的路徑規(guī)劃為例進(jìn)行說(shuō)明。

智能車動(dòng)力學(xué)構(gòu)型空間包含了時(shí)間維度、橫向空間維度和縱向空間維度。在Frenet坐標(biāo)系下既可以將車輛動(dòng)力學(xué)問(wèn)題優(yōu)化成橫向和縱向兩個(gè)時(shí)空方向相互獨(dú)立的問(wèn)題，又可以將車輛動(dòng)力學(xué)問(wèn)題整合成單個(gè)橫向時(shí)空優(yōu)化問(wèn)題，考慮換道過(guò)程中的路徑的舒適性、安全性和平滑性等問(wèn)題。

優(yōu)化目標(biāo)描述

(1)引導(dǎo)線選擇

利用傳感器可檢測(cè)到車道線數(shù)據(jù)信息，其中相鄰車道線目標(biāo)車道

中心線方程信息：

目標(biāo)車道中心線為：

其中

表示向左換道避障，

表示向右換道避障，式(8)為換道避障的目標(biāo)車道線。

目標(biāo)車道中心線作為參考線并不能直接用于車輛控制，需經(jīng)優(yōu)化處理成目標(biāo)軌跡即引導(dǎo)線方可使用。如圖4所示，在Frenet坐標(biāo)系下，引導(dǎo)線為待優(yōu)化的期望車輛行駛軌跡。在實(shí)際環(huán)境下，引導(dǎo)線需滿足多重約束條件，并且平滑過(guò)渡到參考線上。

圖4 規(guī)劃引導(dǎo)線

(2)優(yōu)化目標(biāo)

Frenet坐標(biāo)系下，利用非解耦五次多項(xiàng)式描述目標(biāo)行駛路徑：

其中，

為待定多項(xiàng)式系數(shù)。

關(guān)于非解耦五次多項(xiàng)式路徑規(guī)劃優(yōu)化目標(biāo)主要是換道過(guò)程中的誤差值、橫向位移、航向角、道路曲率、道路曲率變化率以及換到完成后的狀態(tài)誤差組成：

其中，

是目標(biāo)優(yōu)化過(guò)程中的權(quán)重系數(shù)，X其中是運(yùn)動(dòng)過(guò)程中的縱向距離窗寬度。

其中定義待優(yōu)化變量

，優(yōu)化目標(biāo)可轉(zhuǎn)換為：

其中，

關(guān)于非解耦的五次多項(xiàng)式路徑規(guī)劃的優(yōu)化目標(biāo)可等價(jià)于：

其中，

約束條件設(shè)置

其中約束主要包括外部、內(nèi)部約束，內(nèi)部約束是指車輛動(dòng)力學(xué)或運(yùn)動(dòng)學(xué)限制從而帶來(lái)的非完整約束，外部約束主要是外部環(huán)境、檢測(cè)到的障礙物帶來(lái)的約束。

1. 邊界約束：假設(shè)車輛實(shí)際軌跡會(huì)一直沿著規(guī)劃的路徑向前運(yùn)動(dòng)，則對(duì)應(yīng)的邊界約束條件為

其中

表示橫向偏移量，

表示航向偏移量。根據(jù)邊界約束條件，路徑規(guī)劃等式約束等價(jià)于:

2.安全性約束：在車輛避障換道過(guò)程中，為確保換道的安全性，路徑規(guī)劃滿足的約束條件為:

換道過(guò)程中以左為正，則以車輛向左換道為例，如圖5所示。其中當(dāng)

時(shí)，表示向右換道；當(dāng)

時(shí)，表示向左換道。

代表路面寬度。

圖5安全性約束示意圖

非完整性約束：

對(duì)于車輛行駛軌跡的曲率約束最主要的是非完整約束。其中考慮到Fernet坐標(biāo)系下曲率計(jì)算的非線性，則在一個(gè)規(guī)劃窗內(nèi)車輛動(dòng)力學(xué)狀態(tài)的變化較小可直接忽略，即曲率可表示為：

對(duì)于車輛Ackerman轉(zhuǎn)向幾何的曲率約束最重要的是非完整性約束:

其中，

表示與車輛轉(zhuǎn)向相關(guān)的最大曲率。即:

非完整性約束即曲率約束主要是針對(duì)汽車橫向軌跡進(jìn)行約束，即將非線性的非完整約束(13)轉(zhuǎn)換成非線性約束條件(14)。

依據(jù)安全性約束和非完整性約束條件等價(jià)于:

針對(duì)非解耦五次多項(xiàng)式路徑規(guī)劃的約束條件包含邊界約束、安全性約束、非完整性約束即曲率約束。綜上，在Frenet坐標(biāo)系下，關(guān)于車輛非解耦的路徑規(guī)劃可以轉(zhuǎn)換成求解標(biāo)準(zhǔn)的QP問(wèn)題：

在標(biāo)準(zhǔn)QP模型(17)中，綜合考慮非解耦五次多項(xiàng)式擬合目標(biāo)路徑的安全性約束、邊界約束以及非完整性約束。因此，本節(jié)主要設(shè)計(jì)了換道過(guò)程中非解耦五次多項(xiàng)式路徑規(guī)劃模型，并成功轉(zhuǎn)換為標(biāo)準(zhǔn)QP問(wèn)題。

小結(jié)

在符合交通規(guī)則約束和行駛安全約束的條件下設(shè)計(jì)基于動(dòng)力學(xué)的行為決策風(fēng)險(xiǎn)評(píng)估模型，利用非解耦五次多項(xiàng)式進(jìn)行路徑規(guī)劃設(shè)計(jì)并將非解耦路徑規(guī)劃的非凸優(yōu)化問(wèn)題利用QP轉(zhuǎn)化為標(biāo)準(zhǔn)的凸優(yōu)化問(wèn)題，從而求解出最優(yōu)路徑，最后利用已經(jīng)確定好的LQR控制算法進(jìn)行橫向動(dòng)力學(xué)控制，最終實(shí)現(xiàn)車輛的安全換道避障。