在線校正的無人車五次樣條實(shí)時(shí)路徑規(guī)劃方法*

張立雄，郭 艷，李 寧，宋曉祥，薛 端

（陸軍工程大學(xué)，南京 210001）

0 引言

目前，依靠傳感技術(shù)和信息技術(shù)的進(jìn)步，無人車的研制已經(jīng)取得了巨大進(jìn)展，其智能化水平不斷提高。國(guó)外研究的無人車主要基于感知、決策、規(guī)劃、控制等模塊實(shí)現(xiàn)。相比于歐美國(guó)家，我國(guó)的無人車研究雖然起步較晚，但是也取得了長(zhǎng)足的發(fā)展，所研發(fā)的無人車已經(jīng)具備了高精度地圖、定位、感知、智能決策與控制等模塊。無人車具有無人操縱的低風(fēng)險(xiǎn)性、可協(xié)作性、機(jī)動(dòng)性強(qiáng)等優(yōu)點(diǎn)，可大大緩解交通系統(tǒng)的運(yùn)行壓力，也可以執(zhí)行復(fù)雜危險(xiǎn)的軍事偵察任務(wù)，避免發(fā)生人員傷害。

無人車是當(dāng)前國(guó)內(nèi)外人工智能研究的重點(diǎn)領(lǐng)域，而路徑規(guī)劃是其得以實(shí)現(xiàn)的關(guān)鍵技術(shù)。無人車路徑規(guī)劃的技術(shù)路線源自于輪式機(jī)器人技術(shù)，在保持全局導(dǎo)航規(guī)劃的同時(shí)，無人車依靠傳感器獲取周圍的行駛環(huán)境信息及車輛的狀態(tài)，路徑規(guī)劃系統(tǒng)依此信息規(guī)劃，并實(shí)時(shí)更新安全可行的路徑以完成相應(yīng)的行駛?cè)蝿?wù)。

無人車的路徑規(guī)劃旨在滿足路徑平滑度、連續(xù)性、安全性、乘坐舒適性和車輛運(yùn)動(dòng)學(xué)約束等方面。路徑規(guī)劃算法主要分為3 類：?jiǎn)l(fā)式搜索方法、隨機(jī)采樣方法和數(shù)值優(yōu)化方法?；趩l(fā)式搜索的方法通常是將路徑規(guī)劃問題轉(zhuǎn)化為優(yōu)化問題，對(duì)最優(yōu)解進(jìn)行搜索。LUO Q 等針對(duì)蟻群算法存在的易陷入局部最優(yōu)、收斂性差、搜索效率較低等問題，提出了一種改進(jìn)的蟻群算法，根據(jù)中間點(diǎn)與初始點(diǎn)、目標(biāo)點(diǎn)的距離將初始信息素非均勻分布，減少了初始階段的盲目性問題，在迭代中減弱啟發(fā)式函數(shù)的作用，以加快收斂速度，通過增加懲罰因子減少丟失螞蟻數(shù)量，保證了算法的多樣性。LI J 等設(shè)計(jì)了一種混合路徑規(guī)劃算法優(yōu)化無人車的行駛距離，采用遺傳算法生成全局路徑，根據(jù)局部滾動(dòng)優(yōu)化算法不斷優(yōu)化遺傳算法的結(jié)果。啟發(fā)式搜索方法可以根據(jù)具體的約束條件進(jìn)行復(fù)雜的算法設(shè)計(jì)，但是優(yōu)化過程通常需要耗費(fèi)較多的時(shí)間。

隨機(jī)采樣方法是在地圖上生成可行駛區(qū)域，以起始點(diǎn)為基點(diǎn)進(jìn)行采樣，生成大量的候選路徑，從中選取最佳的路徑。HU X 等采用三次樣條曲線對(duì)地圖上的道路中心點(diǎn)進(jìn)行擬合，根據(jù)弧長(zhǎng)和與中心線的偏移量生成大量候選路徑，并且在考慮靜態(tài)安全、舒適性和動(dòng)態(tài)安全等因素的條件下設(shè)計(jì)了成本函數(shù)，根據(jù)總成本最小原則選取最優(yōu)的路徑?？焖偬剿麟S機(jī)樹（RRT）算法可以找到與目標(biāo)點(diǎn)重合的一條路徑，但路徑的平滑性通常無法保證。隨機(jī)采樣法生成的大量路徑可能包括較多的冗余路徑，容易造成計(jì)算資源的浪費(fèi)，而且找到的路徑可能并非是最優(yōu)的。

