自動泊車系統規劃與控制方法研究

孫柯陽,王兆強,梁威,劉斌山,孫崇智

（1.上海工程技術大學機械與汽車工程學院,上海 201620；2.甘肅省特種設備檢驗檢測研究院,甘肅 蘭州 730050）

由于民用汽車擁有量的增長,在有限的生活空間內,司機在泊車過程中容易產生泊車事故,自動泊車系統是解決此類問題的一種有效方法[1],通過傳感器與控制算法技術的融合,自動泊車系統可以提升城市空間的利用效率,降低事故發生率及保障行人安全.

自動泊車系統主要有環境感知、決策規劃及控制執行[2-3]三大關鍵技術.現已有許多與自動泊車系統相關的研究,如：Vorobieva 等[4]分別基于單個和多個圓弧規劃不同的泊車路徑,且根據回旋線進行平滑處理,然而此方法規劃的泊車路徑起點與終點處的曲率不為零,會造成車輛原地轉向；Zips 等[5]將運動學微分方程離散化制定靜態優化問題,通過迭代求解,縮短了泊車路徑,但此方法由于忽略了路徑曲率的連續性,因此泊車過程車輛行駛不平順；錢立軍等[6]基于最短泊車時間,提出最優控制問題,且使用高斯偽譜法提升了求解效率,然而此方法缺乏對控制器的研究,因此跟蹤精度低.針對上述研究中泊車路徑曲率不連續與缺乏控制器跟蹤控制的問題,本文重點對泊車的路徑規劃、車輛跟蹤控制及控制器的優化進行研究.

1 動力學模型建立

車輛是由車身、底盤及發動機組成的復雜的非線性系統,需要繁雜的非線性微分方程來表示車輛數學模型,然而此方法會增加系統的計算量及降低實時性,因此本文在簡化車輛模型的基礎上,結合阿克曼轉向幾何建立了車輛動力學模型,如圖1 所示.

圖1 車輛動力學模型Fig.1 Vehicle dynamics model

圖1 中,R為質心處轉彎半徑,Vf、Vr、V分別為前后輪速度及質心速度,la和lb分別是前后軸到質心處的距離,αf、αr、β、δ、φ、θ 分別是前輪側偏角、后輪側偏角、質心側偏角、前輪等效轉角、橫擺角及航向角、Fyf和Fyr分別為前后輪所受側偏力.對圖中車輛模型進行受力分析可得

式（1）中：m是整車質量,I為轉動慣量.當輪胎側偏角很小時,輪胎側偏力與側偏角呈線性關系,可表示為Fy=Cα,其中C是側偏剛度,因此前輪等效轉角較小時,根據式（1）可得

對圖中速度關系進行分析可得

由于車輛行駛過程中,輪胎側偏角較小,因此可以求得前后輪側角αf與αr的大小

根據公式（2）（3）和（4）,可以得到關于Vy和的狀態方程

2 泊車路徑規劃

2.1 B-spline 曲線

B-spline 曲線具有易于控制、節點間連接平滑且可以進行局部修改曲線形狀的優點,因此本文采用B-spline 曲線用以規劃泊車曲線,n次B-spline 曲線的公式見式（6）

式（6）中：u∈[0,1]；n為樣條曲線的階數；Pi表示第i個控制點；Bi,n（u）表示第i個n次基函數,如式（7）所示

由于三階B-spline 曲線的二階導數連續,滿足泊車過程中,參考曲線曲率連續的要求,因此,公式（6）與公式（7）中n=3,可求得三階B-spline 曲線的表達式,如式（8）

其中P1、P2、P3與P4為曲線的控制點,根據公式（8）即可得出參考路徑的斜率與曲率公式,如式（9）（10）

2.2 曲線控制點的計算

在計算控制點的過程中考慮碰撞約束且采用逆向泊車法對操作過程進行研究,具體過程如圖2所示.

圖2 逆向泊車過程Fig.2 Reverse parking process

車輛從車位駛出總共分為三個階段：P1P3、P3P4 與P4P6.在P1P3 段,車輛行駛至虛線位置時,車輛外輪廓點c與車位拐點B有一定的安全距離Lsafe2=Lsafe1=200 mm,此時車身轉動β 度,可求得

根據β 的數值可以求得P1 與P3 之間坐標差值ΔP13 為

為避免右側車身與拐點B發生碰撞,需要回正方向盤,使車輛沿直線行駛至圖中點線位置,根據圖中關系可求得P3 與P4 之間坐標差值ΔP34 為

在P4P6 階段,可以求得P5 與P6 之間坐標差值ΔP56 為

由于參考曲線的曲率不能過大,因此取P3P4 的延長線與P1 點切線的交點為P2 點,同理可得P5點,P2 與P3 之間坐標差值ΔP23 為

P4 和P5 點之間的坐標差值ΔP45=ΔP23,因此確定P1 點坐標即可確定各個控制點的坐標.設P1=（x,y）,則各控制點的坐標計算如下

