本發(fā)明涉及車輛控制領(lǐng)域，尤其涉及一種車輛路徑跟隨誤差視覺(jué)測(cè)量?jī)?yōu)化方法。

背景技術(shù)：

長(zhǎng)軸距車輛或列車，如公交巴士、商用車、重型車及拖掛車輛等，車輛質(zhì)心較高，車身長(zhǎng)度較長(zhǎng)，因而其可操控性以及低速通過(guò)性較差。在低速轉(zhuǎn)彎工況下，此類車輛尾部相對(duì)于車輛前部會(huì)產(chǎn)生相對(duì)于轉(zhuǎn)彎半徑內(nèi)側(cè)的側(cè)向偏移距離。車身長(zhǎng)度越長(zhǎng)，轉(zhuǎn)彎半徑越小，該側(cè)向偏移距離越大，車輛相應(yīng)的通過(guò)性也越差。為了提高此類車輛的低速安全性能，一些后軸主動(dòng)轉(zhuǎn)向系統(tǒng)的應(yīng)用可以使得整個(gè)車輛更好地跟隨駕駛員的期望行駛路徑。車輛尾部相對(duì)于車輛前部的側(cè)向偏移距離是衡量此類系統(tǒng)表現(xiàn)的重要指標(biāo)，此參數(shù)很難通過(guò)傳感系統(tǒng)測(cè)量直接獲得，通常需要依據(jù)車輛航向角以及橫擺角速度等狀態(tài)觀測(cè)，通過(guò)車輛運(yùn)動(dòng)學(xué)模型估算而得。

部分視覺(jué)系統(tǒng)可以根據(jù)路面特征通過(guò)直接測(cè)量方法或間接測(cè)量方法得出車輛尾部側(cè)向偏移距離。間接測(cè)量方法，可依據(jù)單目相機(jī)測(cè)量車輛牽引點(diǎn)的速度、側(cè)偏角以及橫擺角速度，利用牽引點(diǎn)位置移位寄存器以及車輛幾何參數(shù)，推算出車輛跟隨點(diǎn)的路徑，從而獲得車輛尾部側(cè)向偏移距離。此方法測(cè)量范圍不受側(cè)向偏移距離限制，但精度易受橫擺角速度積分累積誤差的影響。直接測(cè)量方法，可依據(jù)牽引點(diǎn)相機(jī)與跟隨點(diǎn)相機(jī)行駛路徑的特征匹配，直接確定二者之間的側(cè)向偏移距離。此方法精度較高，但測(cè)量范圍易受兩相機(jī)視野重合范圍的影響。對(duì)于基于慣性傳感器(陀螺儀)的計(jì)算方法，在光滑路面，縱向以及側(cè)向坡道存在的工況下車輛尾部路徑跟隨測(cè)量誤差較大。

準(zhǔn)確地測(cè)量此類車輛尾部相對(duì)于車輛前部的側(cè)向偏移距離對(duì)于后軸主動(dòng)轉(zhuǎn)向系統(tǒng)的應(yīng)用具有十分重要的意義。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明為了解決上述技術(shù)問(wèn)題，提供一種車輛路徑跟隨誤差視覺(jué)測(cè)量?jī)?yōu)化方法，其利用傳感器融合技術(shù)，將兩種基于視覺(jué)傳感的車輛尾部側(cè)向偏移距離的測(cè)量方法相結(jié)合，對(duì)車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離進(jìn)行實(shí)時(shí)測(cè)量，能夠在更大的測(cè)量范圍內(nèi)獲取更準(zhǔn)確的車輛尾部側(cè)向偏移距離測(cè)量值，此測(cè)量值可作為后軸主動(dòng)轉(zhuǎn)向系統(tǒng)控制器輸入，以提高長(zhǎng)軸距車輛的通過(guò)性。

本發(fā)明的上述技術(shù)問(wèn)題主要是通過(guò)下述技術(shù)方案得以解決的：本發(fā)明包括如下步驟：

①通過(guò)車輛牽引點(diǎn)單目相機(jī)單目間接測(cè)量系統(tǒng)獲取車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y；

