基于多传感器的机器人实时定位和彩色地图融合映射方法技术

本发明专利技术提出一种基于多传感器的机器人实时定位和彩色地图融合映射方法，快速采集周边环境信息，并实时构建彩色全局地图。视觉、激光、惯性传感器采集模块通过读取传感器的信息，对激光点云数据进行累加，形成点云扫描帧，然后进行预处理，得到提取后的特征点云，同时对不同传感器采集到的数据进行时间同步。利用采集到的激光和惯性数据实时对机器人进行自定位，同时构建单帧点云地图。利用视觉传感器采集到的RGB信息，对构建的全局点云地图进行纹理和颜色渲染，构建单帧彩色地图。构建单帧彩色地图。构建单帧彩色地图。

【技术实现步骤摘要】
基于多传感器的机器人实时定位和彩色地图融合映射方法


[0001]本专利技术涉及机器人
，尤其涉及一种基于多传感器的机器人实时自定位和彩色稠密地图融合映射方法。

技术介绍

[0002]随着近年来科技的发展，移动机器人在工业领域和民用领域的应用不断深入，而定位与导航能力是移动机器人的关键和基础。
[0003]目前主流的同时定位与建图技术主要有基于激光雷达的方法、基于相机的方法和多传感器融合的方法，其中多传感器融合方法多使用激光里程计和视觉里程计的结果直接耦合优化，从而得到一个较为准确的位姿，然后输出一个经过位姿矫正后未经渲染的点云地图或者语义地图，这样做的优点在于可以适应更为多样性的环境，避免传感器退化，缺陷在于计算成本较高，并且只利用了视觉信息中的特征点部分，没有实现完全的利用。

技术实现思路

