(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211074370.0 (22)申请日 2022.09.02 (71)申请人 燕山大学 地址 066000 河北省秦皇岛市海港区河北 大街438号 (72)发明人 丁伟利　邵长宇　 (74)专利代理 机构 石家庄众志华清知识产权事 务所(特殊普通 合伙) 13123 专利代理师 张建 (51)Int.Cl. G06T 7/269(2017.01) G06T 7/73(2017.01) G01C 21/16(2006.01) G01S 17/88(2006.01) G01S 17/86(2020.01)G06F 16/29(2019.01) G06N 3/08(2006.01) (54)发明名称 基于多传感器的机器人实时定位和彩色地 图融合映射方法 (57)摘要 本发明提出一种基于多传感器的机器人实 时定位和彩色地图融合映射方法， 快速采集周边 环境信息， 并实时构建彩色全局地图。 视觉、 激 光、 惯性传感器采集模块通过读取传感器的信 息， 对激光点云数据进行累加， 形成点云扫描帧， 然后进行预处理， 得到提取后的特征点云， 同时 对不同传感器采集到的数据进行时间同步。 利用 采集到的激光和惯性数据实时对机器人进行自 定位， 同时构建单帧点云地图。 利用视觉传感器 采集到的RGB信息， 对构建的全局点云地图进行 纹理和颜色渲染， 构建单帧彩 色地图。 权利要求书5页 说明书9页 附图3页 CN 115526914 A 2022.12.27 CN 115526914 A 1.基于多传感器的机器人实时定位和彩色地图融合映射方法， 其特征在于包括以下步 骤： 步骤1： 读取视觉传感器、 激光雷达传感器、 惯性传感器的数据， 对不同源的图像、 激光 点云和imu位姿 数据进行时间同步， 获得 统一的时间系统下的待融合数据， 并利用惯性传感 器采集到的前20帧数据进行初始化， 建立初始位姿估计； 步骤2： 基于imu传感器的高工作频率， 惯性传感器选用imu传感器， 对imu传感器采集到 的惯性数据进行前向传播， 得到各个时刻机器人位姿的先验估计； 得到各个时刻间对机器 人的位姿的先验估计后， 通过对时刻进行插值， 计算出每个激光点采集时刻和激光点云帧 采集结束时刻间机器人的相对运动， 将激光点从采集时刻的机器人坐标系转移到点云帧采 集结束时刻的坐标系中， 对当前帧中每一个激光点进行上述操作后， 得到消除帧间运动畸 变的点云帧采集结束时刻点云； 步骤3： 使用误差状态迭代卡尔曼滤波器对不同帧间采集到的信 息进行比较， 在机器人 运动过程中， 一个地标物往往会在连续很多帧上同时存在， 并且和机器人 的相对位置不断 变化， 对新采集到的点云帧中的每一点， 在地图中提取和当前位置最近的五个点拟合出一 个小平面， 通过迭代优化， 不断缩小激光点和它对应的拟合平面间的距离， 直到小于阈值， 从而得到点云帧采集结束时刻对机器人运动的最优估计； 利用计算出 的最优估计， 将点云 帧采集结束时刻视觉传感器采集到的图像转移归一化坐标系并最 终转移到雷达坐标系中， 对当前帧中的每一个点， 利用预标定过 的激光雷达传感器和视觉传感器的旋转平移关系， 从三维空间坐标转移到像素坐标系， 给视觉传感器视场范围内的每个激光点赋予RGB信息， 得到经过RGB渲 染的彩色点云； 使用基于时间的插帧处理， 利用激光雷达传感器采集时刻的 前后两帧图片， 使用稠密光流法估计出激光雷达传感器采集时刻的视觉数据， 实现激光雷 达传感器和视 觉传感器的时间同步； 步骤4： 对经过RGB渲染的彩色点云通过点云帧采集结束时刻对机器人运动的最优估 计， 将点云从点云帧采集结束时刻的机器人运动坐标系转移到独立的世界坐标系， 从而构 建出当前帧的彩色点云地图； 将地图中的点以K维树的形式组织起来， 使得新采集到的每一 个点都可以用最短的时间找到它在地图中的最邻近点； 通过时间累积， 将机器人采集到的 多帧信息累加， 得到连续各帧间的机器人运动最优估计和各帧的彩色点云地图， 实现机器 人的自我定位并建立全局彩色地图。 2.根据权利要求1所述的基于多传感器的机器人实时定位和彩色地图融合映射方法， 其特征在于所述 步骤2具体包括以下步骤： 步骤2.1： 建立imu运动学模型， 对于机器人运动的描述中最重要 的两个物理量是当前 时刻的位姿和相对于坐标系原点的位移量， 使用imu传感器测量得到的角速度和加速度数 据对时间维度积分得到所求的位姿和位移状态， 因此建立在世界坐标系下的状态表达方 程： 在建立世界坐标系的状态表达方程中一共有6个状态量， 分别是机器人在世界坐标系 下的位姿RI， 即机器人相对于初始状态的旋转量， 使用一个3*3的旋转矩阵来表示； 机器人 在世界坐标系下相对于原点的位移量pI， 使用一个3*1的平移向量来表示； 机器人在x、 y、 z权　利　要　求　书 1/5 页 2 CN 115526914 A 2三个轴上的瞬时速度vI,使用用一个3*1的速度向量来表示； 机器人惯性imu传感器采集数 据的在角速度和加速度上的偏差bω和ba； 机器人世界坐标系相对于真实世界坐标系下的重 力矢量g； 步骤2.2： 对于建立好的状态方程， 对于imu数据进行预积分， 建立imu运动学连续模型； 其中， 机器人在世界坐标系下的位姿的一阶微分量等于机器人相对于初始状态的旋转矩阵 右乘消除噪音后的角速度向量的反对称矩阵； 机器人在世界坐标系下的位移的一阶微分量 等于机器人在x、 y、 z三个轴上的瞬时速度； 机器人的速度的一阶微分量等于机器人相 对于 初始状态的旋转矩阵乘以消除噪音后的加速度向量再加上机器人世界坐标系下的重力矢 量； 机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba的模型为带高斯 噪声nbω和nba的随机游动过程； 机器人世界坐标系下的重力矢量g的一阶微分量为0， 由此可 得机器人的imu运动学 方程如下： 对上述方程组离 散化可得 步骤2.3： 步骤1中进行初始化， 得到了机器人运动状态的初始估计， 当机器人收到imu 传感器的输入时， 即可根据步骤2.2中建立的离散运动学模 型进行前向传播， 通过初始估计 和imu传感器每一帧采集到的加速度和角速度数据， 迭代出每一帧时刻的机器人位姿的先 验估计； 具体的迭代方程如下 xi+1＝xi+(Dtf(xi,ui,wi)) 其中控制量ui为imu传感器采集到 的的加速度数据 am和角速度数据ωm,误差量w为imu 传感器的累积误差nω、 na和imu传感器采集到的数据的误差nbω、 nba； 步骤2.4： 激光雷达传感器采集完完整一帧点云后， 根据imu传感器数据前向传播得到 的各个时刻的机器人运动状态进 行插值计算， 得到每个激光点采集时刻 对应的机器人运动 状态， 并计算出每个激光点采集时刻到点云帧采集结束时刻的机器人相对运动状态 其中j为每个雷达点的采集时刻， k为点云帧的采集时刻； 将j时刻采集到的激光点先从j时 刻的激光雷达坐标系转移到imu传感器坐标系， 再利用j时刻到k时刻的机器人的相 对运动 将激光点 从j时刻的imu传感器坐标系转移到k时刻的imu传感器坐标系， 最后再将 激光点先权　利　要　求　书 2/5 页 3 CN 115526914 A 3