但孝杰，鄭 迎

一種基于直線圓弧路徑的全自動泊車控制方法

但孝杰，鄭 迎

（華域汽車系統(tǒng)股份有限公司，上海 200041）

為了實現(xiàn)車輛在全自動泊車過程中，能夠安全穩(wěn)定的沿著給定的直線圓弧路徑行駛，文章結(jié)合經(jīng)典比例-積分-微分（PID）控制思想，提出了一種依據(jù)提前轉(zhuǎn)向距離和目標角速度進行方向盤角度控制的方法，再基于當前車輛后軸中心點位置及航向信息與目標路徑點之間的偏差進行修正的控制方法。同時根據(jù)規(guī)劃的泊車路徑特點，提出了一種基于勻減速度來反推速度的規(guī)劃方法。實驗結(jié)果表明，該方法能夠平穩(wěn)控制車輛泊車，并且控制誤差滿足精度要求。

全自動泊車；路徑跟蹤；速度規(guī)劃；直線圓弧路徑

全自動泊車系統(tǒng)可以有效避免在人為泊車過程中的安全事故，也是未來自動駕駛最后一公里的關(guān)鍵技術(shù)。全自動泊車系統(tǒng)主要技術(shù)包括車位掃描、泊車路徑規(guī)劃及車輛控制。

車輛控制的目的是使車輛能夠平穩(wěn)地沿著泊車路徑行駛，并最終停在指定的目標點。文獻[1]使用傳統(tǒng)比例-積分-微分（Proportional Integral Derivative, PID）控制器控制算法，將車輛當前的路徑跟蹤偏差作為輸入量，對跟蹤偏差進行PID控制得到轉(zhuǎn)向控制量。文獻[2]使用了一種純追蹤控制算法，基于當前車輛后軸中心位置，在規(guī)劃路徑上向前一定距離選擇預瞄點。假設車輛后軸中心可以按照一定轉(zhuǎn)彎半徑行駛到該預瞄點，通過車輛的運動學公式計算前輪轉(zhuǎn)角。文獻[3]提出了一種基于非時間參考的無模型自適應路徑跟蹤轉(zhuǎn)型控制策略，通過建立車輛非時間運動模型，來擺脫車輛加減速帶來的時域約束。隨著自動駕駛技術(shù)的發(fā)展，根據(jù)車輛動力學模型和最優(yōu)控制理論，業(yè)界提出了線性二次型控制器（Linear Quadratic Regulator, LQR），該方法使用車輛的狀態(tài)來使控制誤差最小化[4]。此外，模型預測控制（Model Predictive Control, MPC）方法也不斷地應用在車輛控制上，雖然能得到很好的控制效果，但需要有一定算力的控制器[5]。

考慮使用的嵌入式控制器算力有限，只生成一條由直線圓弧構(gòu)成的路徑。以上常用控制方法均需要光滑的路徑，曲率突變會引起轉(zhuǎn)向不平順，甚至發(fā)散。因此，本文基于全自動泊車低速行駛的特點，以及直線圓弧構(gòu)成路徑的特征，提出一種先提前轉(zhuǎn)向后自修正的控制方法，即在經(jīng)過直線圓弧銜接點前一段距離，開始提前打方向盤至下一段軌跡的目標角度。當車輛行駛過銜接點后，以此目標方向盤角度為基準進行修正，從而提高路徑跟蹤的智能性和魯棒性。此外，根據(jù)泊車路徑在換擋點和終點處速度必為0的特點，通過每段軌跡長度反推計算目標車速，從而保證車輛平穩(wěn)行駛。

1 車輛運動模型

1.1 泊車過程運動模型

圖1 車輛運動學模型示意圖

車輛泊車運動過程為低速運動過程，忽略車輪的側(cè)向滑動，同時為方便計算，將四輪車簡化為虛擬的兩輪車運動模型，如圖1所示。

圖1中，為車輛車身與坐標系軸的夾角，即為車輛的航向角；為等效前輪的阿克曼轉(zhuǎn)角；為車輛軸距；為車輛的轉(zhuǎn)彎半徑；()為車輛后軸中心點坐標。由此可得車輛運動學公式為