[0004]本专利技术提出了基于多传感器的机器人实时定位和彩色地图融合映射方法，能够生成基于机器人运动的自我定位信息和实时彩色全局地图，可以通过网络上传到服务器端，可以远程调用查看，摆脱操作人员的视野局限，全方位的掌握整个环境的变化信息。
[0005]为解决上述技术问题，本专利技术所采用的技术方案是：
[0006]基于多传感器的机器人实时定位和彩色地图融合映射方法，包括以下步骤：
[0007]步骤1：读取视觉传感器、激光雷达传感器、惯性传感器的数据，对不同源的图像、激光点云和imu位姿数据进行时间同步，获得统一的时间系统下的待融合数据，并利用惯性传感器采集到的前20帧数据进行初始化，建立初始位姿估计。
[0008]步骤2：由于激光雷达的逐点扫描特性，点云数据中的每个点都有自己独立的采集时间，一帧激光点云往往包含有数万个点，也就有数万个连续的不同时间戳，同一帧中采集到的第一个点和最后一个点之间存在着不可忽视的时间误差，对于运动中的机器人，这种时间误差必然导致不可避免的空间误差，为了消除这种误差，我们使用了采集频率远远超过激光雷达传感器的imu传感器，对imu传感器采集到的惯性数据进行前向传播，得到各个时刻机器人位姿的先验估计；得到各个时刻间对机器人的位姿的先验估计后，通过对时刻进行插值，计算出每个激光点采集时刻和激光点云帧采集结束时刻间机器人的相对运动，将激光点从采集时刻的机器人坐标系转移到点云帧采集结束时刻的坐标系中，对当前帧中每一个激光点进行上述操作后，得到消除帧间运动畸变的点云帧采集结束时刻点云；
[0009]步骤3：由于点云帧采集结束时刻点云是对经过下采样的点云数据直接利用imu信息进行运动畸变消除得到的，因此必然会受到激光雷达传感器和惯性imu传感器的采集数据误差的影响，误差随着时间不断累积，最终导致跟踪丢失，定位和建图的精度都完全得不到保障。为了消除传感器误差的影响，得到更精确的位姿估计，我们使用误差状态迭代卡尔曼滤波器对不同帧间采集到的信息进行比较，在机器人运动过程中，一个地标物往往会在
连续很多帧上同时存在，并且和机器人的相对位置不断变化，对新采集到的点云帧中的每一点，在地图中提取和当前位置最近的五个点拟合出一个小平面，通过迭代优化，不断缩小激光点和它对应的拟合平面间的距离，直到小于阈值，从而得到点云帧采集结束时刻对机器人运动的最优估计；利用计算出的最优估计，将点云帧采集结束时刻视觉传感器采集到的图像转移归一化坐标系并最终转移到雷达坐标系中，对当前帧中的每一个点，利用预标定过的激光雷达传感器和视觉传感器的旋转平移关系，从三维空间坐标转移到像素坐标系，给视觉传感器视场范围内的每个激光点赋予RGB信息，得到经过RGB渲染的彩色点云；为了解决激光雷达和相机数据采集频率不同的问题，我们使用了基于时间的插帧处理，利用激光雷达传感器采集时刻的前后两帧图片，使用稠密光流法估计出激光雷达传感器采集时刻的视觉数据，实现激光雷达传感器和视觉传感器的时间同步；
[0010]步骤4：对经过RGB渲染的彩色点云通过点云帧采集结束时刻对机器人运动的最优估计，将点云从点云帧采集结束时刻的机器人运动坐标系转移到独立的世界坐标系，从而构建出当前帧的彩色点云地图；为了便于对地图中的点进行最近邻搜索，将地图中的点以K维树的形式组织起来，使得新采集到的每一个点都可以用最短的时间找到它在地图中的最邻近点；通过时间累积，将机器人采集到的多帧信息累加，得到连续各帧间的机器人运动最优估计和各帧的彩色点云地图，实现机器人的自我定位并建立全局彩色地图。
[0011]本专利技术技术方案的进一步改进在于：所述步骤2具体包括以下步骤：
[0012]步骤2.1：建立imu运动学模型，对于机器人运动的描述中最重要的两个物理量是当前时刻的位姿和相对于坐标系原点的位移量，但是位姿和位移并不能通过传感器直接测量得到，我们选择使用惯性imu传感器测量得到的角速度和加速度数据对时间维度积分得到所求的位姿和位移状态，因此建立在世界坐标系下的状态表达方程：
[0013][0014]在建立世界坐标系的状态表达方程中一共有6个状态量，分别是机器人在世界坐标系下的位姿R
I
，即机器人相对于初始状态的旋转量，使用一个3*3的旋转矩阵来表示；机器人在世界坐标系下相对于原点的位移量p
I
，使用一个3*1的平移向量来表示；机器人在x、y、z三个轴上的瞬时速度v
I
,使用用一个3*1的速度向量来表示；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差b
ω
和b
a
；机器人世界坐标系相对于真实世界坐标系下的重力矢量g；
[0015]步骤2.2：对于建立好的状态方程，对于imu数据进行预积分，建立imu运动学连续模型；其中，机器人在世界坐标系下的位姿的一阶微分量等于机器人相对于初始状态的旋转矩阵右乘消除噪音后的角速度向量的反对称矩阵；机器人在世界坐标系下的位移的一阶微分量等于机器人在x、y、z三个轴上的瞬时速度；机器人的速度的一阶微分量等于机器人相对于初始状态的旋转矩阵乘以消除噪音后的加速度向量再加上机器人世界坐标系下的重力矢量；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差b
ω
和b
a
的模型为带高斯噪声n
bω
和n
ba
的随机游动过程；机器人世界坐标系下的重力矢量g的一阶微分量为0，由此可得机器人的imu运动学方程如下：
[0016][0017][0018][0019][0020][0021][0022]对上述方程组离散化可得
[0023][0024]步骤2.3：步骤1中进行初始化，得到了机器人运动状态的初始估计，当机器人收到imu传感器的输入时，即可根据步骤2.2中建立的离散运动学模型进行前向传播，通过初始估计和imu传感器每一帧采集到的加速度和角速度数据，迭代出每一帧时刻的机器人位姿的先验估计；具体的迭代方程如下
[0025]x
i+1
＝x
i
+(Δtf(x
i
，u
i...

【技术保护点】

【技术特征摘要】
1.基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于包括以下步骤：步骤1：读取视觉传感器、激光雷达传感器、惯性传感器的数据，对不同源的图像、激光点云和imu位姿数据进行时间同步，获得统一的时间系统下的待融合数据，并利用惯性传感器采集到的前20帧数据进行初始化，建立初始位姿估计；步骤2：基于imu传感器的高工作频率，惯性传感器选用imu传感器，对imu传感器采集到的惯性数据进行前向传播，得到各个时刻机器人位姿的先验估计；得到各个时刻间对机器人的位姿的先验估计后，通过对时刻进行插值，计算出每个激光点采集时刻和激光点云帧采集结束时刻间机器人的相对运动，将激光点从采集时刻的机器人坐标系转移到点云帧采集结束时刻的坐标系中，对当前帧中每一个激光点进行上述操作后，得到消除帧间运动畸变的点云帧采集结束时刻点云；步骤3：使用误差状态迭代卡尔曼滤波器对不同帧间采集到的信息进行比较，在机器人运动过程中，一个地标物往往会在连续很多帧上同时存在，并且和机器人的相对位置不断变化，对新采集到的点云帧中的每一点，在地图中提取和当前位置最近的五个点拟合出一个小平面，通过迭代优化，不断缩小激光点和它对应的拟合平面间的距离，直到小于阈值，从而得到点云帧采集结束时刻对机器人运动的最优估计；利用计算出的最优估计，将点云帧采集结束时刻视觉传感器采集到的图像转移归一化坐标系并最终转移到雷达坐标系中，对当前帧中的每一个点，利用预标定过的激光雷达传感器和视觉传感器的旋转平移关系，从三维空间坐标转移到像素坐标系，给视觉传感器视场范围内的每个激光点赋予RGB信息，得到经过RGB渲染的彩色点云；使用基于时间的插帧处理，利用激光雷达传感器采集时刻的前后两帧图片，使用稠密光流法估计出激光雷达传感器采集时刻的视觉数据，实现激光雷达传感器和视觉传感器的时间同步；步骤4：对经过RGB渲染的彩色点云通过点云帧采集结束时刻对机器人运动的最优估计，将点云从点云帧采集结束时刻的机器人运动坐标系转移到独立的世界坐标系，从而构建出当前帧的彩色点云地图；将地图中的点以K维树的形式组织起来，使得新采集到的每一个点都可以用最短的时间找到它在地图中的最邻近点；通过时间累积，将机器人采集到的多帧信息累加，得到连续各帧间的机器人运动最优估计和各帧的彩色点云地图，实现机器人的自我定位并建立全局彩色地图。2.根据权利要求1所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤2具体包括以下步骤：步骤2.1：建立imu运动学模型，对于机器人运动的描述中最重要的两个物理量是当前时刻的位姿和相对于坐标系原点的位移量，使用imu传感器测量得到的角速度和加速度数据对时间维度积分得到所求的位姿和位移状态，因此建立在世界坐标系下的状态表达方程：在建立世界坐标系的状态表达方程中一共有6个状态量，分别是机器人在世界坐标系下的位姿R
I
，即机器人相对于初始状态的旋转量，使用一个3*3的旋转矩阵来表示；机器人在世界坐标系下相对于原点的位移量p
I
，使用一个3*1的平移向量来表示；机器人在x、y、z
三个轴上的瞬时速度v
I
,使用用一个3*1的速度向量来表示；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差b
ω
和b
a
；机器人世界坐标系相对于真实世界坐标系下的重力矢量g；步骤2.2：对于建立好的状态方程，对于imu数据进行预积分，建立imu运动学连续模型；其中，机器人在世界坐标系下的位姿的一阶微分量等于机器人相对于初始状态的旋转矩阵右乘消除噪音后的角速度向量的反对称矩阵；机器人在世界坐标系下的位移的一阶微分量等于机器人在x、y、z三个轴上的瞬时速度；机器人的速度的一阶微分量等于机器人相对于初始状态的旋转矩阵乘以消除噪音后的加速度向量再加上机器人世界坐标系下的重力矢量；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差b
ω
和b
a
的模型为带高斯噪声n
bω
和n
ba
的随机游动过程；机器人世界坐标系下的重力矢量g的一阶微分量为0，由此可得机器人的imu运动学方程如下：方程如下：方程如下：方程如下：方程如下：方程如下：对上述方程组离散化可得步骤2.3：步骤1中进行初始化，得到了机器人运动状态的初始估计，当机器人收到imu传感器的输入时，即可根据步骤2.2中建立的离散运动学模型进行前向传播，通过初始估计和imu传感器每一帧采集到的加速度和角速度数据，迭代出每一帧时刻的机器人位姿的先验估计；具体的迭代方程如下x
i+1
＝x
i
+(Dtf(x
i
,u
i
,w
i
))其中控制量u
i
为imu传感器采集到的的加速度数据a
m
和角速度数据ω
m
,误差量w为imu传感器的累积误差n
ω
、n
a
和imu传感器采集到的数据的误差n
bω
、n
ba
；步骤2.4：激光雷达传感器采集完完整一帧点云后，根据imu传感器数据前向传播得到的各个时刻的机器人运动状态进行插值计算，得到每个激光点采集时刻对应的机器人运动状态，并计算出每个激光点采集时刻到点云帧采集结束时刻的机器人相对运动状态其中j为每个雷达点的采集时刻，k为点云帧的采集时刻；将j时刻采集到的激光点先从j时刻的激光雷达坐标系转移到imu传感器坐标系，再利用j时刻到k时刻的机器人的相对运动将激光点从j时刻的imu传感器坐标系转移到k时刻的imu传感器坐标系，最后再将激光点先
从k时刻的imu传感器坐标系转移到激光雷达坐标系，激光点的坐标系转换过程如下公式所示：其中
I
T
L...

【专利技术属性】
技术研发人员：丁伟利，邵长宇，
申请(专利权)人：燕山大学，
类型：发明
国别省市：