②通過(guò)車輛牽引點(diǎn)單目相機(jī)及跟隨點(diǎn)單目相機(jī)的雙目直接測(cè)量系統(tǒng)，獲取車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC；

③進(jìn)行基于無(wú)軌卡爾曼濾波的雙系統(tǒng)融合算法，對(duì)單目間接測(cè)量系統(tǒng)輸出的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正。

利用信息融合技術(shù)，將單目間接測(cè)量系統(tǒng)和雙目直接測(cè)量系統(tǒng)的測(cè)量方法相結(jié)合，再通過(guò)基于無(wú)軌卡爾曼濾波的雙系統(tǒng)融合算法對(duì)測(cè)量結(jié)果進(jìn)行修正，能夠在更大的測(cè)量范圍內(nèi)獲取更準(zhǔn)確的車輛尾部側(cè)向偏移距離測(cè)量值，此測(cè)量值可作為后軸主動(dòng)轉(zhuǎn)向系統(tǒng)控制器輸入，以提高長(zhǎng)軸距車輛的通過(guò)性。

作為優(yōu)選，所述的步驟①包括如下步驟：

(11)對(duì)牽引點(diǎn)單目相機(jī)所獲取圖像進(jìn)行預(yù)處理，接著進(jìn)行FAST特征點(diǎn)提取，再生成SURF特征描述向量，然后利用FLANN特征匹配庫(kù)對(duì)相鄰兩幀所提取的SURF特征描述向量進(jìn)行特征匹配，利用RANSAC選取正確匹配樣本，計(jì)算Homography矩陣；

(12)對(duì)計(jì)算所得Homography矩陣進(jìn)行奇異值分解，獲得平動(dòng)信息和轉(zhuǎn)動(dòng)信息，利用平動(dòng)信息推算出牽引點(diǎn)測(cè)偏角β_f以及絕對(duì)速度v_f，利用轉(zhuǎn)動(dòng)信息推算出牽引點(diǎn)的橫擺角ψ_f；

(13)根據(jù)橫擺平面車輛運(yùn)動(dòng)學(xué)模型，按如下公式計(jì)算出車輛行駛距離S_f以及牽引點(diǎn)位置(X_f，Y_f)與跟隨點(diǎn)位置(X_r，Y_r)的全局位置信息：

S_f＝∫v_fdt

γ_f＝ψ_f+β_f

X_f＝∫v_f cos(γ_f)dt

Y_f＝∫v_f sin(γ_f)dt

X_r＝X_f-l cos(ψ_f)

Y_r＝Y(jié)_f-l sin(ψ_f)

其中，γ_f為航向角，l為牽引點(diǎn)與跟隨點(diǎn)之間的距離；

(14)將牽引點(diǎn)單目相機(jī)提取的SURF特征、牽引點(diǎn)位置信息及車輛行駛距離S_f存入道路特征內(nèi)存緩沖區(qū)，根據(jù)牽引點(diǎn)與跟隨點(diǎn)之間的距離l，讀取跟隨點(diǎn)當(dāng)前位置對(duì)應(yīng)牽引點(diǎn)行駛過(guò)的全局坐標(biāo)，進(jìn)行坐標(biāo)變換，轉(zhuǎn)換到當(dāng)前時(shí)刻車輛坐標(biāo)系下，計(jì)算出車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y。

作為優(yōu)選，所述的步驟②包括如下步驟：

(21)對(duì)跟隨點(diǎn)單目相機(jī)所獲取圖像進(jìn)行預(yù)處理，接著進(jìn)行FAST特征點(diǎn)提取，再生成SURF特征描述向量，然后利用FLANN特征匹配庫(kù)對(duì)生成的SURF特征向量與從道路特征內(nèi)存緩沖區(qū)中讀取的SURF特征進(jìn)行匹配，利用RANSAC選取正確匹配樣本，計(jì)算Homography矩陣；

