改進(jìn)的多項(xiàng)式曲線擬合軌跡預(yù)測(cè)算法

黃萬炎, 杜萬和,2,*, 楊淑珍,2, 俞 濤

(1. 上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院, 上海 200444;2. 上海第二工業(yè)大學(xué)智能制造與控制工程學(xué)院, 上海 201209)

0 引 言

目前,想要快速、準(zhǔn)確地預(yù)測(cè)無規(guī)律、復(fù)雜多變的運(yùn)動(dòng)目標(biāo)軌跡,仍然具有很大的挑戰(zhàn);然而這樣的軌跡預(yù)測(cè),在很多領(lǐng)域都有需要。在自動(dòng)駕駛領(lǐng)域,為了避免發(fā)生事故,需要對(duì)路上車輛、行人的運(yùn)動(dòng)軌跡進(jìn)行實(shí)時(shí)預(yù)測(cè),然而車輛的軌跡充滿不確定性,行人的軌跡主觀性很強(qiáng),比較隨意,使得預(yù)測(cè)比較困難[1-2]。在圍捕運(yùn)動(dòng)目標(biāo)時(shí),為了更快速、準(zhǔn)確地圍捕運(yùn)動(dòng)目標(biāo),需要對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行實(shí)時(shí)預(yù)測(cè),然而圍捕方無法準(zhǔn)確獲知運(yùn)動(dòng)目標(biāo)的運(yùn)動(dòng)參數(shù),而且運(yùn)動(dòng)目標(biāo)具有較強(qiáng)的逃逸能力,使得圍捕方很難完成圍捕任務(wù)[3-4]。在涂膠應(yīng)用中,目標(biāo)輪廓彎曲復(fù)雜。激光傳感器在獲取目標(biāo)輪廓時(shí),目標(biāo)輪廓容易偏離掃描區(qū)域,因此需要對(duì)目標(biāo)輪廓進(jìn)行軌跡預(yù)測(cè),使得傳感器更加準(zhǔn)確地獲取輪廓數(shù)據(jù)。對(duì)于無規(guī)律、復(fù)雜多變的軌跡預(yù)測(cè),設(shè)計(jì)合適的算法主要面臨兩個(gè)難點(diǎn):其一,軌跡不斷變化,動(dòng)態(tài)更新,很難實(shí)時(shí)、快速地預(yù)測(cè)運(yùn)動(dòng)目標(biāo)的軌跡,因此對(duì)算法的實(shí)時(shí)性要求較高;其二,軌跡無規(guī)律,彎彎曲曲,復(fù)雜多變,很難準(zhǔn)確地預(yù)測(cè)運(yùn)動(dòng)目標(biāo)的軌跡,因此對(duì)預(yù)測(cè)算法的準(zhǔn)確性要求較高。