3 基于LQR 與FLA 的控制器的設計與優化

3.1 基于LQR 的控制器設計

自動駕駛車輛在行駛過程中,由于外部環境與自身因素的影響,車輛實際運動路徑與參考路徑之間存在一定的誤差,見圖3.

圖3 橫向誤差示意Fig.3 Schematic diagram of lateral error

令φ=eφ+θr,根據公式（17）可以得到以下公式

實際道路曲率變化較為平緩,因此可以忽略的計算,將公式（18）帶入公式（5）可得

公式（19）可以簡化為

分析公式（20）可知,控制器的目標為選取合適的控制輸入u,使得實際路徑與規劃路徑之間的誤差e盡量小,因此可以將此類問題轉化為求解線性二次型調節器（linear quadratic regulator,LQR）問題[7],即

式（21）中：X為誤差量err,Q=[q1,q2,q3,q4]和R=[r]分別為誤差與輸入對應的權重矩陣,由于參考路徑是由大量離散化的參考點所組成,需要將=AX+Bu離散化為Xk+1=AXk+Buk,然后進行求解代價函數的極小值,因此可得如下公式

式（22）中：ξ∈[t,t+dt],進一步利用歐拉法處理公式（22）并化簡可得

基于拉格朗日乘數法構造如式（24）所示函數,即可求得代價函數的極值

為了簡化式（24）,可構造Hamilton 方程[8],令可得

通過求解上述公式,最終可以得到一個Riccati 方程

進一步可求得控制量uk的表達式

式（27）可以簡寫為u=-KX,其中,系數和R已知.

分析公式（20）可知,K取任意值時,系統無法同時消除誤差及其變化量,因此,引入一個控制量δf,有,當e˙rr=0時,,因此可以選擇合適的控制量δf使得err=0,式err為

式（28）中：系數K1和K3由LQR 求出,當橫向誤差ed=0 時,對于橫擺角誤差eφ,根據圖1 進行分析,由于質心處轉彎半徑R遠大于后軸到質心的距離lb,因此,有eφ=-γ-αr,根據包含γ 角的虛線三角形的角度關系有β=γ+αr=-eφ,帶入eφ=φ-θr,可得θ=θr,車輛的航向角誤差總是為零.

3.2 基于FLA 的控制器優化

通過對LQR 控制器的設計過程進行研究,分析得出影響泊車系統性能的因素主要有泊車行駛速度及控制器的權重矩陣Q=[q1,q2,q3,q4]和R=[r],具體影響效果如圖4、圖5 所示.

圖4 不同速度下狀態矩陣Fig.4 State matrix at different speeds

圖5 不同系數下控制效果Fig.5 Control effects under different coefficient

圖4 為不同速度下狀態矩陣A的變化,對比圖4-a、b、c 和d 可以得出隨著速度的增大,狀態矩陣中的部分系數隨之減小,因此,在不同速度場景下泊車精度不同.圖5 為不同權重系數條件下車輛跟蹤五次多項式S 型曲線,車輛的跟蹤誤差與橫擺角誤差,對比圖5-a、b、c 和d 可以發現q1 與q4 作用下的控制器穩定且跟蹤誤差與橫擺角誤差較小,相反的,q2 與q3 對控制器的精度影響較小且會降低控制器的穩定性.對于權重系數R,當R=0.1 與R=10 時,控制器的跟蹤效果不佳,因此直接取R=1,即有較好的控制效果.

現階段可以通過不斷試驗調整權重矩陣以達到良好的跟蹤效果,然而此方法人工成本較高,隨著計算機技術的發展,有各類優化算法被廣泛地應用于權重系數的選取,如灰狼算法[9]、藍鯨算法[10]與神經網絡[11],而智能控制算法中的模糊邏輯算法（Fuzzy Logic Algorithm,FLA）是基于人的經驗進行設計的,因此,對于精度低,非線性與時變性的被控對象也有很好的控制效果,可通過模糊邏輯算法代替人工進行參數調整.模糊邏輯算法的設計過程主要有三步：模糊化、模糊推理與解模糊[12].以誤差e為輸入,控制因子r為輸出,最終通過q=10r計算得出最終的權重系數.首先,確定模糊詞匯為：負大（NB）、負?。∟S）、零（ZO）,正?。≒S）和正大（PB）,定義輸入量的論域為[-0.6,0.6],輸出量的論域分別為[-1,1]和[-1,0.5],得到隸屬度函數,見圖6.

圖6 各隸屬度函數Fig.6 Membership functions

為了后續的模糊推理,需要制定相應的模糊規則庫,研究圖5 中各系數作用下的控制規律可得到規則庫,見表1 和表2.

表1 red 的規則庫Tab.1 The rule base of red

表2 reφ的規則庫Tab.2 The rule base of reφ

由模糊推理得到的值為模糊值,需要相應的解模糊方法將模糊值轉化為實際值,本文使用加權平均法進行處理,得到控制器所需的實際值如圖7 所示.

