(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211143294.4 (22)申请日 2022.09.20 (71)申请人 成都睿芯行 科技有限公司 地址 610000 四川省成 都市高新区府 城大 道西段39 9号7栋2单 元12层1209号 (72)发明人 周军　黄佳遥　龙羽　徐菱　 (74)专利代理 机构 成都佳划信知识产权代理有 限公司 5126 6 专利代理师 邹翠 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/16(2006.01) G01C 21/20(2006.01) G06F 17/10(2006.01) G06V 10/80(2022.01) (54)发明名称 一种基于多传感器融合的轻量级定位与建 图方法 (57)摘要 本发明公开了一种基于多传感器融合的轻 量级定位与建图方法， S1： 是否为定位模式， 否， 进入建图模式； S2： 建图模式下， 激光雷达传感 器、 里程计传感器和惯性传感器数据进行算法运 行； S3： 激光雷达传感器采集的激光数据转换到 地图坐标系构建环境局部地图， S4： 概率栅格构 建环境地图； S5： 激光数据、 里程计数据和惯性单 元数据进行位姿推测， 与局部地图匹配； S6： 位姿 优化， 得到激光在子图下最优位姿， S7： 设定子图 内激光数量， 有后续激光数据时， 新建子图， 重复 操作， 从而得到环境地图。 多传感器来构建环境 地图， 实时输 出机器人位姿数据， 对其进行优化， 保证建图效果， 多传感器融合可以使定位精度较 高。 权利要求书1页 说明书4页 附图2页 CN 115218892 A 2022.10.21 CN 115218892 A 1.一种基于多传感器融合的轻量级定位与建图方法， 其特 征在于： 包括以下步骤： S1： 确定是否为定位模式， 若是， 进入定位模式， 若否， 则进入建图模式； S2： 建图模式下， 激光雷达传感器、 里程计传感器和惯性传感器的数据来进行建图算法 的运行； S3： 默认当前的机器人的初始位置为 （0， 0， 0） ， 通过将激光雷达传感器采集的激光数据 转换到地图坐标系下来构建 当前环境的局部地图， 转换公式为： ； 其中 为当前机器人的位置数据， P为每一个激光点的坐标， 表示的是当前机器人位 置的横坐标， 表示的是当前机器人位置的纵坐标， 为表示的是当前机器人位置的角度 坐标； S4： 通过概 率栅格的方式来构建环境 地图； S5： 通过上一时刻的激光数据、 里程计数据和惯性单元数据来进行位姿推测， 并与当前 的局部地图进行匹配； S6： 对当前激光在当前局部地图下的位姿进行优化， 得到当前激光在当前子图下的最 优位姿， 优化方式为： ； 其中 表示一帧激光雷达数据中的雷达数据点， K表示一帧激光 雷达数据下总的雷达数 据点的个数， 表示对栅格的数值进行插值平 滑操作； S7： 通过设定一个子图内的激光数量， 当有后续激光数据时， 就会新建子图， 重复操作， 直到将所有的激光束都插 入到对应的子图内， 从而得到环境 地图。 2.根据权利要求1所述的一种基于多传感器融合的轻量级定位与建图方法， 其特征在 于： 所述S4中， 激光击中的栅格做hit标记， 激光没有击中的栅格做miss标记， 最终来统计每 一个栅格的hit概 率和miss概率来作为该栅格的概 率数值。 3.根据权利要求2所述的一种基于多传感器融合的轻量级定位与建图方法， 其特征在 于： 所述S5中， 通过融合惯性单元 的角度数据以及里程计数据的平移数据来推测得到当前 时刻估计位姿。 4.根据权利要求3所述的一种基于多传感器融合的轻量级定位与建图方法， 其特征在 于： 所述S6中， 将对应栅格的数值进行平 滑操作， 并做归一 化处理。 5.根据权利要求1所述的一种基于多传感器融合的轻量级定位与建图方法， 其特征在 于： 在定位模式下， 包括以下步骤： A1： 加载建好的环境 地图； A2： 构建相邻的子图， 当机器人移动时， 只构建机器人当前位姿相邻前后的子图； A3： 通过机器人当前的激光雷达数据来进行激光与地图匹配， 来获取当前激光在子图 下的位姿数据； A4： 对当前子图下的位姿数据进行优化， 得到最优的实时位姿数据。权　利　要　求　书 1/1 页 2 CN 115218892 A 2一种基于多传感器融合的轻量级定位 与建图方 法 技术领域 [0001]本发明涉及基于多传感器的建图定位方法， 特别是一种基于多传感器融合的轻量 级定位与建图方法。 背景技术 [0002]在定位与建图技术中， 常用的传感器主要有激光雷达、 相机、 里程计和惯性单元 等， 其中相机和激光雷达作为主要的传感器， 由于相机会受到光照等条件的影响， 因此其构 建地图的质量以及定位精度会较差， 无法应用于高精度定位。 激光雷达主要分为二维激光 雷达与三维激光雷达， 二 维激光雷达具有低成本的优势， 广泛应用在室内移动机器人上。 传 统方法中通常使用单一的二 维激光雷达传感器来定位和建图， 这样的缺点是定位和建图的 精度不高； 且单一的算法只能用于单一的建图应用和单一的定位应用， 无法进行自适应切 换， 算法的成本较高， 而且整个算法具有较高的计算复杂度。 发明内容 [0003]本发明的目的在于克服现有技术的缺点， 提供一种基于多传感器融合的轻量级定 位与建图方法。 [0004]本发明的目的通过以下技术方案来实现： 一种基于多传感器融合的轻量级定位与 建图方法， 包括以下步骤： S1： 确定是否为定位模式， 若是， 进入定位模式， 若否， 则进入建图模式； S2： 建图模式下， 激光雷达传感器、 里程计传感器和惯性传感器的数据来进行建图 算法的运行； S3： 默认当前的机器人的初始位置为 （0， 0， 0） ， 通过将激光雷达传感器采集的激光 数据转换到地图坐标系下来构建 当前环境的局部地图， 转换公式为： ； 其中 为当前机器人的位置数据， P为每一个激光点的坐标， 表示的是当前机器 人位置的横坐标， 表示的是当前机器人位置的纵坐标， 为表示的是当前机器人位置的 角度坐标； S4： 通过概 率栅格的方式来构建环境 地图； S5： 通过上一时刻的激光数据、 里程计数据和惯性单元数据来进行位姿推测， 并与 当前的局部地图进行匹配； S6： 对当前激光在当前局部地图下的位姿进行优化， 得到当前激光在当前子图下 的最优位姿， 优化方式为： ；说　明　书 1/4 页 3 CN 115218892 A 3