基于兩段圓弧的倒推式平行泊車路徑規(guī)劃方法

張 持 張永林

（江蘇科技大學(xué)電子信息學(xué)院 鎮(zhèn)江 212003）

1 引言

隨著世界各國(guó)汽車技術(shù)的飛速發(fā)展，汽車的家庭使用量急劇增加。車輛的增多，導(dǎo)致最直接的結(jié)果就是停車位越來越緊張，想要泊車穩(wěn)準(zhǔn)快難度比較大，對(duì)駕駛員也是一種考驗(yàn)［1］。泊車所引發(fā)的事故占交通事故的比例越來越大，調(diào)查顯示我國(guó)保險(xiǎn)公司接到交通事故理賠的申請(qǐng)中，有不少于35%是由泊車失誤造成的［2］。那么，在汽車業(yè)強(qiáng)調(diào)電動(dòng)化、自動(dòng)化以及互聯(lián)化的潮流下，為了緩解城市街道的壓力，自動(dòng)泊車系統(tǒng)（Automatic Parking Sys?tem，APS）的研究勢(shì)在必行，這給出了研究自動(dòng)泊車的第一個(gè)研究動(dòng)機(jī)。

自動(dòng)泊車系統(tǒng)主要有傳感探測(cè)機(jī)構(gòu)、決策生成機(jī)構(gòu)和命令執(zhí)行機(jī)構(gòu)［3］。其中決策生成機(jī)構(gòu)作為泊車控制器的組成核心，其生成的自動(dòng)泊車路徑對(duì)泊車成功具有重要意義。路徑規(guī)劃就是解決了車輛泊車過程中按照怎樣路徑引導(dǎo)這個(gè)問題［4］。路徑規(guī)劃方法主要有以下兩種：第一種是基于駕駛員經(jīng)驗(yàn)而研發(fā)出的模糊控制動(dòng)態(tài)規(guī)劃方法。如文獻(xiàn)［5～6］提出了根據(jù)駕駛員的主觀駕駛經(jīng)驗(yàn)提取出控制規(guī)則，該規(guī)則能有效減小泊車過程中的控制誤差。但是該規(guī)則對(duì)泊車區(qū)域規(guī)劃不明確，且約束條件不易考慮周全，很難量化，因而不具有實(shí)用性。另一種方式就是通過對(duì)路徑進(jìn)行幾何圖形的規(guī)劃，生成固定參考路徑的靜態(tài)規(guī)劃方法。這種幾何路徑的規(guī)劃方式能夠根據(jù)車輛與車位之間的相對(duì)位置準(zhǔn)確規(guī)劃出無碰撞且充分考慮約束條件的泊車最佳路徑，易于精確控制。REEDS等［7］采用了圓弧與直線相結(jié)合的泊車路徑，但其方法路徑曲率不連續(xù)，在曲率轉(zhuǎn)折處需停車進(jìn)行轉(zhuǎn)向，這會(huì)造成很大的路徑跟蹤誤差。PAROMTCHIK等［8］提出了回歸算法，該算法采用正弦曲線將起始點(diǎn)和泊車終止點(diǎn)連接起來，若起止點(diǎn)變化了，正弦曲線也要隨之改變，就需要重新計(jì)算一次，計(jì)算過程繁瑣。文獻(xiàn)［9～10］采用B樣條曲線進(jìn)行路徑規(guī)劃。文獻(xiàn)［11～12］利用了五次多項(xiàng)式進(jìn)行泊車路徑規(guī)劃，但是對(duì)泊車約束條件沒有給出明確的規(guī)定，泊車時(shí)需要較大泊車空間，實(shí)際中很難實(shí)現(xiàn)。文獻(xiàn)［13～14］對(duì)兩段曲率連續(xù)的圓弧平行車位路徑規(guī)劃展開了研究，將泊車路徑看成是兩段相切的圓弧組成的序列，但是對(duì)組成兩段相切圓弧的轉(zhuǎn)彎半徑?jīng)]有給出建設(shè)性的方法。這給出了本文第二個(gè)研究動(dòng)機(jī)。

