[发明专利]一种视觉与雷达融合的测量方法及终端

权利要求书

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优化器，得到载体相对于导航坐标系的位姿矩阵