融合激光雷达、双目相机和ToF深度相机数据的智能机器人深度图像构建方法技术

本发明专利技术公开了一种融合激光雷达、双目相机和ToF深度相机数据的智能机器人深度图像构建方法。所述方法包括获取多源传感器数据，并对各传感器数据进行数据注册，将其统一到双目相机坐标系下；对注册后的各传感器数据进行关联，获得关联的多源深度数据；基于关联的多源深度数据，采用梯度信息的深度补偿融合算法获得更准确、更稠密的深度数据。本发明专利技术通过融合多源深度数据，不但充分利用多源传感器数据，还保证了所获深度图像的准确性和稠密性。

【技术实现步骤摘要】
融合激光雷达、双目相机和ToF深度相机数据的智能机器人深度图像构建方法
本专利技术涉及智能机器人的深度图像技术方法领域，具体而言，设计一种融合激光雷达、双目相机和ToF深度相机数据进行智能机器人的同时定位与深度图像的构建方法。
技术介绍
同时定位与地图创建(SimultaneouslyLocalizationandMapping,简称SLAM)是指机器人依靠其携带的传感器进行自身的定位，同时增量式地创建环境的地图，是智能机器人在未知环境中自主完成任务的前提和基础。而机器人定位问题，需要直接或间接获取环境的距离信息才可求解。因此距离测量是机器人技术的一项重要要求。在摄像机坐标系中，距离信息是沿深度方向定义的，所以距离测量也被称为深度测量。目前，主流的深度测量传感器有双目相机、ToF深度相机、激光雷达。双目相机是由一对固定基线的相机组成的，深度图像可根据两张彩色图像进行特征匹配来获取。在光照强烈的条件下，双目相机可以正常工作。但当环境光线昏暗时或所处环境缺乏纹理信息时，两张彩色图像无法匹配到鲁棒的特征，双目相机难以获取准确的本文档来自技高网...

【技术保护点】
1.一种融合激光雷达、双目相机和ToF深度相机数据的智能机器人深度图像构建方法，其特征在于深度图像构建包括有下列步骤：/n步骤一：多源传感器数据注册；/n在启动激光雷达(11)、ToF深度相机(12)、双目相机(13)之后，在采样时间t

【技术特征摘要】
20200105 CN 20201000776681.一种融合激光雷达、双目相机和ToF深度相机数据的智能机器人深度图像构建方法，其特征在于深度图像构建包括有下列步骤：
步骤一：多源传感器数据注册；
在启动激光雷达(11)、ToF深度相机(12)、双目相机(13)之后，在采样时间t采样里，多源传感器数据注册单元(20)第一方面用于接收激光雷达(11)输出的点云数据M11；由于激光雷达(11)扫描获得的点云数据中包含有环境数据，需要工作人员手动选取前标定物(2)与后标定物(3)同时存在的区域的点云数据，记为所述中存在的点云总个数，记为N；
第二方面用于接收ToF深度相机(12)输出的ToF深度图像信息M12；由于ToF深度相机(12)拍摄获得的ToF深度图像信息中包含有环境数据，需要工作人员手动选取前标定物(2)与后标定物(3)同时存在的区域的ToF深度图像信息，记为所述中以任意一个像素点的像素值来确定深度信息；
第三方面用于接收双目相机(13)输出的双目图像信息和双目深度信息M13；由于双目相机(13)拍摄获得的双目图像信息和双目深度信息中包含有环境数据，需要工作人员手动选取前标定物(2)与后标定物(3)同时存在的区域的双目深度信息，记为所述中以任意一个像素点的像素值来确定深度信息；
第四方面，根据像素点的深度信息与ToF深度相机(12)的内参值，利用相机成像模型将投影为ToF深度相机(12)的坐标系OToF-XToFYToFZToF下的点云信息，记为
根据像素点的深度信息与双目相机(13)的内参值，利用相机成像模型将投影为双目相机(13)坐标系O双目-X双目Y双目Z双目下的点云信息，记为
第五方面，完成异质传感器的位姿标定；
利用点云ICP算法，计算出和之间的位姿变换矩阵RZL、QZL、RZT、QZT，即可得到激光雷达(11)、ToF深度相机(12)、双目相机(13)之间的位置关系，从而完成此三个传感器的位姿标定；
RZL为到的旋转矩阵；
QZL为到的平移矩阵；
RZT为到的旋转矩阵；
QZT为到的平移矩阵；
具体的，求解到的位姿变换矩阵：



RLT为到的旋转矩阵；
QLT为到的平移矩阵；

为不断迭代的所述RLT的中间量；

为不断迭代的所述QLT的中间量；
N为中点云的总个数；
p为中的任意一个点；
q为中的一个点且与所述中的点p距离最近；
利用梯度下降法计算使得公式(1)等式右边最小，即可获得RLT、QLT；
具体的，求解到的位姿变换矩阵：



RLZ为到的旋转矩阵；
QLZ为到的平移矩阵；

为不断迭代的所述RLZ的中间量；

为不断迭代的所述QLZ的中间量；
N为中点云的总个数；
p为中的任意一个点；
w为中的一个点且与所述中的点p距离最近；
利用梯度下降法计算使得公式(2)等式右边最小，即可获得RLZ、QLZ；
通过公式(3)、公式(4)、公式(5)、公式(6)可获得从坐标系O雷达-X雷达Y雷达Z雷达到坐标系O双目-X双目Y双目Z双目的位姿变换关系RZL、QZL以及从坐标系OToF-XToFYToFZToF到坐标系O双目-X双目Y双目Z双目的位姿变换关系RZT、QZT：












