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

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

前言

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

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

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

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

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

表1 自動(dòng)泊車路徑規(guī)劃方法對(duì)比

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

1 車輛運(yùn)動(dòng)學(xué)模型

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

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

圖1 汽車運(yùn)動(dòng)學(xué)模型

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

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

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

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

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

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

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

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

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

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

圖2 平行泊車場(chǎng)景

2.1 泊車路徑設(shè)計(jì)

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

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

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

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

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

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

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

圖4 第二段泊車路徑

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

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

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

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

2.2 最佳泊車起始點(diǎn)的選取

2.2.1 起始泊車點(diǎn)的采樣

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

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

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

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

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

2.2.2 路徑約束分析

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

圖7 路徑碰撞

2.2.3 多目標(biāo)評(píng)價(jià)函數(shù)設(shè)計(jì)

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

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

式中：s為第i個(gè)采樣點(diǎn)的第一段路徑曲線弧長(zhǎng)；s為第i個(gè)采樣點(diǎn)的第二段路徑曲線的弧長(zhǎng)。

從而，第i個(gè)采樣點(diǎn)泊車路徑曲線的弧長(zhǎng)為

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

結(jié)合上述分析，對(duì)于一個(gè)生成路徑曲線滿足路徑約束的采樣點(diǎn)，可以采用以下評(píng)價(jià)函數(shù)計(jì)算其代價(jià)值：

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

比較各采樣點(diǎn)生成的泊車路徑曲線代價(jià)值，代價(jià)值最低的采樣點(diǎn)即為最佳泊車起始點(diǎn)。

2.3 速度規(guī)劃

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

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

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

圖8 速度規(guī)劃

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

3 仿真分析

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)車試驗(yàn)

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

圖13 試驗(yàn)平臺(tái)軟件架構(gòu)

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

圖14 試驗(yàn)過(guò)程

圖15 實(shí)車軌跡與規(guī)劃軌跡對(duì)比

5 結(jié)論

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

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

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