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

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

【技术实现步骤摘要】
一种视觉与雷达融合的测量方法及终端
本专利技术涉及无人驾驶测量领域，尤其涉及一种视觉与雷达融合的测量方法及终端。
技术介绍
目前，在无人驾驶领域，主要通过应用激光雷达融合视觉进行局部定位，其中视觉摄像机主要是单目，现有的方法下单目能保证计算的效率，但是单目自身无法通过时差直接估计测量物的尺度信息，间接进行估计得到的数据又无法保证精确度，直接使用会影响无人驾驶的安全性，一般采用激光雷达对单目视觉进行辅助，但激光雷达的价格昂贵，需要在车辆外安装、影响车辆外观美观，并且容易受外界环境制约，如雨天、雾天和风沙大的天气，都会较大影响激光雷达的测量精度从而影响到驾驶安全。
技术实现思路
本专利技术所要解决的技术问题是：提供一种视觉与雷达融合的测量方法及终端，在降低成本的同时提高定位的精度。为了解决上述技术问题，本专利技术采用的一种技术方案为：一种视觉与雷达融合的测量方法，包括步骤：S1、将双目摄像机与毫米波雷达放置于载体上；S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。为了解决上述技术问题，本专利技术采用的另一种技术方案为：一种视觉与雷达融合的测量终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：S1、将双目摄像机与毫米波雷达放置于载体上；S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。本专利技术的有益效果在于：利用双目视觉和毫米波雷达共同提供信息对载体的位姿状态进行估计，测量数据能够互相验证，保证了测量的精度，并且在一方出现问题时，另一方可以单独完成基本数据的测量，同时毫米波雷达的造价相对较低，能够有效节约成本，并且毫米波雷达适用范围较广，可以在一些特殊环境下正常工作，保证了自动驾驶的安全性，提升了测量方法的鲁棒性；将双目摄像机测量得到的第一位置误差和雷达跟踪得到的第二位置误差进行联合后估计载体的位姿，实现在降低成本的同时提高定位精度。附图说明图1为本专利技术实施例的一种视觉与雷达融合的测量方法的步骤流程图；图2为本专利技术实施例的一种视觉与雷达融合的测量终端的结构示意图；图3为本专利技术实施例的一种视觉与雷达融合的测量方法的总流程图；标号说明：1、一种视觉与雷达融合的测量终端；2、处理器；3、存储器。具体实施方式为详细说明本专利技术的
技术实现思路
、所实现目的及效果，以下结合实施方式并配合附图予以说明。请参照图1，一种视觉与雷达融合的测量方法，包括步骤：S1、将双目摄像机与毫米波雷达放置于载体上；S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。本专利技术的有益效果在于：利用双目视觉和毫米波雷达共同提供信息对载体的位姿状态进行估计，测量数据能够互相验证，保证了测量的精度，并且在一方出现问题时，另一方可以单独完成基本数据的测量，同时毫米波雷达的造价相对较低，能够有效节约成本，并且毫米波雷达适用范围较广，可以在一些特殊环境下正常工作，保证了自动驾驶的安全性，提升了测量方法的鲁棒性；将双目摄像机测量得到的第一位置误差和雷达跟踪得到的第二位置误差进行联合后估计载体的位姿，实现在降低成本的同时提高定位精度。进一步的，所述S2中所述提取双目摄像机所拍摄图像上的特征点包括：提取双目摄像机拍摄的图像上的角点作为图像特征点；通过KLT光流法进行不同时刻的摄像机拍摄的图像上同个角点的跟踪，对不同时刻图像上的特征点进行匹配；计算匹配上的特征点在不同时刻相对于摄像机的深度信息；根据所述匹配上的特征点的位置信息和深度信息确定其第一位置误差。由上述描述可知，以角点作为图像特征点进行跟踪，角点的特征较明显，方便实现匹配，并且保证了匹配的精度，且匹配角点位置之后，能够较方便得到其他点的匹配的结果，利用KLT光流法进行不同时刻特征点的匹配，进一步加大了匹配的准确度。进一步的，所述S2中确定所述特征点的第一位置误差具体为：其中，ucj和vcj是j时刻双目摄像机测量到的归一化深度的特征点相对于双目摄像机的位置信息，xcj，ycj和zcj是由j时刻摄像头测量值计算得出，与i时刻双目摄像机测量到的归一化深度的特征点相对于双目摄像机的位置信息的关系为：由上式估计出Rbjn；其中，λ为逆深度，其值为z为j时刻双目摄像机测量出的所述特征点对应的深度信息，Rcb为摄像机相对于载体坐标系的位姿矩阵，Rbin和Rbjn分别为i时刻和j时刻所述载体相对于导航坐标系的位姿矩阵。由上述描述可知，建立本时刻双目摄像机测量到的归一化深度的特征点与其估计值的差值，并将所述差值与载体的位姿矩阵相关联，实现对载体位姿矩阵的最优估计。进一步的，所述S3中确定所述跟踪的静态物体的第二位置误差具体为：上式中，xmrj和ymrj为毫米波雷达对于所述周边静态物体的水平面扫描测量得到的位置信息，和是由j时刻毫米波雷达测量值计算得出，为所述周边静态物体的水平面位置信息，其与i时刻周边静态物体的水平面位置信息的关系如下：由上式估计出Rbjn,hori；上式中，Rrb,hori表示毫米波雷达相对于载体坐标系的水平方向的位姿矩阵，Rbin,hori和Rbjn,hori分别为i时刻和j时刻所述载体相对于导航坐标系的水平方向位姿矩阵。由上述描述可知，建立本时刻毫米波雷达对于周边静态物体的水平面扫描测量得到的位置信息及其估计值的差值，并将所述差值与载体位姿相关联，实现在差值最小的情况下估计出最优的载体位姿。进一步的，所述S4具体为：设定目标函数：上式中resprior＝ρ(||rsw,c||)+ρ(||rsw,r||)；rescam＝ρ(residualc)；resmrad＝ρ(residualr)；resprior为先验误差，rsw,c为滑动窗口包含的起始时刻前的相机定位误差，rsw,r为滑动窗口包含的起始时刻前的雷达定位误差；rescam为第一位置误差通过柯西鲁棒核函数后的值，resmard第二位置误差通过柯西鲁棒核函数后的值，ρ表示柯西鲁棒核函数；本文档来自技高网...