本文創(chuàng)新性的提出了倒推式的計(jì)算策略，對(duì)兩段圓弧的轉(zhuǎn)彎半徑進(jìn)行研究。該策略的工作原理為：首先根據(jù)對(duì)泊車環(huán)境的推斷，確定第二段圓弧的轉(zhuǎn)彎半徑為最小轉(zhuǎn)彎半徑Rmin。然后人為提出最大轉(zhuǎn)彎半徑Rmax的概念，對(duì)第一段圓弧的轉(zhuǎn)彎半徑在最大值與最小值之間利用二分法進(jìn)行尋值。從而求出兩個(gè)相切圓弧曲線的轉(zhuǎn)彎半徑，并在碰撞約束條件下確定泊車路徑。該方法簡(jiǎn)化了路徑規(guī)劃過程的計(jì)算，能實(shí)現(xiàn)泊車路徑曲率連續(xù)的要求，并針對(duì)不同的泊車起始點(diǎn)均能有效的完成泊車并滿足泊車約束條件。

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

在模型分析之前，先對(duì)文中所用符號(hào)進(jìn)行說明，如表1所示。

表1 符號(hào)說明

如圖1所示，在大地坐標(biāo)系下進(jìn)行車輛模型分析。其中車輛后軸中心點(diǎn)坐標(biāo)記為（x，y），車輛四個(gè)頂點(diǎn)A、B、C、D的坐標(biāo)記為（xA，yA）、（xB，yB）、（xC，yC）、（xD，yD）。

圖1 車輛運(yùn)動(dòng)學(xué)大地坐標(biāo)系

由于車輛在泊車過程中速度較低，所以車輛后軸側(cè)滑可忽略，此時(shí)的運(yùn)動(dòng)系統(tǒng)是一個(gè)非完整約束系統(tǒng)，約束方程可表示為［15］

通過對(duì)式（1）推導(dǎo)得出下式的運(yùn)動(dòng)學(xué)方程：

此外，將車身前方兩個(gè)頂點(diǎn)的運(yùn)動(dòng)軌跡方程表示為

3 平行泊車路徑規(guī)劃

3.1 倒推式平行泊車路徑規(guī)劃

如圖2所示，以車位左下角為坐標(biāo)原點(diǎn)，以車輛后軸中心點(diǎn)作為參考點(diǎn)，建立泊車運(yùn)動(dòng)路徑直角坐標(biāo)系。車輛從P0行駛到泊車起始點(diǎn)P1的過程中，車身周圍的傳感器模塊探測(cè)右側(cè)的車位情況，并計(jì)算出車位大小深度。結(jié)合泊車起始點(diǎn)P1點(diǎn)所在位置，控制器根據(jù)車位情況計(jì)算確定泊車的終止點(diǎn)P2的位置，從而根據(jù)車位環(huán)境確定了起始點(diǎn)和終止點(diǎn)，開始泊車路徑的規(guī)劃。

圖2 兩段圓弧曲線泊車軌跡示意圖

圖2中，將兩段曲率連續(xù)的相切圓弧作為泊車路徑，泊車路徑從泊車起始點(diǎn)P1過渡到終止點(diǎn)P2，且過程中保證車輛不會(huì)碰到車位邊界，兩段弧半徑大于等于車輛最小轉(zhuǎn)彎半徑。泊車起始點(diǎn)P1記為（x，y），從泊車起始點(diǎn)起，第一段圓弧記為圓弧1，其圓心坐標(biāo)為O1（x1，y1），第二段圓弧記為圓弧2，其圓心坐標(biāo)為O2（x2，y2），泊車路徑終止點(diǎn)P1（x3，y3），停車區(qū)域左前方拐點(diǎn)為P（Px，Py），E為兩段圓弧的相切點(diǎn)即兩圓心連線與泊車路徑的交點(diǎn)，記泊車區(qū)域上邊界的縱坐標(biāo)為y=yup。