1.2 泊車過程誤差模型

泊車車輛的運動包括平面移動和平面旋轉(zhuǎn)，因此，可以利用車輛局部坐標系和大地坐標系來描述車輛在某一時刻的位置和方向，如圖2所示。

圖2 泊車過程誤差模型示意圖

圖2中，為車輛的后軸中心點；為規(guī)劃的直線圓弧路徑；點為規(guī)劃路徑中與點距離最近的點；定義路徑跟蹤的位置偏差為點到點的距離；和d分別為車輛縱軸線和規(guī)劃路徑在點的切線與軸的夾角。定義路徑跟蹤的航向偏差為-d，假設車輛向后行駛，定義點到路徑末點的長度為縱向距離偏差。

2 車輛控制器設計

2.1 控制器功能概述

圖3為控制器總體框圖，輸入當前車輛位姿、速度、方向盤角度以及規(guī)劃路徑信息，其中路徑信息包括直線圓弧段端點的坐標和航向、圓弧半徑大小、直線圓弧段的長度以及規(guī)劃的總段數(shù)等。目標轉(zhuǎn)角控制模塊計算目標方向盤角度，為了控制方向盤轉(zhuǎn)速，同時防止電動助力轉(zhuǎn)向系統(tǒng)（Electric Power Steering, EPS）因當前與目標角度差過大而失效，增加EPS轉(zhuǎn)角控制模塊，以一定的轉(zhuǎn)向步長控制EPS轉(zhuǎn)向。速度規(guī)劃模塊根據(jù)當前所處路徑位置輸出目標控制車速，將當前目標方向盤角度和目標車速通過控制器局域網(wǎng)絡（Controller Area Network, CAN）總線發(fā)送給汽車底盤執(zhí)行器，控制車輛行駛。

圖3 控制器總體框圖

2.2 目標轉(zhuǎn)角控制模塊設計

泊車規(guī)劃的軌跡由直線圓弧段構(gòu)成，實現(xiàn)較好跟隨軌跡，需要在銜接點處停車并原地調(diào)整轉(zhuǎn)角姿態(tài)，這不符合正常泊車駕駛習慣且乘坐體驗很差。因此，本文先提前一段距離打方向盤至下一段路徑的目標角度。

當下段路徑為直線，則輸出目標方向盤轉(zhuǎn)角為0°；當下段路徑為圓弧時，通過圓弧半徑大小以及車輛運動學公式，計算得到目標阿克曼轉(zhuǎn)角，通過車輛標定，得到相應阿克曼轉(zhuǎn)角對應的目標方向盤轉(zhuǎn)角。計算公式為

式中，為下一段圓弧路徑的半徑大??；為車輛軸距；為阿克曼轉(zhuǎn)角。

(一)工資、薪金所得，是指個人因任職或者受雇而取得的工資、薪金、獎金、年終加薪、勞動分紅、津貼、補貼以及與任職或者受雇有關(guān)的其他所得。

考慮EPS的遲滯響應時間、轉(zhuǎn)角響應時間及車速等因素，計算提前轉(zhuǎn)向距離。當規(guī)劃的某段路徑為最小半徑圓弧時，車輛以最大方向盤角度轉(zhuǎn)向，由于車輛的不足轉(zhuǎn)向特性，車輛行駛過某弧長閾值后，跟蹤誤差會超過容許范圍。因方向盤已打到極限，無法通過自修正來減小誤差，因此，本文在提前轉(zhuǎn)向距離計算公式中加入額外距離計算項，在控制誤差范圍內(nèi)，適當增加提前轉(zhuǎn)向距離來抵消因不足轉(zhuǎn)向產(chǎn)生的誤差。提前轉(zhuǎn)向距離為

式中，為當前車速；sr為EPS遲滯響應時間，由EPS實車測試得到；σe為下一路徑段期望方向盤轉(zhuǎn)角；σc為當前方向盤轉(zhuǎn)角；為方向盤角速度，由當前車速和EPS控制步長計算標定得到；為選擇參數(shù)，當下段路徑為最小半徑圓弧且弧長超過閾值sh時，置為1，否則為0；為規(guī)劃的最小半徑圓弧弧長；為標定參數(shù)。