(22)對(duì)計(jì)算所得Homography矩陣進(jìn)行奇異值分解，獲得平動(dòng)信息，將平動(dòng)信息從相機(jī)坐標(biāo)系轉(zhuǎn)換到車輛坐標(biāo)系下，跟隨點(diǎn)的Y向分量即為車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC，跟隨點(diǎn)的X向分量用于修正牽引點(diǎn)與跟隨點(diǎn)之間的距離l。

Y向分量即為車輛坐標(biāo)系下跟隨點(diǎn)的側(cè)向分量，X向分量即為車輛坐標(biāo)系下跟隨點(diǎn)的縱向分量。

作為優(yōu)選，所述的步驟③為：當(dāng)雙目直接測(cè)量系統(tǒng)測(cè)得的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC小于設(shè)定值時(shí)，則利用雙目直接測(cè)量系統(tǒng)的輸出預(yù)測(cè)單目間接測(cè)量系統(tǒng)的橫擺角測(cè)量誤差，從而對(duì)單目間接測(cè)量系統(tǒng)輸出的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正，獲得修正后的側(cè)向偏移距離z_k；所使用的UKF無(wú)軌卡爾曼濾波更新以及測(cè)量方程如下：

狀態(tài)量包括車輛橫擺角ψ_k、橫擺角速度誤差牽引點(diǎn)側(cè)偏角β_k以及速度V_k；輸入量由牽引點(diǎn)單目間接測(cè)量系統(tǒng)提供，輸入量包括車輛橫擺角速度牽引點(diǎn)側(cè)偏角β_SC，k以及牽引點(diǎn)車速V_SC，k；

狀態(tài)量x_k：

輸入量u_k：

狀態(tài)量是指客觀存在的車輛狀態(tài)，比如車速等，并不是指由某個(gè)特定傳感器所得的測(cè)量值，狀態(tài)量是指車輛的狀態(tài)，是由觀測(cè)器根據(jù)輸入量以及觀測(cè)模型估算出來(lái)的。輸入量是指從傳感器獲得的測(cè)量值，如由輪速傳感器測(cè)得的車速值，輸入量的下標(biāo)SC，表示測(cè)量值源自單目視覺(jué)系統(tǒng)。

狀態(tài)更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt為采樣時(shí)間間隔，w_i，k為過(guò)程噪聲，通過(guò)手動(dòng)調(diào)節(jié)可得；

協(xié)方差矩陣P_k更新：

其中，

α，β，L為UKF參數(shù)，默認(rèn)值分別為0.01，2，0，4；

將步驟(13)和步驟(14)所描述的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離的計(jì)算過(guò)程定義為觀測(cè)方程F，則修正后的側(cè)向偏移距離由以下公式表示：

z_k＝F(x_k)

觀測(cè)更新：

卡爾曼增益K計(jì)算：

狀態(tài)與協(xié)方差修正：

x_k+1＝x_(k+1|k)+K(y_DC-z_(k+1|k))

P_k+1＝P_(k+1|k)-K P_{zz，(k+1|k)}K^T

其中，y_DC為雙目直接測(cè)量系統(tǒng)測(cè)得的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離。

作為優(yōu)選，所述的步驟③包括：當(dāng)雙目直接測(cè)量系統(tǒng)測(cè)得的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC大于設(shè)定值時(shí)，則利用雙目直接測(cè)量系統(tǒng)的輸出預(yù)測(cè)單目間接測(cè)量系統(tǒng)的橫擺角測(cè)量誤差，對(duì)單目間接測(cè)量系統(tǒng)輸出的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正，獲得修正后的側(cè)向偏移距離z_k，修正過(guò)程如下：

狀態(tài)量包括車輛橫擺角ψ_k、橫擺角速度誤差牽引點(diǎn)側(cè)偏角β_k以及速度V_k；輸入量由牽引點(diǎn)單目間接測(cè)量系統(tǒng)提供，輸入量包括車輛橫擺角速度牽引點(diǎn)側(cè)偏角β_SC，k以及牽引點(diǎn)車速V_SC，k；

狀態(tài)量x_k：

輸入量u_k：

狀態(tài)更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt為采樣時(shí)間間隔，w_i，k為過(guò)程噪聲；

將狀態(tài)更新后的各參數(shù)代入觀測(cè)方程F，則獲得修正后的側(cè)向偏移距離：

