基于三次B樣條曲線擬合的主車軌跡預(yù)測算法的研究

鄧琬云、曲延羽、楊子鈺、林智桂、廖尉華

（上汽通用五菱汽車股份有限公司，柳州 545007）

0 引言

自適應(yīng)巡航控制功能（Adaptive Cruise Control，簡稱ACC）是利用攝像頭、雷達(dá)等傳感器識別周圍環(huán)境，從前方眾多目標(biāo)中選擇一個有效目標(biāo)，作為本車需要跟隨的目標(biāo)。然后系統(tǒng)通過控制驅(qū)動和制動系統(tǒng)調(diào)整車速，使本車和有效目標(biāo)之間保持一定的安全距離，從而達(dá)到提升車輛主動安全能力和舒適性的目的[1]。

ACC 功能的系統(tǒng)框架如圖1所示，其中,選擇有效目標(biāo)模塊、速度控制模塊及距離控制模塊皆由選擇有效目標(biāo)模塊決定。當(dāng)選擇有效目標(biāo)模塊檢測到前方探測范圍內(nèi)不存在有效目標(biāo)時，則按照駕駛員設(shè)定的巡航速度進(jìn)行定速控制；當(dāng)選擇有效目標(biāo)模塊檢測到前方存在有效目標(biāo)時，則按照駕駛員設(shè)定的安全距離進(jìn)行跟隨控制。通常選擇有效目標(biāo)的策略是預(yù)測主車行駛軌跡，在預(yù)測軌跡上選擇較近目標(biāo)作為前方有效目標(biāo)。所以，主車軌跡預(yù)測算法是ACC 功能中識別前方有效目標(biāo)的重要環(huán)節(jié)。

圖1 ACC 功能的系統(tǒng)框架

1 主車軌跡預(yù)測算法介紹

當(dāng)前常用的主車軌跡預(yù)測算法包括定曲率識別算法[2]、基于前方目標(biāo)擬合車道線算法[3-4]和基于前方目標(biāo)和主車軌跡分兩段預(yù)測主車行駛軌跡算法[5]等。定曲率識別算法計算簡單，它將當(dāng)前車頭所指方向作為主車預(yù)期行駛軌跡，此方法導(dǎo)致主車行駛在彎道時會存在嚴(yán)重的誤差?；谇胺侥繕?biāo)擬合車道線算法是將前方不同目標(biāo)的運動軌跡進(jìn)行融合得到主車預(yù)期行駛軌跡，但在交叉路口、車輛換道等復(fù)雜工況下，不能體現(xiàn)出很好的適應(yīng)性。用兩段回旋線預(yù)測主車行駛軌跡的算法是將主車預(yù)期軌跡分為兩段，近端由主車行駛軌跡的歷史狀態(tài)擬合得出，遠(yuǎn)端由前方車輛行駛軌跡的歷史狀態(tài)擬合得出，該方法的預(yù)測準(zhǔn)確性較高，但由于要充分保證分段點處的曲率一致性，分段點較難選取。

上述算法普遍使用主車行駛軌跡和目標(biāo)軌跡對主車預(yù)期行駛軌跡進(jìn)行擬合，但忽略了在行駛過程中車道信息對預(yù)期軌跡的影響。因此，本文基于分兩段預(yù)測主車行駛軌跡的算法，提出了采用三次B 樣條曲線對主車預(yù)期行駛軌跡進(jìn)行擬合的方法，以達(dá)到提高運算效率、提升ACC 性能的目的。

2 基于曲線擬合的主車軌跡預(yù)測算法

本文提出的主車軌跡預(yù)測算法模型架構(gòu)如圖2所示。將攝像頭、雷達(dá)和底盤讀取的信號分別用于計算車道中心線、識別前方目標(biāo)運動軌跡及擬合主車行駛軌跡，在軌跡離散模塊中將計算出的路徑和軌跡進(jìn)行離散化處理，在軌跡融合模塊中引入距離權(quán)重的概念對3 條軌跡線進(jìn)行融合處理，最后用三次B 樣條曲線擬合主車預(yù)期行駛軌跡。

圖2 主車軌跡預(yù)測算法模型架構(gòu)

圖3描述了軌跡融合模塊中進(jìn)行融合操作的3 種情況，即根據(jù)軌跡和路徑的有效性，選擇有效的軌跡或路徑進(jìn)行融合。為了保證主車行駛軌跡和本車道或者前方目標(biāo)運動軌跡的方向一致，需先判斷所選擇的有效軌跡或路徑是否皆同號，如果同號則對軌跡進(jìn)行融合，否則不進(jìn)行融合。為了保證預(yù)期行駛軌跡可以包含所有潛在有效目標(biāo)，預(yù)期行駛軌跡的長度應(yīng)為主車到有效目標(biāo)的縱向距離。但在有效目標(biāo)不存在的情況下，選擇用車道中心線的長度作為預(yù)期行駛軌跡的長度，保證預(yù)期行駛軌跡參數(shù)的完整性。

在融合過程中，將同一時間戳上依據(jù)圖3描述的3 種融合情況，選取軌跡線的離散點作為融合對象。使用2 條有效軌跡進(jìn)行融合的公式如下：

圖3 軌跡融合的三種情況

式中 k——步長

W——權(quán)重，權(quán)重的大小為預(yù)期軌跡長度的倒數(shù)

（x1，y1）、（x2，y2）——同一時間戳上離散點的坐標(biāo)

（x，y）——融合點的坐標(biāo)

