基于目標運動軌跡預測的自動緊急制動避撞策略

童 燮，萬茂松，趙奉奎

（南京林業大學汽車與交通工程學院，江蘇南京 210037）

0 引言

我國汽車擁有量持續增加，各種交通事故頻發，給家庭和社會帶來巨大損失。傳統的被動安全措施原理是減小事故發生后傳遞到駕駛員及乘客身體的沖擊，從而減小傷害程度，但卻難以避免事故發生。隨著交通系統的發展，行車速度得到了很大提高，單獨依靠被動安全措施難以保證高速行車事故下的人身安全。自動緊急制動系統（AEB）是汽車高級駕駛輔助系統（Advanced Driving Assistance System，ADAS）的重要組成部分，是一種主動安全措施。通過信息與傳感技術感知周圍環境信息，自動判斷汽車當前的安全狀態，提醒駕駛員制動或采取強制制動措施，避免碰撞事故發生，AEB 系統已經逐步成為汽車的標準配置。

避撞策略是汽車AEB 系統核心，精確合理的避撞策略對AEB 系統實現更好的控制效果起著至關重要的作用，而避撞策略的制定則依據車輛安全狀態判斷。目前有兩種方式判斷汽車安全狀態：①計算出即碰時間，與系統計算的閾值比較，稱為安全時間模型；②通過距離判斷汽車的安全狀態，稱為安全距離模型［1］。

安全時間模型中應用較多的為基于碰撞時間（TTC）的比較模型，碰撞時間參數最早由Hayward 于1971 年提出。徐杰等［2］提出一種考慮本車與目標車相對加速度的二階TTC 公式，彌補了經典TTC 公式在兩車車速非常接近時的不足。安全距離模型主要有Honda 模型、Mazda 模型、Berkeley 模型等［3］。此外，有研究提出其它因素的安全距離模型，如駕駛員預估安全距離模型［4-5］、基于車間時距的安全距離模型［6-8］、基于制動過程的安全距離模型［9-11］、改進的駕駛員預估安全模型［12］等；楊為等［13］搭建了對行人保護的分層控制避撞策略，但其制動減速度和風險評估模型是基于經驗值判斷的；唐陽山等［14］通過采集駕駛員的反應時間確定不同駕駛員的反應時間對安全距離的影響；姜順明等［15］基于駕駛員狀態識別建立自動緊急制動控制策略。

上述模型均只考慮了車與目標的安全時間或者安全距離，沒有對目標的運動狀態進行深入研究，而且判斷目標多為同車道或鄰近車道車輛，對于復雜的交通環境，比如交叉路口下的行人、車輛以及其它有可能威脅車輛安全的目標不能準確判斷。本文針對上述模型對目標判斷的局限性，以駕駛員的自車為原點建立坐標系，預測目標下一段時間的運動軌跡，并將目標的運動軌跡加入汽車安全狀態的判斷條件，提出一種基于目標運動軌跡的自動緊急制動避撞策略。

1 基于卡爾曼濾波的目標運動軌跡預測算法

1.1 卡爾曼濾波算法

卡爾曼濾波能從一系列包含噪聲的測量中估計動態系統狀態，是一種高效的遞歸濾波器。卡爾曼濾波會根據各種測量在不同時間下的值，考慮各時間下的聯合分布對未知變數進行估計，比單一測量為基礎的估計方式要準。

對于運動狀態頻繁變化的系統，卡爾曼濾波能通過輸入輸出觀測數據對系統進行最優狀態估計，從而實時預測目標的運動狀態變化［16］。

卡爾曼濾波狀態方程一般為：

觀測方程一般為：

其中，Xk為系統當前時刻的狀態向量，Ak-1為系統的狀態轉移矩陣，Xk-1為系統上一時刻的狀態向量，Bk-1為系統的控制矩陣，uk-1為系統上一時刻的控制向量，wk-1為系統的過程噪聲，Zk為系統當前時刻的觀測量，Hk-1為系統的觀測矩陣，sk-1為系統的測量噪聲。