z_k＝F(x_k)。

本技術(shù)方案中用于判斷的設(shè)定值，為雙目直接測(cè)量系統(tǒng)測(cè)量范圍上限，和相機(jī)的內(nèi)外部參數(shù)有關(guān)，根據(jù)實(shí)驗(yàn)標(biāo)定獲得。如相機(jī)安裝高度為0.8米，焦距為3mm時(shí)，設(shè)定值為0.3米。

本發(fā)明的有益效果是：利用信息融合的方法，能夠有效消除單一視覺(jué)系統(tǒng)應(yīng)用過(guò)程中存在的問(wèn)題。相對(duì)于單目視覺(jué)系統(tǒng)，融合后的視覺(jué)系統(tǒng)有效消除了橫擺角速度的積分誤差，測(cè)量精度更高；相對(duì)于雙目視覺(jué)系統(tǒng)，融合后的視覺(jué)系統(tǒng)可以利用濾波后的車輛狀態(tài)有效估計(jì)較大測(cè)量值的側(cè)向偏移距離，測(cè)量范圍更大。融合后的視覺(jué)系統(tǒng)能夠在光滑路面、縱向或側(cè)向坡道存在的工況下有效地測(cè)量長(zhǎng)軸距車輛尾部的側(cè)向偏移距離，能夠在更大的測(cè)量范圍內(nèi)獲取更準(zhǔn)確的車輛尾部側(cè)向偏移距離測(cè)量值，從而精確描述長(zhǎng)軸距車輛的低速通過(guò)性，此測(cè)量值可作為后軸主動(dòng)轉(zhuǎn)向系統(tǒng)控制器輸入，以提高此類車輛的通過(guò)性。

附圖說(shuō)明

圖1是本發(fā)明的算法流程總圖。

圖2是本發(fā)明中車輛在低速轉(zhuǎn)彎工況下的一種俯視結(jié)構(gòu)示意圖。

圖3是本發(fā)明中牽引點(diǎn)單目相機(jī)單目間接測(cè)量系統(tǒng)算法流程圖。

圖4是本發(fā)明中雙目直接測(cè)量系統(tǒng)算法流程圖。

圖中1.車輛，2.車輛牽引點(diǎn)，3.車輛跟隨點(diǎn)。

具體實(shí)施方式

下面通過(guò)實(shí)施例，并結(jié)合附圖，對(duì)本發(fā)明的技術(shù)方案作進(jìn)一步具體的說(shuō)明。

實(shí)施例：本實(shí)施例的一種車輛路徑跟隨誤差視覺(jué)測(cè)量?jī)?yōu)化方法，算法流程總圖如圖1所示，包括如下步驟：

①通過(guò)車輛牽引點(diǎn)單目相機(jī)單目間接測(cè)量系統(tǒng)獲取車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y；

②通過(guò)車輛牽引點(diǎn)單目相機(jī)及跟隨點(diǎn)單目相機(jī)的雙目直接測(cè)量系統(tǒng)，獲取車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC；

③進(jìn)行基于無(wú)軌卡爾曼濾波的雙系統(tǒng)融合算法，對(duì)單目間接測(cè)量系統(tǒng)輸出的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正。

兩個(gè)單目相機(jī)獲取的圖像為本方法的輸入，車輛跟隨點(diǎn)3(車輛尾部)相對(duì)于車輛牽引點(diǎn)2(車輛前部)行駛路徑的側(cè)向偏移距離為本方法的輸出。如圖2所示，牽引點(diǎn)單目相機(jī)安裝在車輛1最前端(通常為牽引點(diǎn)位置)，跟隨點(diǎn)單目相機(jī)安裝在車輛最尾端(通常為跟隨點(diǎn)位置)，兩單目相機(jī)均以垂直向下朝向路面方向安裝，本實(shí)施例中單目相機(jī)的離地高度約為0.5m。圖2中虛線為車輛牽引點(diǎn)行駛路徑，l為牽引點(diǎn)和跟隨點(diǎn)之間的距離，D為此刻實(shí)際發(fā)生的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離。