在進(jìn)行路徑規(guī)劃前，首先需對(duì)泊車路徑的約束條件進(jìn)行論述：

1）圓弧2的圓心O2到車輛右前頭的距離需小于O2到拐點(diǎn)P的距離：

2）拐點(diǎn)P到圓弧1圓心O1的距離需小于圓弧1圓心O1到車輛右后方的距離：

3）圓心O1到車輛左前方的距離需小于圓心O1到上邊界的距離。

4）車輛自身也存在約束條件，由于車輛存在最小轉(zhuǎn)彎半徑Rmin，當(dāng)車輛處于最小轉(zhuǎn)彎半徑時(shí)，曲率最大。所以后軸中心點(diǎn)軌跡曲線的曲率K需滿足K≤1 Rmin。同時(shí)前輪轉(zhuǎn)向角速度應(yīng)有 φ?≤φ?max。

基于倒推的方式就是首先確定第二段圓弧的半徑為最小轉(zhuǎn)彎半徑，再根據(jù)其推論出第一段圓弧的半徑大小。根據(jù)以上約束條件，對(duì)泊車路徑中兩段圓弧的半徑分別進(jìn)行分析判斷。

1）圓弧2的圓心及半徑大小確定

在起始點(diǎn)和終止點(diǎn)位置確定且設(shè)定合理的前提情況下，有以下推斷。

在終點(diǎn)位置確定的條件下，第二段圓弧的半徑越小，車輛右前方車頭距離停車位的拐點(diǎn)P則越遠(yuǎn)，發(fā)生碰撞的可能性越??；在起始點(diǎn)位置確定的條件下，第一段圓弧半徑越大，車輛左前方車頭與泊車區(qū)域上邊界碰撞的可能性越低，且與P點(diǎn)的距離越遠(yuǎn)。因此，取車輛最小轉(zhuǎn)彎半徑作為圓弧2的半徑。從而可確定圓弧2的圓心坐標(biāo)O2（x3，y3+Rmin）。

2）圓弧1的圓心及半徑大小的確定

車輛只有最小轉(zhuǎn)彎半徑而不存在最大轉(zhuǎn)彎半徑。為了便于數(shù)值的計(jì)算，在平行泊車的前提下人為設(shè)定最大轉(zhuǎn)彎半徑以達(dá)到確定圓弧1半徑的目的。

根據(jù)初始點(diǎn)和終止點(diǎn)的相關(guān)信息來求解最大轉(zhuǎn)彎半徑：

所以，通過求解最大轉(zhuǎn)彎半徑，此處圓弧1半徑存在最大和最小轉(zhuǎn)彎半徑。

圖3 圓弧1半徑求解分析圖

如圖3所示，E為圓弧1圓心和圓弧2圓心連線與泊車路徑的交點(diǎn)。理論上，若圓弧1與圓弧2相切，有O1E=R1，所以令此處的R1值即為所求的半徑。此處，對(duì)圓弧1的半徑R1，有 Rmin≤R1≤Rmax，因此，只需找到介于最大和最小半徑之間的合理R值即可。

令li為介于最大和最小半徑之間的值，有Rmin≤li≤Rmax，如果當(dāng)li=R1時(shí)，此li值即為所求的圓弧1半徑，取l1和l2，令其取值范圍為 l1≤Rmax，l2≥Rmin。

3.2 二分法查找介于兩者之間的最終解

當(dāng)圓弧1的半徑為最小轉(zhuǎn)彎半徑時(shí)，圓弧1的圓心坐標(biāo)為Omin（x，y-Rmin）。假設(shè)此時(shí)使圓弧Omin與圓弧2相切的半徑為l2，如式（9），比較l2與Rmin的大小。

并求解直線O2Omin與X軸所成的夾角ωmin，如式（10），此處的夾角為銳角。

