基于目標(biāo)運(yùn)動(dòng)軌跡預(yù)測的自動(dòng)緊急制動(dòng)避撞策略

童 燮，萬茂松，趙奉奎

（南京林業(yè)大學(xué)汽車與交通工程學(xué)院，江蘇南京 210037）

0 引言

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

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

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

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

1 基于卡爾曼濾波的目標(biāo)運(yùn)動(dòng)軌跡預(yù)測算法

1.1 卡爾曼濾波算法

卡爾曼濾波能從一系列包含噪聲的測量中估計(jì)動(dòng)態(tài)系統(tǒng)狀態(tài)，是一種高效的遞歸濾波器?？柭鼮V波會根據(jù)各種測量在不同時(shí)間下的值，考慮各時(shí)間下的聯(lián)合分布對未知變數(shù)進(jìn)行估計(jì)，比單一測量為基礎(chǔ)的估計(jì)方式要準(zhǔn)。

對于運(yùn)動(dòng)狀態(tài)頻繁變化的系統(tǒng)，卡爾曼濾波能通過輸入輸出觀測數(shù)據(jù)對系統(tǒng)進(jìn)行最優(yōu)狀態(tài)估計(jì)，從而實(shí)時(shí)預(yù)測目標(biāo)的運(yùn)動(dòng)狀態(tài)變化［16］。

卡爾曼濾波狀態(tài)方程一般為：

觀測方程一般為：

其中，Xk為系統(tǒng)當(dāng)前時(shí)刻的狀態(tài)向量，Ak-1為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，Xk-1為系統(tǒng)上一時(shí)刻的狀態(tài)向量，Bk-1為系統(tǒng)的控制矩陣，uk-1為系統(tǒng)上一時(shí)刻的控制向量，wk-1為系統(tǒng)的過程噪聲，Zk為系統(tǒng)當(dāng)前時(shí)刻的觀測量，Hk-1為系統(tǒng)的觀測矩陣，sk-1為系統(tǒng)的測量噪聲。

本文預(yù)測過程沒有對目標(biāo)的控制，因此忽略控制輸入，即：假設(shè)系統(tǒng)的過程噪聲wk-1和測量噪聲sk-1是相互獨(dú)立的，且為高斯白噪聲，其協(xié)方差分別為Q 和R?？柭鼮V波計(jì)算分為兩步：時(shí)間更新和狀態(tài)更新，利用遞歸算法獲得系統(tǒng)的最優(yōu)狀態(tài)估計(jì)。

時(shí)間更新方程為：

狀態(tài)更新方程為：

因此，將目標(biāo)上一時(shí)刻的狀態(tài)代入時(shí)間更新方程便可得到當(dāng)前狀態(tài)的先驗(yàn)估計(jì)，通過計(jì)算卡爾曼增益得到當(dāng)前狀態(tài)的后驗(yàn)估計(jì)，即最優(yōu)估計(jì)。然后將當(dāng)前時(shí)刻的最優(yōu)估計(jì)代入時(shí)間更新方程便可得到下一時(shí)刻的先驗(yàn)估計(jì)，完成對目標(biāo)運(yùn)動(dòng)軌跡的預(yù)測。

1.2 目標(biāo)運(yùn)動(dòng)軌跡建模

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

其中，pij為第j時(shí)刻的位置矢量，vij為第i個(gè)目標(biāo)在第j個(gè)時(shí)刻的速度，aij為第i個(gè)目標(biāo)在第j個(gè)時(shí)刻的加速度。

根據(jù)運(yùn)動(dòng)學(xué)公式：

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

將公式（12）和公式（13）代入卡爾曼濾波的時(shí)間更新方程和狀態(tài)更新方程中，即可完成對目標(biāo)當(dāng)前狀態(tài)的最優(yōu)估計(jì)以及對下一時(shí)刻的軌跡預(yù)測，通過調(diào)整不同的Δt即可實(shí)現(xiàn)對未來一段時(shí)間的軌跡預(yù)測。

2 風(fēng)險(xiǎn)評估模型設(shè)計(jì)

2.1 報(bào)警及制動(dòng)閾值設(shè)定