然而，為了保證車輛的穩(wěn)定性和安全性，獲得具有連續(xù)曲率的路徑是有必要的。數(shù)值優(yōu)化方法是根據(jù)路徑規(guī)劃的初始狀態(tài)生成軌跡，通?？紤]車輛的狀態(tài)和受到的外部環(huán)境約束，并衡量每個(gè)目標(biāo)的重要性，根據(jù)實(shí)際需要使設(shè)計(jì)的目標(biāo)函數(shù)最大化或最小化，完成對(duì)生成軌跡的參數(shù)優(yōu)化，路徑規(guī)劃中的優(yōu)化問題通常可以轉(zhuǎn)化為半無限約束規(guī)劃問題。例如，文獻(xiàn)［7］使用模型預(yù)測(cè)控制（MPC）來獲得符合車輛運(yùn)動(dòng)學(xué)的路徑。在文獻(xiàn)［8］中，路徑規(guī)劃問題被轉(zhuǎn)化為凸優(yōu)化問題，在每個(gè)運(yùn)動(dòng)狀態(tài)下都需要進(jìn)行目標(biāo)函數(shù)的高維度優(yōu)化。數(shù)值優(yōu)化方法為無人車的路徑規(guī)劃提供了很好的解決方法，然而優(yōu)化方法的實(shí)時(shí)性難以保證。在CPU 上處理優(yōu)化問題需要較多的時(shí)間，查找表的建立可以減少參數(shù)優(yōu)化過程中花費(fèi)的時(shí)間，但參考空間中的方向變化范圍是有限的，這導(dǎo)致查找表無法直接應(yīng)用于方向變化較大的路徑規(guī)劃中。

因此，無人車在時(shí)變行駛環(huán)境中，需要保證路徑規(guī)劃的乘坐舒適性、穩(wěn)定性、實(shí)時(shí)性和連續(xù)性，為解決上述問題，提出了一種在線校正的無人車五次樣條路徑規(guī)劃與實(shí)時(shí)優(yōu)化方法，通過完成對(duì)五次樣條曲線的路徑優(yōu)化，根據(jù)優(yōu)化結(jié)果設(shè)計(jì)基于映射查找表的動(dòng)態(tài)迭代路徑規(guī)劃方法，從而快速地規(guī)劃出符合要求的路徑。

1 主要工作

為解決無人車在路徑規(guī)劃中需同時(shí)滿足乘坐舒適性、穩(wěn)定性、實(shí)時(shí)性和連續(xù)性的問題，本文將五次樣條曲線的生成方法作為路徑的求解原理，在此基礎(chǔ)上進(jìn)行參數(shù)優(yōu)化，根據(jù)乘坐舒適性、穩(wěn)定性和執(zhí)行效率的要求，以平均曲率最小和距離最短為目標(biāo)，設(shè)計(jì)了路徑優(yōu)化的目標(biāo)函數(shù)。其中，考慮到橫縱向的速度、加速度和橫擺角速度的限制，設(shè)計(jì)了路徑優(yōu)化的約束條件。針對(duì)參數(shù)優(yōu)化問題，采用了序列二次規(guī)劃（sequential quadratic programming，SQP）算法進(jìn)行求解。

優(yōu)化過程通常需要耗費(fèi)較多的時(shí)間，將基于SQP 算法在一定方向變化范圍內(nèi)進(jìn)行參數(shù)優(yōu)化的結(jié)果存儲(chǔ)并建立映射表，利用映射表查找法來解決路徑優(yōu)化的實(shí)時(shí)性問題。針對(duì)路徑規(guī)劃時(shí)起止點(diǎn)的方向變化超過映射表最大方向變化范圍的問題，采用迭代更新規(guī)則對(duì)初始路徑進(jìn)行分割，便于直接根據(jù)分割后的起止?fàn)顟B(tài)信息，通過映射表查找法快速地規(guī)劃路徑，并保證路徑曲率的連續(xù)性。

