自動駕駛車輛的平行泊車軌跡規(guī)劃*

胡 杰，張敏超，徐文才，陳瑞楠，鐘鑫凱，朱令磊

（1.武漢理工大學(xué)，現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室，武漢 430070；2.武漢理工大學(xué)，汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070；3.新能源與智能網(wǎng)聯(lián)車湖北工程技術(shù)研究中心，武漢 430070）

前言

泊車場景下的自動駕駛技術(shù)漸漸成為研究重點，其中代客泊車便是典型的應(yīng)用。目前，對自動泊車路徑規(guī)劃方法的研究大致可以分為幾何法、多步法、采樣法和數(shù)值優(yōu)化法4類。

幾何法大多考慮采用圓弧加直線來構(gòu)造路徑，但是采用該法構(gòu)造的路徑曲率不連續(xù)，這會導(dǎo)致車輛在進(jìn)行路徑跟蹤時出現(xiàn)原地轉(zhuǎn)向問題，并且該方法對泊車初始位姿要求比較嚴(yán)格。針對這一問題，文獻(xiàn)［6］～文獻(xiàn)［14］中采用擬合曲線對路徑進(jìn)行優(yōu)化，解決了路徑曲率不連續(xù)問題，但增加了對泊車位大小的要求，而且并未解決初始位姿的問題。文獻(xiàn)［15］中將平行泊車路徑分為兩段，采用五次多項式生成路徑曲線解決了泊車初始位姿問題，但其第二段曲線也增加了對泊車位大小的要求。

多步法是將自動泊車路徑約束進(jìn)行逐步考慮，然后分步獲取滿足約束的路徑。文獻(xiàn)［16］和文獻(xiàn)［17］中采用一種非完整系統(tǒng)的三步路徑規(guī)劃方法，該方法首先計算無碰撞路徑，然后通過考慮非完整約束和其他運動學(xué)約束的局部規(guī)劃器計算的線段連接來逼近第一步中獲得的路徑，最后將第二步生成的路徑優(yōu)化為更短、更平滑路徑，但是多步法通常會導(dǎo)致規(guī)劃出的路徑有較多的轉(zhuǎn)折點，這會使車輛頻繁地前進(jìn)后退。

采樣法是機器人和計算機科學(xué)中研究的基于采樣的尋徑算法。在文獻(xiàn)［18］中開發(fā)了一種基于有序采樣的算法，該算法對汽車從泊車位后退的可能軌跡進(jìn)行有序采樣，然后根據(jù)安全、機動等性能標(biāo)準(zhǔn)對停車路徑進(jìn)行優(yōu)化。該方法對各種初始起始位姿以及泊車場景有很好的適應(yīng)能力，但在每個計算周期內(nèi)都須對軌跡進(jìn)行采樣計算，計算時間較長。

數(shù)值優(yōu)化方法通過引入最優(yōu)目標(biāo)，將非完整系統(tǒng)的運動規(guī)劃問題重新表述為開環(huán)最優(yōu)控制問題，并進(jìn)行數(shù)值求解。文獻(xiàn)［19］中找到了所有3種停車情況下的最小時間的路徑，并使用序列二次規(guī)劃（SQP）來求解優(yōu)化問題。在文獻(xiàn)［20］中也進(jìn)行了類似的研究，采用內(nèi)點法求解優(yōu)化問題。但數(shù)值優(yōu)化方法計算時間非常長，在當(dāng)前典型的CPU上進(jìn)行計算可能需要40 s以上的時間。表1所示為自動泊車路徑規(guī)劃各種方法對比。

表1 自動泊車路徑規(guī)劃方法對比

針對以上方法存在的問題，本文中對自動駕駛車輛平行泊車場景進(jìn)行分析，結(jié)合采樣法對場景的適應(yīng)能力較強和幾何法計算簡單的特點，提出一種基于自動駕駛車輛的平行泊車軌跡規(guī)劃方法。該方法對各種泊車初始位姿有較強的適應(yīng)能力，同時計算時間較短，能較好地滿足自動駕駛車輛平行泊車的需求。

1 車輛運動學(xué)模型

