[發明專利]一種車輛路徑跟隨誤差視覺測量優化方法

有效

• 申請號: 201710171955.7
• 申請日: 2017-03-21
• 公開（公告）號: CN106885523B
• 公開（公告）日: 2019-03-08
• 發明（設計）人: 繆其恒;許煒
• 申請（專利權）人: 浙江零跑科技有限公司
• 主分類號: G01B11/03
• 分類號: G01B11/03
• 代理公司: 杭州杭誠專利事務所有限公司 33109
• 代理人: 尉偉敏;楊燕霞

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

①通過車輛牽引點單目相機單目間接測量系統獲取車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y；

所述的步驟①包括如下步驟：

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

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

(13)根據橫擺平面車輛運動學模型，按如下公式計算出車輛行駛距離S_f以及牽引點位置(X_f，Y_f)與跟隨點位置(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_f-l sin(ψ_f)

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

(14)將牽引點單目相機提取的SURF特征、牽引點位置信息及車輛行駛距離S_f存入道路特征內存緩沖區，根據牽引點與跟隨點之間的距離l，讀取跟隨點當前位置對應牽引點行駛過的全局坐標，進行坐標變換，轉換到當前時刻車輛坐標系下，計算出車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y；

②通過車輛牽引點單目相機及跟隨點單目相機的雙目直接測量系統，獲取車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y_DC；

所述的步驟②包括如下步驟：

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

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

③進行基于無軌卡爾曼濾波的雙系統融合算法，對單目間接測量系統輸出的車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y進行修正；

所述的步驟③為：

當雙目直接測量系統測得的車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y_DC小于設定值時，則利用雙目直接測量系統的輸出預測單目間接測量系統的橫擺角測量誤差，從而對單目間接測量系統輸出的車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y進行修正，獲得修正后的側向偏移距離z_k；所使用的UKF無軌卡爾曼濾波更新以及測量方程如下：

狀態量包括車輛橫擺角ψ_k、橫擺角速度誤差牽引點側偏角β_k以及速度V_k；輸入量由牽引點單目間接測量系統提供，輸入量包括車輛橫擺角速度牽引點側偏角β_SC，k以及牽引點車速V_SC，k；

狀態量x_k：

輸入量u_k：

狀態更新：

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

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

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

協方差矩陣P_k更新：

其中，

α，β，L為UKF參數，默認值分別為0.01，2，0，4；

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

z_k＝F(x_k)

觀測更新：

卡爾曼增益K計算：

狀態與協方差修正：

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為雙目直接測量系統測得的車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離。

2.根據權利要求1所述的一種車輛路徑跟隨誤差視覺測量優化方法，其特征在于所述的步驟③包括：當雙目直接測量系統測得的車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y_DC大于設定值時，則利用雙目直接測量系統的輸出預測單目間接測量系統的橫擺角測量誤差，對單目間接測量系統輸出的車輛跟隨點相對于車輛牽引點行駛路徑的側向偏移距離y進行修正，獲得修正后的側向偏移距離z_k，修正過程如下：

狀態量包括車輛橫擺角ψ_k、橫擺角速度誤差牽引點側偏角β_k以及速度V_k；輸入量由牽引點單目間接測量系統提供，輸入量包括車輛橫擺角速度牽引點側偏角β_SC，k以及牽引點車速V_SC，k；

狀態量x_k：

輸入量u_k：

狀態更新：

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

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

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

將狀態更新后的各參數代入觀測方程F，則獲得修正后的側向偏移距離：

z_k＝F(x_k)。