圖7 輸出精確值Fig.7 The exact value of output

3.3 基于Frobenius 范數的更新方法

由于實際泊車場景的復雜性,泊車過程中車輛在不同場景下行駛速度不同,且模糊邏輯算法計算過程是實時的,需要相應的更新方法以降低計算量,提升對不同速度的泊車場景的適應度,因此,本文將基于Frobenius 范數的相似度計算引入優化過程中,通過對比范數的接近程度,可以確定兩個矩陣之間的差異.

以狀態矩陣A為例,矩陣A為當前速度下的狀態矩陣,A1 為另一速度下的狀態矩陣,則有差異矩陣D=A-A1,則D的Frobenius 范數為,兩個狀態矩陣的相似度為

式（29）中：差異矩陣的范數值越小,狀態矩陣A和A1 之間的相似度越高,同理,權重矩陣Q的更新方法亦是如此,更新方法制定完成后需選擇相應的更新閾值,當相似度小于閾值,則更新權重矩陣與狀態矩陣,反之,則不更新矩陣,考慮到計算量與精度的要求,本文設置的更新閾值為0.8.

4 仿真與分析

為了驗證本文規劃的參考路徑與控制器的有效性,使用MATLAB/Simulink 與Carsim 進行聯合仿真測試,仿真采用的車輛參數為軟件內部車輛模型,具體參數如表3 所示.

表3 車輛參數Tab.3 Vehicle parameters

選取車位尺寸為[7.5 m,2.5 m],計算得出以后軸中心為參考點的參考路徑與路徑曲率,如圖8 所示.

圖8 參考路徑Fig.8 The reference path

從圖8 可以看出規劃的泊車路徑滿足車輛避障約束且路徑曲率連續,保證了泊車過程中方向盤轉動連續且平順.由于仿真軟件以前軸中心為參考點,因此取控制點P1 的坐標為（2.91,0）,則各控制點的坐標見表4.

表4 控制點坐標Tab.4 Control point coordinates

引入更新機制后,在設計的程序中加入tic 函數,得到更新前后程序運算的時間,如圖9 所示.

圖9 程序運算時間Fig.9 Program operation time

由圖9 可知,引入更新機制前,從整體上來看,程序運算的時間在2.5 ms 上下波動,加入更新機制后,程序運算時間約為0.2 ms,程序運行效率提升了92%,因此,在降低人工調整參數成本的同時,控制器的實時性有所提升.

設定速度為1 m/s,對優化后控制器的性能進行仿真驗證,具體仿真結果如圖10 所示.

圖10 仿真結果Fig.10 Simulation results

結合圖10-a,可以發現跟蹤誤差最大約為0.15 m,在泊車起始階段與即將進入車位的階段,車輛產生的跟蹤誤差較大,在最終駛入車位后,車輛與預定位置相差約0.13 m.分析泊車過程中產生較大跟蹤誤差的原因是設計模糊邏輯算法的過程中,模糊詞匯數量選取較少,導致控制器的跟蹤精度下降.對于橫擺角誤差,從圖10-b 中可以看出車輛橫擺角誤差最大約為0.08 rad,角度誤差較小,車輛停止時與車位的夾角約為0.03 rad,近似與車位平行.圖10-c 為車輛的前輪轉角,從圖中可以看出車輛的前輪轉角變化連續且較為平順,然而在最后階段前輪轉角出現小范圍的波動,分析后可知在泊車最后階段車輛調整車身角度的時候出現誤差.

圖11 為Carsim 中展示的動態泊車過程,圖中可以看出,雖然行駛過程中出現跟蹤誤差,然而并未與障礙車輛出現碰撞,可以完成安全無碰撞的泊入車位的要求.

圖11 路徑跟蹤圖Fig.11 Path tracking

圖12 為優化前后,在不同速度條件下控制器的跟蹤結果,對比圖12-a 與b 中的跟蹤誤差與橫擺角誤差,得出優化后的跟蹤誤差降低了約30%,橫擺角誤差降低幅度較小,表明優化后的控制器提升了在不同速度的泊車場景的適應度.

圖12 控制器適應度仿真結果Fig.12 Simulation results of controller fitness

5 小結

首先,針對兩段圓弧式參考曲線曲率不連續的問題,使用B-spline 曲線計算出一條曲率連續的路徑,仿真結果表明規劃路徑曲率連續,滿足泊車需求；其次,結合車輛動力學模型與LQR 理論設計出線性二次型控制器,利用模糊邏輯算法優化控制器,解決了人工調整參數耗時耗力的問題；最后,引入相似度的理論,提升控制系統的實時性與不同速度條件下的適應度,使用MATLAB 與Carsim 進行聯合仿真測試,仿真結果表明,優化后控制器的運算速率由改進前的2.5 ms 變為改進后的0.2 ms,運算效率提升了92%,同時改進后的控制器在不同速度下的跟蹤誤差相較于改進前的降低了約30%,因此改進后的控制器的適應度提升了30%.