目前比較常見的有兩種AEB 系統(tǒng)風(fēng)險(xiǎn)評估模型：①通過分析本車與目標(biāo)車之間的制動(dòng)過程設(shè)定相應(yīng)的安全預(yù)碰撞距離閾值，當(dāng)本車與目標(biāo)車之間的距離小于設(shè)定的安全閾值時(shí)開始緊急制動(dòng)，稱為安全距離模型；②計(jì)算本車和目標(biāo)車之間的碰撞時(shí)間（TTC），設(shè)置相應(yīng)的碰撞時(shí)間閾值，當(dāng)兩車的碰撞時(shí)間低于設(shè)定的碰撞閾值時(shí)開始制動(dòng)，稱為安全時(shí)間模型。

但上述兩種模型都無法對運(yùn)動(dòng)軌跡復(fù)雜的目標(biāo)進(jìn)行有效判斷，汽車仍然存在安全隱患。本文提出一種基于目標(biāo)運(yùn)動(dòng)軌跡的自動(dòng)緊急制動(dòng)避撞策略，對自車前方的目標(biāo)進(jìn)行軌跡預(yù)測，判斷自車當(dāng)前的安全狀態(tài)，實(shí)施過程如下：

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

Fig.1 Self-driving coordinate system圖1 自車坐標(biāo)系

已知條件為物體G 相對于本車的速度、加速度、距離、角度等信息，使用卡爾曼濾波算法對目標(biāo)接下來的Tg 秒時(shí)間的軌跡進(jìn)行預(yù)測，分為圖1 所示4 種情況?？梢钥闯?，目標(biāo)的軌跡與自車坐標(biāo)系y 軸有不同的交點(diǎn)，將目標(biāo)軌跡與y 軸交點(diǎn)稱為YL。

定義線段S1 為緊急制動(dòng)區(qū)，表示接下來Tg 秒內(nèi)經(jīng)過此區(qū)域的目標(biāo)有可能嚴(yán)重威脅自車安全。

定義線段S2 為預(yù)警區(qū)，表示接下來Tg 秒內(nèi)經(jīng)過此區(qū)域的目標(biāo)有可能危及車輛安全，但駕駛員有足夠的時(shí)間采取措施。

定義線段S1 與S2 之外的區(qū)域?yàn)榘踩珔^(qū)，表示接下來Tg 秒內(nèi)經(jīng)過此區(qū)域的目標(biāo)不會對自車安全造成影響。

對于軌跡L1 與L4，可以看出物體A 在接下來的Tg秒內(nèi)的運(yùn)動(dòng)軌跡完全在汽車的警報(bào)范圍之外，這種情況的運(yùn)動(dòng)對本車的安全不造成任何影響，此時(shí)AEB 不采取任何措施。

對于軌跡L2，可以看出物體A 的運(yùn)動(dòng)軌跡與S2 有交點(diǎn)，說明物體G 可能在一個(gè)比較危險(xiǎn)的時(shí)間段內(nèi)經(jīng)過汽車前方，此時(shí)需要警報(bào)提示駕駛員有潛在的安全隱患。

對于軌跡L3，可以看出物體A 的運(yùn)動(dòng)軌跡與S1 有交點(diǎn)，說明汽車此時(shí)已經(jīng)處于一種比較危險(xiǎn)的狀態(tài)，應(yīng)當(dāng)立即采取制動(dòng)措施，避免碰撞。

2.2 分級報(bào)警制動(dòng)系統(tǒng)

本文所設(shè)計(jì)的風(fēng)險(xiǎn)評估模型根據(jù)目標(biāo)的運(yùn)動(dòng)軌跡與自車坐標(biāo)系y 軸的交點(diǎn)縱坐標(biāo)大小，對各個(gè)工況下的行車安全程度進(jìn)行3 個(gè)安全等級劃分。

一級為正常行駛級別，如圖1 中的軌跡L1 和L4。目標(biāo)軌跡經(jīng)過安全區(qū)處于正常行駛級別，此時(shí)系統(tǒng)仍然對目標(biāo)實(shí)時(shí)檢測，但無預(yù)警無制動(dòng)。