當車輛后軸中心點行駛過路徑銜接點后，需要相應的控制策略保證車輛行駛在軌跡誤差范圍內(nèi)。PID控制是最早發(fā)展起來的控制策略之一，由于其算法簡單、魯棒性好和可靠性高，被廣泛應用于工業(yè)過程控制[5]。傳統(tǒng)的離散型位置式PID為

式中，()為控制器輸出；()當前周期誤差；(-1)為上一周期誤差；()為歷史累計誤差；p為比例項系數(shù)；i為積分項系數(shù)；d為微分項系數(shù)。

傳統(tǒng)PID控制只針對一個誤差量進行控制，無法較好地同時保證泊車過程中的位置和航向誤差。其次，誤差數(shù)量級遠小于控制量，容易造成PID參數(shù)過大且不易整定。因此，本文在PID控制思想基礎上，提出基于車輛當前位置和航向偏差，在目標方向盤轉(zhuǎn)角附近進行修正的控制方法，計算公式為

=e+p×pos+h×head（5）

式中，t為輸出方向盤轉(zhuǎn)角；e為當前路徑段的期望方向盤轉(zhuǎn)角；pos為位置偏差項，控制車輛靠近參考路徑行駛；head為航向偏差，控制車輛航向角與參考路徑控制點航向角一致，同時避免系統(tǒng)因超調(diào)而造成車輛蛇形行駛；p為位置偏差項系數(shù)；h為航向偏差項系數(shù)；max為最大方向盤轉(zhuǎn)角。為了進一步保證方向盤控制的穩(wěn)定，當位置航向誤差小于設定閾值時，不進行控制，直接輸出上一周期方向盤控制轉(zhuǎn)角。

2.3 EPS轉(zhuǎn)角控制模塊設計

為了控制方向盤轉(zhuǎn)速度，同時防止EPS因當前與目標角度差過大而失效，增加EPS轉(zhuǎn)角控制模塊，即以一定的轉(zhuǎn)向步長控制EPS轉(zhuǎn)向。當前周期輸出目標方向盤轉(zhuǎn)角為

2.4 速度規(guī)劃模塊設計

依據(jù)泊車路徑[6-8]在換擋點和終點處速度必為0的特點，可將泊車行駛視為勻減速度的過程，通過每段軌跡長度反推計算目標速度。為了保證較精確地停在目標點，必須考慮車輛底盤控制系統(tǒng)延遲響應時間和控制響應時間，提前一段距離開始制動。提前制動距離為

式中，f為提前制動距離；r為控制系統(tǒng)延遲響應時間，通過車輛測試得到；d為當前開始制動車速；d為發(fā)送給底盤的目標控制減速度。計算目標車速為

式中，t為目標車速；p為標定加速度；e為縱向距離誤差，即車輛規(guī)劃路徑上的控制目標點到換擋點或終點的距離；max為泊車最大車速；min為泊車最小車速。

泊車過程中，目標車速通過CAN總線發(fā)送給汽車底盤，控制車輛行駛。同時在每個控制周期中計算判斷e與f大小，當e≤f時，發(fā)送目標減速度，控制車輛制動。

3 實車測試及分析

為驗證本控制器在真實泊車環(huán)境中的有效性和可靠性，開展了實車測試。采用上汽榮威MARVELX為測試平臺，車上搭載公司自研的6個高分辨率多模態(tài)毫米波雷達和4個攝像頭感知獲取車位信息。泊車測試場景包括平行泊車和垂直泊車，其中平行車位大小為車長+1.5 m，垂直車位大小為車寬+1.2 m。實驗時泊車最高車速設為 3 km/h，最低車速設為1 km/h。

圖4為車輛在平行泊車和垂直泊車時的方向盤轉(zhuǎn)角控制圖。由圖4可見，車輛當前方向盤轉(zhuǎn)角能夠很好跟隨目標方向盤度。

