基于彈性繩理論的自主車輛防碰撞的路徑規劃*

曹昊天，宋曉琳，黃 江

(湖南大學，汽車車身先進設計與制造國家重點實驗室，長沙 410082)

前言

目前研發的車輛主動避障系統有碰撞預警系統(collision warning system, CWS)、自適應巡航控制系統(adaptive cruise control system, ACC)和車輛停走系統(stop & go)[1]等。歐洲EuroFOT提供的研究數據顯示，裝有ACC系統和前碰撞預警系統(FCW)等先進駕駛員輔助系統(ADAS)的車輛，能減少交通事故的發生概率達5.7%[2]。根據2012年中國統計年鑒交通事故的數據，我國2011年由于交通事故而死亡的人數達到六萬余人[3]，盡管與前幾年比較死亡人數呈下降趨勢，但總體而言，這個傷亡數目仍然巨大。而車輛主動避障系統可以輔助駕駛員對潛有危險的交通場景規劃出安全的避障路徑，從而可減少甚至避免交通事故的發生，因此車輛主動避障系統的研究具有理論意義和實用價值。

文獻[4]中提出的人工勢場理論和彈性繩理論最早運用于機器人領域，文獻[5]和文獻[6]中加以改進運用到自主車輛路徑規劃領域，并成功實現了在低風險路況下動態車輛的避障路徑規劃算法。為擴大障礙車勢場的作用范圍，作者曾對影響避障路徑生成的幾個因素，即障礙車引導勢場的形狀、引導勢場因子、彈性繩剛度和主車速度進行了探討，結果表明彈性繩剛度和主車車速的影響最大，然后是引導勢場因子，而引導勢場形狀的影響最小[7]。本文中在先前研究和文獻[6]算法的基礎上，分別在障礙車的前后增加一個引導勢場，研究引入引導勢場后對主車避障路徑生成的改善情況。通過在直道和彎道兩種不同道路環境下，結合Carsim車輛動力學模型進行仿真分析。結果表明，添加引導勢場后，對于直線道路上行駛的車輛作用最為明顯，車輛能夠提前避障，從而給駕駛員更為寬裕的操作時間，并且避障過程中在縱向上與障礙車輛保持充分的安全距離，從而使避障過程更安全。

1 道路模型

道路環境的信息由車載雷達、GPS和車載攝像頭獲得。在路徑決策的初始階段，位于道路上的慣性坐標系的原點選取在道路中心線上與車輛質心縱向對應的位置，如圖1所示。道路中心線由一回旋線構造，其曲率變化率為一常數，即其曲率與弧長成正比。假定一條總弧長為lD的回旋線線段，其起始曲率為c0，則自起點計弧長為l處一點的曲率為c(l)=c0+c1l。由于回旋線的精確表達須計算Fresnel積分，因此更常用的方法是采用多項式逼近來表達，即道路中心線的橫向位移可表示為

(1)

式中：x為道路坐標系下的縱向位移；y0為道路中心線初始的橫向偏移；τ0為初始方向角；c1為曲率變化率或曲率常數，c1=(cf-c0)/lD，cf為終點曲率。

道路中心線上的點在道路坐標系中可表示為

Rc=xex+ycey

(2)

式中：ex、ey為道路坐標系的縱向和橫向單位向量。

根據圖1，結合邊界條件yc(0)=0和yc′(0)=0可將y0和τ0消掉。為計算簡便，引入建立在道路中心線上的自然坐標系，其切向和法向單位向量為

(3)

因此對于雙車道，道路邊界上的點可表示為

(4)

式中：B為道路總寬度；下標q∈[l,r]，表示左右邊界。

此外考慮到構成道路中心線的回旋線方向角較小，基于小曲率的假設下，整個道路上任意點的位置R近似表示為

R=Rc+(y-yc)en

(5)

2 彈性繩模型

根據文獻[5]中提出的彈性繩模型，假定車輛的緊急避障路徑是由一根虛擬的彈性繩模擬，它由N個節點的線性彈簧串聯在一起，如圖2所示。節點的位置向量在道路坐標系中可表示為

ri=xiex+yiey

(6)

路徑規劃開始時，確定好起始點和目標點，這樣彈性繩的首尾處的節點坐標是已知的。假設彈簧的拉伸滿足胡克定律，那么彈性繩節點處的彈性勢能可由與相鄰兩個節點之間的拉伸長度給出，即

(7)

式中：i=1,2,3,…,N-1,N為節點個數；ki為第i段彈簧的剛度；li為第i段彈簧的自然長度。li定義為歐式距離，即

