張 持 張永林

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

1 引言

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

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

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

2 泊車運動學(xué)模型

在模型分析之前，先對文中所用符號進行說明，如表1所示。

表1 符號說明

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

圖1 車輛運動學(xué)大地坐標(biāo)系

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

通過對式（1）推導(dǎo)得出下式的運動學(xué)方程：

此外，將車身前方兩個頂點的運動軌跡方程表示為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 碰撞判斷

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

圖4 碰撞點示意圖

1）碰撞判斷1

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

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

圖5 上邊界碰撞判斷

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

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

2）碰撞判斷2

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

圖6 車位左前方碰撞判斷

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

其中，線段O2B的長度d2可以表示為

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

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

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

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

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

表2 車位和車輛尺寸

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

圖7 后軸中心點泊車軌跡和車輛運行軌跡

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

圖8 后軸中心點泊車軌跡和車輛運行軌跡

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

圖9 后軸中心點泊車軌跡和車輛運行軌跡

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

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

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

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

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

圖10 泊車路徑擬合

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

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

5 結(jié)語

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