同理，當(dāng)圓弧1的半徑為最大轉(zhuǎn)彎半徑時(shí)，可求得相關(guān)的l1和ωmax。

若 l2＞Rmin，l1＜Rmax，則計(jì)算 ωmax和 ωmin的中間值，有：

求解過圓弧2圓心且斜率為-tanω的直線與以(x，y)為起始點(diǎn)且沿y軸負(fù)方向的射線的交點(diǎn)到圓心O2構(gòu)成線段的長(zhǎng)度。假設(shè)交點(diǎn)為Om，則lm=O2Om-Rmin，有：

比較lm與OmP1的大小。若lm＞OmP1，則以線段OmP1的長(zhǎng)度替代最小轉(zhuǎn)彎半徑；若lm＜OmP1，則以線段OmP1的長(zhǎng)度替代最大轉(zhuǎn)彎半徑。

迭代上述過程直到lm=OmP1，此時(shí)，所求解得的半徑R1使得圓弧1與圓弧2相切。

3.3 碰撞判斷

為實(shí)現(xiàn)泊車路徑的避障，需進(jìn)行碰撞判斷。在起始點(diǎn)和終止點(diǎn)時(shí)，車輛均平行于X軸，一般存在如圖4所示碰撞點(diǎn)①和②的碰撞情況。

圖4 碰撞點(diǎn)示意圖

1）碰撞判斷1

在進(jìn)行圓弧1的泊車時(shí)，車輛左前方可能會(huì)與上邊界①發(fā)生碰撞。如圖5所示，圓心點(diǎn)O1到車輛左前方A的距離為d1，圓心O1到上邊界的垂直距離為 yup-y1，比較兩者大小。距離d1可表示為

上圖中，θ為兩段圓弧所對(duì)的圓心角，α0為線段O1A與圓心O1到上邊界的垂直線段之間形成的夾角。以此兩角作為判斷條件：

圖5 上邊界碰撞判斷

（1）若θ＞α0，說明車輛在泊車過程中d1所在線段會(huì)存在與X軸垂直的情況。比較 yup-y1和d1值的大小。

（2）若θ＜α0，說明車輛在泊車過程中d1所在線段不會(huì)存在與X軸垂直的情況，即與X軸正方向夾角為銳角。比較 yup-y1和 d1?cos(α0-θ)值大小。

2）碰撞判斷2

如圖6所示，當(dāng)車輛泊車到圓弧2時(shí)，車輛右前方B可能會(huì)與拐點(diǎn)P碰撞。給出如下判斷條件：

圖6 車位左前方碰撞判斷

將此時(shí)判斷條件由點(diǎn)到點(diǎn)的距離比較轉(zhuǎn)換為投影到X軸上水平線段大小的比較，即將O2P與O2B的比較轉(zhuǎn)換為線段O2P投影在X軸上的水平距離與當(dāng)車輛右頂點(diǎn)行駛到線段O2P所在的直線上時(shí)線段O2B投影在X軸上的水平距離的比較。

其中，線段O2B的長(zhǎng)度d2可以表示為

4 平行泊車路徑規(guī)劃數(shù)值分析

本節(jié)通過多個(gè)案例驗(yàn)證所提出的倒推式兩段圓弧自動(dòng)泊車路徑規(guī)劃方案在各種泊車工況下的可行性。

4.1 倒推式泊車路徑規(guī)劃的仿真

一般的家用轎車中型車的長(zhǎng)度在4.3m～4.7m，寬度在1.7m～1.8m；中大型轎車長(zhǎng)度在4.6m～4.9m，寬度在1.7m～1.9m之間。通常情況，平行車位的長(zhǎng)度應(yīng)是車身長(zhǎng)度的1.4倍～1.6倍，車位的長(zhǎng)度一般在6m～7m，車位寬度一般在2m～2.4m之間［16］。

