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

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

中圖分類(lèi)號(hào)：U463.6 文獻(xiàn)標(biāo)識(shí)碼：A

0引言

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

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

1主車(chē)軌跡預(yù)測(cè)算法介紹

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

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

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

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

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

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

式中k——步長(zhǎng)

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

（x₁，y₁）、（x₂，y₂）——同一時(shí)間戳上離散點(diǎn)的坐標(biāo)

（x，v）——融合點(diǎn)的坐標(biāo)

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

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

3 MATLAB算法仿真分析

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

3.1場(chǎng)景一：主車(chē)前方無(wú)目標(biāo)車(chē)輛且存在車(chē)道線時(shí)

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

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

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

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

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

3.4總結(jié)與分析

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

4結(jié)束語(yǔ)

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

作者簡(jiǎn)介：

鄧琬云，本科，工程師，研究方向?yàn)橹悄荞{駛軟件開(kāi)發(fā)。