泊車是一個低速行駛且基本不受側(cè)向力的過程，行駛中輪胎沒有發(fā)生滑移作純滾動，其轉(zhuǎn)向軸線均交于一點作圓周運動。因此將整車視為一個剛體，從而能夠基于阿克曼轉(zhuǎn)向原理來進(jìn)行運動學(xué)研究。

車輛運動學(xué)模型和車輛尺寸參數(shù)如圖1所示，其中x、y為車輛后軸中心坐標(biāo)，θ為車輛航向角，φ為前輪等效轉(zhuǎn)向角，v為后軸中心速度，L為整車長度，W為整車寬度，L為車輛前懸長度，L為軸距，L為車輛后懸長度。A(x，y)、B(x，y)、C(x，y)、D(x，y)為汽車車身4個頂點坐標(biāo)。

圖1 汽車運動學(xué)模型

以車輛后軸中心為參考點，（x，y，θ，φ）為車輛的位姿向量，車輛在沿給定路徑轉(zhuǎn)向行駛時，根據(jù)車輛運動學(xué)關(guān)系可知：

（1）車輛的后軸中心瞬時轉(zhuǎn)向半徑R與路徑曲線的曲率半徑r相等；

（2）路徑曲線對x的1階導(dǎo)數(shù)與車輛的航向角正切值相等。

根據(jù)以上兩點可推出如下關(guān)系：

式中：y′為路徑曲線方程對x的1階導(dǎo)數(shù)；y″為路徑曲線方程對x的2階導(dǎo)數(shù)。

同時，由于車輛本身機械結(jié)構(gòu)的限制，前輪等效轉(zhuǎn)向角φ存在如下關(guān)系：

式中：φ為前輪最大等效轉(zhuǎn)向角；φ˙為前輪最大轉(zhuǎn)向角速度。

泊車過程為了避免車輛與周圍其他物體發(fā)生碰撞，須計算車身4個頂點的坐標(biāo)。根據(jù)圖1所示的幾何關(guān)系，可以得出車身各頂點坐標(biāo)與后軸中心坐標(biāo)的關(guān)系表達(dá)式：

2 自動平行泊車軌跡規(guī)劃

在平行泊車場景中，自動駕駛車輛需要在沒有人工干預(yù)的情況下完成在停車場中尋找車位并進(jìn)行泊車的任務(wù)。通常泊車位前方有一條結(jié)構(gòu)化道路，道路為雙向單車道，如圖2所示。自動駕駛車輛沿著一側(cè)的道路邊緩慢行駛邊檢測空閑泊車位。當(dāng)行駛到泊車位附近P處時，自動駕駛車輛檢測到泊車位，此時車輛需要繼續(xù)向前行駛至合適的起始泊車點P。車輛到達(dá)起始泊車點P處之后，駕駛員將車輛擋位切入倒擋并進(jìn)行泊車，直到車輛到達(dá)P點泊車成功。根據(jù)上述描述場景，本文中把平行泊車軌跡分為兩段，第一段為前進(jìn)軌跡，第二段為泊車軌跡，通過選取合適的起始泊車點生成平行泊車軌跡。

圖2 平行泊車場景

2.1 泊車路徑設(shè)計

如圖3所示，以泊車位中心為坐標(biāo)原點，建立泊車路徑規(guī)劃直角坐標(biāo)系。其中D為道路寬度，W為泊車位寬度，L為泊車位長度。

圖3 泊車路徑設(shè)計示意圖

2.1.1 第一段泊車路徑設(shè)計

泊車路徑的第一段路徑曲線要求是一段能連接P點位姿（x，y，θ，φ）和P點位姿（x，y，θ，φ）的曲線，并且車輛在沿第一段路徑曲線行駛時，其后軸中心坐標(biāo)（x，y）、航向角θ和前輪等效轉(zhuǎn)角φ都應(yīng)連續(xù)變化。考慮到這些約束，本文中采用五次多項式來生成第一段路徑曲線。

本文中采用的是五次多項式曲線，故需要6個約束方程，聯(lián)立求解式（10）、式（11）、式（13）、式（14）、式（16）和式（17）即可得五次多項式系數(shù)a～a。

