CN111538029A - 一种视觉与雷达融合的测量方法及终端 - Google Patents

Abstract

本发明公开了一种视觉与雷达融合的测量方法及终端，将双目摄像机与毫米波雷达放置于载体上；提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；联合所述第一位置误差和第二位置误差，估计所述载体的位姿；通过毫米波雷达和双目摄像机共同测量目标的深度信息，提升了测量系统的鲁棒性，计算双目摄像机所拍摄图像上特征点的匹配误差，及毫米波雷达跟踪载体周边静态物体时的匹配误差，并将其进行联合优化，做出载体的最优位姿估计，确保了载体位姿估计结果的准确性，实现在降低成本的同时提高定位精度。

Description

一种视觉与雷达融合的测量方法及终端

技术领域

本发明涉及无人驾驶测量领域，尤其涉及一种视觉与雷达融合的测量方法及终端。

背景技术

目前，在无人驾驶领域，主要通过应用激光雷达融合视觉进行局部定位，其中视觉摄像机主要是单目，现有的方法下单目能保证计算的效率，但是单目自身无法通过时差直接估计测量物的尺度信息，间接进行估计得到的数据又无法保证精确度，直接使用会影响无人驾驶的安全性，一般采用激光雷达对单目视觉进行辅助，但激光雷达的价格昂贵，需要在车辆外安装、影响车辆外观美观，并且容易受外界环境制约，如雨天、雾天和风沙大的天气，都会较大影响激光雷达的测量精度从而影响到驾驶安全。

发明内容

本发明所要解决的技术问题是：提供一种视觉与雷达融合的测量方法及终端，在降低成本的同时提高定位的精度。

为了解决上述技术问题，本发明采用的一种技术方案为：

一种视觉与雷达融合的测量方法，包括步骤：

S1、将双目摄像机与毫米波雷达放置于载体上；

S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；

S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；

S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。

为了解决上述技术问题，本发明采用的另一种技术方案为：

一种视觉与雷达融合的测量终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、将双目摄像机与毫米波雷达放置于载体上；

S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；

S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；

S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。

本发明的有益效果在于：利用双目视觉和毫米波雷达共同提供信息对载体的位姿状态进行估计，测量数据能够互相验证，保证了测量的精度，并且在一方出现问题时，另一方可以单独完成基本数据的测量，同时毫米波雷达的造价相对较低，能够有效节约成本，并且毫米波雷达适用范围较广，可以在一些特殊环境下正常工作，保证了自动驾驶的安全性，提升了测量方法的鲁棒性；将双目摄像机测量得到的第一位置误差和雷达跟踪得到的第二位置误差进行联合后估计载体的位姿，实现在降低成本的同时提高定位精度。

附图说明

图1为本发明实施例的一种视觉与雷达融合的测量方法的步骤流程图；

图2为本发明实施例的一种视觉与雷达融合的测量终端的结构示意图；

图3为本发明实施例的一种视觉与雷达融合的测量方法的总流程图；

标号说明：

1、一种视觉与雷达融合的测量终端；2、处理器；3、存储器。

具体实施方式

为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并配合附图予以说明。

请参照图1，一种视觉与雷达融合的测量方法，包括步骤：

S1、将双目摄像机与毫米波雷达放置于载体上；

S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；

S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；

S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。

本发明的有益效果在于：利用双目视觉和毫米波雷达共同提供信息对载体的位姿状态进行估计，测量数据能够互相验证，保证了测量的精度，并且在一方出现问题时，另一方可以单独完成基本数据的测量，同时毫米波雷达的造价相对较低，能够有效节约成本，并且毫米波雷达适用范围较广，可以在一些特殊环境下正常工作，保证了自动驾驶的安全性，提升了测量方法的鲁棒性；将双目摄像机测量得到的第一位置误差和雷达跟踪得到的第二位置误差进行联合后估计载体的位姿，实现在降低成本的同时提高定位精度。

进一步的，所述S2中所述提取双目摄像机所拍摄图像上的特征点包括：

提取双目摄像机拍摄的图像上的角点作为图像特征点；

通过KLT光流法进行不同时刻的摄像机拍摄的图像上同个角点的跟踪，对不同时刻图像上的特征点进行匹配；

计算匹配上的特征点在不同时刻相对于摄像机的深度信息；

根据所述匹配上的特征点的位置信息和深度信息确定其第一位置误差。