常用的動(dòng)態(tài)軌跡預(yù)測(cè)算法主要有3類[5-6]。第1類是基于曲線擬合的軌跡預(yù)測(cè)算法,所選取的曲線通常為多項(xiàng)式曲線。傳統(tǒng)的多項(xiàng)式曲線擬合軌跡預(yù)測(cè)算法(polynomial curve fitting trajectory prediction algorithm, PCFTPA)具有原理簡(jiǎn)單、計(jì)算量小、預(yù)測(cè)速度快等特點(diǎn),在對(duì)實(shí)時(shí)性要求較高的場(chǎng)合,具有很好的優(yōu)勢(shì)。胡俊等[7]在研究圍捕機(jī)器人對(duì)移動(dòng)目標(biāo)的快速圍捕時(shí),考慮到移動(dòng)目標(biāo)運(yùn)動(dòng)軌跡復(fù)雜多變,機(jī)器人需要先通過預(yù)測(cè)算法快速地預(yù)判移動(dòng)目標(biāo)未來一小部分的運(yùn)動(dòng)軌跡,然后才能根據(jù)預(yù)測(cè)的軌跡對(duì)運(yùn)動(dòng)目標(biāo)實(shí)施圍捕這一情況,選擇曲線擬合軌跡預(yù)測(cè)算法,該算法很好地滿足了快速預(yù)測(cè)的需求。張強(qiáng)等[8]在嵌入式態(tài)勢(shì)顯示設(shè)備中,針對(duì)運(yùn)動(dòng)目標(biāo)軌跡更新延遲的問題,提出基于曲線擬合的軌跡預(yù)測(cè)算法。該算法只需將靠近預(yù)測(cè)點(diǎn)的部分采樣點(diǎn)進(jìn)行擬合,無需存儲(chǔ)大量的歷史數(shù)據(jù),使得算法計(jì)算量小、所需存儲(chǔ)空間較小,很好地解決了嵌入式態(tài)勢(shì)顯示設(shè)備對(duì)運(yùn)動(dòng)目標(biāo)軌跡更新延遲的問題。李世杰等[9]需要對(duì)高超聲速再入滑翔目標(biāo)進(jìn)行軌跡預(yù)測(cè)。考慮到運(yùn)動(dòng)目標(biāo)具有較強(qiáng)的機(jī)動(dòng)能力,其軌跡是復(fù)雜多變的,并且預(yù)測(cè)方無法準(zhǔn)確獲知運(yùn)動(dòng)目標(biāo)實(shí)際機(jī)動(dòng)模式的信息,作者采用最小二乘曲線擬合算法進(jìn)行預(yù)測(cè)。Cao等[10]充分利用多項(xiàng)式曲線擬合的快速性優(yōu)勢(shì),實(shí)時(shí)預(yù)測(cè)動(dòng)態(tài)目標(biāo)在未來一小段時(shí)間內(nèi)的運(yùn)動(dòng)軌跡,進(jìn)而實(shí)現(xiàn)對(duì)動(dòng)態(tài)目標(biāo)的圍捕。可見,PCFTPA應(yīng)用于軌跡預(yù)測(cè)的優(yōu)勢(shì)非常明顯,即計(jì)算量小、預(yù)測(cè)速度快。然而傳統(tǒng)的多項(xiàng)式曲線擬合主要是通過輸入樣本點(diǎn)求出多項(xiàng)式的系數(shù),這樣一條理論曲線對(duì)于真實(shí)、復(fù)雜多變的軌跡并不能很好地逼近,導(dǎo)致多項(xiàng)式曲線擬合對(duì)于復(fù)雜多變的軌跡預(yù)測(cè)無法取得較好的預(yù)測(cè)效果。因此,很多學(xué)者嘗試研究第2類、第3類軌跡預(yù)測(cè)算法。第2類是基于自適應(yīng)濾波器的軌跡預(yù)測(cè)算法,其優(yōu)點(diǎn)是對(duì)采樣點(diǎn)的異常情況適應(yīng)性強(qiáng)[11-14]。為了更準(zhǔn)確地預(yù)測(cè)飛機(jī)爬升的軌跡,Thipphavong等[15]提出了一種基于觀測(cè)數(shù)據(jù)動(dòng)態(tài)調(diào)整模型參數(shù)的自適應(yīng)軌跡預(yù)測(cè)算法。Han等[16]為了解決具有動(dòng)態(tài)時(shí)間尺度的浮萍飄移軌跡預(yù)測(cè)問題,設(shè)計(jì)了一種自適應(yīng)參數(shù)變時(shí)間尺度卡爾曼濾波器,可精確地預(yù)估浮萍的軌跡。第3類是基于神經(jīng)網(wǎng)絡(luò)的動(dòng)態(tài)軌跡預(yù)測(cè)算法[17-21],該算法對(duì)于非線性系統(tǒng)具有很好的映射能力。Zhang等[22]為了準(zhǔn)確地預(yù)測(cè)乒乓球在未來時(shí)刻的位置,提出基于多層神經(jīng)網(wǎng)絡(luò)的軌跡預(yù)測(cè)算法。Akabane等[23]為了使移動(dòng)機(jī)器人更準(zhǔn)確地跟隨人,提出了基于長(zhǎng)短期記憶(long short-term memory, LSTM)網(wǎng)絡(luò)的軌跡預(yù)測(cè)算法。Yang等[24]為了更合理地預(yù)測(cè)行人的軌跡,提出了一種基于圖注意和卷積LSTM的模型;該模型具有較高的預(yù)測(cè)精度,生成的軌跡更符合社會(huì)理性和物理約束。雖然第2類、第3類算法對(duì)于復(fù)雜多變的軌跡預(yù)測(cè)準(zhǔn)確率較高,但實(shí)時(shí)性較差,因此本文選擇第1類算法進(jìn)行研究。第1類軌跡預(yù)測(cè)算法研究的不足為:① 沒有對(duì)預(yù)測(cè)誤差較大的軌跡部位進(jìn)行準(zhǔn)確識(shí)別并加以處理;② 未考慮采樣點(diǎn)間隔不一對(duì)軌跡預(yù)測(cè)的負(fù)面影響;原始點(diǎn)數(shù)據(jù)采集時(shí),由于采集速度不均,導(dǎo)致點(diǎn)與點(diǎn)之間的間隔大小不一以及軌跡變化的連續(xù)性受影響,對(duì)軌跡預(yù)測(cè)的結(jié)果有負(fù)面影響;③ 忽略了采樣點(diǎn)之間時(shí)序上及空間上的關(guān)系。數(shù)據(jù)采集是有先后順序的,而且相鄰兩個(gè)采樣點(diǎn)變化并不會(huì)很大,因此相鄰兩次預(yù)測(cè)的誤差較接近。