3 勢場模型

3.1 道路邊界勢場

道路邊界勢場的建立是為確保車輛在道路上正常行駛而不至于出界，因此勢場模型的選擇是關鍵之一。根據人工勢場模型在機器人領域運用的經驗，選取如式(8)所示的勢場，很顯然，當車輛接近于道路邊界，它的勢能值趨向于無窮大，以達到限制車輛運動的目的。

(8)

式中ηq為道路邊界剛度值。

3.2 障礙車勢場

本文中采用的人工勢場法考慮的是車輛在R2空間中的路徑規劃，且障礙車采用凸多邊形表示，具體而言，采用矩形來描述障礙車的幾何結構。構建障礙車勢場的目的是防止它與其他車輛發生碰撞，并保持一定的安全距離。因此在設計障礙車勢場函數時，應使主車越接近該障礙車時，障礙車的勢能值越大，即

(9)

式中：kobst為障礙車勢場的剛度值；r0(m)為第m個障礙車輛的質心坐標；d為安全圓直徑。所謂安全圓就是以障礙車輛的質心為圓心包含整輛車在內的一個圓，以保證主車不會與障礙車發生接觸，見圖3。

3.3 引導勢場

為使主車能夠在運動中盡力避免與障礙車接近，考慮在障礙車前后加入引導勢場，并且將勢場的影響范圍“放大”，從而使主車能夠提前做出避障措施。如圖3所示，在障礙車前后加入半圓形的引導勢場，對于后引導勢場，選取其勢場表達式為

(10)

式中：kgr為車輛勢場剛度值；τ為引導勢場半徑；xr為障礙車后方邊界的中心位置的橫坐標。引導勢場的形式與障礙車勢場的表達式相近，但是R′中多出了一個放大因子ξ，通過在以障礙車后方邊界中點為原點的坐標系中進行縮放，擴大引導勢場在道路縱向上的影響范圍(側向上保持不變)。經“放大”后的縱向坐標值在道路坐標系中用x*′表示，在相應的道路坐標系中表示成x′。此外以左邊界中點為原點的坐標系中的X方向的數值表示成x*，該坐標系的單位向量與道路坐標系中單位向量的轉換關系為

(11)

(12)

式中：ψ0為障礙車的方向角，如圖3所示。后方放大因子ξr與主車和障礙車的速度差v-vm有直接關系，將βr稱作后方引導勢場因子。當障礙車在直線道上行駛，ψ0=π。式(11)和式(12)可簡化為

(13)

同樣，對于前方引導勢場也有類似的設定，ξf稱作前方放大因子，βf稱作前方引導勢場因子。總而言之，通過引導勢場的“放大”作用使原障礙車勢場的影響范圍更大，原本離障礙車較遠的點，經過坐標變化后離障礙車更近了，從而促使車輛在離障礙車更遠的地方就開始采取避障措施。β的取值決定了引導勢場的影響程度，β越大，所能影響的距離也就越遠。

3.4 道路風險圖

對于M個障礙車，將所有的障礙車勢場和前后引導勢場疊加起來得到最終的障礙車勢場，如式(14)所示。外界勢場為道路邊界勢場和障礙車勢場之和，如式(15)所示。

(14)

Uext=Uborder+Uobst

(15)

Uext的大小體現了道路風險的情況。由于障礙車的位置和狀態隨時間而變化，所以Uext大小也隨著時間而變化。圖4為初始時刻在引入引導勢場前后外界勢場圖的對比。

4 平衡方程的建立和求解

4.1 平衡方程的建立

由向量微積分的相關知識可知，勢場作用于每一個節點產生的虛擬力可由對勢場進行梯度運算得到，即

(16)

Fobj,i= -iUobst,i=

(17)

(18)

根據彈性繩理論的假設，作用于每一個節點的合力應處于平衡狀態，由此可得到方程式為

(19)

對于N個彈性繩節點，由于首尾節點均已固定，坐標已知，將式(19)分解到橫向和縱向兩個方向，從而產生2(N-2)個方程組成的聯立方程組，因此節點位置的求解需解此非線性方程組。為使計算更加簡便，將節點的x方向位移進行約束，因為在車輛進行車道保持、換道或緊急避障時，節點的縱向位移并非必要，因此可以將其進行約束[6]，即

ri·ex=xi=i·Δx,i=1,2,…,N

式中：Δx為節點之間在縱向上的間隔。這樣，式(19)可以簡化為

(20)