【技术保护点】
1.一种视觉与雷达融合的测量方法，其特征在于，包括步骤：/nS1、将双目摄像机与毫米波雷达放置于载体上；/nS2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；/nS3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；/nS4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。/n

【技术特征摘要】
1.一种视觉与雷达融合的测量方法，其特征在于，包括步骤：
S1、将双目摄像机与毫米波雷达放置于载体上；
S2、提取双目摄像机所拍摄图像上的特征点，对所述特征点进行追踪并匹配，确定所述特征点的第一位置误差；
S3、通过所述毫米波雷达跟踪载体运动时周边的静态物体，确定所述跟踪的静态物体的第二位置误差；
S4、联合所述第一位置误差和第二位置误差，估计所述载体的位姿。


2.根据权利要求1所述的一种视觉与雷达融合的测量方法，其特征在于，所述S2中所述提取双目摄像机所拍摄图像上的特征点包括：
提取双目摄像机拍摄的图像上的角点作为图像特征点；
通过KLT光流法进行不同时刻的摄像机拍摄的图像上同个角点的跟踪，对不同时刻图像上的特征点进行匹配；
计算匹配上的特征点在不同时刻相对于摄像机的深度信息；
根据所述匹配上的特征点的位置信息和深度信息确定其第一位置误差。


3.根据权利要求2所述的一种视觉与雷达融合的测量方法，其特征在于，所述S2中确定所述特征点的第一位置误差具体为：



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



由上式估计出Rbjn；
其中，λ为逆深度，其值为z为j时刻双目摄像机测量出的所述特征点对应的深度信息，Rcb为摄像机相对于载体坐标系的位姿矩阵，Rbin和Rbjn分别为i时刻和j时刻所述载体相对于导航坐标系的位姿矩阵。


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



上式中，xmrj和ymrj为毫米波雷达对于所述周边静态物体的水平面扫描测量得到的位置信息，和是由j时刻毫米波雷达测量值计算得出，为所述周边静态物体的水平面位置信息，其与i时刻周边静态物体的水平面位置信息的关系如下：



由上式估计出Rbjn,hori；
上式中，Rrb,hori表示毫米波雷达相对于载体坐标系的水平方向的位姿矩阵，Rbin,hori和Rbjn,hori分别为i时刻和j时刻所述载体相对于导航坐标系的水平方向位姿矩阵。


5.根据权利要求4所述的一种视觉与雷达融合的测量方法，其特征在于，所述S4具体为：
设定目标函数：
上式中resprior＝ρ(||rsw,c||)+ρ(||rsw,r||)；
rescam＝ρ(residualc)；
resmrad＝ρ(residualr)；
resprior为先验误差，rsw,c为滑动窗口包含的起始时刻前的相机定位误差，rsw,r为滑动窗口包含的起始时刻前的雷达定位误差；
rescam为第一位置误差通过柯西鲁棒核函数后的值，resmard第二位置误差通过柯西鲁棒核函数后的值，ρ表示柯西鲁棒核函数；
计算所述目标函数的雅可比矩阵，通过LM优化器，得到载体相对于导航坐标系的位姿矩阵


6.一...

【专利技术属性】
技术研发人员：余平，项崴，
申请(专利权)人：江苏盛海智能科技有限公司，
类型：发明
国别省市：江苏;32