本優(yōu)化方法的具體步驟如下：

一、單目間接測(cè)量系統(tǒng)，如圖3所示：

(11)對(duì)牽引點(diǎn)單目相機(jī)所獲取圖像進(jìn)行預(yù)處理(灰度化和除畸變)，接著進(jìn)行FAST特征點(diǎn)提取，提取車輛前表面或側(cè)表面平面特征，再生成SURF特征描述向量，然后利用FLANN特征匹配庫(kù)對(duì)相鄰兩幀所提取的SURF特征描述向量進(jìn)行特征匹配，利用RANSAC選取正確匹配樣本，計(jì)算Homography矩陣；

通過(guò)m個(gè)循環(huán)，隨機(jī)選取4個(gè)匹配特征，計(jì)算Homography矩陣，對(duì)剩余特征按該矩陣匹配結(jié)果進(jìn)行打分，像素點(diǎn)匹配距離小于某閾值M，則視為正確匹配，選取打分最高的Homography矩陣，利用其對(duì)應(yīng)的所有正確匹配特征對(duì)，重新計(jì)算得到最終Homography矩陣；中循環(huán)數(shù)m與距離閾值M均為預(yù)設(shè)值；

Homography矩陣表示為：其中，R為相機(jī)平動(dòng)信息，T為相機(jī)轉(zhuǎn)動(dòng)信息，d為圖像平面對(duì)應(yīng)的深度，N為圖像平面對(duì)應(yīng)的法向信息，K’為相機(jī)內(nèi)部參數(shù)矩陣，α’為比例系數(shù)，α’取決于相機(jī)安裝高度；

(12)對(duì)計(jì)算所得Homography矩陣進(jìn)行奇異值分解，獲得平動(dòng)信息和轉(zhuǎn)動(dòng)信息，利用平動(dòng)信息推算出牽引點(diǎn)測(cè)偏角β_f以及絕對(duì)速度v_f，利用轉(zhuǎn)動(dòng)信息推算出牽引點(diǎn)的橫擺角ψ_f；

對(duì)計(jì)算所得Homography矩陣進(jìn)行奇異值分解，獲得相機(jī)平動(dòng)信息T與轉(zhuǎn)動(dòng)信息R；令：

∑＝diag(σ1，σ2，σ3)，V＝[v1，v2，v3]

這是對(duì)奇異值分解結(jié)果，∑為對(duì)角矩陣，V為向量σ1，σ2，σ3以及v1，v2，v3為對(duì)應(yīng)數(shù)值；

上述奇異值分解理論上有四組解，如下所示：

解1：

解2：

解3：

R₃＝R₁，N₃＝-N₁，

解4：

R₄＝R₂，N₄＝-N₂，

選擇方向最接近于[0，0，1]的法向量N對(duì)應(yīng)的該組解；

通過(guò)公式：計(jì)算得出實(shí)時(shí)車速V的絕對(duì)值v_f，v_f即為平動(dòng)信息；

通過(guò)公式：計(jì)算得出車輛的實(shí)時(shí)側(cè)偏角β_f；

通過(guò)公式：計(jì)算車輛橫擺角速度Ψ_f；

公式中：T_x為x軸方向牽引點(diǎn)單目相機(jī)的實(shí)時(shí)平動(dòng)速度；T_y為y軸方向牽引點(diǎn)單目相機(jī)的實(shí)時(shí)平動(dòng)速度；R_z為牽引點(diǎn)單目相機(jī)繞z軸的轉(zhuǎn)動(dòng)分量；t_s為單位時(shí)間步長(zhǎng)；

(13)根據(jù)橫擺平面車輛運(yùn)動(dòng)學(xué)模型，按如下公式計(jì)算出車輛行駛距離S_f以及牽引點(diǎn)位置(X_f，Y_f)與跟隨點(diǎn)位置(X_r，Y_r)的全局位置信息：

S_f＝∫v_fdt

γ_f＝ψ_f+β_f

X_f＝∫v_f cos(γ_f)dt

Y_f＝∫v_f sin(γ_f)dt

X_r＝X_f-l cos(ψ_f)

