技術(shù)特征：

1.一種車輛路徑跟隨誤差視覺測量優(yōu)化方法，其特征在于包括如下步驟：

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

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

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

2.根據(jù)權(quán)利要求1所述的一種車輛路徑跟隨誤差視覺測量優(yōu)化方法，其特征在于所述的步驟①包括如下步驟：

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

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

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

S_f＝∫v_fdt

γ_f＝ψ_f+β_f

X_f＝∫v_fcos(γ_f)dt

Y_f＝∫v_fsin(γ_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)前位置對應(yīng)牽引點(diǎn)行駛過的全局坐標(biāo)，進(jìn)行坐標(biāo)變換，轉(zhuǎn)換到當(dāng)前時刻車輛坐標(biāo)系下，計算出車輛跟隨點(diǎn)相對于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y。

3.根據(jù)權(quán)利要求2所述的一種車輛路徑跟隨誤差視覺測量優(yōu)化方法，其特征在于所述的步驟②包括如下步驟：

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

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

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

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

狀態(tài)量x_k：

輸入量u_k：

狀態(tài)更新：

${\mathrm{\ψ}}_{(k+1|k)}={\mathrm{\ψ}}_k+({\stackrel{\·}{\mathrm{\ψ}}}_{SC,k}+{\stackrel{\·}{\mathrm{\ψ}}}_{bias,k})\mathrm{\Δ}t+w_{1,k}$

${\stackrel{\·}{\mathrm{\ψ}}}_{bias,(k+1|k)}={\stackrel{\·}{\mathrm{\ψ}}}_{bias,k}+w_{2,k}$

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

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

其中，Δt為采樣時間間隔，w_i，k為過程噪聲；

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

$x_k^a=\[x_k,x_k\±\sqrt{(L+\mathrm{\λ})P_k}\]$

$x_{(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^m{x}_{(k+1|k),i}^a$

$P_{(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^c(x_{(k+1|k),i}^a-x_{(k+1|k)}){(x_{(k+1|k),i}^a-x_{(k+1|k)})}^T$

其中，

$W_0^m=\frac{\mathrm{\λ}}{L+\mathrm{\λ}}$

$W_0^c=\frac{\mathrm{\λ}}{L+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β})$

$W_i^m=W_i^c=\frac{1}{2(L+\mathrm{\λ})},i=1,\mathrm{...},2L$

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

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

z_k＝F(x_k)

觀測更新：

$z_{(k+1|k)}^a=F\left(x_{(k+1|k)}^a\right)$

$z_{(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^m{z}_{(k+1|k),i}^a$

$P_{zz,(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_1^c(z_{(k+1|k),i}^a-z_{(k+1|k)}){(z_{(k+1|k),i}^a-z_{(k+1|k)})}^T$

$P_{xz,(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^c(x_{(k+1|k),i}^a-x_{(k+1|k)}){(z_{(k+1|k),i}^a-z_{(k+1|k)})}^T$

卡爾曼增益K計算：

$K=P_{xz,(k+1|k)}{P}_{zz,(k+1|k)}^{-1}$

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

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

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

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

5.根據(jù)權(quán)利要求4所述的一種車輛路徑跟隨誤差視覺測量優(yōu)化方法，其特征在于所述的步驟③包括：當(dāng)雙目直接測量系統(tǒng)測得的車輛跟隨點(diǎn)相對于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y_DC大于設(shè)定值時，則利用雙目直接測量系統(tǒng)的輸出預(yù)測單目間接測量系統(tǒng)的橫擺角測量誤差，對單目間接測量系統(tǒng)輸出的車輛跟隨點(diǎn)相對于車輛牽引點(diǎn)行駛路徑的側(cè)向偏移距離y進(jìn)行修正，獲得修正后的側(cè)向偏移距離z_k，修正過程如下：

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

狀態(tài)量x_k：

輸入量u_k：

狀態(tài)更新：

${\mathrm{\ψ}}_{(k+1|k)}={\mathrm{\ψ}}_k+({\stackrel{\·}{\mathrm{\ψ}}}_{SC,k}+{\stackrel{\·}{\mathrm{\ψ}}}_{bias,k})\mathrm{\Δ}t+w_{1,k}$

${\stackrel{\·}{\mathrm{\ψ}}}_{bias,(k+1|k)}={\stackrel{\·}{\mathrm{\ψ}}}_{bias,k}+w_{2,k}$

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

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

其中，Δt為采樣時間間隔，w_i，k為過程噪聲；

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

z_k＝F(x_k)。