二級為預(yù)警級別，如圖1 中的軌跡L2。目標(biāo)軌跡經(jīng)過預(yù)警區(qū)處于碰撞預(yù)警級別，此時(shí)若駕駛員未意識到危險(xiǎn)而采取加速措施極有可能發(fā)生碰撞，故此時(shí)應(yīng)發(fā)出報(bào)警信號，提醒駕駛員減速行駛。

三級為緊急制動(dòng)級別，如圖1 中的軌跡L3。目標(biāo)軌跡經(jīng)過緊急制動(dòng)區(qū)處于緊急制動(dòng)級別，此時(shí)盲目加速或者不采取措施都有極大可能發(fā)生碰撞事故，AEB 系統(tǒng)應(yīng)實(shí)施自動(dòng)緊急制動(dòng)，保障車內(nèi)外人員安全。

3 自動(dòng)緊急制動(dòng)控制系統(tǒng)設(shè)計(jì)

3.1 AEB 系統(tǒng)工作流程

本文采用上層模糊下層PID 控制的方式對AEB 系統(tǒng)硬件進(jìn)行控制。圖2 為AEB 系統(tǒng)工作流程，車載傳感器實(shí)時(shí)檢測前方目標(biāo)信息，通過卡爾曼濾波預(yù)測目標(biāo)軌跡并將結(jié)果輸入風(fēng)險(xiǎn)評估模型中，得到當(dāng)前的報(bào)警及制動(dòng)閾值，將此結(jié)果輸入到模糊控制器得到當(dāng)前期望的減速度，通過底層PID 控制器控制制動(dòng)管路油壓，實(shí)現(xiàn)對AEB 系統(tǒng)的硬件控制。

Fig.2 System workflow圖2 系統(tǒng)工作流程

3.2 AEB 上層模糊控制器設(shè)計(jì)

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

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

Fig.3 YL membership function圖3 YL 隸屬度函數(shù)

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

Fig.4 Vz membership function圖4 Vz 隸屬度函數(shù)

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

Fig.5 T membership function圖5 T 隸屬度函數(shù)

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

Fig.6 a membership function圖6 a 隸屬度函數(shù)

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

Fig.7 Signal membership function圖7 Signal 隸屬度函數(shù)

根據(jù)隸屬度函數(shù)分析和仿真研究，設(shè)計(jì)部分模糊控制器規(guī)則如表1 所示。

Table 1 Fuzzy control rules表1 模糊控制規(guī)則

3.3 AEB 下層PID 制動(dòng)控制系統(tǒng)設(shè)計(jì)

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

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

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

4 系統(tǒng)仿真及結(jié)果分析

4.1 目標(biāo)運(yùn)動(dòng)軌跡預(yù)測仿真

本文選取網(wǎng)絡(luò)上的一組數(shù)據(jù)集對卡爾曼濾波軌跡預(yù)測算法進(jìn)行驗(yàn)證，數(shù)據(jù)集包括目標(biāo)的橫向位置、縱向位置以及加速度等信息，實(shí)驗(yàn)軟件為MATLAB，通過編寫MATLAB 程序?qū)λ惴ㄟM(jìn)行驗(yàn)證。實(shí)驗(yàn)結(jié)果如圖8 所示（彩圖掃描OSID 碼可見）。

Fig.8 Track prediction results圖8 軌跡預(yù)測結(jié)果

從圖8 可以看出，本文基于卡爾曼濾波的軌跡預(yù)測算法能夠?qū)崿F(xiàn)對目標(biāo)運(yùn)動(dòng)軌跡的預(yù)測，預(yù)測結(jié)果與實(shí)際運(yùn)動(dòng)趨勢基本一致，可以作為自動(dòng)緊急制動(dòng)系統(tǒng)控制的依據(jù)。

4.2 系統(tǒng)避撞策略仿真

系統(tǒng)運(yùn)行時(shí)，通過車載雷達(dá)等傳感器對目標(biāo)進(jìn)行檢測，然后通過軌跡預(yù)測程序?qū)δ繕?biāo)的運(yùn)動(dòng)軌跡進(jìn)行預(yù)測，結(jié)合當(dāng)前車速和目標(biāo)軌跡與自車坐標(biāo)系縱軸的交點(diǎn)縱坐標(biāo)，利用模糊控制器判斷自車的安全狀態(tài)，確定危險(xiǎn)等級，得到期望減速度；然后通過PID 控制系統(tǒng)控制自車的制動(dòng)壓力，進(jìn)而實(shí)現(xiàn)系統(tǒng)的避撞控制。本文根據(jù)系統(tǒng)工作流程建立CarSim 和Simulink 聯(lián)合仿真模型，如圖9 所示。