本文預測過程沒有對目標的控制，因此忽略控制輸入，即：假設系統的過程噪聲wk-1和測量噪聲sk-1是相互獨立的，且為高斯白噪聲，其協方差分別為Q 和R。卡爾曼濾波計算分為兩步：時間更新和狀態更新，利用遞歸算法獲得系統的最優狀態估計。

時間更新方程為：

狀態更新方程為：

因此，將目標上一時刻的狀態代入時間更新方程便可得到當前狀態的先驗估計，通過計算卡爾曼增益得到當前狀態的后驗估計，即最優估計。然后將當前時刻的最優估計代入時間更新方程便可得到下一時刻的先驗估計，完成對目標運動軌跡的預測。

1.2 目標運動軌跡建模

汽車檢測到的目標，無論是行人還是車輛均可看作是移動對象。以自車為原點建立直角坐標系，通過毫米波雷達和視覺等車載傳感器得到目標相對于自車的位置、速度和加速度等信息，將這些信息存儲到數據集Tri中，即：

其中，pij為第j時刻的位置矢量，vij為第i個目標在第j個時刻的速度，aij為第i個目標在第j個時刻的加速度。

根據運動學公式：

加速度實時測量模型中只需要使用該值，不需要預測。將公式（10）和（11）寫成矩陣形式即為狀態方程：

將公式（12）和公式（13）代入卡爾曼濾波的時間更新方程和狀態更新方程中，即可完成對目標當前狀態的最優估計以及對下一時刻的軌跡預測，通過調整不同的Δt即可實現對未來一段時間的軌跡預測。

2 風險評估模型設計

2.1 報警及制動閾值設定

目前比較常見的有兩種AEB 系統風險評估模型：①通過分析本車與目標車之間的制動過程設定相應的安全預碰撞距離閾值，當本車與目標車之間的距離小于設定的安全閾值時開始緊急制動，稱為安全距離模型；②計算本車和目標車之間的碰撞時間（TTC），設置相應的碰撞時間閾值，當兩車的碰撞時間低于設定的碰撞閾值時開始制動，稱為安全時間模型。

但上述兩種模型都無法對運動軌跡復雜的目標進行有效判斷，汽車仍然存在安全隱患。本文提出一種基于目標運動軌跡的自動緊急制動避撞策略，對自車前方的目標進行軌跡預測，判斷自車當前的安全狀態，實施過程如下：

在自車行駛過程中，以自車為原點建立平面直角坐標系，如圖1 所示。假設自車前方某位置存在一障礙物G，自車安裝有毫米波雷達和機器視覺等傳感器，可以檢測目標相對于自車的距離、角度、相對速度、相對加速度。

Fig.1 Self-driving coordinate system圖1 自車坐標系

已知條件為物體G 相對于本車的速度、加速度、距離、角度等信息，使用卡爾曼濾波算法對目標接下來的Tg 秒時間的軌跡進行預測，分為圖1 所示4 種情況。可以看出，目標的軌跡與自車坐標系y 軸有不同的交點，將目標軌跡與y 軸交點稱為YL。

定義線段S1 為緊急制動區，表示接下來Tg 秒內經過此區域的目標有可能嚴重威脅自車安全。

定義線段S2 為預警區，表示接下來Tg 秒內經過此區域的目標有可能危及車輛安全，但駕駛員有足夠的時間采取措施。

定義線段S1 與S2 之外的區域為安全區，表示接下來Tg 秒內經過此區域的目標不會對自車安全造成影響。

對于軌跡L1 與L4，可以看出物體A 在接下來的Tg秒內的運動軌跡完全在汽車的警報范圍之外，這種情況的運動對本車的安全不造成任何影響，此時AEB 不采取任何措施。

對于軌跡L2，可以看出物體A 的運動軌跡與S2 有交點，說明物體G 可能在一個比較危險的時間段內經過汽車前方，此時需要警報提示駕駛員有潛在的安全隱患。

對于軌跡L3，可以看出物體A 的運動軌跡與S1 有交點，說明汽車此時已經處于一種比較危險的狀態，應當立即采取制動措施，避免碰撞。

2.2 分級報警制動系統

本文所設計的風險評估模型根據目標的運動軌跡與自車坐標系y 軸的交點縱坐標大小，對各個工況下的行車安全程度進行3 個安全等級劃分。