上角标T为坐标转置符；
利用坐标系的位姿变换关系，将变换到坐标系O双目-X双目Y双目Z双目下，从而完成多源传感器数据的注册；
步骤二：多源传感器数据关联；
步骤21：异质传感器的统一时间戳；
首先，计算机器人的实际移动速度v；在机器人平台以速度v相对于标记物移动的过程中，双目相机(13)会以一定帧率获得标记物的位姿，通过一段时间的位置微分，即可求得机器人的实际移动速度v；
其次，计算ToF深度相机(12)和双目相机(13)的数据采集时间间隔；
ToF深度相机(12)与双目相机(13)各有一时间轴，双目相机(13)在A时刻观测到标记所在位置为tag_hA；ToF深度相机(12)在b时刻观测到标记所在位置为tag_hb，其中A时刻为双目相机(13)返回数据的时间戳，b时刻为ToF深度相机(12)返回数据的时间戳；但由于ToF深度相机(12)数据采集存在较大延迟，因此ToF深度相机(12)的时间轴应整体向左移动；对于双目相机(13)的A时刻而言，在ToF深度相机(12)时间轴上应处于d时刻，若ToF深度相机(12)与双目相机(13)时间不存在时间戳偏移，那么双目相机(13)的时间轴上的A时刻在ToF深度相机(12)的时间轴上应处于c时刻；然而，由于时间戳偏移的存在，双目相机(13)的时间轴上的A时刻对应在ToF深度相机(12)的时间轴上是d时刻；则A时刻与d时刻数值的差值即为两传感器时间戳之间的偏移量Δt；已知ToF深度相机(12)的坐标系到双目相机(13)的坐标系的旋转矩阵为RZT，平移向量为QZT，有以下关系：



t2＝tb-tc＝tb-tA(8)
t1表示ToF深度相机(12)的时间轴上d时刻与b时刻的时间间隔；
t2表示ToF深度相机(12)的时间轴上c时刻与b时刻的时间间隔；
tb表示ToF深度相机(12)的时间轴上b时刻的数值；
td表示ToF深度相机(12)的时间轴上d时刻的数值；
tc表示ToF深度相机(12)的时间轴上c时刻的数值；
tA表示双目相机(13)的时间轴上A时刻的数值；
ToF深度相机(12)与双目相机(13)时间戳的偏移量Δt为：
Δt＝t2-t1(9)
步骤22：异质传感器数据同步选取；
双目相机(13)的数据采集频率表最高，ToF深度相机(12)次之，激光雷达(11)采集频率最慢；
由于双目相机(13)的工作频率最高，因此主要以双目相机(13)时间轴为参考，选取与双目相机(13)深度图像数据时间点接近的数据；当接收到双目相机(13)的数据时，判断在前后10ms的时间差内有无接收到ToF深度相机(12)的数据和激光雷达(11)的数据，若有，则将此三帧数据选取为有效地同步数据；若没有，则当下一次接收到双目相机(13)的数据时再进行同步数据选取；
以双目相机(13)的时间轴上B时刻为例，在B时刻双目相机(13)输出深度图像数据，与B时刻最近的ToF深度相机(12)的深度数据在e时刻，最近的激光雷达(11)数据在Ⅲ时刻；在进行深度数据融合时，只选取与双目数据时间点B时刻最近且时间差不超过10ms的ToF深度数据和激光雷达数据，以减小或消除由传感器频率不同所造成的数据误差，保证不同传感器所捕获的数据在时间和空间上实现对齐；
步骤三：多源深度数据融合；
在采样时间t采样里，激光雷达(11)扫描获得的点云数据，记为由于点云信息无法与图像信息直接融合，故利用相机成像模型将所述转换为深度图像，记为
在采样时间t采样里，ToF深度相机(12)拍摄获得的ToF深度图像信息，记为
在采样时间t采样里，双目相机(13)拍摄获得的双目深度信息，记为
目标深度图像MPR的优化目标：



下角标i表示像素点在水平方向的坐标；
下角标j表示像素点在垂直方向的坐标；
MPR表示最终优化得到的在双目相机13坐标系O双目-X双目Y双目Z双目下成像平面上的融合深度图像；
MPR*表示优化过程中所述MPR的中间量；
ki,j表示最终优化得到的所述MPR在像素坐标点(i,j)处的像素值；

表示优化过程中所述MPR*在像素坐标点(i,j)处的像素值；

表示优化过程中所述MPR*在像素坐标点(i,j)处的梯度值；
li,j表示雷达深度图像在像素坐标点(i,j)处的像素值；
ri,j表示依据状态选取的深度图(或)在像素坐标点(i,j)处的像素值；

表示依据状态选取的深度图(或)在像素坐标点(i,j)处的梯度值，简称为状态下的梯度值；
由于ToF深度相机12或双目相机13的深度图像在像素坐标点(i,j)处存在没有深度数据的情况下，故需依据状态对状态下的梯度值进行选择；
梯度值状态A是指在像素坐标点(i,j)处存在深度数据，此时，等于在像素坐标点(i,j)处的梯度值；
梯度值状态B是指在像素坐标点(i,j)处不存在深度数据，在像素坐标点(i,j)处存在深度数据，此时等于在像素坐标点(i,j)处的梯度值；
梯度值状态C是指和在像素坐标点(i,j)处均不存在深度数据，此时为0；
为了获得最佳的目标深度图像MPR，公式(10)的等式右边取值最小，将公式(10)书写为便于求解的二次形形式；直接用初始化目标深度图像MPR，因此公式...

【专利技术属性】
技术研发人员：吴星明，孔令坤，陈伟海，王建华，赵小铭，马鑫至，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：北京;11