[發明專利]一種視覺與雷達融合的測量方法及終端

在審

• 申請號: 202010329732.0
• 申請日: 2020-04-24
• 公開（公告）號: CN111538029A
• 公開（公告）日: 2020-08-14
• 發明（設計）人: 余平;項崴
• 申請（專利權）人: 江蘇盛海智能科技有限公司
• 主分類號: G01S17/86
• 分類號: G01S17/86;G01S17/931;G05D1/02;G06T7/73
• 代理公司: 福州市博深專利事務所(普通合伙) 35214
• 代理人: 林振杰

1.一種視覺與雷達融合的測量方法，其特征在于，包括步驟：

S1、將雙目攝像機與毫米波雷達放置于載體上；

S2、提取雙目攝像機所拍攝圖像上的特征點，對所述特征點進行追蹤并匹配，確定所述特征點的第一位置誤差；

S3、通過所述毫米波雷達跟蹤載體運動時周邊的靜態物體，確定所述跟蹤的靜態物體的第二位置誤差；

S4、聯合所述第一位置誤差和第二位置誤差，估計所述載體的位姿。

2.根據權利要求1所述的一種視覺與雷達融合的測量方法，其特征在于，所述S2中所述提取雙目攝像機所拍攝圖像上的特征點包括：

提取雙目攝像機拍攝的圖像上的角點作為圖像特征點；

通過KLT光流法進行不同時刻的攝像機拍攝的圖像上同個角點的跟蹤，對不同時刻圖像上的特征點進行匹配；

計算匹配上的特征點在不同時刻相對于攝像機的深度信息；

根據所述匹配上的特征點的位置信息和深度信息確定其第一位置誤差。

3.根據權利要求2所述的一種視覺與雷達融合的測量方法，其特征在于，所述S2中確定所述特征點的第一位置誤差具體為：

其中，u_cj和v_cj是j時刻雙目攝像機測量到的歸一化深度的特征點相對于雙目攝像機的位置信息，x_cj，y_cj和z_cj是由j時刻攝像頭測量值計算得出，與i時刻雙目攝像機測量到的歸一化深度的特征點相對于雙目攝像機的位置信息的關系為：

由上式估計出R_bjn；

其中，λ為逆深度，其值為z為j時刻雙目攝像機測量出的所述特征點對應的深度信息，R_cb為攝像機相對于載體坐標系的位姿矩陣，R_bin和R_bjn分別為i時刻和j時刻所述載體相對于導航坐標系的位姿矩陣。

4.根據權利要求3所述的一種視覺與雷達融合的測量方法，其特征在于，所述S3中確定所述跟蹤的靜態物體的第二位置誤差具體為：

上式中，x_mrj和y_mrj為毫米波雷達對于所述周邊靜態物體的水平面掃描測量得到的位置信息，和是由j時刻毫米波雷達測量值計算得出，為所述周邊靜態物體的水平面位置信息，其與i時刻周邊靜態物體的水平面位置信息的關系如下：

由上式估計出R_bjn,hori；

上式中，R_rb,hori表示毫米波雷達相對于載體坐標系的水平方向的位姿矩陣，R_bin,hori和R_bjn,hori分別為i時刻和j時刻所述載體相對于導航坐標系的水平方向位姿矩陣。

5.根據權利要求4所述的一種視覺與雷達融合的測量方法，其特征在于，所述S4具體為：

設定目標函數：

上式中res_prior＝ρ(||r_sw,c||)+ρ(||r_sw,r||)；

res_cam＝ρ(residual_c)；

res_mrad＝ρ(residual_r)；

res_prior為先驗誤差，r_sw,c為滑動窗口包含的起始時刻前的相機定位誤差，r_sw,r為滑動窗口包含的起始時刻前的雷達定位誤差；

res_cam為第一位置誤差通過柯西魯棒核函數后的值，res_mard第二位置誤差通過柯西魯棒核函數后的值，ρ表示柯西魯棒核函數；

計算所述目標函數的雅可比矩陣，通過LM優化器，得到載體相對于導航坐標系的位姿矩陣