當(dāng)使用主車行駛軌跡、前方目標(biāo)運動軌跡和車道中心線三者進(jìn)行融合時，應(yīng)優(yōu)先保證主車在本車道上行駛，故使用車道中心線和主車行駛軌跡的離散點代入式（1）進(jìn)行計算。然后將融合之后的點（x，y）作為（x1，y1），將前方目標(biāo)運動軌跡的點（x2，y2）代入式（2）進(jìn)行計算，即可得到最終規(guī)劃軌跡上的點（x，y）。

對于規(guī)劃軌跡上的點（x，y）需要使用曲線進(jìn)行擬合。三次B 樣條曲線是一種參數(shù)曲線，它由多段貝塞爾曲線組成，并在交點處具有連續(xù)性，擁有精度高、數(shù)值穩(wěn)定性好等優(yōu)點，常常作為數(shù)據(jù)平滑擬合的方法應(yīng)用于路徑規(guī)劃中。故本文選擇使用三次B樣條曲線對主車預(yù)期行駛軌跡進(jìn)行擬合，可得到曲率連續(xù)、滿足車輛行駛要求的主車預(yù)期行駛軌跡。

3 MATLAB 算法仿真分析

在Matlab 軟件的Simulink 環(huán)境下，基于三次B 樣條曲線對主車預(yù)期行駛軌跡進(jìn)行仿真研究。主車的預(yù)瞄距離應(yīng)與車速和參考軌跡的形狀特征有關(guān)，所以要依據(jù)駕駛員駕駛行為統(tǒng)計數(shù)據(jù)進(jìn)行分析歸納，形成駕駛行為預(yù)瞄距離統(tǒng)計表。在該方法的實際應(yīng)用中，使用插值法對預(yù)瞄距離進(jìn)行計算。

3.1 場景一：主車前方無目標(biāo)車輛且存在車道線時

場景一模擬的是藍(lán)車行駛在車道線清晰且前方?jīng)]有目標(biāo)車輛的情況（圖4a）。從圖4b 中可以看到，擬合的軌跡從主車當(dāng)前位置出發(fā)，遠(yuǎn)端靠近車道中心線，滿足一般駕駛員將遠(yuǎn)方車道中心作為預(yù)瞄點的駕駛習(xí)慣。圖4c 得到主車預(yù)期行駛軌跡的曲率與車道中心線的曲率偏差較小，故擬合的軌跡形狀特征與車道中心線一致。從圖4d 可知，主車與車道中心線的橫向偏差逐漸減小，說明主車逐漸靠近車道中心。

圖4 使用車道中心線和主車歷史軌跡預(yù)測主車預(yù)期行駛軌跡

3.2 場景二：主車前方有目標(biāo)車輛且不存在車道線時

場景二模擬的是藍(lán)車行駛在不存在車道線，且前方綠車為目標(biāo)車輛的情況（圖5a）。從圖5b 中可以看出，擬合的軌跡從主車當(dāng)前位置出發(fā)，遠(yuǎn)端靠近前方目標(biāo)，滿足一般駕駛員的駕駛習(xí)慣。圖5c 得到主車預(yù)期行駛軌跡的曲率與前方目標(biāo)運動軌跡的曲率偏差較小，故擬合的軌跡形狀特征與前方目標(biāo)運動軌跡一致。從圖5d 可知，主車與前方目標(biāo)運動軌跡的橫向偏差逐漸減小，說明主車逐漸靠近前方目標(biāo)車輛。

圖5 使用前方目標(biāo)運動軌跡和主車歷史軌跡預(yù)測主車預(yù)期行駛軌跡

3.3 仿真場景三：主車前方有目標(biāo)車輛且存在車道線時

場景三模擬的是藍(lán)車行駛在車道線清晰，且前方綠車為目標(biāo)車輛的情況（圖6a）。從圖6b 中可以看出擬合的軌跡從主車當(dāng)前位置出發(fā)，將前方目標(biāo)運動軌跡作為參考軌跡，遠(yuǎn)端靠近車道中心線，滿足一般駕駛員的駕駛習(xí)慣。圖6c 得到主車預(yù)期行駛軌跡的曲率與車道中心線及前方目標(biāo)運動軌跡曲率的變化趨勢相近，故擬合的軌跡形狀特征與已知軌跡一致。圖6d 可知，主車與參考軌跡橫向偏差的最大值小于國家標(biāo)準(zhǔn)車道寬度3 m，故主車仍在本車道上行駛，滿足行駛軌跡的安全性的要求。

圖6 使用車道中心線、前方目標(biāo)運動軌跡和主車歷史軌跡預(yù)測主車預(yù)期行駛軌跡

3.4 總結(jié)與分析

上述仿真實驗結(jié)果表明，本文提出的方法對于僅存在車道線或前方目標(biāo)的場景，仍可使用三次B 樣條曲線對主車預(yù)期的行駛軌跡進(jìn)行預(yù)測。預(yù)測的行駛軌跡曲率變化平滑，且其最大值小于車道中心線或前方目標(biāo)運動軌跡的最大曲率值，主車預(yù)期行駛軌跡與參考軌跡的橫向偏差值逐漸減小。預(yù)測軌跡的近端以主車歷史軌跡為基準(zhǔn)，遠(yuǎn)端以車道中心線或前方目標(biāo)運動軌跡為基準(zhǔn)，符合一般駕駛員的駕駛習(xí)慣。

4 結(jié)束語

針對分兩段預(yù)測主車行駛軌跡算法的局限性，本文提出了一種基于三次B 樣條曲線對主車預(yù)期行駛軌跡進(jìn)行擬合的方法，并將其在MATLAB 軟件的Simulink 中予以實現(xiàn)。通過仿真實驗證明此方法具有可行性，應(yīng)用場景廣泛，在不同情況下皆可對主車預(yù)期軌跡進(jìn)行擬合。并且，擬合出的軌跡具有良好的連續(xù)性和可控性，計算過程簡單，滿足車輛對實時性的要求。