緊急避撞工況下的路徑規劃與跟蹤
2020-11-09 03:16:43周兵萬希吳曉建陳曉龍曾凡沂
湖南大學學報·自然科學版 2020年10期
周兵 萬希 吳曉建 陳曉龍 曾凡沂
摘? ?要：高速工況下規劃側向換道緊急避撞路徑需考慮避撞與穩定性問題，本文提出了一種基于非均勻B樣條曲線、滿足多約束要求的路徑規劃方法. 該方法通過兩段非均勻B樣條曲線構造出具有曲率連續且加速度呈梯形變化、滿足起止點斜率和曲率約束的避撞路徑模型，再結合環境信息生成由兩個參數控制路徑形狀的避撞路徑簇;在此基礎上，構建乘員舒適性目標函數，旨在滿足避撞要求的情形下，從路徑簇中選取舒適性最優的局部路徑;在完成避撞路徑規劃同時，通過目標函數以及車輛多約束得到優化縱向加速度，完成避撞運動規劃;最后采用長于處理多約束控制的模型預測控制算法，通過建立線性時變模型預測控制器控制車輛跟蹤規劃的路徑，驗證了路徑規劃方法的可行性.
關鍵詞：緊急避撞;非均勻B樣條曲線;路徑規劃;多約束;路徑跟蹤
中圖分類號：U463.1? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻標志碼：A
Path Planning and Tracking in Scenario
of Emergency Collision Avoidance
ZHOU Bing1，WAN Xi1?，WU Xiaojian2，CHEN Xiaolong1，ZENG Fanyi1
（1. State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body，Hunan University，Changsha? 410082，China;
2. School of Mechanical & Electrical Engineering，Nanchang University，Nanchang 330031，China）
Abstract：To consider the problem of collision avoidance and stability when planning the emergency collision avoidance path of lateral lane under high speed scenarios，a path planning algorithm based on non-uniform B-spline curve is proposed，which meets the requirements of multi-constraint. The proposed collision-free path model constructed by two non-uniform B-spline curves has continuous curvature with trapezoidal change and satisfies the slope and curvature constraint at the starting point and end point; and then，the environmental information is combined to generate a collision-avoidance path cluster whose path shape is controlled by two parameters. On this basis，a passenger comfort objective function is constructed，aiming to select a local path with the best comfort from the path cluster when the collision avoidance requirement is met. Furthermore，the objective function and multi-constrains of the vehicle are used to obtain the optimized longitudinal acceleration so as to complete the collision avoidance motion planning. Finally，the model predictive control （MPC） algorithm specialized in multi-constrained problem is selected to develop a linear time-varying MPC controller，and the feasibility of the proposed path planning method is verified.
Key words：emergency collision avoidance;non-uniform B-spline curve;path planning;multi-constraint;path tracking
統計數據表明，人為駕駛不可避免地存在反應不及時、操控不精確、誤操作等局限性，導致90%以上的交通事故與人為因素有關. 智能車輛能夠通過高效的環境感知、決策和精準的控制，有效地消除上述局限，提高行駛安全性. 目前，智能車輛相關研究已成為熱點.
局部路徑規劃是智能駕駛的基礎和核心技術之一. 根據規劃的原理可以將其劃分為3大類[1]：基于特定函數的路徑規劃、基于搜索的路徑規劃和基于優化的路徑規劃. 其中，基于特定函數的路徑規劃是一種采用梯形加速度曲線[2]、多項式[3-5]、B樣條曲線[6]等函數計算車輛避撞路徑的方法.該方法具有計算高效、曲率變化連續平滑等優點，適用于本文研究的高速避撞這類極端工況. 目前，圍繞該方法已開展了大量研究工作. Shim等[4]采用多項式函數建立6次多項式方程，求解得出多項式系數，使路徑滿足起點和終點約束，但該方法未考慮車輛動力學約束;Gonzalez等[6]采用貝塞爾曲線構造具有曲率連續的避撞路徑，通過最大側向加速度約束得出最大縱向速度，從而實現對縱向速度的規劃;孫浩等[7]提出一種基于三次多項式曲線的最優換道軌跡，考慮了乘坐舒適性和安全性，但前提是車輛恒速行駛;陳成等[8]提出了一種由3 個參數確定的四階貝塞爾曲線來規劃路徑，并考慮了加速度連續和加速度有界等約束，但忽略了縱側向加速度耦合約束;金之熔等[9]使用B樣條曲線對Dijkstra算法得到的最短路徑進行平滑，并考慮時間最優，在規劃路徑的同時求解速度曲線，最終得到符合車輛動力學特性的最優運動路徑;Suzuki等[10]將利用A*算法得到的B樣條節點進行平移生成曲率大于最小轉彎半徑的避障路徑，并對控制點位置進行處理，以使車輛盡可能減少轉向操作;Wu等[11]使用B樣條曲線對人工勢場法規劃結果進行平滑，實現了減小路徑震蕩的效果;Choi等[12]在規劃雙臂機器人路徑通過多次刪選B樣條控制點以達到減少不必要路徑抖動的效果;Wan等[13]在B樣條曲線控制點連線平行方向增加控制點，使車輛能夠通過指定路徑點，從而更好地控制車輛可以通過狹窄通道.
學者們在基于特定函數的路徑規劃方法做了廣泛而深入的探索，然而，仍有兩個問題需要關注：1） 為了保證生成路徑的可行性，通常通過調整函數的參數值生成若干條備選路徑，再通過目標函數從中選擇. 當確定軌跡函數的參數較多時，生成的備選軌跡會成倍增加，導致篩選耗時;2） 目前，對該類方法的研究主要集中在中低速，且規劃軌跡時考慮車輛約束不夠全面，如缺少對縱向加速度的約束，以及對縱、側向加速度耦合的約束等，而這些被忽略的約束在車輛高速避撞控制時往往至關重要.
針對以上問題，本文利用兩段非均勻B樣條曲線構造出具有曲率連續、滿足起（終）點的斜率與曲率約束的避撞路徑模型，且其路徑兼有梯形加速度曲線的優點，同時路徑的形狀僅由兩個參數確定，減少備選軌跡的生成;而后通過構建乘員舒適性目標函數，添加車輛速度、加速度連續約束，防側滑約束，縱、側向加速度耦合約束等多約束，對生成的避撞路徑簇進行篩選，最終完成車輛高速避撞路徑規劃和運動規劃.
1? ?路徑規劃
1.1? ?非均勻 B樣條曲線的介紹
1.1.1? ?非均勻B樣條曲線的遞推式
相較于其它類型的B樣條曲線通過控制點和曲線次數改變曲線形狀，非均勻B樣條曲線形狀變化更加靈活，可以通過它的節點矢量加以改變.
k次非均勻B樣條曲線定義為：
C（j） = Ni，k（j）Pi，a≤j≤b? ? ? ?（1）
式中：{Pi}（Pi = （Xi，Yi））是決定樣條曲線形狀的控制點的位置坐標;{Ni，k（j）}是在非周期且非均勻節點矢量χ上的k次樣條基函數.
χ = [a，…，a，jk+1，…，jm-k-1，b，…，b]? ? （2）
式中的節點個數為m+1，非均勻B樣條曲線的次數k、控制點數（n+1）、節點數（m+1）之間的關系為：m = n + k + 1.
k次非均勻B樣條曲線基函數Ni，k（j）定義為在給定一個非遞減的節點矢量χ = [ j0，j1，…，jn]，有：
Ni，0（j）=1，ji ≤j≤ji+1
0，else
Ni，k（j）=
Ni，k-1（j）+
Ni+1，k-1（j）（3）
當式（3）中出現%的形式時，約定其比值為0.
一般地，路徑規劃采用3次或3次以上的非均勻B樣條函數，使生成的路徑能夠在整個區間具有連續的二階導數，即具有連續的路徑曲率. 需要注意的是，增加樣條函數次數可以提高曲線的自由度，但勢必增加計算消耗. 考慮到本文研究的高速緊急避撞工況對路徑規劃的實時性要求較高，在此，將采用3次非均勻B樣條進行路徑規劃.
1.1.2? ?非均勻B樣條曲線控制點的確定
為使路徑擁有較好的平滑性，文獻[14]提出了一種非均勻B樣條曲線控制點確定方法. 本文在該方法基礎上，通過融入路徑信息，進行路徑規劃. 具體而言，如圖1所示，通過車輛的初始位置（X0，Y0，φ0）和目標位置（Xend，Yend，φend）確定兩條相交的直線，構成過渡路徑以配置樣條曲線的控制點. 為使路徑起止點的方向與車輛方向相同，且保證從初始位置到目標位置之間的曲線曲率連續，本文選用6個對稱的控制點確定每段B樣條曲線，控制點的定位由式（4）確定.
P0 =B+l1 a，P1 =B+
a，P2 =B+
a，
P3 =B+
b，P4 =B+
b，P5 =B+l2 a（4）
式中：B點為兩直線的交點;a和b為兩直線的單位向量;l1和l2為前、后轉彎準備距離，其中，l1為車輛當前位置與交點B的距離，l2為車輛目標點與交點B的距離;τ為控制點的定位參數.
當前、后轉彎準備距離確定后，由參數τ確定B樣條曲線的控制點及對應的節點矢量χ.
圖1? ?非均勻B樣條曲線控制點的確定
Fig.1? ?Determination of control points
of non-uniform B-spline curve
車輛的側向加速度與道路曲率的關系滿足ay = v2
xρ，在車輛縱向速度變化不太大的情況下，應盡量使道路曲率的變化與車輛側向加速度的變化趨勢相同. 本文選用的確定B樣條曲線控制點的方法能夠達到這一目的. 因車輛的加速度常采用梯形加速度模型，本文的B樣條曲線控制點，其曲率函數可表達為式（5），如圖2所示，根據式（5）得到的曲率變化曲線也呈現梯形.
1.2? ?局部最優路徑規劃及速度規劃
緊急避撞路徑規劃的工作原理可概括為圖3. 詳細而言，為了使整段避撞路徑的曲率呈梯形變化，路徑采用兩段B樣條曲線構成. 同時，由于控制B樣條曲線的參數是可變的，因此可以生成一組路徑簇. 本文在避撞路徑簇的基礎上，進一步構建乘員舒適性目標函數，旨在滿足避撞要求的情形下，選取產生一條舒適性最優的局部路徑;與此同時，還將由目標函數得出車輛的最優縱向加速度.
1.2.1? ?場景信息的提取與緊急避撞路徑模型的建立
設車輛行駛于如下工況：車輛當前坐標位置為（0，0，0），車速V0 = 90 km/h，車寬wv = 2 m，靜止障礙物位置坐標為（40，0，0），障礙物的寬度wobs = 2 m，車輛與障礙物的期望安全距離Sd = 1 m. 通過上述路徑規劃方法得出的避撞路徑如圖8所示.
圖9展示了最優、次優路徑對應的縱、側向加速度. 實線為最優路徑的縱、側向加速度，虛線則為次優路徑的縱、側向加速度. 最優路徑的側向加速度變化較快，但最大側向加速度值小于次優路徑的最大側向加速度值. 同時，最優路徑的最大縱向加速度也小于次優路徑的最大加速度. 因此，從舒適性角度來說，最優路徑的舒適性更好.
隨后，通過最優路徑的縱向加速度可得出縱向期望速度，如圖10所示. 在該仿真工況中，由于車輛利用轉向進行側向避撞，輪胎的縱向制動力所需不大，故使車輛的縱向速度減小較少. 圖中，車輛的縱向速度由初始時刻的90 km/h下降到完成避撞時的83.7 km/h，可以保持較高速度避障通過.
2? ?路徑跟蹤
路徑規劃后需通過路徑跟蹤完成避撞控制，本文路徑跟蹤控制的框架如圖11所示. 由于高速避撞工況下，車輛的縱向動力學與側向動力學存在復雜的耦合關系，車輛縱向動態對側向動態產生影響，需要將車輛縱向控制器中輸出的加速度傳遞到車輛側向控制器中，得到更加準確的控制效果.
車輛的縱向速度控制器采用PID控制，側向控制則采用模型預測控制方法. 模型預測控制（model predictive control，MPC）方法是一種滾動優化控制方法，該方法能夠系統地處理控制約束問題，非常適合用于車輛的軌跡跟蹤控制. 考慮到非線性模型的MPC方法雖然控制效果好，但計算量大，實時性變差[15]，本文采用Falcone等[16]提出的線性時變模型預測控制算法，將車輛模型在工作點進行局部線性化，可降低求解器的求解難度，加快求解器運算速度，同時保證模型的誤差.
2.1? ?二自由度車輛模型
如圖10所示，縱向期望速度在避撞過程中變化很小，利用模型預測控制器控制車輛轉向避撞時，在一個時長很短的預測時域內，車輛模型可采用縱向速度恒定的非線性二自由度模型，如圖12所示.
式中：ξ（t） = [[y] ，φ，[φ] ，Y，X]T為系統的狀態量;u（t） = δf為系統的輸入量;ξ（t） = [φ，Y]T為系統的輸出量;C為系統輸出矩陣;φ表示車輛的航向角;[φ] 表示橫擺角速度;X、Y表示車輛在慣性坐標系中的位置;δf表示車輛的前輪轉角;lf和lr分別表示車輛質心到前、后軸的距離;Ccf和Ccr分別為前、后輪的側偏剛度;m為車輛的總質量;Iz為車輛橫擺轉動慣量.
2.2? ?線性時變模型預測控制
為了保證模型預測控制器能夠實時地輸出控制信號，需要簡化模型預測控制器中使用的非線性車輛模型，降低控制器的求解計算量. 先通過對非線性車輛模型進行離散化，得到非線性離散車輛模型.
再將非線性離散車輛模型在工作點（ξi，ut）處進行線性化處理后，得到線性時變車輛模型[17].
式中：ΔU（t）=[Δu（t|t），Δu（t+1|t），…，Δu（t+Nc-1|t）]T;Np和Nc分別為預測時域和控制時域;Q和R為權重矩陣;ξr為參考軌跡點. 通過優化問題簡化后可以重新表達為：
約束： MΔU≤N.
2.3? ?跟蹤效果的仿真與分析
根據1.4節所述工況下規劃的避撞路徑，采用Simulink-Carsim聯合進行路徑跟蹤仿真控制，車輛初試速度為90 km/h，路面附著系數為0.8，Carsim中的部分車輛參數如表1所示.
圖13為在1.4節所述緊急避撞工況下，根據本文提出的方法規劃出的期望速度和跟蹤速度對比. 圖中，虛線為規劃的期望速度，實線為車輛跟蹤速度. 可以發現：由于在路徑規劃過程中加入了加速度約束，車輛的期望速度沒有出現突變;即使車輛在高速行駛，并且有大的轉向時，車輛的速度變化仍能夠跟上期望速度的變化.
圖14為規劃路徑與跟蹤路徑的對比. 總體上，車輛能夠準確地跟蹤規劃的路徑，由于控制器使用線性化處理的車輛模型，造成跟蹤時出現小的偏差;其中，最大的側向誤差為0.185 m（高速時，此誤差能夠接受），車輛與障礙物的最小距離為0.353 m（考慮了自車的寬度），能夠保證車輛避開障礙物.
規劃的車輛側向加速度與跟蹤側向加速度的對比，如圖15所示. 跟蹤路徑控制時的側向加速度變化呈梯形加速度模型變化，其值與規劃的側向加速度比較接近，其中，后半段路徑由于車輛的跟蹤速度稍大于規劃速度，造成跟蹤的側向加速度稍大于規劃的側向加速度.
圖16和圖17為路徑跟蹤過程中的橫擺角速度和質心側偏角的變化. 整個跟蹤過程中，車輛的橫擺角速度的絕對值小于16.34 deg/s，且質心側偏角的絕對值小于1.55°，這表明車輛在路徑跟蹤過程中能夠保持良好的穩定性.
綜合以上分析，本文提出的多約束及多目標最優條件下緊急避撞路徑規劃方法，能夠在保證避撞的前提下，盡可能的保證乘員的舒適性;同時，因在構造路徑簇和選擇最優路徑時，充分考慮了車輛的動力學約束，可確保車輛能夠良好地跟蹤規劃路徑.
3? ?結? ?論
1）提出了一種基于非均勻B樣條曲線構造緊急避撞路徑的方法，充分利用傳感器所得到的環境信息，得到一組少量且準確的避撞路徑簇（備選路徑）;在此基礎上，利用構建的乘員舒適性目標函數和車輛縱、側向加速度耦合約束等條件，從避撞路徑簇中選取產生一條既能保證乘員舒適性又適合車輛高速跟蹤的避撞路徑.
2）Simulink-Carsim聯合進行路徑跟蹤控制結果表明：本文在路徑規劃時將車輛約束融入規劃過程，能夠確保車輛在高速緊急避撞時也能精確跟蹤路徑.
參考文獻
[1]? ? 江慶坤. 智能汽車避障危險評估和軌跡規劃研究[D]. 長春：吉林大學汽車工程學院，2016：5—11.
JIANG Q K. Threat assessment and trajectory planning for intelligent vehicle obstacle avoidance system[D]. Changchun：College of Automotive Engineering，Jilin University，2016：5—11. （In Chinese）
[2]? SOUDBAKHSH D，ESKANDARIAN A，CHICHKA D. Vehicle collision avoidance maneuvers with limited lateral acceleration using optimal trajectory control[J]. Journal of Dynamic Systems Measurement & Control，2013，135（4）：041006.
[3]? ? YANG J，QU Z，WANG J，et al. Comparison of optimal solutions to real-time path planning for a mobile vehicle[J].IEEE Transactions on Systems，Man and Cybernetics，Part A：Systems and Humans，2010，40（4）：721—731.
[4]? ? SHIM T，ADIREDDY G，YUAN H. Autonomous vehicle collision avoidance system using path planning and model-predictive-control-based active front steering and wheel torque control[J]. Proceedings of the Institution of Mechanical Engineers，Part D：Journal of Automobile Engineering，2012，226（6）：767—778.
[5]? ? 宋曉琳，周南，黃正瑜，等. 改進RRT在汽車避障局部路徑規劃中的應用[J]. 湖南大學學報（自然科學版），2017，44（4）：30—37.
SONG X L，ZHOU N，HUANG Z Y，et al. An improved rrt algorithm of local path planning for vehicle collision avoidance[J]. Journal of Hunan University （Natural Sciences），2017，44（4）：30—37. （In Chinese）
[6]? GONZALEZ D，PEREZ J，LATTARULO R，et al. Continuous curvature planning with obstacle avoidance capabilities in urban scenarios[C]//International Conference on Intelligent Transportation Systems. 2014：1430—1435.
[7]? ? 孫浩，鄧偉文，張素民，等. 考慮全局最優性的汽車微觀動態軌跡規劃[J]. 吉林大學學報（工學版），2014，44（4）：918—924.
SUN H，DENG W W，ZHANG S M，et al. Micro vehicle dynamic trajectory plan with global optimality[J]. Journal of Jilin University （Engineering and Technology Edition），2014，44（4）：918—924. （In Chinese）
[8]? ? 陳成，何玉慶，卜春光，等. 基于四階貝塞爾曲線的無人車可行軌跡規劃[J]. 自動化學報，2015，41（3）：486—496.
CHEN C，HE Y Q，BU C G，et al. Feasible trajectory generation for autonomous vehicles based on quartic bezier curve[J].Acta Automatica Sinica，2015，41（3）：486—496. （In Chinese）
[9]? ? 金之熔，申立勇. 基于二次B樣條的時間最優軌跡規劃[J]. 系統科學與數學，2018，38（12）：1364—1375.
JIN Z R，SHEN L Y. Time optimal trajectory planning based on quadratic B-spline [J]. Journal of Systems Science and Mathematical Sciences，2018，38（12）：1364 —1375. （In Chinese）
[10]? SUZUKI K，USAMI R，MAEKAWA T. Automatic two-lane path generation for autonomous vehicles using quartic B-spline curves[J]. IEEE Transactions on Intelligent Vehicles，2018，3（4）：547—558.
[11]? WU Z，SU W，LI J. Multi-robot path planning based on improved artificial potential field and B-spline curve optimization[C]//2019 Chinese Control Conference （CCC）. Piscataway N J：IEEE，2019：4691—4696.
[12]? CHOI Y，KIM D，HWANG S，et al. Dual-arm robot motion planning for collision avoidance using B-spline curve[J]. International Journal of Precision Engineering and Manufacturing，2017，18（6）：835—843.
[13]? WAN N，XU D，YE H. Improved cubic B-spline curve method for path optimization of manipulator obstacle avoidance[C]//2018 Chinese Automation Congress （CAC）. Piscataway N J：IEEE，2018：1471—1476.
[14]? 王振華，章衛國，李廣文，等. 基于非均勻B-樣條的G2路徑平滑方法[J]. 系統工程與電子技術，2011，33（7）：1539—1543.
WANG Z H，ZHANG W G，LI G W，et al. G2 path smoothing using non-uniform B-spline[J]. Systems Engineering and Electronics， 2011，33（7）：1539—1543. （In Chinese）
[15] BORRELLI F，FALCONE P，KEVICZKY T，et al. MPC-based approach to active steering for autonomous vehicle systems[J]. International Journal of Vehicle Autonomous Systems，2005，3（2/4）：265—291.
[16]? FALCONE P，BORRELLI F，ASGARI J，et al. Predictive active steering control for autonomous vehicle systems[J]. IEEE Transactions on Control Systems Technology，2007，15（3）：566—580.
[17]? ABBAS M A. Non-linear model predictive control for autonomous vehicles[D]. Oshawa：University of Ontario Institute of Technology，2011：22—63.