根據(jù)上述平行車位尺寸的標(biāo)準(zhǔn)與要求，為了模擬真實(shí)車位環(huán)境，將選取的車位尺寸和車輛長(zhǎng)寬尺寸列如表2。

表2 車位和車輛尺寸

仿真結(jié)果如下，圖7為在車位6.8m×2m，上邊界縱坐標(biāo)為6.5m的車身環(huán)境中，車輛大小為4.7m×1.8m的不同泊車起始點(diǎn)泊車軌跡。

圖7 后軸中心點(diǎn)泊車軌跡和車輛運(yùn)行軌跡

圖8為在車位6.8m×2m，上邊界縱坐標(biāo)為7m的車身環(huán)境中，車輛大小為4.5m×1.8m的不同泊車起始點(diǎn)泊車軌跡。

圖8 后軸中心點(diǎn)泊車軌跡和車輛運(yùn)行軌跡

圖9為在車位7m×2m，上邊界縱坐標(biāo)為7.5m的車身環(huán)境中，車輛大小為4.5m×1.8m的不同起始點(diǎn)泊車運(yùn)行軌跡。

圖9 后軸中心點(diǎn)泊車軌跡和車輛運(yùn)行軌跡

通過比較圖7和圖8，可總結(jié)出在車位相同的情況下，不同大小的車都能夠?qū)崿F(xiàn)泊車入位。比較圖8和圖9，當(dāng)車位變大時(shí)，車輛停入車位的后方距離會(huì)隨之增大，但對(duì)泊車效果沒有影響。

實(shí)驗(yàn)仿真證明，此路徑規(guī)劃方法可實(shí)現(xiàn)不同車位、不同大小車輛和不同起始點(diǎn)位置的泊車運(yùn)動(dòng)，能應(yīng)對(duì)實(shí)際泊車中遇到的各種車位情況，為自動(dòng)泊車提供可靠可行的泊車路徑。

4.2 路徑規(guī)劃的擬合仿真結(jié)果

在倒車過程中，車輛運(yùn)行軌跡呈現(xiàn)階躍式變化。當(dāng)車輛行駛到兩段圓弧相切點(diǎn)時(shí)，轉(zhuǎn)向角值會(huì)有一個(gè)較大的跳變，所行駛的路徑也會(huì)有波動(dòng)，對(duì)泊車效果會(huì)有影響。所以需對(duì)兩段圓弧路徑進(jìn)行擬合，擬合出一條連續(xù)曲線。

利用反正切函數(shù)對(duì)泊車路徑進(jìn)行擬合，擬合的結(jié)果如圖10所示，擬合曲線能夠有效地貼近兩段圓弧泊車路徑。

圖10 泊車路徑擬合

反正切函數(shù)形式如下：

式中的a、b、c、d為可改變曲線形狀的參數(shù)，修改這些參數(shù)可使曲線接近所要的兩段圓弧泊車軌跡。將坐標(biāo)點(diǎn)（8.5，3.8）作為起始點(diǎn)，適當(dāng)?shù)倪x取轉(zhuǎn)彎半徑的R1以及R2的大小，擬合得反正切函數(shù)為

5 結(jié)語

本文研究了自動(dòng)泊車路徑規(guī)劃問題。首先在大地坐標(biāo)系下結(jié)合車輛在低速行駛狀態(tài)下的特性，建立了車輛運(yùn)動(dòng)學(xué)模型。然后依據(jù)所建運(yùn)動(dòng)模型提出了一種基于兩段圓弧的倒推式平行泊車路徑規(guī)劃方案，該方案采用二分法尋找最佳半徑并給出了車輛碰撞判斷條件。最后通過多個(gè)案例驗(yàn)證了所提方案可實(shí)現(xiàn)不同泊車起始點(diǎn)的路徑規(guī)劃，并對(duì)兩段圓弧泊車路徑使用反正切函數(shù)進(jìn)行擬合，得出一段曲率連續(xù)的泊車路徑，有效加強(qiáng)了泊車的連貫性。