主要工作架構(gòu)如圖1 所示。

圖1 主要工作架構(gòu)

2 基于五次樣條曲線的路徑優(yōu)化

2.1 求解原理

與路徑縱向偏移量有關(guān)的各系數(shù)為：

與路徑橫向偏移量有關(guān)的各系數(shù)為：

其中，θ及θ分別是初始點(diǎn)和目標(biāo)點(diǎn)處的方向角；κ和κ分別是初始點(diǎn)和目標(biāo)點(diǎn)處的曲率；η為無人車在初始點(diǎn)處的速度v；η表示無人車在目標(biāo)點(diǎn)處的速度；η表示無人車在初始點(diǎn)處的加速度；η表示無人車在目標(biāo)點(diǎn)處的加速度。

2.2 目標(biāo)函數(shù)及約束條件

為使得無人車在行駛過程中滿足乘坐舒適性和穩(wěn)定性，應(yīng)使得路徑的平均曲率盡可能小。同時(shí)為提高行駛的執(zhí)行效率，無人車總是希望以最短的距離完成行駛?cè)蝿?wù)，因此，參數(shù)優(yōu)化的目標(biāo)函數(shù)設(shè)計(jì)為

約束應(yīng)滿足如下：

其中，s 為沿著路徑p（μ）測(cè)量所得的距離。

其中，ζ和ζ分別為路徑平均曲率和距離在目標(biāo)函數(shù)中所占的比重，由于無人車在行駛過程中更注重乘坐舒適性，因此，取ζ=10 000，ζ=1。式（6）是為了保證路徑曲線的規(guī)律性。顯然，η 表示路徑距離s（或ds/dμ）的變化率：

無人車在行駛過程中，需要考慮自身的穩(wěn)定性。無人車的橫擺角速度是反映車輛行駛穩(wěn)定性的主要指標(biāo)。假設(shè)無人車的質(zhì)心側(cè)偏角近似為0，那么需考慮無人車分別在橫向和縱向上的橫擺角，近似為

因此，通過上述目標(biāo)函數(shù)和約束條件，通過調(diào)節(jié)參數(shù)，可以規(guī)劃出符合乘坐舒適性、穩(wěn)定性和執(zhí)行效率且平均曲率最小的較短路徑。

2.3 參數(shù)優(yōu)化方法

極值優(yōu)化問題可以轉(zhuǎn)為半無限規(guī)劃形式，將其轉(zhuǎn)化為一組具有峰值的有限約束，這種優(yōu)化問題一般可以通過序列二次規(guī)劃（SQP）算法求解。SQP算法通過將具有非線性約束的原優(yōu)化問題轉(zhuǎn)換為在迭代點(diǎn)處的二次規(guī)劃子問題，求解子問題獲取新的解點(diǎn)，利用新的解點(diǎn)在原優(yōu)化問題上再次利用二次規(guī)劃求解。過程如下：

對(duì)于非線性約束的規(guī)劃問題，一般由以下公式表示。

約束為：

其中，d 是全局搜索變量； 是梯度；H是拉格朗日函數(shù)的Hessain 矩陣的正定擬牛頓近似值。通過稠密半正定牛頓近似法求解，式（15）可以通過任意一種QP 算法得到結(jié)果，所得的解將被用于新的迭代過程中：

其中，d是δ指向δ的一個(gè)向量；a為標(biāo)量步長(zhǎng)，通過適當(dāng)?shù)木€性搜索過程來確定，從而使函數(shù)的某一個(gè)指標(biāo)達(dá)到足夠小的減小量。

由于SQP 算法是目前公認(rèn)的適于解決具有非線性約束優(yōu)化問題的方法，相比于其他的優(yōu)化方法，它具有計(jì)算效率高、收斂性好、邊界搜索能力強(qiáng)等優(yōu)勢(shì)，故此選用該方法針對(duì)本文中的非線性約束優(yōu)化問題進(jìn)行參數(shù)尋優(yōu)。

3 基于映射查找表的動(dòng)態(tài)迭代路徑規(guī)劃方法

