(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221078426 5.X (22)申请日 2022.07.05 (71)申请人 东南大学 地址 210096 江苏省南京市玄武区四牌楼 2 号 (72)发明人 高旺　刘宏　潘树国　乔龙雷　 (74)专利代理 机构 南京众联专利代理有限公司 32206 专利代理师 周蔚然 (51)Int.Cl. G01S 17/89(2020.01) G01S 17/86(2020.01) G01C 21/16(2006.01) G01S 19/49(2010.01) G01S 19/48(2010.01)G06T 7/73(2017.01) G06F 16/29(2019.01) (54)发明名称 基于大场景的低线束激光雷达-IMU-RTK定 位建图算法 (57)摘要 本发明公开了一种基于大场景的低线束激 光雷达‑IMU‑RTK定位建图算法， 包括： 初始化阶 段使用RTK位置信息 快速精准的完成IMU对 准， 并 解算激光雷达 ‑IMU‑RTK系统在全局坐标下的正 确初始位姿； 提取周围点云环境特征， 根据曲率 区分角点和面点； 实时构建局部点云地图， 利用 当前帧点云信息与局部地图动态匹配， 计算当前 激光雷达位姿； 图优化模型构建激光雷达里程计 因子、 IMU预积分 因子、 RTK绝对 位置因子， 优化求 解定位结果并进行实时点云地图拼接。 本发明通 过图优化模 型， 能够解决低线束激光雷达在大场 景特征稀 疏环境下定位和建图问题， 同时能够保 障实时采集。 权利要求书3页 说明书6页 附图2页 CN 115407357 A 2022.11.29 CN 115407357 A 1.基于大场景的低线 束激光雷达 ‑IMU‑RTK定位建图算法， 其特 征在于， 具体方法如下： 步骤1、 初始化利用RTK位置信息完成IMU对准， 并解算激光雷达 ‑IMU‑RTK系统在全局坐 标下的正确初始位姿； 步骤2、 提取周围点云环境特征， 根据扫描点曲率区分角点和面点， 采用IMU数据进行点 云去畸变校正； 步骤3、 实时构建局部点云地图， 对地图与当前帧进行体素滤波提升后续计算效率， 并 利用当前帧点云信息与局部地图动态匹配， 迭代计算当前激光雷达位姿； 步骤4、 图优化模型构 建激光雷达里程计因子、 IMU预积 分因子、 RTK绝对位置因子， 优化 求解定位结果并进行实时点云地图拼接， 获得一 致性较好的全局地图。 2.如权利要求1所述的基于大场景的低线束激光雷达 ‑IMU‑RTK定位建图算法， 其特征 在于， 步骤1所述 IMU初始对准按下式解得： 式中， 表示导航坐标系到载体坐标系的转换矩阵； ab、 bb、 cb分别表示载体坐标系下的 位置坐标； an、 bn、 cn表示对应点在导航坐标系下的坐标值， 初始对准确定惯 性器件在导航坐 标系下的姿态， 获得激光雷达 ‑IMU‑RTK融合定位在全局坐标 下的姿态初值。 3.如权利要求1所述的基于大场景的低线束激光雷达 ‑IMU‑RTK定位建图算法， 其特征 在于， 步骤2所述提取环境点云特征， 根据扫描点曲率区分角点和面点， 曲率c计算 公式如下 式： |S|代表此扫描点的同线束周围点集合， ri表示此点的深度值， rj表示此点附近同线束 点深度值， 通过此式依次计算每帧激光雷达扫描到的所有点的曲率， 同时， 利用每个点的深 度信息剔除平行点和遮挡点， 为了提升遍历效率， 每帧点云坐标信息投影成二 维图像， 并分 为六个子图， 对子图分别提取角点和面点， 根据阈值cth进行判定， c＜cth为角点， c ＞cth为面 点， 由于载体运动， 每帧扫描过程会出现点云畸变， 采用高频IMU数据短时间积分运算实现 运动补偿。 4.如权利要求1所述的基于大场景的低线束激光雷达 ‑IMU‑RTK定位建图算法， 其特征 在于， 步骤3所述的实时构建局部点云地图， 对地图与当前帧进 行体素滤波提升后续计算效 率， 利用当前帧点云信息与局部地图动态匹配， 迭代计算当前激光雷达位姿， 采用滑窗的方 式合并一定范围内的关键 帧信息， 为了提升点云匹配速率， 局部地图的点云信息都存储在 kd‑tree的数据结构中， 方便后续搜索， 运动估计模块将当前帧与局部地图匹配求解运动状态， 对于角点， 搜索其在局部地图 最近的五个点， 计算五点均值和协方差矩阵， 当有满足条件的线 特征， 计算当前角点到线的 距离， 距离计算公式如下： 权　利　要　求　书 1/3 页 2 CN 115407357 A 2其中·和×分别表示点积和叉乘运算， pn是单位向量， pε代表当前角点， 代表特征值 对应的特征向量， 同理， 面点ps在局部地图中搜索出五点构成平面， 此时五点的协方差矩阵 的最小特 征值对应的特 征向量 为这个平面所对应法向量， 点 面距离计算如式(5)： 式子 和 分别表示局部地图提取角点和面点的几何中心， 通过联合最小化点线残 差f(pε)和点面残差f(ps)， 得到当前点在局部地图的最佳位姿估计Tk， 求解过程如下式： min{∑f(pε)+f(ps)}            (6)。 5.如权利要求1所述的基于大场景的低线束激光雷达 ‑IMU‑RTK定位建图算法， 其特征 在于， 步骤4所述的图优化模型构建激光雷达里程计因子、 IMU预积分因子、 RTK绝对位置因 子， 优化求解定位结果并进 行实时点云地图拼接， 对于一定窗口内的关键帧， 数量为n， 优化 状态量 最小化求解方程如下式： 式中Rp(x)代表边缘化残差， 边缘化的目的是减少优化方程规模， 移出滑窗的关键帧信 息保存为当前优化的先验信息， 代表滑窗内激光雷达里程计的帧间约束， 残差计算如下式： 式中i和j分别表示上一帧和当前帧， T是位姿的欧式矩阵表示， ΔT即为 位姿变化 量， 代表滑窗内IMU预积分约束， 残差按下式解得： 式中 是旋转矩阵表示的姿态， W是世界坐标系， bi和bj指第i帧与第j帧所在载体系， δ α 、 δ β 和 δ θ 分别表示位置、 速度和角度的变 化量， δba和 δbw分别表示加速度计和陀螺仪的偏置 变化,Δt指两帧之间的时间间隔， gW表示世界坐标系下的重力值， pW、 vW和qW是世界系下的权　利　要　求　书 2/3 页 3 CN 115407357 A 3