基于駕駛員經驗的自動泊車規劃算法研究*

胡 文，譚運生，康龍云，于宗光

（1.華南理工大學電力學院，廣州 510640； 2.領途汽車有限公司智能駕駛開發部，無錫 214000；3.中國電子科技集團公司第五十八研究所，無錫 214000）

前言

近年來，隨著汽車保有量的增加，停車空間越來越小，對駕駛員的操作要求也越來越高，對于經驗不足的駕駛員來說，很難輕松地將車泊入擁擠的停車位中。基于這些因素，自動泊車技術得以發展，很多國內外學者開始對其進行研究。

目前，在自動泊車技術領域，國內外學者的研究主要集中在兩個方面：（1）路徑規劃和路徑跟蹤，首先借助傳感器獲取泊車可行駛區域，并結合車輛的幾何參數，預先規劃出一條理想的泊車軌跡，然后設計各種控制器跟蹤該泊車軌跡［1-3］；（2）利用先進的控制算法模擬駕駛員操作，分析駕駛員在泊車時用到的經驗知識，采用模糊控制、神經網絡等先進控制算法［4-6］，進行模擬建模以實現自動泊車。但是，這些算法過于復雜［3-8］，滿足不了系統的實時性要求，不利于實際應用。其中，雖然有些算法簡單易行，但容易出現曲率不連續問題［9-10］。此外，部分算法［11-13］解決了曲率不連續的問題，但在路徑跟蹤過程中，需要反饋，計算量較大，對系統的實時性要求高。本文中在考慮算法實時性要求的基礎上，借鑒熟練駕駛員的泊車經驗，提出了一種簡單易行的自動泊車規劃算法。

1 問題描述與泊車分析

1.1 問題描述

泊車路徑曲率不連續會導致汽車停車原地轉向，容易造成輪胎磨損問題。而導致路徑曲率不連續問題的原因，大多是在路徑規劃過程中沒有考慮轉向盤轉向的連續性。本文中在解決該問題時，借鑒熟練駕駛員的泊車經驗，對經驗駕駛員的泊車過程進行機理分析，并在分析成果的基礎上，對該過程進行建模。

1.2 泊車分析

在平行車位的泊車入庫過程中，熟練駕駛員的泊車過程可分為6段，即S0、S1、S2、S3、S4、S5，如圖1所示，其中，T0、T1、T2、T3、T4為各段之間的連接點。S0段：將車輛行駛至泊車起始點T0，其路徑可為直線或曲線，視工況而定；S1段：將車速控制在最小穩定車速vmin，并將轉向盤以最大角速度勻速向右打滿；S2段：轉向盤停留在最右端的位置，提升車速至最大泊車車速vpmax（以減小泊車時間），車輛以最小轉彎半徑做圓周運動，直至到達一定位置（即點T2）；S3段：降低車速至最小穩定車速vmin，并以最大角速度反方向勻速打滿轉向盤；S4段：轉向盤停留在最左端的位置，提升車速至最大泊車車速vpmax，車輛以最小轉彎半徑做圓周運動，直至方向角回零；S5段：轉向盤快速回正，車輛以最小穩定車速vmin前向、后向行駛，同時適當操作轉向盤，將車輛調整至合適的位置與方位。由于本文研究的重點在于泊車入庫過程，S0準備階段和S5回正調整階段將不對其進行研究。

圖1 泊車過程

本文中在參考上述泊車過程的基礎上，建立如圖2所示的自動泊車控制系統框圖。其中，環境信息為環境感知提供的車位信息、可行駛區域信息、車輛位置信息等；泊車控制器根據感知環節提供的環境信息進行自動泊車控制，輸出期望轉向盤轉向角θd和期望車速vd；轉向控制器為車輛線控轉向控制器，可有效控制轉向盤達到目標轉向角；速度控制器為車輛速度控制器，通過控制加速和減速使車速穩定在目標車速附近。由于在泊車過程中車速不超過5 km／h，車輛速度較低，可以通過車輛自身的控制器實現目標控制，滿足期望的轉向盤轉向角和車輛速度要求。因此，本文重點在于設計泊車控制器，輸入環境信息，輸出期望轉向盤轉向角和期望車速，對速度控制器和轉向控制器將不做進一步研究。

圖2 自動泊車控制系統框圖

2 控制器建模