一級為正常行駛級別，如圖1 中的軌跡L1 和L4。目標軌跡經過安全區處于正常行駛級別，此時系統仍然對目標實時檢測，但無預警無制動。

二級為預警級別，如圖1 中的軌跡L2。目標軌跡經過預警區處于碰撞預警級別，此時若駕駛員未意識到危險而采取加速措施極有可能發生碰撞，故此時應發出報警信號，提醒駕駛員減速行駛。

三級為緊急制動級別，如圖1 中的軌跡L3。目標軌跡經過緊急制動區處于緊急制動級別，此時盲目加速或者不采取措施都有極大可能發生碰撞事故，AEB 系統應實施自動緊急制動，保障車內外人員安全。

3 自動緊急制動控制系統設計

3.1 AEB 系統工作流程

本文采用上層模糊下層PID 控制的方式對AEB 系統硬件進行控制。圖2 為AEB 系統工作流程，車載傳感器實時檢測前方目標信息，通過卡爾曼濾波預測目標軌跡并將結果輸入風險評估模型中，得到當前的報警及制動閾值，將此結果輸入到模糊控制器得到當前期望的減速度，通過底層PID 控制器控制制動管路油壓，實現對AEB 系統的硬件控制。

Fig.2 System workflow圖2 系統工作流程

3.2 AEB 上層模糊控制器設計

為了在制動過程中盡可能保證乘車人員舒適性，本文通過模糊控制器獲取期望減速度。本文所述的AEB 模糊控制器是以目標運動軌跡與自車坐標系y 軸交點的縱坐標YL（m）、自車車速Vz（km/h）以及剩余碰撞時間T（s）為輸入，以制動時的期望減速度a（m/s2）和報警信號Signal 為輸出的三輸入雙輸出模糊控制。

交點縱坐標YL 的隸屬度函數如圖3 所示，取值為［-1，11］。當YL 為小于0 的任何值時不采取任何措施，認為此時YL 為-1 即可；當YL 大于10 時，認為此時汽車處于安全狀態，令YL 為11 即可；當YL 值較小時應采取緊急制動模式；當YL 值較大時，對汽車安全無影響，可正常行駛。隸屬度函數使用高斯型函數，模糊語言變量為負、低、中等、高，YL 的模糊集設置為｛NA，LO，MI，HI｝。

Fig.3 YL membership function圖3 YL 隸屬度函數

自車車速Vz 的取值為［0，80］，Vz 較小時可采取強度較低的制動模式保證車輛行駛安全；當Vz 較高時，為了避免碰撞應加大制動強度，甚至采取緊急制動模式。自車車速Vz 的隸屬度函數取高斯型，如圖4 所示，其模糊語言變量為停止、非常低、低、中等、高、非常高，Vz 的模糊集設置為｛ST，VL，LO，MI，HI，VH｝。

Fig.4 Vz membership function圖4 Vz 隸屬度函數

剩余碰撞時間T 的隸屬度函數如圖5 所示，取值為［-1，6］，表示目標將在T 秒后經過自車坐標系y 軸。當T小于0 時，認為此目標已經過了汽車前方，后面不會再影響汽車安全，故此時令T 為-1 即可；T 越小，表示潛在風險可能更快發生，故此時需要較大的減速度令汽車減速。

Fig.5 T membership function圖5 T 隸屬度函數

期望減速度a 的取值為［0，8］，其隸屬度函數使用高斯型函數，如圖6 所示。模糊語言變量為零、非常低、低、中等、高、非常高，將模糊集設置為｛LI，VL，LO，MI，HI，VH｝。

Fig.6 a membership function圖6 a 隸屬度函數

報警信號Signal 取值為0 和1，其隸屬度函數如圖7所示。模糊語言變量為0、1，將模糊集設置為｛LI，YI｝。

Fig.7 Signal membership function圖7 Signal 隸屬度函數

根據隸屬度函數分析和仿真研究，設計部分模糊控制器規則如表1 所示。

Table 1 Fuzzy control rules表1 模糊控制規則

3.3 AEB 下層PID 制動控制系統設計

