本發(fā)明屬于自動(dòng)化與計(jì)算機(jī)，特別多車廂鏈?zhǔn)杰囕v的防碰撞，具體涉及一種多車廂鏈?zhǔn)杰囕v的防撞預(yù)警系統(tǒng)及方法。

背景技術(shù)：

1、在現(xiàn)代交通系統(tǒng)中，多車廂鏈?zhǔn)杰囕v廣泛應(yīng)用于公共交通和物流運(yùn)輸中。這類車輛由于其結(jié)構(gòu)特點(diǎn)，易受到多種因素的影響，導(dǎo)致碰撞事故的發(fā)生，因此設(shè)計(jì)一種針對(duì)多車廂鏈?zhǔn)杰囕v的防撞預(yù)警系統(tǒng)具有重要的應(yīng)用價(jià)值。

2、中國(guó)專利申請(qǐng)：一種鉸鏈?zhǔn)杰囕v自動(dòng)駕駛碰撞預(yù)測(cè)方法(申請(qǐng)?zhí)枺篶n202211437902.2)公開(kāi)了一種鉸鏈?zhǔn)杰囕v的防撞預(yù)測(cè)方法，通過(guò)建立柵格地圖將車輛和障礙物投影到柵格地圖，通過(guò)運(yùn)動(dòng)學(xué)模型計(jì)算未來(lái)時(shí)刻的車輛位姿信息在柵格地圖的投影，并判斷是否會(huì)和障礙物的投影重合；該方案只考慮了預(yù)測(cè)車輛的位姿信息，車輛對(duì)傳感器誤差的容忍度低。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明旨在解決以上現(xiàn)有技術(shù)的問(wèn)題。提出了一種方法。本發(fā)明的技術(shù)方案如下：

2、一種多車廂的鏈?zhǔn)杰囕v防撞預(yù)警系統(tǒng)，部署于車頭和車廂，其車頭部分包含車頭數(shù)據(jù)采集模塊、車頭通信模塊、地圖模塊、位姿估計(jì)模塊與態(tài)勢(shì)估計(jì)模塊、碰撞預(yù)警模塊，車廂部分包含車廂數(shù)據(jù)采集模塊和車廂通信模塊，其中：

3、所述車頭數(shù)據(jù)采集模塊用于獲取鏈?zhǔn)杰囕v車頭位姿信息，并采集周圍環(huán)境目標(biāo)數(shù)據(jù)；車廂數(shù)據(jù)采集模塊用于獲取車廂的慣性測(cè)量單元imu的數(shù)據(jù)；

4、所述車頭通信模塊用于鏈?zhǔn)杰囕v車頭接收車廂imu數(shù)據(jù)；車廂通信模塊用于給鏈?zhǔn)杰囕v車頭發(fā)送車廂imu數(shù)據(jù)；

5、所述地圖模塊用于讀取已知環(huán)境地圖，并獲取環(huán)境地圖中的已知環(huán)境目標(biāo)信息；

6、所述位姿估計(jì)模塊用于通過(guò)車輛運(yùn)動(dòng)學(xué)模型計(jì)算車頭及各車廂的當(dāng)前位姿信息，并預(yù)測(cè)未來(lái)若干時(shí)刻的車輛位姿數(shù)據(jù)；

7、所述態(tài)勢(shì)估計(jì)模塊用于分別對(duì)環(huán)境目標(biāo)和鏈?zhǔn)杰囕v構(gòu)建對(duì)應(yīng)的態(tài)勢(shì)場(chǎng)，并且根據(jù)車輛位姿估計(jì)信息對(duì)鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)預(yù)測(cè)未來(lái)若干時(shí)刻的態(tài)勢(shì)場(chǎng)，根據(jù)環(huán)境目標(biāo)的運(yùn)動(dòng)狀態(tài)估計(jì)對(duì)目標(biāo)態(tài)勢(shì)場(chǎng)進(jìn)行預(yù)測(cè)，將預(yù)測(cè)得到的態(tài)勢(shì)場(chǎng)分別進(jìn)行融合得到對(duì)應(yīng)的總態(tài)勢(shì)場(chǎng)；