因此,本文在PCFTPA基礎(chǔ)上,提出一種改進(jìn)的PCFTPA(improved PCFTPA, IPCFTPA)。改進(jìn)之處為:① 通常預(yù)測(cè)誤差較大的部位,軌跡的曲率、撓率值也較大。本文通過曲率、撓率閾值確定算法(curvature and torsion thresholds determination algorithm, CTTDA)獲得曲率、撓率閾值,建立曲率、撓率及誤差的對(duì)應(yīng)關(guān)系,從而將可能導(dǎo)致預(yù)測(cè)誤差較大的軌跡部位識(shí)別出來并加以處理;② 多項(xiàng)式曲線擬合軌跡預(yù)測(cè)受軌跡不規(guī)則程度的影響,另外采樣點(diǎn)之間間隔不一,因此本文提出插值滾動(dòng)預(yù)測(cè)算法(interpolation-rolling prediction algorithm, IRPA)。通過樣條曲線插值,得到采樣間隔較小且較均勻的點(diǎn),然后截取末了一小段軌跡進(jìn)行預(yù)測(cè),使得所需預(yù)測(cè)的軌跡更加規(guī)則,采樣點(diǎn)也更加均勻。在這些點(diǎn)的基礎(chǔ)上,得到少于預(yù)測(cè)目標(biāo)點(diǎn)數(shù)、較準(zhǔn)確的預(yù)測(cè)點(diǎn),接著將這些預(yù)測(cè)點(diǎn)當(dāng)做待擬合點(diǎn)再次進(jìn)行預(yù)測(cè),如此循環(huán),直到完成預(yù)測(cè)任務(wù);③ 通過雙誤差預(yù)測(cè)值更新算法(double errors predictive value update algorithm, DEPVUA)建立采樣點(diǎn)之間時(shí)序上及空間上的關(guān)系。該算法不僅將未更新時(shí)的預(yù)測(cè)值誤差進(jìn)行了考慮,還將單誤差預(yù)測(cè)值更新時(shí)的誤差也進(jìn)行了考慮,進(jìn)而建立雙誤差預(yù)測(cè)值更新機(jī)制,實(shí)現(xiàn)對(duì)預(yù)測(cè)值的糾正。

1 多項(xiàng)式曲線擬合軌跡預(yù)測(cè)模型

1.1 問題描述及軌跡表示

圖1(a)中,L為運(yùn)動(dòng)目標(biāo)的實(shí)際軌跡,L′為L(zhǎng)的未來一小段的預(yù)測(cè)軌跡。由于目標(biāo)不斷運(yùn)動(dòng),軌跡L的形狀也不斷變化,因此需要根據(jù)歷史軌跡實(shí)時(shí)、動(dòng)態(tài)地預(yù)測(cè)L′。

圖1 軌跡離散化示意圖Fig.1 Schematic diagram of trajectory discretization

在三維空間中,運(yùn)動(dòng)目標(biāo)的軌跡是一條連續(xù)的曲線,然而在獲取軌跡時(shí),只能通過一定頻率的采樣得到離散的軌跡點(diǎn)。根據(jù)Nyquist-Shannon采樣定理[25]可知,連續(xù)的信號(hào)可用一定時(shí)間間隔的采樣點(diǎn)完整地表示,當(dāng)采樣頻率大于原始信號(hào)最大頻率的2倍時(shí),可根據(jù)采樣點(diǎn)完整地恢復(fù)原始信號(hào)。因此,對(duì)于運(yùn)動(dòng)目標(biāo)軌跡預(yù)測(cè)的研究,可轉(zhuǎn)化為對(duì)軌跡離散點(diǎn)的研究(見圖1(b))。通過對(duì)軌跡離散點(diǎn)的預(yù)測(cè),得到所需要的預(yù)測(cè)軌跡L′。

軌跡L可表示為

L≈{Pi(xi,yi,zi),i=1,2,…,N}

(1)

假設(shè)這些離散的點(diǎn)均在空間參數(shù)曲線上

r(t)={x(t),y(t),z(t)},t∈[a,b]

(2)

其中,t是曲線的參數(shù)。那么,曲線r(t)的空間特征可以反映軌跡的空間特征。因此,可以通過對(duì)曲線r(t)的研究,研究軌跡L的預(yù)測(cè)。

本文選擇確定的空間參數(shù)曲線r(t)表示空間中運(yùn)動(dòng)目標(biāo)的軌跡,并對(duì)r(t)作如下限定:

(1) 曲線為連續(xù)光滑的曲線,即r(t)具有連續(xù)的一階導(dǎo)數(shù),排除曲線包含比較“尖銳”的點(diǎn);

(2) 曲線為正則曲線,即在曲線的每一點(diǎn)處,r′(t)≠0,排除了切向量變化不連續(xù)的特殊點(diǎn),比如帶尖點(diǎn)的曲線;