3.1 映射表查找法

由于求解路徑優(yōu)化問題需要大量的計(jì)算，時(shí)效性不足會(huì)導(dǎo)致優(yōu)化方法在實(shí)際中難以實(shí)用。為了解決該問題，在實(shí)際應(yīng)用中，映射表查找法可以為此提供解決方案，查找表由多維參數(shù)構(gòu)成，包含起始和目標(biāo)的位置、方向和曲率等參數(shù)。

由于每個(gè)參數(shù)都有無限的值，因此，有必要減少參數(shù)的維度并釋放存儲(chǔ)空間，于是構(gòu)造起點(diǎn)和目標(biāo)點(diǎn)分別固定在（0，0）m 和（10，0）m 處的參考空間。利用五次樣條曲線進(jìn)行插值的參數(shù)被簡(jiǎn)化為4 個(gè)自由度參數(shù)η：=［ηηηη］。通過查找表進(jìn)行路徑規(guī)劃的過程如下：

1）形式轉(zhuǎn)換

為了將初始點(diǎn)A 和目標(biāo)點(diǎn)B 映射為位于參考空間中的A'和B'，端點(diǎn)的方向和曲率將轉(zhuǎn)換為

2）查找表

3）反轉(zhuǎn)換

將從參考空間中獲得的路徑反變換為原始坐標(biāo)系。反轉(zhuǎn)換的過程如下：

3.2 迭代更新規(guī)則

當(dāng)方向變化超過查找表參考空間的最大范圍［-θ，θ］時(shí)，很難直接將查找表應(yīng)用于路徑規(guī)劃，因此，本文提出了一種迭代更新規(guī)則。該規(guī)則是選擇曲線上μ=β 處的點(diǎn)作為路徑分割點(diǎn)C，并將分割點(diǎn)作為下一條路徑的初始點(diǎn)，分割點(diǎn)處的曲率為

其中，β∈［0，1］為迭代更新率。因此，在動(dòng)態(tài)路徑規(guī)劃過程中，通過更新初始點(diǎn)，可以得到完整的路徑。

其中，m 表示路徑分割的段數(shù)；β表示第n 段路徑的迭代更新率。在路徑生成過程中，這些分割點(diǎn)會(huì)導(dǎo)致車輛運(yùn)動(dòng)狀態(tài)的大量變化。

1）最佳分割點(diǎn)搜索

具體的分割方法為：對(duì)β 從0 開始以速率r 增加的方式進(jìn)行搜索，直到曲線的方向變化小于或等于2 θ，確定β 的當(dāng)前值β，那么可以確定分割點(diǎn)C的位置。若考慮直接使用映射表查找法進(jìn)行路徑規(guī)劃，那么所選用的迭代更新率應(yīng)小于或等于β。

2）在線校正參數(shù)

完成路徑分割后，此時(shí)就可以得到各段路徑的起止點(diǎn)狀態(tài)信息，其中，分割點(diǎn)處的曲率信息可通過式（21）計(jì)算得到，將起止點(diǎn)狀態(tài)信息輸入到映射表中獲取最優(yōu)的新參數(shù)，完成參數(shù)更正，從而快速地路徑規(guī)劃。

4 仿真分析

為了驗(yàn)證算法的有效性，對(duì)路徑進(jìn)行了優(yōu)化求解，然后利用映射表查找法和迭代更新規(guī)則快速規(guī)劃路徑，并對(duì)路徑規(guī)劃的準(zhǔn)確性進(jìn)行了驗(yàn)證。路徑規(guī)劃的所有實(shí)驗(yàn)均是在Intel Core i7-10750H CPU 2.60 GHz 16GB RAM 上進(jìn)行的，算法是基于MATLAB 軟件編譯實(shí)現(xiàn)的。

4.1 路徑優(yōu)化求解

表1 優(yōu)化結(jié)果

將獲得的上述參數(shù)代入五次樣條曲線中，得到的路徑為：

v=10 m/s 時(shí)：

v=15 m/s 時(shí)：

v=20 m/s 時(shí)：