2.1.2 第二段泊車路徑設(shè)計

為了盡量減小平行泊車時車輛對泊車位大小的需求，本文中將第二段路徑設(shè)計成兩段曲線，如圖4所示。曲線1是一段五次多項式曲線，其作用是連接P點位姿（x，y，θ，φ）和P點的位姿（x，y，θ，φ）。曲線2是一段半徑大小等于車輛最小轉(zhuǎn)彎半徑R的圓弧。

圖4 第二段泊車路徑

圖5 第二段泊車路徑中圓弧段路徑

由于曲線1是五次多項式曲線，令曲線1的表達(dá)式為

式中k～k為表達(dá)式系數(shù)。

采用第一段路徑曲線計算方法，將P點和P的位姿分別代入之后即可確定系數(shù)k～k，故第二段路徑曲線為

2.2 最佳泊車起始點的選取

2.2.1 起始泊車點的采樣

在平行泊車中車輛在泊車起始點時應(yīng)盡量與泊車位平行，同時車輛轉(zhuǎn)向角為零。所以在本文中起始泊車點的航向角θ=0，轉(zhuǎn)向角φ=0。故只需對在泊車起始區(qū)域?qū)囕v后軸中心坐標(biāo)（x，y）進(jìn)行采樣。如圖6所示，在車輛前進(jìn)方向上確定一塊矩形區(qū)域作為泊車起始點采樣區(qū)域，其可以表示為

圖6 泊車起始區(qū)域

式中：x為左邊界橫坐標(biāo)值；x為右邊界橫坐標(biāo)值；y為下邊界縱坐標(biāo)值；y為上邊界縱坐標(biāo)值。

矩形區(qū)域邊界計算方法為

在矩形區(qū)域內(nèi)以左下角（x，y）為起點，橫向間距為Δx，縱向間距為Δy均勻取點。以此每個點P都可以確定一個位姿（x，y，0，0），從而獲得兩段路徑曲線。

2.2.2 路徑約束分析

對泊車的路徑曲線需要滿足兩個約束條件，一是路徑曲線的曲率半徑r不能小于車輛最小轉(zhuǎn)彎半徑R，二是車輛沿著路徑曲線行駛時不能與道路邊界以及車位邊界發(fā)生碰撞。

圖7 路徑碰撞

2.2.3 多目標(biāo)評價函數(shù)設(shè)計

針對生成路徑曲線滿足路徑約束的采樣點，需要建立一個評價函數(shù)來評價每條路徑的優(yōu)劣，從中選取路徑曲線最優(yōu)的采樣點作為最佳泊車起始點。首先，曲線應(yīng)盡量的平滑，曲線越平滑路徑跟蹤越容易，車輛的平順性越好；其次，泊車路徑曲線長度還應(yīng)該盡可能短，較短的泊車路徑曲線能縮短泊車時間；最后，整個泊車過程所占用空間大小應(yīng)該盡可能的小，并且泊車過程應(yīng)該盡量少占對向車道。

路徑曲線平滑程度可以用路徑曲線的最小曲率表示，最小曲率越小路徑曲線的平滑程度越好。根據(jù)路徑曲線方程，可以得到路徑曲線曲率計算公式，離散路徑曲線計算該段路徑的最小曲率，通過比較獲得整個泊車路徑曲線的最小曲率：

式中：s為第i個采樣點的第一段路徑曲線弧長；s為第i個采樣點的第二段路徑曲線的弧長。

從而，第i個采樣點泊車路徑曲線的弧長為

泊車空間占有對向車道的程度可以用起始泊車點的縱坐標(biāo)表示，起始泊車點的縱坐標(biāo)越小，對向車道的占用越少。同時由于目標(biāo)評價函數(shù)中考慮了泊車曲線路徑長度這一指標(biāo)，所以起始泊車點的縱坐標(biāo)越小，泊車空間也越小。

結(jié)合上述分析，對于一個生成路徑曲線滿足路徑約束的采樣點，可以采用以下評價函數(shù)計算其代價值：