如圖3所示，去掉S0準備階段和S5回正結束階段的泊車過程。其中，LP和WP分別為泊車位長寬；R1和R2分別為S4和S2曲線半徑；θ為S4對應的圓周角，也是T3點對應的車輛方向角；ψT為O2T1與縱軸的夾角；Lv和Wv分別為車輛的長寬；L和W分別為車輛的軸距和輪距；Lf和Lr分別為車輛的前懸長和后懸長；Rmin為車輛后軸中點的最小轉彎半徑。車輛在T0點時的泊車起始位置由后保險杠與泊車位沿x方向的距離D1和車輛右平面與泊車位沿y方向的距離D2確定（見圖3）。

由于S2和S4段為轉向盤打滿后的轉向過程，車輛的轉彎半徑為最小轉彎半徑。最小轉彎半徑為：（1）在滿足泊車車位最小的情況下，S4段需滿足最小轉彎半徑［14］；（2）在R1取最小值的情況下，R2取值較大時，將使泊車可行區域（D2取值范圍）變小，而在Rmin值附近的泊車可行區域最大；（3）從控制難度和精度考慮，R2取最小值（即轉向盤打滿）的控制難度較低、控制精度較高。

圖3 泊車路徑規劃

S1段為轉向盤回正到右打滿的過程，S3段為轉向盤右打滿到左打滿的過程，在實際操作過程中可將其看成轉向盤勻速轉向過程，轉向速度為最大角速度。由于S2和S4段轉向盤打滿不動，車輛行駛軌跡固定，與車速無關。因此，可將S2和S4段的泊車車速提高，以降低泊車時間；而S1和S3段轉向盤需要轉向，泊車軌跡受車速和轉向盤轉向影響，為控制泊車精度，可將S1和S3段的泊車車速降低至最小穩定車速。

2.1 轉向控制

2.1.1 S1段建模

S1段為轉向盤從回正到打滿的過程，對S1段的路徑進行分析，如圖4所示。其中，車輛后軸中心點的起始位置為圖示坐標的原點T0，車輛起始方向角為零，并與T0X軸重合；S1為車輛行駛軌跡；T1為軌跡終點；T1Tx為軌跡終點的切線；ψS為T0T1與T0X的夾角；ψT為T1Tx與T0X的夾角，也是車輛方向角。

圖4 S1段路徑軌跡

由車輛運動學關系可得

式中：ψv為車輛橫擺角；δf為等效前輪轉向角；v為車速；ω為等效前輪轉動角速度，可由轉向盤角速度ωs和傳動比i獲取，即ω＝iωs，而與相對應的轉向盤轉角ηd＝i·δf。

由式（1）可得

式中：K1＝-log（cos（ωt1））＝-log（cos（δfmax）），K1取值不隨時間改變；t1為轉向結束時間。

由于泊車過程的速度較低、轉向盤轉速較快，可在1 s內完成轉向盤打滿過程，此時的ψv取值較小（在0.05～0.25之間）。因此，可將泊車軌跡S1簡化成直線T0T1，sinψv可簡化成ψv，同時，泊車軌跡S1可通過車速v乘以時間t1計算，由此可得

由于K1和K2只與δfmax有關，為降低實際應用中的計算量，提升實時性，可根據δfmax預先計算K1和K2的值。

2.1.2 S3段建模

圖5 S3段路徑軌跡

S3段路徑軌跡如圖5所示。其中，OX為水平軸，O點為轉向盤回正時的坐標點；Ox軸為轉向盤回正時的車輛方向，也是S3曲線過O點的切線；T2為軌跡起點；T3為軌跡終點；ψv為車輛在O點時的方向角；ψS為T2T3與Ox的夾角；ψT為過T2和T3點的切線T3Tx與Ox的夾角；φ為過T2或T3點的切線與水平方向的夾角，也是車輛在T2和T3點的方向角；ψaxis為T2T3與Ox的夾角。

由于S3段為轉向盤從右打滿到左打滿的過程，OT2曲線與OT3曲線關于O點點對稱，而S3段的前半段可視為S1段的反過程，也即S3曲線的前半段OT2曲線實際上與S1曲線呈鏡面對稱，因此S3弧長等于S1弧長的兩倍。與S1曲線同理，可將S3曲線簡化成T2T3直線。同時，基于上述簡化條件，可假定ψT和ψS與S1曲線中的值相等，即可由式（2）和式（5）獲得。其它需滿足的幾何關系如下：

2.1.3 S2段和S4段建模

由前面的敘述和分析可知，S2段曲線為以Rmin為半徑的圓弧，如圖3所示。因此本節重點須要確定的是本段中車輛的圓周運動應持續的時間，即求車輛到達本段終點T2的時機或位置，為此，取S2弧線在點T2的半徑線與縱坐標的夾角θ為待求的變量。

由幾何關系可得

結合式（6）～式（13），可得