由上述描述可知，以角点作为图像特征点进行跟踪，角点的特征较明显，方便实现匹配，并且保证了匹配的精度，且匹配角点位置之后，能够较方便得到其他点的匹配的结果，利用KLT光流法进行不同时刻特征点的匹配，进一步加大了匹配的准确度。

进一步的，所述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时刻所述载体相对于导航坐标系的位姿矩阵。

由上述描述可知，建立本时刻双目摄像机测量到的归一化深度的特征点与其估计值的差值，并将所述差值与载体的位姿矩阵相关联，实现对载体位姿矩阵的最优估计。

进一步的，所述S3中确定所述跟踪的静态物体的第二位置误差具体为：

上式中，x_mrj和y_mrj为毫米波雷达对于所述周边静态物体的水平面扫描测量得到的位置信息，

和

是由j时刻毫米波雷达测量值计算得出，为所述周边静态物体的水平面位置信息，其与i时刻周边静态物体的水平面位置信息的关系如下：

由上式估计出R_bjn,hori；

上式中，R_rb,hori表示毫米波雷达相对于载体坐标系的水平方向的位姿矩阵，R_bin,hori和R_bjn,hori分别为i时刻和j时刻所述载体相对于导航坐标系的水平方向位姿矩阵。

由上述描述可知，建立本时刻毫米波雷达对于周边静态物体的水平面扫描测量得到的位置信息及其估计值的差值，并将所述差值与载体位姿相关联，实现在差值最小的情况下估计出最优的载体位姿。

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

由上述描述可知，将第一位置误差、第二位置误差、先验误差都考虑到位姿估计中，保证最终的估计结果误差最小，计算雅可比矩阵并通过LM优化器对所述目标函数进行优化，确保最终估计的位姿矩阵与实际值的差距最小，提高位姿估计的准确度。

请参照图2，一种视觉与雷达融合的测量终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、将双目摄像机与毫米波雷达放置于载体上；

S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；

S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；

S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。

本发明的有益效果在于：利用双目视觉和毫米波雷达共同提供信息对载体的位姿状态进行估计，保证了测量的精度，并且在一方出现问题时，另一方可以单独完成基本数据的测量，可以在一些特殊环境下正常工作，保证了自动驾驶的安全性，提升了测量方法的鲁棒性，将双目摄像机测量得到的第一信息和雷达跟踪得到的第二信息进行匹配，融合了两种传感器的深度测量数据，大大提高了定位精度；双目摄像头可以直接进行深度的估计，误差较小，不用次次都引入雷达的测量数据进行矫正，提高了效率，毫米波雷达的成本大大小于激光雷达，节约了成本。

进一步的，所述S2中所述提取双目摄像机所拍摄图像上的特征点包括：

提取双目摄像机拍摄的图像上的角点作为图像特征点；

通过KLT光流法进行不同时刻的摄像机拍摄的图像上同个角点的跟踪，对不同时刻图像上的特征点进行匹配；

计算匹配上的特征点在不同时刻相对于摄像机的深度信息；

根据所述匹配上的特征点的位置信息和深度信息确定其第一位置误差。

由上述描述可知，以角点作为图像特征点进行跟踪，角点的特征较明显，方便实现匹配，并且保证了匹配的精度，且匹配角点位置之后，能够较方便得到其他点的匹配的结果，利用KLT光流法进行不同时刻特征点的匹配，进一步加大了匹配的准确度。

进一步的，所述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时刻所述载体相对于导航坐标系的位姿矩阵。

由上述描述可知，建立本时刻双目摄像机测量到的归一化深度的特征点与其估计值的差值，并将所述差值与载体的位姿矩阵相关联，实现对载体位姿矩阵的最优估计。

进一步的，所述S3中确定所述跟踪的静态物体的第二位置误差具体为：

上式中，x_mrj和y_mrj为毫米波雷达对于所述周边静态物体的水平面扫描测量得到的位置信息，

和

是由j时刻毫米波雷达测量值计算得出，为所述周边静态物体的水平面位置信息，其与i时刻周边静态物体的水平面位置信息的关系如下：

由上式估计出R_bjn,hori；

上式中，R_rb,hori表示毫米波雷达相对于载体坐标系的水平方向的位姿矩阵，R_bin,hori和R_bjn,hori分别为i时刻和j时刻所述载体相对于导航坐标系的水平方向位姿矩阵。

由上述描述可知，建立本时刻毫米波雷达对于周边静态物体的水平面扫描测量得到的位置信息及其估计值的差值，并将所述差值与载体位姿相关联，实现在差值最小的情况下估计出最优的载体位姿。

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