(3) 對(duì)r(t)的采樣頻率遠(yuǎn)大于r(t)最大頻率的2倍。

1.2 多項(xiàng)式曲線擬合算法原理

對(duì)于空間中的軌跡,可在投影面中分別進(jìn)行曲線擬合。設(shè)投影面上的采樣點(diǎn)為{(xi,yi),i=1,2,…,N},用k(k

y=akxk+ak-1xk-1+…+a1x+a0

(3)

根據(jù)采樣點(diǎn),通過最小二乘法,便可計(jì)算出多項(xiàng)式的系數(shù)。

1.3 基于多項(xiàng)式曲線擬合的空間軌跡預(yù)測(cè)模型

對(duì)于無規(guī)律、復(fù)雜多變的軌跡,未來一小段的軌跡只與較近部分的歷史軌跡有關(guān),而與較遠(yuǎn)的歷史軌跡沒有關(guān)系[8]。因此,在軌跡預(yù)測(cè)時(shí),只需對(duì)較近的軌跡點(diǎn)進(jìn)行擬合。軌跡預(yù)測(cè)模型示意圖如圖2所示,預(yù)測(cè)過程如下:

圖2 軌跡預(yù)測(cè)模型示意圖Fig.2 Schematic diagram of the trajectory prediction model

步驟 1選取N個(gè)歷史軌跡點(diǎn),作為待擬合點(diǎn)(滑動(dòng)窗口S內(nèi));

步驟 2根據(jù)待擬合點(diǎn)進(jìn)行最小二乘擬合,得到一條與軌跡相近的空間曲線;

步驟 3通過一定的方式外推得到未來的一小段軌跡L′上的軌跡點(diǎn);

步驟 4當(dāng)獲取新的采樣點(diǎn)時(shí),滑動(dòng)窗口S往前一步,回到步驟2繼續(xù)進(jìn)行預(yù)測(cè)。

圖3更直觀地展示了軌跡預(yù)測(cè)模型的動(dòng)態(tài)性。

圖3 軌跡動(dòng)態(tài)預(yù)測(cè)模型示意圖Fig.3 Schematic diagram of trajectory dynamic prediction model

1.4 評(píng)價(jià)指標(biāo)

平均位移誤差(average displacement error,ADE)和最終位移誤差(final displacement error,FDE)通常用來評(píng)估軌跡預(yù)測(cè)算法的準(zhǔn)確性[21,24,26]。ADE為平均距離誤差,對(duì)每一個(gè)預(yù)測(cè)點(diǎn)與對(duì)應(yīng)點(diǎn)之間的距離求平均值。其表達(dá)式為

(4)

FDE為最后一個(gè)預(yù)測(cè)點(diǎn)的距離誤差,主要體現(xiàn)最后一個(gè)預(yù)測(cè)點(diǎn)的偏離程度。其表達(dá)式為

(5)

2 改進(jìn)的曲線擬合軌跡預(yù)測(cè)算法

2.1 曲率與撓率閾值的確定

空間曲線的特征可以用曲率和撓率來反映,因此可充分利用曲率、撓率對(duì)曲線的不同部位進(jìn)行表征[27-29]。曲率的幾何意義為曲線的切向量對(duì)弧長(zhǎng)的旋轉(zhuǎn)速度。曲線在某一點(diǎn)的彎曲程度越大,切向量對(duì)于弧長(zhǎng)的旋轉(zhuǎn)速度就越大,即曲率也會(huì)越大。因此,某一點(diǎn)的曲率可以反映曲線在該點(diǎn)的彎曲程度。撓率的絕對(duì)值是曲線的副法向量對(duì)于弧長(zhǎng)的旋轉(zhuǎn)速度。曲線在某一點(diǎn)的扭轉(zhuǎn)程度越大,副法向量對(duì)于弧長(zhǎng)的旋轉(zhuǎn)速度就越大,因此撓率可以反映空間中曲線在某一點(diǎn)處的扭轉(zhuǎn)程度。

對(duì)于空間參數(shù)曲線r(t),其在t處的曲率為

(6)

r(t)在t處的撓率為

(7)

在軌跡預(yù)測(cè)時(shí),預(yù)測(cè)誤差與曲率、撓率值一般具有如下的關(guān)系:當(dāng)預(yù)測(cè)誤差較大時(shí),曲率或撓率值也較大,即曲線彎曲程度較嚴(yán)重的部分或扭曲程度較嚴(yán)重的部分預(yù)測(cè)準(zhǔn)確率較低;反之亦然。因此,可通過確定曲率、撓率閾值,識(shí)別軌跡預(yù)測(cè)誤差較大的部分。CTTDA算法的流程如下。

算法 1 CTTDA輸入 動(dòng)態(tài)更新的待擬合點(diǎn)集P輸出 閾值Kmax1. set errorMax; ∥用于確定局部誤差最大值

2. for each prediction3. ∥K為待擬合點(diǎn)的曲率或撓率4. K=calculate K(P); 5. K_mean=mean(K); ∥計(jì)算K的平均值6. P'=curvefitPredict(P); ∥PCFTPA7. errorMean=getMeanError(P, P'); 8. ∥找到誤差局部最大值9. if errorMean>errorMax and other conditions10. errorMax=errorMean;11. end if12. ∥確定曲率或撓率閾值13. if errorMean>errorMax and other conditions14. Kmax=K_mean;15. end if16. end for17. output Kmax