圖5為車輛在平行泊車和垂直泊車時的位置偏差示意圖。平行泊車過程中位置偏差始終控制在5 cm范圍內(nèi)，泊車完成時距離終點的位置偏差在4 cm左右。垂直泊車過程中，由于第一步泊車最小轉(zhuǎn)向半徑圓弧路徑的存在，方向盤無法通過自修正來減小誤差，目前通過標定提前轉(zhuǎn)向距離，使最大位置偏差不超過10 cm，泊車完成時距離終點的位置偏差在2 cm左右。

圖5 位置偏差圖

圖6為車輛在平行泊車和垂直泊車時的航向角偏差示意圖。平行泊車和垂直泊車過程中，航向偏差始終控制在3°范圍內(nèi)，泊車完成時距離終點的航向偏差在1°左右。

圖6 航向角偏差圖

圖7為車輛在平行泊車和垂直泊車時的速度控制圖。車輛起步時，系統(tǒng)延遲響應時間在1 s左右；車輛剎車制動時，系統(tǒng)延遲響應時間在0.5 s左右。在平行泊車和垂直泊車過程中，速度偏差始終控制在0.1 m/s范圍內(nèi)。

4 結(jié)束語

本文針對全自動泊車規(guī)劃直線圓弧路徑曲率不連續(xù)特點提出了一種先提前轉(zhuǎn)向后自修正的轉(zhuǎn)向控制方法，同時根據(jù)換擋點和終點處速度必為0的特點，提出通過每段路徑長度反推目標車速的速度規(guī)劃方法，并通過實車測試對控制器的可靠性和有效性進行了驗證。測試結(jié)果表明，上述控制方法可保證控制誤差能快速地收斂到合理范圍內(nèi)，保證車輛能夠有效準確實現(xiàn)路徑跟蹤和平穩(wěn)行駛，具有一定的工程實用價值。

[1] 鄭平平.無人駕駛拖拉機路徑跟蹤與轉(zhuǎn)向控制研究[D].合肥:合肥工業(yè)大學,2020.

[2] 王雪犇.自動駕駛車輛垂直車位自動泊車控制算法研究[D].長春:吉林大學,2021.

[3] 張持.基于超聲波的自動泊車控制策略研究[D].鎮(zhèn)江:江蘇科技大學,2018.

[4] 陶冰冰,周海鷹,王思山.自動駕駛車輛LQR軌跡跟蹤控制器設計[J].湖北汽車工業(yè)學院學報,2017,31 (4):1-6.

[5] 黃穎濤.自動駕駛車輛軌跡跟蹤模型預測控制方法研究[D].西安:長安大學,2019.

[6] 趙林峰,徐磊,陳無畏.基于自抗擾控制的自動泊車路徑跟蹤[J].中國機械工程,2017,28(8):966-973.

[7] 徐磊.基于EPS的自動泊車路徑規(guī)劃及跟蹤控制研究[D].合肥:合肥工業(yè)大學,2017.

[8] 張剛.基于模糊控制汽車自動泊車關(guān)鍵技術(shù)設計[J].微型電腦應用,2019,35(6):123-125.

An Automatic Parking Control Method Based on Straight and Arc Path

DAN Xiaojie, ZHENG Ying

( Huayu Automotive Systems Company Limited, Shanghai 200041, China )

In order to realize safely and stably control the vehicle driving along the given straight and arc path in the process of automatic parking, combined with the classical proportion integration differentiation (PID) idea, a new control method is proposed, which reach the target steering wheel angle according to the advance steering distance and target angular speed, and then based on the current vehicle rear axle center position and heading information and the deviation between the target path point correction control method in this paper. Meanwhile according to the characteristics of the planned path, this paper also proposes a speed planning method, which is based on the uniform deceleration. The experiment result shows that the method can control the parking smoothly, and the control error meets the accuracy requirements.

Automatic parking; Path tracking; Speed planning; Straight and arc path

U495

A

1671-7988(2023)03-49-05

10.16638/j.cnki.1671-7988.2023.03.009

但孝杰（1995—），男，助理工程師，研究方向為自動駕駛技術(shù)，E-mail:xiaojied@hasco-group.com。

上海市2020年度“科技創(chuàng)新行動計劃”高新技術(shù)領域項目（20511104400）。