由上述描述可知，将第一位置误差、第二位置误差、先验误差都考虑到位姿估计中，保证最终的估计结果误差最小，计算雅可比矩阵并通过LM优化器对所述目标函数进行优化，确保最终估计的位姿矩阵与实际值的差距最小

请参照图1，本发明的实施例一为：

下文所述导航坐标系：以载体的初始坐标为原点，过所述原点且指向所述载体在所述原点时的东方向的射线为x_n轴，北方向的射线为y_n轴，过所述原点且垂直于x_ny_n平面并指向所述载体顶部的射线为z_n轴；

载体坐标系：以载体的重心为原点，过所述原点且指向所述载体左侧的射线为x_b轴，过所述原点且指向所述载体前方的射线为y_b轴，过所述原点且垂直于x_by_b平面并指向所述载体顶部的射线为z_b轴；

雷达坐标系：以雷达测量零点为原点，过所述原点且指向所述雷达左侧的射线为x_r轴，过所述原点且指向所述雷达前方的射线为y_r轴，过所述原点且垂直于x_ry_r平面并指向所述雷达上方的射线为z_r轴；

相机坐标系：以相机重心为原点，过所述原点且指向所述相机右侧的射线为x_c轴，过所述原点且指向所述相机下方的射线为y_c轴，过所述原点且垂直于x_cy_c平面并指向所述相机前方的射线为z_c轴；

一种视觉与雷达融合的测量方法，具体包括步骤：

S1、将双目摄像机与毫米波雷达放置于载体上；

进行毫米波雷达与双目摄像头外参数的标定，得到摄像机坐标系与雷达坐标系之间的转化关系、摄像机坐标系与载体坐标系之间的转化关系及雷达坐标系与载体坐标系之间的转化关系；

S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；

所述提取双目摄像机所拍摄图像上的特征点包括：

提取双目摄像机拍摄的图像上的角点作为图像特征点；

通过KLT光流法进行不同时刻的摄像机拍摄的图像上同个角点的跟踪，对不同时刻图像上的特征点进行匹配；

计算匹配上的特征点在不同时刻相对于摄像机的深度信息；

根据所述匹配上的特征点的位置信息和深度信息确定其第一位置误差；

S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；

S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。

本发明的实施例二为：

一种视觉与雷达融合的测量方法，其与实施例一的不同之处在于，所述S2中，确定所述特征点的第一位置误差具体为：

其中，u_cj和v_cj是j时刻双目摄像机测量到的归一化深度的特征点相对于双目摄像机的位置信息，x_cj，y_cj和z_cj是由j时刻摄像头测量值计算得出，与i时刻双目摄像机测量到的归一化深度的特征点相对于双目摄像机的位置信息的关系为：

由上式估计出R_bjn；

其中，λ为逆深度，其值为

z为i时刻双目摄像机测量出的所述特征点对应的深度信息，R_cb为摄像机相对于载体坐标系的位姿矩阵，可由标定后的摄像机坐标系与载体坐标系之间的转换关系确定，R_bin和R_bjn分别为i时刻和j时刻所述载体相对于导航坐标系的位姿矩阵，R_bin为已估计出的i时刻载体相对于导航坐标系的位姿矩阵，若i为初始时刻，则R_bin已知；

所述S3中，确定所述跟踪的静态物体的第二位置误差具体为：

上式中，x_mrj和y_mrj为毫米波雷达对于所述周边静态物体的水平面扫描测量得到的位置信息，

和

是由j时刻毫米波雷达测量值计算得出，为所述周边静态物体的水平面位置信息，其与i时刻周边静态物体的水平面位置信息的关系如下：

由上式估计出R_bjn,hori；

上式中，R_rb,hori表示毫米波雷达相对于载体坐标系的水平方向的位姿矩阵，可以由标定后的雷达坐标系与载体坐标系之间的转换关系确定，R_bin,hori和R_bjn,hori分别为i时刻和j时刻所述载体相对于导航坐标系的水平方向位姿矩阵，R_bin,hori为已估计出的i时刻所述载体相对于导航坐标系的水平方向位姿矩阵，若i为初始时刻，则R_bin,hori已知；

所述S4具体为：

视觉估计的是载体的三维位姿R_bn，毫米波雷达估计的是载体的二维位姿R_bn,hori，R_bn,hori被R_bn所包含，因此，所需估计的载体位姿为：