(1) 設(shè)置errorMax初始值(第1行);

(2) 實(shí)時(shí)計(jì)算待擬合點(diǎn)集的曲率或撓率平均值(第4行、第5行);

(3) 根據(jù)待擬合點(diǎn)預(yù)測(cè)未來的軌跡點(diǎn),并計(jì)算誤差(第6行、第7行);

(4) 確定局部誤差最大值(第9～11行);

(5) 當(dāng)誤差值超過errorMax,將當(dāng)前的曲率或撓率平均值作為曲率、撓率的閾值Kmax(第15～17行);

(6) 對(duì)errorMax可以先賦較小值,并保證先確定誤差局部最大值errorMax,然后才執(zhí)行Kmax的賦值。另外,局部誤差最大值和曲率、撓率閾值均只確定一次。

2.2 IRPA

2.2.1 軌跡預(yù)測(cè)與采樣間隔的關(guān)系

縮小采樣間隔可以提高軌跡預(yù)測(cè)準(zhǔn)確率。如圖4所示,當(dāng)輸入的待擬合點(diǎn)縮小采樣間隔時(shí),軌跡采樣點(diǎn)數(shù)增多,滑動(dòng)窗口內(nèi)的軌跡變短,所代表的軌跡段更有規(guī)律,此時(shí)相同階數(shù)的擬合函數(shù)更容易逼近該曲線段,預(yù)測(cè)結(jié)果也會(huì)更準(zhǔn)確。

圖4 縮小采樣間隔的軌跡預(yù)測(cè)示意圖Fig.4 Trajectory prediction schematic diagram with the reduced sampling interval

2.2.2 算法具體步驟

IRPA算法充分利用了采樣間隔對(duì)軌跡預(yù)測(cè)的影響,通過對(duì)采樣點(diǎn)進(jìn)行插值,得到更密集的點(diǎn),然后選擇靠近預(yù)測(cè)位置的部分點(diǎn)進(jìn)行預(yù)測(cè)。由于所預(yù)測(cè)的點(diǎn)準(zhǔn)確率很高,因此可將其近似當(dāng)做真實(shí)的點(diǎn),選取這些點(diǎn)繼續(xù)進(jìn)行預(yù)測(cè),就可以滾動(dòng)地預(yù)測(cè)所需的軌跡。

算法 2 IRPA輸入 待擬合點(diǎn)集P輸出 預(yù)測(cè)點(diǎn)P'(最終預(yù)測(cè)M個(gè)點(diǎn))1. set predictNumber=K; ∥單次預(yù)測(cè)點(diǎn)數(shù)2. newP=interp1(P); ∥插值得到較密的點(diǎn)集3. N=M/K; ∥M為K的倍數(shù),N為滾動(dòng)次數(shù)4. for i=1 to N5. ∥取最后N個(gè)點(diǎn)作為待擬合點(diǎn)6. P_=newP(end-N:end); 7. tempP=curvefitPredict(P_); ∥PCFTPA8. ∥將預(yù)測(cè)點(diǎn)放進(jìn)待擬合點(diǎn),進(jìn)行滾動(dòng)預(yù)測(cè)9. newP=append(tempP); 10. ∥保存每一輪的預(yù)測(cè)結(jié)果11. P'=append(tempP);12. end for13. output P'

具體步驟如下:

(1) 確定單次預(yù)測(cè)點(diǎn)數(shù)K(第1行);

(2) 通過插值,縮小采樣間隔(第2行);

(3) 對(duì)插值后的點(diǎn)集newP,取最后N個(gè)點(diǎn)作為待擬合點(diǎn)P_(第6行);

(4) 對(duì)待擬合點(diǎn)進(jìn)行擬合,得到K個(gè)預(yù)測(cè)點(diǎn)tempP(第7行);

(5) 將所預(yù)測(cè)的點(diǎn)tempP添加到newP點(diǎn)集中,構(gòu)成新的待擬合點(diǎn)集(第9行);

(6) 循環(huán)預(yù)測(cè)N次,輸出預(yù)測(cè)點(diǎn)P′(第13行)。

2.3 DEPVUA