由表1 可知，基于SQP 算法進(jìn)行參數(shù)優(yōu)化的平均所用時(shí)長(zhǎng)為14.7 s。然而，在同等的實(shí)驗(yàn)條件下，若使用基于虛擬編碼策略的遺傳算法對(duì)目標(biāo)函數(shù)進(jìn)行優(yōu)化，需要的時(shí)間為數(shù)個(gè)小時(shí)以上?？梢?，相比于遺傳算法，SQP 算法的計(jì)算效率有著較明顯的優(yōu)勢(shì)。然而，對(duì)于無人車實(shí)際的應(yīng)用，數(shù)十秒的優(yōu)化時(shí)間仍然很長(zhǎng)，難以滿足路徑規(guī)劃實(shí)時(shí)性的要求。

4.2 路徑實(shí)時(shí)生成及精度驗(yàn)證

為解決上述路徑規(guī)劃的實(shí)時(shí)性和查表法不可直接使用的問題，基于映射表查找法和迭代更新規(guī)則進(jìn)行路徑規(guī)劃。

取4.1 節(jié)中初始速度為v=10 m/s 時(shí)得到的路徑為初始路徑，該路徑的方向變化超過了參考空間的最大方向限制。對(duì)于該初始路徑，僅需找到一個(gè)分割點(diǎn)即可通過映射表查找法直接而快速地規(guī)劃路徑。根據(jù)3.2 節(jié)中的分割方法，β 從0 開始以0.01的速率增加對(duì)分割點(diǎn)進(jìn)行搜索。在仿真中，確定的迭代更新率為0.66，得到的分割點(diǎn)狀態(tài)為p（C）=［56.1，163.0］，θ=10.6°，κ=0.032。得到的兩段新路徑分別命名為Path 1 和Path 2，根據(jù)兩段新路徑的起止點(diǎn)狀態(tài)信息，基于SQP 算法進(jìn)行參數(shù)優(yōu)化，從而得到兩段優(yōu)化后的新路徑，以便于驗(yàn)證映射表查找法的精度。為此，根據(jù)初始路徑的端點(diǎn)A、B 和分割點(diǎn)C 的狀態(tài)信息，可基于映射表查找法快速地規(guī)劃路徑，將所得路徑與基于SQP 算法優(yōu)化的路徑進(jìn)行了對(duì)比，如圖2 所示。可見，基于映射表查找法得到的路徑與優(yōu)化路徑幾乎完全重合。

圖2 路徑規(guī)劃

同時(shí)，對(duì)路徑的曲率、曲率變化和多項(xiàng)式系數(shù)進(jìn)行了對(duì)比分析，如圖3 和圖4 所示。

圖3 基于查找表和優(yōu)化結(jié)果得到的曲率κ 及其變化dκ/ds 的比較

圖4 基于映射查找表和優(yōu)化結(jié)果得到的多項(xiàng)式系數(shù)xi 和yi 的比較

由上圖可得，基于映射表查找法的路徑規(guī)劃性能與SQP 算法的優(yōu)化結(jié)果非常接近，這說明了所提方法在路徑規(guī)劃中具有很好的效果，不需要復(fù)雜的優(yōu)化過程，可以進(jìn)一步縮短路徑優(yōu)化的時(shí)間消耗。

5 結(jié)論

本文針對(duì)無人車路徑規(guī)劃在時(shí)變行駛環(huán)境中對(duì)乘坐舒適性、穩(wěn)定性和執(zhí)行效率的要求，在考慮速度、加速度和橫擺角速度等約束條件下，以平均曲率最小和距離最短為目標(biāo)設(shè)計(jì)了目標(biāo)函數(shù)，并基于SQP 算法對(duì)路徑進(jìn)行了優(yōu)化。然而路徑優(yōu)化過程尚不能滿足實(shí)時(shí)性的需求，且現(xiàn)有查表法的方向變化范圍是有限的，為進(jìn)一步提高無人車在行駛時(shí)的路徑規(guī)劃效率，提出了一種基于映射表查找法的動(dòng)態(tài)迭代路徑規(guī)劃方法，該方法用于無人車路徑規(guī)劃中基于查找表的參數(shù)優(yōu)化和最大方向變化超過查找表極限時(shí)的在線校正。結(jié)果表明，該方法具有良好的效果。