8、所述碰撞預(yù)警模塊用于鏈?zhǔn)杰囕v和環(huán)境目標(biāo)對(duì)應(yīng)的總態(tài)勢(shì)場(chǎng)進(jìn)行離散化、二值化得到對(duì)應(yīng)的局部柵格地圖，最后將鏈?zhǔn)杰囕v、環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)局部的柵格地圖坐標(biāo)都轉(zhuǎn)換到全局柵格地圖坐標(biāo)系下計(jì)算交集，判斷車輛是否存在碰撞風(fēng)險(xiǎn)，并在有碰撞風(fēng)險(xiǎn)時(shí)輸出碰撞預(yù)警信息。

9、一種基于所述系統(tǒng)的多車廂的鏈?zhǔn)杰囕v防撞預(yù)警方法，其包括以下步驟：

10、2.1數(shù)據(jù)采集：

11、車頭數(shù)據(jù)采集：車頭數(shù)據(jù)采集模塊0獲取車頭位姿信息并實(shí)時(shí)獲取周圍的環(huán)境信息，獲取環(huán)境目標(biāo)信息目標(biāo)信息的速度、位置坐標(biāo)用五元組表示其中表示k時(shí)刻目標(biāo)的x，y坐標(biāo)，表示k時(shí)刻目標(biāo)x，y的線速度，表示k時(shí)刻目標(biāo)的方向角。

12、車廂數(shù)據(jù)采集：車廂i的數(shù)據(jù)采集模塊采集獲取每個(gè)車廂的imu數(shù)據(jù)，其中i＝1,2,…,n，表示第i節(jié)車廂，車頭編號(hào)為0；

13、2.2通信：車廂i的通信模塊向車頭的通信模塊發(fā)送車廂的imu的數(shù)據(jù)信息，車頭通信模塊接收；

14、2.3地圖導(dǎo)入：地圖模塊導(dǎo)入已知環(huán)境地圖，獲取環(huán)境地圖目標(biāo)信息；

15、2.4車輛位姿估計(jì)：基于鏈?zhǔn)杰囕v的運(yùn)動(dòng)學(xué)模型，并根據(jù)車頭傳感器獲取位姿信息和車廂的imu數(shù)據(jù)信息，估計(jì)鏈?zhǔn)杰囕v當(dāng)前時(shí)刻k車頭及各車廂位姿信息，并預(yù)測(cè)未來(lái)時(shí)刻鏈?zhǔn)杰囕v的位姿；

16、2.5態(tài)勢(shì)估計(jì)：根據(jù)鏈?zhǔn)杰囕v的位姿信息，分別構(gòu)建鏈?zhǔn)杰囕v車頭和車廂的態(tài)勢(shì)場(chǎng)，并根據(jù)鏈?zhǔn)杰囕v的位姿估計(jì)預(yù)測(cè)車輛各節(jié)車廂未來(lái)時(shí)刻τ的態(tài)勢(shì)場(chǎng)，再將各節(jié)車廂態(tài)勢(shì)場(chǎng)融合整體鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)。根據(jù)已知環(huán)境目標(biāo)信息和目標(biāo)檢測(cè)信息構(gòu)建環(huán)境中的靜止、固定和動(dòng)態(tài)目標(biāo)態(tài)勢(shì)場(chǎng)，對(duì)目標(biāo)的運(yùn)動(dòng)估計(jì)預(yù)測(cè)未來(lái)時(shí)刻τ的環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)，將所有目標(biāo)態(tài)勢(shì)場(chǎng)融合得到整體目標(biāo)態(tài)勢(shì)場(chǎng)；

17、2.6碰撞預(yù)警：設(shè)置鏈?zhǔn)杰囕v碰撞安全閾值，將車輛態(tài)勢(shì)場(chǎng)和環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)離散化，并通過(guò)安全閾值將其二值化，得到對(duì)應(yīng)的局部柵格地圖，最后將鏈?zhǔn)杰囕v、環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)局部柵格地圖坐標(biāo)都轉(zhuǎn)換到全局柵格地圖坐標(biāo)系下計(jì)算交集，并通過(guò)碰撞預(yù)警函數(shù)falert來(lái)計(jì)算車輛是否會(huì)發(fā)生碰撞，如果會(huì)發(fā)生碰撞則輸出碰撞預(yù)警信息。

18、進(jìn)一步的，所述步驟2.4的位姿估計(jì)方法，具體包括以下步驟：