式中w、w、w為各權(quán)重系數(shù)，本文中采取的權(quán)重值為w=40，w=3，w=1。

比較各采樣點生成的泊車路徑曲線代價值，代價值最低的采樣點即為最佳泊車起始點。

2.3 速度規(guī)劃

本文中泊車路徑分為兩段，故需要對兩段路徑分別進(jìn)行速度規(guī)劃，但由于對兩段路徑進(jìn)行速度規(guī)劃的方法相同，所以本小節(jié)以第一段路徑為例來討論泊車路徑的速度規(guī)劃。

自動駕駛車輛在泊車時是一個低速的路徑跟蹤過程，在進(jìn)行速度規(guī)劃時，需要限制泊車過程中的最大速度，本文中限定泊車過程中的最大速度v=5 km/h。同時泊車軌跡需要滿足式（4），即泊車過程中前輪轉(zhuǎn)向角速度要小于車輛允許的最大轉(zhuǎn)向角速度φ˙。

如圖8所示，針對一條已知路徑，可以計算出其路徑長度S，當(dāng)自動駕駛車輛以最大速度跟隨這段路徑時，通過這段路徑的時間t=S/v。假定S-t曲線符合三次多項式：

圖8 速度規(guī)劃

將式（34）代入式（32）和式（33）中即可確定一條路徑長度-時間曲線，并通過計算泊車過程中速度和轉(zhuǎn)向角速度是否滿足約束條件來修正S-t曲線，若通過路徑的時間為t時生成的曲線不滿足約束，則以Δt（Δt＞0）為步長，依次向前搜索，直到尋找到滿足約束的S-t，通過對曲線求導(dǎo)獲得速度-時間曲線。

3 仿真分析

為了驗證平行泊車軌跡規(guī)劃方法的可行性，利用Python腳本語言編寫平行泊車軌跡規(guī)劃模塊進(jìn)行仿真分析。同時為保證仿真的真實性，在仿真過程中使用的參數(shù)與實車試驗參數(shù)一致，如表2所示。

表2 車輛以及環(huán)境參數(shù)

通過與雙圓弧式平行泊車路徑對比，分析本文中規(guī)劃的曲率連續(xù)性，結(jié)果如圖9所示。從圖9中可以看出，雙圓弧式平行泊車路徑的曲率在雙圓弧切點處會發(fā)生曲率突變，而本文中規(guī)劃的兩條路徑曲線曲率都是連續(xù)變化，同時在第一段路徑的結(jié)束點和第二段路徑的起始點曲率值相等，這意味著車輛在前進(jìn)和后退切換點處不需要原地調(diào)整轉(zhuǎn)向角。同時，規(guī)劃出的路徑曲率半徑最小值均大于車輛的最小轉(zhuǎn)彎半徑，滿足車輛運動學(xué)要求。

圖9 與雙圓弧平行泊車路徑規(guī)劃對比

通過設(shè)計不同的規(guī)劃起點位姿來驗證方法不同的泊車起始位姿的適應(yīng)能力，如表3所示。經(jīng)過測試，通過Python腳本語言編寫的平行泊車軌跡規(guī)劃模塊規(guī)劃一次的時間約1.19 s。

表3 規(guī)劃起點的位姿

圖10～圖12所示為3種工況的仿真結(jié)果，可以看出在3種工況下軌跡規(guī)劃模塊都能規(guī)劃出兩段滿足約束軌跡，并且能適應(yīng)各種起始位姿。

對比圖10（a）、圖11（a）、圖12（a）可以發(fā)現(xiàn)，當(dāng)規(guī)劃起點的位姿變化在第一段軌跡曲線的調(diào)整范圍內(nèi)（工況②、③）時，規(guī)劃出的第二段軌跡基本一致，但當(dāng)規(guī)劃起點位姿變化較大（工況①）且超出第一段軌跡曲線的調(diào)整范圍時，選擇的二段路徑起點（即泊車起始點）生成的泊車路徑長度和泊車空間的需求就會明顯增大。該方法可以盡量地滿足較短的泊車路徑和較小的泊車空間需求。

圖10 工況①仿真結(jié)果

圖11 工況②仿真結(jié)果