利用預(yù)測(cè)值之間的誤差對(duì)預(yù)測(cè)值進(jìn)行更新是很有必要的[30]。圖5是相鄰兩次預(yù)測(cè)中的x、y、z方向的誤差對(duì)比;兩次預(yù)測(cè)分別預(yù)測(cè)12個(gè)點(diǎn)。由圖5可知,相鄰兩次預(yù)測(cè)的誤差有相同的規(guī)律,即隨著預(yù)測(cè)點(diǎn)數(shù)的增多,誤差值越來越大,并且在相同預(yù)測(cè)點(diǎn)位置誤差大小相近。這是由于相鄰兩次預(yù)測(cè)時(shí),滑動(dòng)窗口內(nèi)的軌跡段整體相似,用相同算法預(yù)測(cè)得到的結(jié)果也會(huì)相近。

圖5 相鄰兩個(gè)預(yù)測(cè)點(diǎn)的誤差比較Fig.5 Comparison of error of two adjacent prediction points

基于相鄰兩次預(yù)測(cè)誤差的規(guī)律,可得到如下的預(yù)測(cè)值更新策略。設(shè)

(8)

則

(9)

(10)

圖6 單步預(yù)測(cè)中雙誤差預(yù)測(cè)值更新示意圖Fig.6 Double error prediction value update schematic diagram in single-step prediction

對(duì)于k(k>1)步預(yù)測(cè),由于第i次預(yù)測(cè)時(shí),第i-1次預(yù)測(cè)中的k個(gè)誤差值只有第一個(gè)誤差可利用,因此errori-1只能取近似組合值。如圖7所示,為errori-1的取值過程(假設(shè)k取3)。

圖7 多步預(yù)測(cè)第i-1次誤差取值過程(k=3)Fig.7 Process of error value extraction in step i-1 in multi-step prediction (k=3)

具體算法流程如下:

算法 3 DEPVUA輸入 預(yù)測(cè)點(diǎn)P'i,上一次預(yù)測(cè)誤差errori-1輸出 更新的預(yù)測(cè)點(diǎn)newDoubleP'i1. for i=1,2,…2. ∥單誤差預(yù)測(cè)值更新3. if i>k4. newP'i=P'i - k1×errori-1;5. newSingleErrori=newSingleP'i-Pi;6. end if7. if i>k+18. ∥雙誤差預(yù)測(cè)值更新9. newDoubleP'i=10. P'i-k1×errori-1-k2×newSingleErrori-1; 11. end if12. end for13. outputnewDoubleP'i

2.4 預(yù)測(cè)值偏差限制

相鄰兩次預(yù)測(cè)滑動(dòng)窗口內(nèi)的數(shù)據(jù)變化并不是很大,預(yù)測(cè)值之間也不會(huì)差別太大,因此可以限制預(yù)測(cè)值之間的偏差在一定的范圍。設(shè)

(11)

(12)

式中:k1、k2為偏差的縮放系數(shù),滿足0

2.5 算法流程

本文所設(shè)計(jì)IPCFTPA算法的流程如圖8所示。

圖8 IPCFTPA流程圖Fig.8 Flowchart of IPCFTPA

(1) 數(shù)據(jù)預(yù)處理階段,對(duì)于不光滑的軌跡數(shù)據(jù)需進(jìn)行平滑處理,比如采用3次B樣條曲線擬合;

(2) 曲率、撓率閾值可設(shè)置較大的初始值,使得算法一開始先按PCFTPA進(jìn)行預(yù)測(cè);

(3) 一旦曲率、撓率閾值最終確定,CTTDA不再執(zhí)行;

(4) 當(dāng)當(dāng)前曲率、撓率值小于閾值時(shí),說明該位置誤差在可接受的范圍,選擇PCFTPA進(jìn)行預(yù)測(cè);當(dāng)曲率、撓率值大于閾值時(shí),說明該位置誤差可能較大,選擇IRPA進(jìn)行預(yù)測(cè);

(5) 對(duì)于PCFTPA及IRPA的預(yù)測(cè)值,均需通過DEPVUA進(jìn)行更新,并將更新過的預(yù)測(cè)值作為最終的預(yù)測(cè)值。

3 算法驗(yàn)證

為充分驗(yàn)證本文算法的可行性和有效性,選擇3組仿真數(shù)據(jù)及3組真實(shí)數(shù)據(jù)進(jìn)行詳細(xì)的對(duì)比分析。本文將所提算法與PCFTPA、基于卡爾曼濾波的軌跡預(yù)測(cè)算法(Kalman filtering trajectory prediction algorithm,KF)、基于反向卷積神經(jīng)網(wǎng)絡(luò)的軌跡預(yù)測(cè)算法(back propagation neural network trajectory prediction algorithm,BPNN)、LSTM作對(duì)比。另外,本文研究的算法針對(duì)空間中復(fù)雜多變、未知的軌跡,因此LSTM和BPNN沒有提前訓(xùn)練模型,而是根據(jù)部分歷史軌跡點(diǎn)實(shí)時(shí)訓(xùn)練并預(yù)測(cè)。本次實(shí)驗(yàn)所使用的仿真軟件為Matlab R2019b。