19、3.1車輛位姿計(jì)算：根據(jù)鏈?zhǔn)杰囕v的運(yùn)動(dòng)學(xué)模型，車廂i+1與車廂i存在如下位姿關(guān)系；

20、

21、其中，i＝0,1,2…；l0是車頭的軸距；li表示車廂i的軸距；表示k時(shí)刻車廂i與車廂i+1之間的轉(zhuǎn)彎折角，是車廂i在k時(shí)刻的朝向角；

22、根據(jù)傳感器獲取k時(shí)刻鏈?zhǔn)杰囕v車頭位姿信息表示車輛車頭k時(shí)刻的位置坐標(biāo)；表示車頭k時(shí)刻的速度；表示車頭k時(shí)刻的朝向角。根據(jù)公式(1)依次計(jì)算各節(jié)車廂的位姿。

23、3.2車輛位姿預(yù)測(cè)：

24、時(shí)刻k+τ車頭位姿：根據(jù)k時(shí)刻獲取的車頭位姿信息預(yù)測(cè)未來(lái)τ時(shí)刻位姿

25、

26、其中，δ是車頭前輪轉(zhuǎn)角；

27、時(shí)刻k+τ車廂位姿：根據(jù)所述鏈?zhǔn)杰囕v的車體參數(shù)和鉸鏈折角已知k+τ車頭的位姿信息，通過(guò)運(yùn)動(dòng)學(xué)模型預(yù)測(cè)車廂i的位姿信息，車廂的位姿信息預(yù)測(cè)同公式(1)。

28、進(jìn)一步的，所述步驟2.5的態(tài)勢(shì)估計(jì)方法，具體包括以下步驟：

29、4.1構(gòu)造環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)：

30、構(gòu)造靜態(tài)目標(biāo)態(tài)勢(shì)場(chǎng)：分別根據(jù)目標(biāo)位置和目標(biāo)特征，構(gòu)建對(duì)應(yīng)的靜態(tài)態(tài)勢(shì)場(chǎng)

31、

32、其中，kobstp表示目標(biāo)特征的比例因子；pobstp表示目標(biāo)特征大??；表示目標(biāo)的位置。

33、并且有：

34、

35、構(gòu)造固定目標(biāo)態(tài)勢(shì)場(chǎng)：同靜態(tài)目標(biāo)；

36、構(gòu)造動(dòng)態(tài)目標(biāo)態(tài)勢(shì)場(chǎng)：根據(jù)目標(biāo)位置和目標(biāo)的運(yùn)動(dòng)學(xué)特征，構(gòu)建動(dòng)態(tài)目標(biāo)勢(shì)場(chǎng)動(dòng)態(tài)目標(biāo)態(tài)勢(shì)場(chǎng)函數(shù)：

37、

38、其中,kobs表示目標(biāo)速度的比例因子；m表示補(bǔ)償系數(shù)，用于補(bǔ)償由目標(biāo)類型引起的勢(shì)場(chǎng)變化；表示目標(biāo)位置指向車輛位置的向量；目標(biāo)速度為記與的夾角為是單位向量；

39、根據(jù)目標(biāo)當(dāng)前狀態(tài)估計(jì)利用卡爾曼濾波器預(yù)測(cè)未來(lái)k+τ時(shí)刻的位置預(yù)測(cè)，構(gòu)建未來(lái)時(shí)刻τ的動(dòng)態(tài)目標(biāo)勢(shì)場(chǎng)

40、

41、其中，表示車輛在未來(lái)τ時(shí)刻的速度。

42、4.2構(gòu)造鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)：

43、車頭態(tài)勢(shì)場(chǎng)構(gòu)建：鏈?zhǔn)杰囕v根據(jù)自身位置和運(yùn)動(dòng)學(xué)特征，則鏈?zhǔn)杰囕v車頭的動(dòng)態(tài)態(tài)勢(shì)場(chǎng)可表示為：

44、

45、其中，kcar表示車輛速度的比例因子；

46、根據(jù)鏈?zhǔn)杰囕v位姿預(yù)測(cè)計(jì)算車頭k+τ時(shí)刻的態(tài)勢(shì)場(chǎng)：

47、

48、車廂態(tài)勢(shì)場(chǎng)構(gòu)建：車廂相對(duì)于前車廂運(yùn)動(dòng)是被鉸鏈所束縛的，鏈?zhǔn)杰囕v車廂相對(duì)于車頭基本處于靜止?fàn)顟B(tài)，構(gòu)建車廂i的靜態(tài)態(tài)勢(shì)場(chǎng)：