Y_r＝Y(jié)_f-l sin(ψ_f)

其中，γ_f為航向角，l為牽引點(diǎn)與跟隨點(diǎn)之間的距離；

(14)將牽引點(diǎn)單目相機(jī)提取的SURF特征、牽引點(diǎn)位置信息及車輛行駛距離S_f存入道路特征內(nèi)存緩沖區(qū)，根據(jù)牽引點(diǎn)與跟隨點(diǎn)之間的距離l，讀取跟隨點(diǎn)當(dāng)前位置對(duì)應(yīng)牽引點(diǎn)行駛過(guò)的全局坐標(biāo)，進(jìn)行坐標(biāo)變換，轉(zhuǎn)換到當(dāng)前時(shí)刻車輛坐標(biāo)系下，計(jì)算出車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y。

二、雙目直接測(cè)量系統(tǒng)，如圖4所示：

(21)對(duì)跟隨點(diǎn)單目相機(jī)所獲取圖像進(jìn)行預(yù)處理(灰度化，除畸變)，接著進(jìn)行FAST特征點(diǎn)提取，再生成SURF特征描述向量，然后利用FLANN特征匹配庫(kù)對(duì)生成的SURF特征向量與從道路特征內(nèi)存緩沖區(qū)中讀取的SURF特征進(jìn)行匹配，即利用FLANN特征匹配庫(kù)對(duì)當(dāng)前時(shí)刻車輛尾部的圖像與內(nèi)存中存儲(chǔ)的相應(yīng)位置處的車輛前端圖像進(jìn)行特征匹配，利用RANSAC選取正確匹配樣本，計(jì)算Homography矩陣；

(22)對(duì)計(jì)算所得Homography矩陣進(jìn)行奇異值分解，獲得平動(dòng)信息，將平動(dòng)信息從相機(jī)坐標(biāo)系轉(zhuǎn)換到車輛坐標(biāo)系下，跟隨點(diǎn)的Y向分量即為車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC，跟隨點(diǎn)的X向分量用于修正牽引點(diǎn)與跟隨點(diǎn)之間的距離l。

三、基于無(wú)軌卡爾曼濾波的雙系統(tǒng)融合算法：

當(dāng)雙目直接測(cè)量系統(tǒng)測(cè)得的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC小于設(shè)定值時(shí)，為第一模式狀態(tài)，則利用雙目直接測(cè)量系統(tǒng)的輸出預(yù)測(cè)單目間接測(cè)量系統(tǒng)的橫擺角測(cè)量誤差，從而對(duì)單目間接測(cè)量系統(tǒng)輸出的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正，獲得修正后的側(cè)向偏移距離z_k；所使用的UKF無(wú)軌卡爾曼濾波更新以及測(cè)量方程如下：

狀態(tài)量包括車輛橫擺角ψ_k、橫擺角速度誤差牽引點(diǎn)側(cè)偏角β_k以及速度V_k；輸入量由牽引點(diǎn)單目間接測(cè)量系統(tǒng)提供，輸入量包括車輛橫擺角速度牽引點(diǎn)側(cè)偏角β_SC，k以及牽引點(diǎn)車速V_SC，k；

狀態(tài)量x_k：

輸入量u_k：

狀態(tài)更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt為采樣時(shí)間間隔，w_i，k為過(guò)程噪聲；

協(xié)方差矩陣P_k更新：

其中，

α，β，L為UKF參數(shù)，默認(rèn)值分別為0.01，2，0，4；

將步驟(13)和步驟(14)所描述的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離的計(jì)算過(guò)程定義為觀測(cè)方程F，則修正后的側(cè)向偏移距離由以下公式表示：

z_k＝F(x_k)

觀測(cè)更新：

卡爾曼增益K計(jì)算：

狀態(tài)與協(xié)方差修正：

x_k+1＝x_(k+1|k)+K(y_DC-z_(k+1|k))

P_k+1＝P_(k+1|k)-K P_{zz，(k+1|k)}K^T

其中，y_DC為雙目直接測(cè)量系統(tǒng)測(cè)得的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離。