Fig.9 Co-simulation model圖9 聯(lián)合仿真模型

聯(lián)合仿真模型包含軌跡預(yù)測模型、模糊控制模型、整車模型以及PID 控制模型。其中整車模型根據(jù)某車輛的真實(shí)數(shù)據(jù)，通過CarSim 軟件建立制動(dòng)系統(tǒng)動(dòng)力學(xué)模型等等。軌跡預(yù)測模型主要根據(jù)車輛的距離、加速度等信息預(yù)測目標(biāo)的運(yùn)動(dòng)軌跡，得到軌跡與自車坐標(biāo)系縱軸的交點(diǎn)縱坐標(biāo)YL。模糊控制器根據(jù)YL、當(dāng)前車速以及剩余碰撞時(shí)間，得到期望減速度和報(bào)警信號，通過PID 控制完成車身制動(dòng)。

4.3 仿真結(jié)果分析

本文取車速為60km/h 對系統(tǒng)進(jìn)行仿真分析，通過Car-Sim 軟件建立汽車模型，AEB 系統(tǒng)收集本車及前車的信息（速度、加速度），由軌跡預(yù)測模型預(yù)測目標(biāo)軌跡，通過模糊控制器進(jìn)行風(fēng)險(xiǎn)評估并輸出期望減速度與報(bào)警信號，從而通過PID 進(jìn)行制動(dòng)控制。仿真結(jié)果如圖10 所示。

Fig.10 Simulation results圖10 仿真結(jié)果

從圖10 可以看出，汽車初始速度為60km/h，此時(shí)系統(tǒng)檢測到某目標(biāo)會在6s 后經(jīng)過汽車前方約1m 的位置，即YL 約為1?？紤]到當(dāng)前車速較快，若不采取措施極有可能發(fā)生事故，故此時(shí)系統(tǒng)采取緊急制動(dòng)措施。初始時(shí)減速度較小，2s 后系統(tǒng)檢測到Y(jié)L 約為3，汽車仍處于危險(xiǎn)狀態(tài)，故系統(tǒng)采取進(jìn)一步制動(dòng)措施。3.5s 左右時(shí)可以看出YL 為8，此時(shí)的距離足夠駕駛員發(fā)現(xiàn)目標(biāo)并采取相應(yīng)的措施，故此時(shí)僅報(bào)警并不采取制動(dòng)措施。6s 后，剩余碰撞時(shí)間為-1，說明此時(shí)目標(biāo)已經(jīng)通過了汽車前方，危險(xiǎn)狀態(tài)解除，報(bào)警信號歸0，汽車勻速行駛。

從仿真結(jié)果可以看出，系統(tǒng)的報(bào)警信號以及制動(dòng)措施均與設(shè)想相符合，報(bào)警信號與汽車安全狀態(tài)保持同步，充分考慮了目標(biāo)運(yùn)動(dòng)軌跡對汽車安全狀態(tài)的影響，制動(dòng)效果良好。

5 結(jié)語

本文結(jié)合卡爾曼濾波提出一種基于目標(biāo)軌跡的自動(dòng)緊急制動(dòng)避撞策略。通過CarSim/Simulink 聯(lián)合仿真，結(jié)果表明在復(fù)雜交通環(huán)境下，系統(tǒng)能準(zhǔn)確預(yù)測目標(biāo)的運(yùn)動(dòng)軌跡并給出合理的減速度，實(shí)現(xiàn)自動(dòng)緊急制動(dòng)，解決了現(xiàn)有模型無法對目標(biāo)進(jìn)行有效判斷的局限性。本文所設(shè)計(jì)的自動(dòng)緊急制動(dòng)避撞策略可以滿足行車安全需求，對AEB 系統(tǒng)開發(fā)設(shè)計(jì)有一定的參考意義。