4.2 方程組的數值解法

本文中采用基本Newton-Raphson算法，它是多維求根方法中最為簡單的一種，編程簡易，但該算法對方程表達式的要求較高，求解的表達式必須連續可微，并且其1階導數是連續的。該方法雖然不具有全局收斂性質，但是對于足夠好的初始值仍然有著明顯的收斂效果。

Newton-Raphson法是一種通過表達式的泰勒級數的1階近似，在方程根可能存在的區間上進行迭代尋根的方法。一般而言方程可以用下式來描述：

F(z)=0,dimz=N

Fitotal=Fiext+Fiinc

選取一組初始值為

F(z)≈F(z(k))+J(z(k))(z-z(k))

(21)

式中：J為F的雅克比矩陣；k為迭代序號。將式(21)中F(z)=0的解記為z(k+1)，因此有

z(k+1)=z(k)-J-1(z(k))F(z(k))

(22)

直到所得到的搜索步長Δz(k)=z(k+1)-z(k)在預期的誤差范圍ε內停止迭代。此外，由于道路的危險勢場隨著時間的變化而變化，因此路徑規劃須以一定的時間間隔進行更新。這樣上一次規劃的路徑可作為下一次路徑規劃的初始值，通過Newton-Raphson算法來進一步修正得到某規劃時刻規劃的路徑值。表1給出Newton-Raphson算法的偽代碼。

表1 Newton-Raphson迭代法偽代碼

4.3 初始解的產生

用Newton-Raphson算法解方程組時須獲得一組初始解，即須得到一組初始路徑。為此本文中采取一種簡單的幾何方法來獲得。如圖5所示，獲取探測到障礙車邊界與離它較遠的車道邊界的中點，然后將起始點P0和這些中點與目標點PN進行線性插值得到對應的插值函數，然后彈性繩各節點的取值便依照該插值函數來取點。這樣就得到一組可用來迭代求解的初始值。

5 仿真

本文中模擬的障礙車設定為一長為4m，寬為2m的矩形，此外主車通過車載激光雷達來獲得與前方障礙車的間隔距離。其最大探測距離可達150m，取決于激光功率和人眼安全規定；最小0.5m，取決于脈沖長度。在防碰撞預警系統中，考慮到自車和前車的絕對速度和相對速度、路況、兩車完全停止時的實際車間距離等因素，一般要求傳感器的探測距離不小于130m，分辨率為1m，激光雷達滿足該要求[8]。但在智能車的行車環境中，雷達的測量值可能來源于目標車輛，也可能來源于其他干擾物；此外由于雷達自身工作的不穩定和目標回波能量的分布不均，還可能出現虛假目標，同時隨著車輛行駛中的隨機顛簸和擺動，雷達測量信號可能出現短暫丟失，從而導致目標物信息的較大波動[9]。因此，須對雷達測量信號進行處理，以準確選取有效目標。為了簡便，假定主車可準確地捕捉到前方運動障礙車，且在仿真場景的設定中主車與障礙車有足夠的安全距離，方便主車進行避障操作。為驗證改進算法的有效性，分別就以下兩種情形進行仿真分析。

5.1 直線道路

在單向雙車道的直線高速公路上，車道寬度為3.5m，主車的初始位置為右車道中心線上，障礙車起始位置同樣在右車道中心線離主車前方60m處。主車速度為25m/s，障礙車速度為15m/s，均勻速行駛，如圖6所示。主車探測到前方出現的障礙車開始采取避障措施，繞過障礙后繼續前進。利用Mathematica軟件編寫算法程序得到避障路徑，然后結合Carsim動力學軟件將路徑導入進行仿真，如圖7所示，觀察車輛在避障過程中的穩定性。仿真參數設置如表2所示。

參數數值描述ηl，ηr/(N/m)30道路左右邊界勢場剛度b/m35單車道寬度βf，βr020前后引導勢場影響因子N166彈性繩節點數l/m250路徑規劃長度l0/m12彈性繩自然長度k/(N/m)350000彈性繩剛度kobst/(N/m)1000障礙車勢場剛度kr，kf/(N/m)1000前后引導勢場剛度ε/m0001Newton?Raphson法求解精度W/m2障礙車橫向寬度L/m4障礙車縱向長度