當(dāng)雙目直接測(cè)量系統(tǒng)測(cè)得的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC大于設(shè)定值時(shí)，為第二模式狀態(tài)，則利用雙目直接測(cè)量系統(tǒng)的輸出預(yù)測(cè)單目間接測(cè)量系統(tǒng)的橫擺角測(cè)量誤差，對(duì)單目間接測(cè)量系統(tǒng)輸出的車輛跟隨點(diǎn)相對(duì)于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正，獲得修正后的側(cè)向偏移距離z_k，修正過(guò)程如下：

狀態(tài)量包括車輛橫擺角ψ_k、橫擺角速度誤差牽引點(diǎn)側(cè)偏角β_k以及速度V_k；輸入量由牽引點(diǎn)單目間接測(cè)量系統(tǒng)提供，輸入量包括車輛橫擺角速度牽引點(diǎn)側(cè)偏角β_SC，k以及牽引點(diǎn)車速V_SC，k；

狀態(tài)量x_k：

輸入量u_k：

狀態(tài)更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt為采樣時(shí)間間隔，w_i，k為過(guò)程噪聲；

將狀態(tài)更新后的各參數(shù)代入觀測(cè)方程F，則獲得修正后的側(cè)向偏移距離：

z_k＝F(x_k)。

即第二模式狀態(tài)下，優(yōu)化過(guò)程不進(jìn)行卡爾曼增益計(jì)算與狀態(tài)修正，直至車輛重新恢復(fù)到第一模式狀態(tài)。

本方法的目的就是使測(cè)得的側(cè)向偏移距離z_k更接近于實(shí)際發(fā)生的側(cè)向偏移距離D。

相關(guān)技術(shù)術(shù)語(yǔ)的名詞解釋：

FAST：FAST特征檢測(cè)算法來(lái)源于corner的定義，這個(gè)定義基于特征點(diǎn)周圍的圖像灰度值，檢測(cè)候選特征點(diǎn)周圍一圈的像素值，如果候選點(diǎn)周圍鄰域內(nèi)有足夠多的像素點(diǎn)與該候選點(diǎn)的灰度值差別夠大，則認(rèn)為該候選點(diǎn)為一個(gè)特征點(diǎn)。此特征點(diǎn)檢測(cè)是公認(rèn)的比較快速的特征點(diǎn)檢測(cè)方法，只利用周圍像素比較的信息就可以得到特征點(diǎn)，簡(jiǎn)單，有效。該方法多用于角點(diǎn)檢測(cè)。

SURF：一種具有尺度與旋轉(zhuǎn)特征不變性的特征描述算法，描述性強(qiáng)，速度快。

FLANN：一種快速近似最近鄰搜索函數(shù)庫(kù)，自動(dòng)選擇兩個(gè)近似最近鄰算法中最優(yōu)的算法。

RANSAC：一種魯棒的回歸方法，用于排除不匹配特征信息。

Homography：兩張圖像中對(duì)應(yīng)匹配特征點(diǎn)的投影變換矩陣

SIFT：尺度不變特征轉(zhuǎn)換(SIFT)算法是一種特征提取的方法。它在尺度空間中尋找極值點(diǎn)，并提取出其位置、尺度、旋轉(zhuǎn)不變量，并以此作為特征點(diǎn)并利用特征點(diǎn)的鄰域產(chǎn)生特征向量。SIFT算法對(duì)于光線、噪聲、和微小視角改變的容忍度相當(dāng)高，且對(duì)于部分遮擋的物體也有較高的識(shí)別相率。

本發(fā)明利用傳感器融合技術(shù)，將兩種基于視覺(jué)傳感的車輛尾部側(cè)向偏移距離測(cè)量方法相結(jié)合，相比于單獨(dú)應(yīng)用其中某一視覺(jué)系統(tǒng)，本方法能夠在更大的測(cè)量范圍內(nèi)獲取更準(zhǔn)確的車輛尾部側(cè)向偏移距離。而且本發(fā)明運(yùn)算量小，可移植性強(qiáng)，實(shí)時(shí)性好，不需要引入額外硬件投入。