3.1 數(shù)據(jù)描述

(1) 表1為3類仿真數(shù)據(jù)的參數(shù)方程及其描述,圖9為仿真數(shù)據(jù)的三維圖。

表1 3類仿真數(shù)據(jù)及其描述Table 1 Three types of simulation data and their descriptions

圖9 仿真數(shù)據(jù)三維圖Fig.9 3D diagram of simulation data

(2) 3組真實(shí)數(shù)據(jù)分別從3組不同的三維輪廓數(shù)據(jù)源提取,數(shù)據(jù)特征類似,處理方式也類似。本文對(duì)真實(shí)數(shù)據(jù)1作詳細(xì)說明。真實(shí)數(shù)據(jù)1如圖10所示。該數(shù)據(jù)是通過三維激光傳感器掃描得到目標(biāo)輪廓的點(diǎn)云,然后提取目標(biāo)輪廓的中心線而得。軌跡數(shù)據(jù)點(diǎn)為370個(gè)。

圖10 真實(shí)三維空間軌跡Fig.10 Real 3D trajectory

由于真實(shí)數(shù)據(jù)存在一系列問題,包括局部數(shù)據(jù)不光滑、部分軌跡點(diǎn)跳躍性比較大等,因此在數(shù)據(jù)預(yù)處理階段需對(duì)其進(jìn)行樣條擬合,使軌跡相對(duì)光滑[31-33]。預(yù)處理后的軌跡如圖11所示,數(shù)據(jù)點(diǎn)為370個(gè)。

圖11 預(yù)處理后的軌跡Fig.11 Preprocessed trajectory

3.2 重要參數(shù)選取

以仿真數(shù)據(jù)1為例,探究IPCFTPA中重要參數(shù)(多項(xiàng)式擬合系數(shù)、插值間隙、粗調(diào)系數(shù)和微調(diào)系數(shù))對(duì)軌跡預(yù)測(cè)的影響。

圖12為不同擬合階數(shù)對(duì)軌跡預(yù)測(cè)的情況。由圖12可知,擬合階數(shù)并不是越大越好。擬合階數(shù)過大,ADE、FDE、單次預(yù)測(cè)耗時(shí)均增大。綜合擬合階數(shù)對(duì)3個(gè)因變量的影響,對(duì)于仿真數(shù)據(jù)1,本文選擇擬合階數(shù)為3。

圖12 不同擬合階數(shù)的軌跡預(yù)測(cè)情況Fig.12 Trajectory prediction with different fitting orders

圖13為粗調(diào)系數(shù)及微調(diào)系數(shù)對(duì)軌跡預(yù)測(cè)的影響。其中,圖13(a)為SEPVUA中粗調(diào)系數(shù)對(duì)軌跡預(yù)測(cè)結(jié)果的影響,圖13(b)為DEPVUA中微調(diào)系數(shù)對(duì)軌跡預(yù)測(cè)的影響。

圖13 調(diào)節(jié)系數(shù)對(duì)軌跡預(yù)測(cè)的影響Fig.13 Influence of adjustment coefficient on trajectory prediction

由圖13可知,k1取0.5、k2取0.55時(shí)對(duì)軌跡預(yù)測(cè)效果最好。

3.3 對(duì)比實(shí)驗(yàn)

圖14用仿真數(shù)據(jù)1對(duì)比了PCFTPA、IRPA、SEPVUA及DEPVUA的ADE情況。其中,相對(duì)于PCFTPA,IRPA的ADE降低了40.3%,SEPVUA的ADE降低了51.71%,DEPVUA的ADE降低了59.7%,驗(yàn)證了DEPVUA的可行性和有效性。后續(xù)IPCFTPA均采用IRPA+DEPVUA。

圖14 仿真數(shù)據(jù)1不同算法不同位置的ADE情況Fig.14 ADE situation of different algorithms at different positions in simulation data 1

圖15用仿真數(shù)據(jù)1探究不同方法、不同預(yù)測(cè)步長(zhǎng)對(duì)軌跡預(yù)測(cè)的影響。

圖15 仿真數(shù)據(jù)1用不同方法在不同預(yù)測(cè)步長(zhǎng)下的軌跡預(yù)測(cè)情況Fig.15 Trajectory prediction with different prediction steps by different methods in simulation data 1