49、

50、其中，i＝1,2…；kcartp表示車廂特征的比例因子；pcartp表示車廂特征大小。

51、并且有：

52、

53、4.3態(tài)勢(shì)場(chǎng)融合：

54、鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)：鏈?zhǔn)杰囕v對(duì)車頭和各車廂分別構(gòu)建態(tài)勢(shì)場(chǎng)，再將各節(jié)車廂態(tài)勢(shì)場(chǎng)融合成整體車輛的態(tài)勢(shì)場(chǎng)

55、

56、其中n為鏈?zhǔn)杰囕v車廂總數(shù)；

57、環(huán)境態(tài)勢(shì)場(chǎng)：環(huán)境中的靜態(tài)、固定和運(yùn)動(dòng)目標(biāo)的態(tài)勢(shì)場(chǎng)融合得到整體目標(biāo)態(tài)勢(shì)場(chǎng)

58、

59、進(jìn)一步的，所述步驟2.6碰撞預(yù)警方法，其步驟如下：

60、5.1態(tài)勢(shì)場(chǎng)離散化：將鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)和環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)分別投影到多值局部柵格地圖；鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)投影生成的局部柵格地圖為gridveh,local＝{grid(i,j)∣-m≤i≤m,-n≤j≤n}表示；環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)投影生成的局部柵格地圖為gridobs,local＝{grid(i,j)∣-m≤i≤m,-n≤j≤n}；

61、5.2態(tài)勢(shì)場(chǎng)二值化：設(shè)置安全閾值s0，通過(guò)安全閾值將鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)和環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)的柵格地圖進(jìn)行二值化：

62、

63、5.3碰撞預(yù)警：設(shè)計(jì)碰撞預(yù)警函數(shù)falert，計(jì)算方法為：將鏈?zhǔn)杰囕v自身、環(huán)境目標(biāo)態(tài)勢(shì)場(chǎng)局部柵格地圖坐標(biāo)都轉(zhuǎn)換到全局柵格地圖坐標(biāo)系下，在全局坐標(biāo)系下兩個(gè)柵格地圖gridveh,global與gridobs,global的交集，計(jì)算兩張地圖柵格坐標(biāo)值與的結(jié)果并累加；

64、碰撞預(yù)警函數(shù)falert：

65、

66、如果falert≥1，則認(rèn)為兩個(gè)地圖的交集不為空集，車輛行駛可能會(huì)發(fā)生碰撞，并輸出碰撞預(yù)警信息，否則falert＝0，兩個(gè)柵格地圖的交集為空集，無(wú)碰撞危險(xiǎn)。

67、本發(fā)明的優(yōu)點(diǎn)及有益效果如下：

68、本發(fā)明提出一種多車廂鏈?zhǔn)杰囕v的防撞預(yù)警系統(tǒng)及方法，通過(guò)對(duì)各車廂位姿估計(jì)與預(yù)測(cè)，并結(jié)合鏈?zhǔn)杰囕v和環(huán)境目標(biāo)的態(tài)勢(shì)估計(jì)與預(yù)測(cè)，再分別將鏈?zhǔn)杰囕v和環(huán)境目標(biāo)的態(tài)勢(shì)場(chǎng)融合得到對(duì)應(yīng)總的態(tài)勢(shì)場(chǎng)，將鏈?zhǔn)杰囕v態(tài)勢(shì)場(chǎng)和環(huán)境目標(biāo)的態(tài)勢(shì)場(chǎng)離散化、二值化，計(jì)算車輛各車廂和環(huán)境目標(biāo)在柵格地圖全局坐標(biāo)系下交集，根據(jù)交集結(jié)果判斷是否存在碰撞風(fēng)險(xiǎn)，若有碰撞風(fēng)險(xiǎn)則進(jìn)行告警，從而降低事故風(fēng)險(xiǎn)。該系統(tǒng)能顯著提高多車廂鏈?zhǔn)杰囕v在復(fù)雜環(huán)境下的行駛的安全性，系統(tǒng)中增加了前向預(yù)測(cè)的方法，能夠提前規(guī)避風(fēng)險(xiǎn)，降低了車輛的操控難度，可以提升鏈?zhǔn)杰囕v運(yùn)輸效率。