上式中，X表示滑动窗口，N为滑动窗口内待估计系数矩阵起始的时刻，M为滑动窗口内待估计系数矩阵由起始到最后时刻的时间间隔，也可以表示所述滑动窗口的数目；

设定目标函数：

上式中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第二位置误差通过柯西鲁棒核函数后的值，ρ表示柯西鲁棒核函数，参照下式：

其中，c为系数，一般设定为2.3849，s为输入值；

使用柯西鲁棒核函数，在误差过大时(可能特征提取错误，匹配错误，或者测量值误差过大等造成的误差过大)，可以减小其误差值，并且不会对误差值小的数据有影响，这样可以通过这个柯西鲁棒核函数减低误差过大的值对于优化过程中的正确收敛点产生影响，从而影响定位精度；

计算所述目标函数的雅可比矩阵(为代价函数对于带估参数的求导)，通过LM优化器，得到载体相对于导航坐标系的位姿矩阵

LM优化器：

(J^TJ+μI)ΔXlm＝-J^Tf withμ≥0

上式中，J为其代价函数

的雅可比矩阵，f为res_prior+res_cam+res_mrad；的值，μ为阻尼系数，设定初值后，一直迭代计算，μ会根据每次迭代后的ΔXlm的变化量进行调整，当ΔXlm变化值小于额定阈值，并且其值小于特定阈值后，收敛，得到估计出f中带估参数

的值；

然后滑动窗口进行滑动，剔除头一个点，把新的点的观测值加入窗口内，然后把剔除点的误差加入到先验误差内，再次计算加入新点的滑动窗口内代价函数的误差值和雅可比矩阵的值，然后再次优化；

重复上述步骤直至结束对载体位置的确定。

请参照图2，本发明的实施例三为：

一种视觉与雷达融合的测量终端1，所述终端1包括处理器2、存储器3及存储在存储器3上并可在所述处理器2上运行的计算机程序，所述处理器2执行所述计算机程序时实现实施例一或实施例二中的各个步骤。

综上所述，本发明提供了一种视觉与雷达融合的测量方法及终端，通过毫米波雷达和双目摄像机共同对目标的深度信息进行测量，提升了系统的鲁棒性，通过计算双目摄像机所拍摄图像上的特征点的匹配误差，及毫米波雷达跟踪载体周边静态物体时的匹配误差，并将所述两项误差与载体的位姿相关联，从而实现在进行误差最小的优化时，做出载体的最优位姿估计，同时将雷达扫描获得的水平面位姿与摄像机获得的三维位姿进行联合，确保了载体位姿估计结果的准确性，同时在最后采用LM方法进一步进行优化，保证最后获得的载体位姿与实际值的误差最小。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

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

6.一种视觉与雷达融合的测量终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现以下步骤：

S1、将双目摄像机与毫米波雷达放置于载体上；

S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；

S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差。

7.根据权利要求6所述的一种视觉与雷达融合的测量终端，其特征在于，所述S2中所述提取双目摄像机所拍摄图像上的特征点包括：

提取双目摄像机拍摄的图像上的角点作为图像特征点；

通过KLT光流法进行不同时刻的摄像机拍摄的图像上同个角点的跟踪，对不同时刻图像上的特征点进行匹配；

计算匹配上的特征点在不同时刻相对于摄像机的深度信息；

根据所述匹配上的特征点的位置信息和深度信息确定其第一位置误差。

8.根据权利要求7所述的一种视觉与雷达融合的测量终端，其特征在于，所述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时刻所述载体相对于导航坐标系的位姿矩阵。

9.根据权利要求8所述的一种视觉与雷达融合的测量终端，其特征在于，所述S3中确定所述跟踪的静态物体的第二位置误差具体为：

上式中，x_mrj和y_mrj为毫米波雷达对于所述周边静态物体的水平面扫描测量得到的位置信息，

和

是由j时刻毫米波雷达测量值计算得出，为所述周边静态物体的水平面位置信息，其与i时刻周边静态物体的水平面位置信息的关系如下：

由上式估计出R_bjn,hori；

上式中，R_rb,hori表示毫米波雷达相对于载体坐标系的水平方向的位姿矩阵，R_bin,hori和R_bjn,hori分别为i时刻和j时刻所述载体相对于导航坐标系的水平方向位姿矩阵。

10.根据权利要求9所述的一种视觉与雷达融合的测量终端，其特征在于，所述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优化器，得到载体相对于导航坐标系的位姿矩阵

Images