在車輛的速度v和轉向盤角速度ωs固定的情況，式（13）中的S1、ψT和ψS為固定值，在D2已知的情況下，可由式（14）求得θ值。借助Mathematica軟件的推導可得

由于ψT和ψS數值較小，式（15）中的sinψS≈ψS，sin（ψT-ψS）≈ψT-ψS、cos（ψT-ψS）≈1，式（15）可進一步簡化為

至于S4段，它與S2段類似，不再存在待求的未知量，在此無須贅述。

2.1.4 路徑約束

現對車輛在泊車過程中可能發生的碰撞情況進行分析，如圖6所示，為保證泊車過程中的安全，設定一些安全距離：Df，Dr，Di，Do和Drb。

（1）根據汽車車身頂點D的碰撞分析，可確定泊車位的寬度WP。如圖6所示，在S4段的D點圓弧半徑為

圖6 泊車路徑約束示意圖

車位寬度WP為

式中WPmin為最小車位寬度。

（2）根據汽車車身頂點C的碰撞分析，可確定泊車位的長度LP。如圖6所示，在S4段的C點圓弧半徑為

車位長度LP為

式中LPmin為最小車位長度。

（3）根據汽車車身頂點B與泊車位另一側障礙物的碰撞分析，可確定泊車空間所需的寬度H。由于S1段的側向位移較小，為方便計算，對B點的運動軌跡進行簡化，如圖6所示，簡化后的B點圓弧半徑為

泊車空間寬度H為

式中Hmin為泊車空間寬度。

（4）車輛在S2段的泊車過程中，可能會與泊車位頂點B′發生碰撞，最可能的碰撞點為車身E點，E點為后軸延長線與車身側邊的交點。如圖6所示，只有在O2點的坐標x2＞LP時，才會發生碰撞，因此可得以下幾何關系：

式中：DEB為E點至B′點的安全距離；K4為圓心O1與O2的距離。結合圖7中的幾何關系可得

圖7 圓心O1與O2距離示意圖

2.1.5 泊車位置

在泊車信息已知的情況下，可根據上節中的路徑約束條件，計算得到泊車起始位置D1和D2的范圍。

由于在泊車轉向過程中的車速v和轉向角速度ωs事前設定，S1、ψT和ψS可提前求得，而R1和R2采用最小半徑Rmin。因此，可由式（22）求得D2的最大值：

由式（23）～式（26）求得D2的最小值：

其中，上式中的O1點坐標（x1，y1）可由圖6所示的幾何關系得到

而當x2＜LP時，車輛側向CD邊應與車位邊A′B′保持DEB的安全距離，即

結合式（28）和式（31）可得D2的最小值：

在D2已知的情況下，可以根據式（23）和式（24）求得D1的情況：

此外，后軸的最小轉彎半徑Rmin和前輪等效轉角δfmax可由車輛幾何關系求得

對于熟練駕駛員來說，在泊車之前會將車停在熟悉的泊車區域，即由D1和D2確定的區域。而大多數駕駛員的側向區域D2的范圍為0.4～1.0 m，這是由于側向距離D2較小可以縮短停車時間，同時，也方便給另一側留有更多的可行空間，便于他人行走。為此，在實際應用中，當泊車系統自動尋找車位時，可將車停在靠近車位的位置，即D2的可行區域為0.5～1.0 m范圍內。

2.2 速度控制

參考熟練駕駛員的駕駛經驗，在車輛轉向盤轉動的過程中，將車速控制在最小穩定車速vmin；在轉向盤穩定過程中，將車速提升至最高泊車車速vmax，一般在5 km／h左右。為此可得如圖8所示的車速控制過程。

圖8 車速控制過程

2.3 控制流程

上文對泊車控制的轉向控制和速度控制進行了分析，現將泊車控制器的整個控制流程進行歸納。其中，由于車輛的幾何參數、最小穩定車速vmin、轉向盤最大穩定轉速已知且安全距離預先設定，由此可計算得到S1、ψT、ψS、K1、K2、K4和K5等信息，泊車控制器可預存這些控制信息，便于減少重復計算量。

泊車前的參考計算過程如下：

（1）首先將車輛停在D2可行區域內，同時，感知系統進行泊車空間探測；

（2）判斷泊車空間是否滿足LPmin和WPmin，不滿足則提示信息并退出泊車過程；

（3）判斷泊車位置是否滿足D2min和D2max，不滿足則提示信息并退出泊車過程；

（4）根據D2計算θ、D1和T2的橫坐標xT2。

3 仿真結果