圖12 工況③仿真結(jié)果

同樣，觀察3種工況下的速度變化圖和轉(zhuǎn)向角速度變化圖可以知道，3種工況下速度曲線變化平滑，轉(zhuǎn)向角速度滿足要求。

4 實車試驗

試驗車輛具有線控底盤控制系統(tǒng)，能夠?qū)崿F(xiàn)對轉(zhuǎn)向盤轉(zhuǎn)角、油門、制動踏板和擋位的控制，可直接通過CAN通信對車輛進(jìn)行控制。同時車輛配備有激光雷達(dá)、毫米波雷達(dá)、控制器、GPS、IMU等設(shè)備，因此不需要再對車輛進(jìn)行相應(yīng)的改裝。整車結(jié)構(gòu)參數(shù)在表2中已經(jīng)列出。試驗平臺軟件架構(gòu)如圖13所示，底層驅(qū)動程序采用LabVIEW編寫，通過NI控制器經(jīng)過CAN信號直接與試驗車輛進(jìn)行通信。上層程序基于ROS平臺建立軌跡規(guī)劃和路徑跟蹤節(jié)點，將車輛參數(shù)和環(huán)境信息傳入軌跡規(guī)劃節(jié)點規(guī)劃出軌跡，然后將軌跡輸入至路徑跟蹤節(jié)點輸出車輛控制信號。ROS平臺部署在上位機上通過UDP與Labview驅(qū)動程序通信。其中，路徑跟蹤模塊中橫向控制采用單點預(yù)瞄控制，縱向控制采用PID進(jìn)行控制。

圖13 試驗平臺軟件架構(gòu)

車輛的試驗過程如圖14所示，其中（a）為車輛的初始位置，（b）為車輛第一段軌跡結(jié)束的位置，（c）為車輛入庫的位置，（d）為車輛泊車結(jié)束的位置。試驗過程中實車軌跡與規(guī)劃軌跡如圖15所示，其中紅色為實車軌跡。從圖中可以看出試驗效果良好，在不同的起始位姿都能規(guī)劃出一條滿足車輛行駛的無碰撞路徑，并且最后都成功泊車。但是由于跟蹤誤差的存在，泊車結(jié)束時車輛的航向角還是存在一定的偏差。同時由于泊車的兩段軌跡是通過一次規(guī)劃出來的，所以在試驗中發(fā)現(xiàn)車輛在跟蹤第一段軌跡時的誤差會影響第二段軌跡的跟蹤。另外，在試驗過程中還出現(xiàn)了車輛無法準(zhǔn)確在軌跡切換點停車問題，從圖中可以看出車輛跟蹤第一段軌跡時每次停車點都超出第一段軌跡。

圖14 試驗過程

圖15 實車軌跡與規(guī)劃軌跡對比

5 結(jié)論

（1）針對自動駕駛車輛平行泊車軌跡規(guī)劃問題，本文中將軌跡規(guī)劃解耦成路徑規(guī)劃和速度規(guī)劃。規(guī)劃的路徑在保證平行泊車路徑的曲率連續(xù)性的同時利用圓弧曲線降低了泊車時對最小泊車位大小的要求，并且通過對泊車起始區(qū)域采樣和引入多目標(biāo)評價函數(shù)實現(xiàn)了車輛任意起始位姿的平行泊車路徑規(guī)劃。在路徑規(guī)劃的基礎(chǔ)上，通過對時間采樣，選取時間最短、滿足約束的S-t曲線生成平行泊車軌跡。

（2）設(shè)計3種不同的起始位姿對所提出的軌跡規(guī)劃方法進(jìn)行仿真驗證，結(jié)果表明：本文提出的方法在不同的起始位姿下都能求解出滿足汽車運動學(xué)約束、碰撞約束和計算實時性等要求的軌跡信息，具有較好的場景適應(yīng)能力。

（3）搭建實車試驗平臺進(jìn)行實車試驗，試驗結(jié)果表明：本文提出的方法能為自動駕駛車輛提供有效的平行泊車參考軌跡，指導(dǎo)自動駕駛車輛進(jìn)行自主平行泊車。