下層PID 控制系統主要功能是接收模糊控制器的期望減速度，通過計算得到車輛制動管路壓力值從而對車輛進行減速控制。PID 制動控制系統計算公式如下：

其中，Δa(k)為減速度變化量，kp為比例系數，ki為積分系數，kd為微分系數，e(k)為第k 時刻期望減速度與實際減速度之差。

調整比例系數、積分系數以及微分系數大小，使得實際減速度能快速收斂到期望減速度。選取收斂速度最快的一組PID 參數為最優參數。通過實驗，本文PID 的3 個參數最終確定為：kp=3,ki=3.7,kd=0。

4 系統仿真及結果分析

4.1 目標運動軌跡預測仿真

本文選取網絡上的一組數據集對卡爾曼濾波軌跡預測算法進行驗證，數據集包括目標的橫向位置、縱向位置以及加速度等信息，實驗軟件為MATLAB，通過編寫MATLAB 程序對算法進行驗證。實驗結果如圖8 所示（彩圖掃描OSID 碼可見）。

Fig.8 Track prediction results圖8 軌跡預測結果

從圖8 可以看出，本文基于卡爾曼濾波的軌跡預測算法能夠實現對目標運動軌跡的預測，預測結果與實際運動趨勢基本一致，可以作為自動緊急制動系統控制的依據。

4.2 系統避撞策略仿真

系統運行時，通過車載雷達等傳感器對目標進行檢測，然后通過軌跡預測程序對目標的運動軌跡進行預測，結合當前車速和目標軌跡與自車坐標系縱軸的交點縱坐標，利用模糊控制器判斷自車的安全狀態，確定危險等級，得到期望減速度；然后通過PID 控制系統控制自車的制動壓力，進而實現系統的避撞控制。本文根據系統工作流程建立CarSim 和Simulink 聯合仿真模型，如圖9 所示。

Fig.9 Co-simulation model圖9 聯合仿真模型

聯合仿真模型包含軌跡預測模型、模糊控制模型、整車模型以及PID 控制模型。其中整車模型根據某車輛的真實數據，通過CarSim 軟件建立制動系統動力學模型等等。軌跡預測模型主要根據車輛的距離、加速度等信息預測目標的運動軌跡，得到軌跡與自車坐標系縱軸的交點縱坐標YL。模糊控制器根據YL、當前車速以及剩余碰撞時間，得到期望減速度和報警信號，通過PID 控制完成車身制動。

4.3 仿真結果分析

本文取車速為60km/h 對系統進行仿真分析，通過Car-Sim 軟件建立汽車模型，AEB 系統收集本車及前車的信息（速度、加速度），由軌跡預測模型預測目標軌跡，通過模糊控制器進行風險評估并輸出期望減速度與報警信號，從而通過PID 進行制動控制。仿真結果如圖10 所示。

Fig.10 Simulation results圖10 仿真結果

從圖10 可以看出，汽車初始速度為60km/h，此時系統檢測到某目標會在6s 后經過汽車前方約1m 的位置，即YL 約為1。考慮到當前車速較快，若不采取措施極有可能發生事故，故此時系統采取緊急制動措施。初始時減速度較小，2s 后系統檢測到YL 約為3，汽車仍處于危險狀態，故系統采取進一步制動措施。3.5s 左右時可以看出YL 為8，此時的距離足夠駕駛員發現目標并采取相應的措施，故此時僅報警并不采取制動措施。6s 后，剩余碰撞時間為-1，說明此時目標已經通過了汽車前方，危險狀態解除，報警信號歸0，汽車勻速行駛。

從仿真結果可以看出，系統的報警信號以及制動措施均與設想相符合，報警信號與汽車安全狀態保持同步，充分考慮了目標運動軌跡對汽車安全狀態的影響，制動效果良好。

5 結語

本文結合卡爾曼濾波提出一種基于目標軌跡的自動緊急制動避撞策略。通過CarSim/Simulink 聯合仿真，結果表明在復雜交通環境下，系統能準確預測目標的運動軌跡并給出合理的減速度，實現自動緊急制動，解決了現有模型無法對目標進行有效判斷的局限性。本文所設計的自動緊急制動避撞策略可以滿足行車安全需求，對AEB 系統開發設計有一定的參考意義。