圖15(a)、圖15(b)具有相同的規(guī)律:步長(zhǎng)不同時(shí),IPCFTPA相較于其他方法,ADE、FDE均最低;隨著步長(zhǎng)的增大,不同方法ADE、FDE均變大。從耗時(shí)上看,BPNN、LSTM由于在預(yù)測(cè)時(shí)還需訓(xùn)練模型,因此耗時(shí)都比較長(zhǎng),而PCFTPA、IPCFTPA、KF耗時(shí)均比較小。

對(duì)于第2組、第3組仿真數(shù)據(jù)及真實(shí)數(shù)據(jù)1的預(yù)測(cè)結(jié)果如表2、表3、表4所示(選取10個(gè)歷史軌跡點(diǎn),預(yù)測(cè)步長(zhǎng)為12,耗時(shí)均指單次預(yù)測(cè)耗時(shí);真實(shí)數(shù)據(jù)2、3不同方法的對(duì)比情況與真實(shí)數(shù)據(jù)1類似,預(yù)測(cè)結(jié)果在表5中展示)。

表2 仿真數(shù)據(jù)2不同方法的預(yù)測(cè)情況Table 2 Simulation data 2 prediction of different methods

表3 仿真數(shù)據(jù)3不同預(yù)測(cè)方法的預(yù)測(cè)情況Table 3 Simulation data 3 prediction of different methods

表4 真實(shí)數(shù)據(jù)1不同方法預(yù)測(cè)情況Table 4 Real data 1 prediction of different methods

表5 不同數(shù)據(jù)IPCFTPA相較于PCFTPA的ADE、FDE降低比例Table 5 ADE and FDE decreasement rate of IPCFTPA of different data compared with PCFTPA %

由表2、表3、表4可知,本文算法的ADE、FDE均小于PCFTPA、KF、BPNN、LSTM。從預(yù)測(cè)耗時(shí)上看,改進(jìn)的曲線擬合軌跡預(yù)測(cè)算法相較于BPNN和LSTM具有明顯優(yōu)勢(shì),相較于PCFTPA略有增大。

表5統(tǒng)計(jì)了不同數(shù)據(jù)IPCFTPA相較于PCFTPA的ADE、FDE降低比例情況。其中,KADE為ADE降低比例,KFDE為FDE降低比例。

由表5可知,真實(shí)數(shù)據(jù)的ADE、FDE降低比例較仿真數(shù)據(jù)低,主要是因?yàn)檎鎸?shí)數(shù)據(jù)表示的軌跡,雖經(jīng)過光滑處理,但依然不如仿真數(shù)據(jù)光滑,并且真實(shí)數(shù)據(jù)有較多的彎曲部分,使得整條軌跡的曲率變化較大。由表5可知,本文算法相較于PCFTPA,對(duì)于仿真數(shù)據(jù),ADE平均降低了49.38%,FDE平均降低了39.26%。對(duì)于真實(shí)數(shù)據(jù),ADE平均降低了36.15%,FDE平均降低了33.99%,充分說明了本文算法的可行性和有效性。在運(yùn)動(dòng)目標(biāo)軌跡預(yù)測(cè)中,對(duì)于對(duì)高實(shí)時(shí)性、高準(zhǔn)確率有較高要求的場(chǎng)合,本文算法具有一定的參考價(jià)值。

4 結(jié)束語

本文提出基于PCFTPA的算法IPCFTPA。改進(jìn)之處主要有3點(diǎn):① 通過CTTDA確定曲率、撓率閾值,并根據(jù)該閾值識(shí)別預(yù)測(cè)誤差較大的軌跡部位;② 通過IRPA對(duì)預(yù)測(cè)誤差較大的部位進(jìn)行插值滾動(dòng)預(yù)測(cè);③ 通過DEPVUA對(duì)預(yù)測(cè)值進(jìn)行更新,建立相鄰兩次預(yù)測(cè)中時(shí)間及空間上的關(guān)系,提高預(yù)測(cè)準(zhǔn)確率。實(shí)驗(yàn)結(jié)果表明,IPCFTPA相較于PCFTPA,ADE平均下降了42.77%,FDE平均下降了36.62%,單次平均預(yù)測(cè)時(shí)間略有增加;與LSTM、BPNN軌跡預(yù)測(cè)算法相比,IPCFTPA在單次預(yù)測(cè)耗時(shí)上具有非常大的優(yōu)勢(shì);與KF、LSTM、BPNN相比,IPCFTPA預(yù)測(cè)準(zhǔn)確率具有明顯的優(yōu)勢(shì)。另外,在多項(xiàng)式曲線擬合用于軌跡預(yù)測(cè)的算法中,多項(xiàng)式系數(shù)也呈現(xiàn)出一定的規(guī)律。下一步將研究多項(xiàng)式系數(shù)的變化規(guī)律,進(jìn)一步提高軌跡預(yù)測(cè)的準(zhǔn)確性,以充分發(fā)揮多項(xiàng)式曲線擬合在軌跡預(yù)測(cè)中的優(yōu)勢(shì)。