避障路徑生成的動態過程如圖8所示。仿真結果表明，在引入前后引導勢場后，規劃出的路徑改善明顯，具體表現在：障礙車勢場對主車的影響范圍變大，因此主車在避障開始駛入另一車道行駛，且避障時側向與障礙車保持著更為安全的距離。沒有加引導勢場的模型雖然也能“成功”避障，但是近乎與障礙車“擦身而過”，因而實際駕駛中非常容易導致與障礙車的碰撞，如圖7(b)所示。所以引導勢場的引入很好地避免了這類情況的發生，從而使主車在避障過程中更為安全。

圖9(a)給出了加入和不加入引導勢場這兩種情況下生成的避障路徑在橫向上距離之差的絕對值。可以看出，在加入引導勢場后，主車與障礙車的橫向距離比未加入引導勢場時更遠，其最大差值達到約1.4m，可見效果明顯。

此外，由圖9(b)給出的車輛在路徑跟隨過程中主車橫擺角速度隨時間變化的關系可知,主車在加入和未加入引導勢場在避障過程中都能夠保持車輛的穩定前行，但加入引導勢場后，由于避障初始轉向角的增大，橫擺角速度也會增大，但是整個過程中相差并不太多，且沒有出現失穩的情況。因此該方法的可行性和有效性得到驗證。

5.2 彎道情形

彎道場景如圖10(a)所示，道路參數設置如表3所示。規劃初始時刻主車在右車道距離車道中心線2.5m處，以25m/s的速度勻速行駛。障礙車的初始位置為水平距離50m，與車道中心線相距2m的地方，并且以15m/s的速度勻速行駛。

從圖10(b)的仿真結果來看，兩種情形下都能夠安全成功避障，雖然加入引導勢場的效果與不加入引導勢場沒有直線車道情形下差別明顯，但在加入引導勢場后，主車的避障路徑在橫向上距離最大差值約為0.4m，見圖10(c)。

參數數值描述b/m35單車道寬度l/m200路徑規劃長度x0/m-10彎道開始處曲率xf/m-1000125彎道末尾處曲率

在彎道情形中，由于彈性繩起始位置的差異，在沒有加入引導勢場下主車也能夠較早駛入另一車道，且與障礙車在縱向上保持足夠的安全距離，是否加入引導勢場的區別不大。在直線車道情形中，由于引導勢場的加入也是為使主車在縱向上與障礙車保持更為安全的距離，所以位置會離障礙車更遠。由此可見引導勢場的加入對直線道路上的避障作用更為明顯。此外從圖10(d)中也可以觀察到在避障過程中主車橫擺角速度大小，可知主車在整個過程中狀態穩定。

6 結論

在人工勢場和彈性繩模型的基礎上，分別在障礙車前后添加引導勢場模型，實現了在低風險交通場景的緊急避障路徑規劃。仿真實驗表明，改進后的模型與改進前相比，特別在直線道路情形下，會有更加安全的避障路徑。通過Carsim仿真模擬可知，主車在避障過程中可保持車輛的動力學穩定性。

[1] 宋曉琳,馮廣剛,楊濟匡.汽車主動避撞系統的發展現狀及趨勢[J].汽車工程,2008,30(4):285-290.

[2] 中華人民共和國統計局.中國統計年鑒(2012年):交通事故統計情況(2011年)[DB/OL].http://www.stats.gov.cn/tjsj/ndsj/2012/indexch.htm,2012-09-19.

[3] SAE International. Driver Assistance Technology: What Really Matters[EB/OL].www.sae.org/mags/sve/11152/,2012-06-02.

[4] Quinlan S. Real-time Modification of Collision-free Path[D]. Stanford University, USA,1995.

[5] Hilgert J, Hirsch K, Bertram T, et al. Emergency Path Planning for Autonomous Vehicles Using Elastic Band Theory[C]. IEEE/ASME International Conference on Advanced Intelligent Mechatronics AIM 2003, Kobe, Japan,20 -24 July 2003.

[6] Sattel T Brandt. From Robotics to Automotive: Lane-keeping and Collision Avoidance Based on Elastic Bands[J]. Vehicle System Dynamics,2008,46(7):597-619.

[7] Song Xiaolin, Cao Haotian, Huang Jiang. Influencing Factors Research on Vehicle Path Planning Based on Elastic Bands for Collision Avoidance[J]. SAE International Journal of Passenger Cars- Electronic and Electrical Systems,2012,5(2):625-637.

[8] 李曉霞,江宗法,李百川,等.車載距離探測技術比較[J].長安大學學報(自然科學版),2002,22(2):73-76.

[9] 劉志峰,王建強,李克強.具有魯棒特性的車載雷達有效目標確定方法[J].清華大學學報(自然科學版),2008,48(5):875-878.