為驗證泊車算法的可行性，本文中以天津一汽夏利N5車型為例，利用Matlab／Simnlink建立本文提出的規劃算法，并結合泊車過程中的極限和正常工況，對算法進行驗證，參數如表1所示。選擇車位尺寸LP×WP為7 m×2.4 m，道路寬度H為4 m，車輛的最低穩定車速vmin為2km／h，最大泊車車速vpmax為5 km／h。不同車輛的最低穩定車速和最大泊車車速有所不同，可根據實際情況標定獲得，這里僅作參考。根據車輛相對車位的不同側向距離D2，選擇3種泊車情況進行分析。

表1 乘用車與泊車有關的主要參數 m

為顯性驗證控制器滿足碰撞約束條件的有效性，現將各安全距離均設為0，進行研究分析。如圖9所示，在泊車側向距離D2取最小值時，在泊車過程中，車輛側向與車位之間的最小間隙DEB為0.003 m，車輛后側與車位之間的最小間隙Dr為-0.003 m。造成誤差的原因在于為提高實時性，對算法進行了部分簡化處理。同時，如圖10所示，在泊車側向距離D2取最大值時，在泊車過程中，車輛側向與障礙物之間的最小間隙Drb幾乎為0。由此可見，本文中設計的泊車控制器誤差非常小，能對車輛進行精確無誤的控制。

圖9 D2為D min的仿真結果

圖10 D2為D max的仿真結果

此外，為結合實際泊車過程中的情況，將算法中的安全距離Df、Dr、Di、Do、Drb和DEB設為0.2 m，將Dr設為0.4 m，確保泊車安全并再次進行仿真分析。如圖11所示，本文中設計的泊車控制器能使車輛精確、無碰撞地泊在設計的泊車終點區域內，且能獲得比較理想的泊車效果。同時，如圖12所示，轉向盤在轉向過程中，車速控制在2 km／h；轉向盤打滿后，車速迅速提升至5 km／h；整個泊車過程時間不足7 s。由此可見，本文中設計的控制器與熟練駕駛員的駕駛經驗一致，在對車速進行變速控制的同時，既滿足了精度需求，又有效控制了泊車時間。

圖11 D2為0.5 m的仿真結果

圖12 D2為0.5 m時的轉向盤轉角和車速變化過程

為進一步研究，現將本文中所提算法與其他泊車算法（B算法）進行對比。B算法主要通過控制車輛橫擺角從0°-45°-0°，從而實現平行車位的自動泊車，如圖13所示。為方便對比，泊車終止點和各安全距離與圖11的仿真條件保持一致。

圖13 D2為D min（B算法）的仿真結果

從圖13和圖11可以看出，在D2取Dmin時，其泊車軌跡離上邊沿只有0.1 m，并不滿足0.2 m的要求，這說明該算法對泊車空間有更高的要求，降低其實用性。同時，在泊車終止點相同的情況下，該算法所需最小側向距離Dmin為2.155 m，而本文中所提算法最小側向距離Dmin＝DEB，即只需要0.2 m，這說明本文中所提算法對泊車空間要求較小，更容易在狹窄的環境中實現泊車。此外，從圖14可以看出，其泊車時間較長，完成泊車需要8.1 s，大于本文的算法6.8 s，若采取提高泊車速度來縮短泊車時間，這將進一步提升對泊車空間的要求，導致泊車空間不足。

圖14 D2為D min（B算法）的轉向盤轉角和車速變化過程

綜合以上的仿真結果可以看出，本文中所提算法在不犧牲精度的情況下，減少了運算量，進而減輕對泊車系統的性能要求，實用性更強。同時在泊車過程中，對速度進行控制，進一步減少了對泊車空間的要求。

4 結論

（1）為解決泊車過程中曲率非連續問題，避免原地轉向，本文中借鑒熟練駕駛員的駕駛經驗，對其過程進行分析，由此提出了一種基于駕駛員經驗的自動泊車規劃算法。該算法主要基于駕駛員經驗對泊車路徑進行規劃研究，并對泊車過程中各個階段和可能發生的碰撞情況進行建模分析，并由此推導出泊車區域的要求。

（2）利用Matlab／Simulink搭建所提出的規劃算法，并結合泊車過程中的極限和正常工況，對算法進行驗證。仿真結果表明：本文中提出的算法能實現精確的自動泊車控制；算法的部分簡化對控制器控制精度影響較小，簡化結果行之有效；控制器中的速度控制，既能滿足精度要求，又能達到泊車時間要求，與預期設計思路一致。

（3）本文中解決的是一般工況下的自動泊車問題，對于一些特殊工況下的自動泊車問題未必適用，比如車輛前方有障礙物致使泊車可行區域D2不滿足要求。因此，為應對各工況下的自動泊車問題，后續研究中將在優化本文算法的同時，著手解決特殊工況下的自動泊車問題。