一种基于IEKF的固态激光雷达建图方法技术

本发明专利技术公开了一种基于IEKF的固态激光雷达建图方法，本方法利用IMU辅助固态激光雷达，提高激光雷达自身位姿估计的准确性，首先求解点云帧间运动，作为全局位姿的观测；其次将IMU原始数据进行积分运算，作为全局位姿的预测；然后计算协方差矩阵和卡尔曼增益，不断迭代优化位姿。本方法解决了小视场固态激光雷达里程计估计不准确、建图精度低的问题，为后续机器人自主作业提供了保障。人自主作业提供了保障。人自主作业提供了保障。

【技术实现步骤摘要】
一种基于IEKF的固态激光雷达建图方法


[0001]本专利技术属于机器人
，具体涉及一种基于IEKF的固态激光雷达建图方法。

技术介绍

[0002]自主作业机器人作业时，需实时得到自身的位姿及定位信息，高可信度的里程计估计和建图是其自主完成工作的重要前提。
[0003]目前激光雷达与IMU融合定位和建图方法主要分为两类，一是基于滤波理论，此类融合方式在短时间、小范围内具有较好的精度，同时由于使用的是传感器各自的位姿信息，消耗计算资源相对较少，但在长时间大范围作业时，容易产生累积误差使得位姿可信度降低。二是基于优化理论。此类融合方式考虑移动载体移动过程中的全部位姿状态和环境观测信息，通过构造节点和边缘对机器人的位姿进行约束，可以获得较好的全局地图，但消耗更多的计算资源。

技术实现思路

[0004]针对上述问题，本专利技术提出一种基于IEKF的固态激光雷达建图方法，通过固态激光雷达采集雷达点云信息，经预处理后提取点云特征，进行scan
‑
to
‑
map匹配得到基于雷达的相对位姿矩阵，同时，对IMU原始数据积分处理，预测新的位姿信息，将二者的位姿信息通过IEKF进行数据融合，获得可信度更高的位姿信息，最终结合雷达点云数据，将各帧点云转换到统一的世界坐标系下，更新地图。
[0005]本专利技术解决其技术问题所采用的技术方案是：包括以下步骤：步骤一：使用激光雷达对周围环境进行感知，获取激光点云数据；步骤二：对激光点云数据进行预处理，去除杂点；步骤三：根据雷达点云帧提取特征信息；步骤四：对当前帧点云与特征子地图进行scan
‑
to
‑
map匹配，得到激光雷达位姿信息；步骤五：融合步骤四得到的位姿信息以及将IMU原始数据进行积分计算，得到的全局位姿预测值，前者作为观测值，后者作为预测值，进行迭代扩展卡尔曼滤波，估计优化后的位姿；步骤六：利用激光雷达点云数据和优化后的位姿信息，更新地图。
[0006]步骤一包括：本专利技术的研究对象为激光雷达与惯性测量单元的迭代扩展卡尔曼滤波融合问题，将激光雷达和IMU固连，采用手持方式，通过激光束飞行时差的原理获取点云数据。
[0007]步骤二包括：在提取线特征前，为保证特征点的正确提取，将入射偏转角、点云强度以及激光入射角大于一定阈值的点视为杂点进行剔除。
[0008]步骤三包括：固态激光雷达扫描视场小，花瓣式的扫描方式，特征提取不直观，故提取线特征和面特征时，根据平滑度的大小划分线、面特征，将不平滑的点划分为线特征点，平滑的点划分为面特征点。
[0009]步骤四包括：采用特征点在特征子地图中的映射点指向与其欧式距离最近的线或者平面的方向作为梯度下降的方向，点到线或者面的距离作为残差，即
[0010][0011]式中，G(
·
)为求残差的函数，J
k，i
为第k帧点云、第i个特征点对应的雅可比矩阵。同时，根据残差大小设定特征点在增量方程中的权重，其增量方程为
[0012][0013]式中P
k，i
‑1为第k帧点云、第i个特征点的权重，e
k，i
为残差，J
k，i
为第k帧点云、第i个特征点对应的雅可比矩阵，以Δx
k
为第n次迭代时的增量来更新估计位姿。
[0014]步骤五包括：将IMU原始数据积分计算得到的位姿作为预测值，将激光雷达点云scan
‑
to
‑
map匹配得到的位姿矩阵作为观测值，代入IEKF模型进行数据融合，得到较高精度的位姿估计值。
[0015]步骤六包括：利用优化后的位姿信息，将步骤二处理后的点云数据，变换到统一的世界坐标系下，用Livox
‑
loam建图模块更新地图。
[0016]本专利技术的有益效果是，提出了一种基于IEKF的固态激光雷达建图方法，解决了现有激光雷达与IMU融合定位和建图方法生成高置信度位姿信息以及高计算资源消耗二者矛盾的问题，具有一定实际意义。
附图说明
[0017]图1为本专利技术的整体流程图
[0018]图2为本专利技术实验平台
[0019]图3为IEKF预测更新过程流程图
[0020]图4为对比实验下，本专利技术方法和原生Livox
‑
LOAM定位精度对比
具体实施方式
[0021]参照图1，一种基于IEKF的固态激光雷达建图方法，其特征在于：包括以下步骤：步骤一：使用激光雷达对周围环境进行感知，获取激光点云数据；步骤二：对激光点云数据进行预处理，去除杂点；步骤三：根据雷达点云帧提取特征信息；步骤四：对当前帧点云与特征子地图进行scan
‑
to
‑
map匹配，得到激光雷达位姿信息；步骤五：融合步骤四得到的位姿信息以及将IMU原始数据进行积分计算，得到的全局位姿预测值，前者作为观测值，后者作为预测值，进行迭代扩展卡尔曼滤波，估计优化后的位姿；步骤六：利用激光雷达点云数据和优化后的位姿信息，更新地图。
[0022]参照图2，步骤一包括：本专利技术的研究对象为激光雷达与惯性测量单元的迭代扩展卡尔曼滤波融合问题，激光雷达采用LIVOX的觅道Mid
‑
40工业级激光雷达，IMU采用ALUBI公司的IMU惯性测量模块LPMS
‑
IG1，计算平台采用英伟达Jetson AGX Xavier，采用手持方式采集环境点云数据。
[0023]步骤二包括：在提取线特征前，为保证特征点的正确提取，将入射偏转角、点云强度以及激光入射角大于一定阈值的点视为杂点进行剔除。
[0024]步骤三包括：提取线特征和面特征时，将不平滑的点划分为线特征点，平滑的点划
分为面特征点，其点云平滑度表示为
[0025][0026]上式中，S表示参与计算的点集；
L
X
k，i
表示在雷达坐标系(L系)下第k帧、第i个点的深度。
[0027]步骤四包括：在特征子地图中，将在L系下，第k帧、第i个特征点
L
p
k，i
的映射点指向与其欧式距离最近的线或者平面的方向作为梯度下降的方向，点到线或者面的距离作为残差，即
[0028][0029]式中，G(
·
)为求残差的函数，J
k，i
为第k帧点云、第i个特征点对应的雅可比矩阵。同时，根据残差大小设定特征点在增量方程中的权重，其增量方程为
[0030][0031]式中P
k，i
‑1为第k帧点云、第i个特征点的权重，Δx
k
为第n次迭代时的增量，综上，更新估计位姿为：式中，为迭代后的状态估计。
[0032]步骤五包括：IEKF算法主要分为状态预测以及迭代更新两个步骤，状态预测过程：每接收到一帧IMU数据，根据运动模型，对IMU的状态以及协方差矩阵本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于IEKF的固态激光雷达建图方法，其特征在于：包括以下步骤：步骤一：使用激光雷达对周围环境进行感知，获取激光点云数据；步骤二：对激光点云数据进行预处理，去除杂点；步骤三：根据雷达点云帧提取特征信息；步骤四：对当前帧点云与特征子地图进行scan
‑
to
‑
map匹配，得到激光雷达位姿信息；步骤五：融合步骤四得到的位姿信息以及将IMU原始数据进行积分计算，得到的全局位姿预测值，前者作为观测值，后者作为预测值，进行迭代扩展卡尔曼滤波，估计优化后的位姿；步骤六：利用优化后的位姿信息，将步骤二处理后的点云数据，变换到统一的世界坐标系下，用Livox
‑
loam建图模块更新地图。2.根据权利要求1所述的一种基于IEKF的固态激光雷达建图方法，其特征在于：步骤一包括：本发明的研究对象为激光雷达与惯性测量单元的迭代扩展卡尔曼滤波融合问题，激光雷达采用LIVOX的觅道Mid
‑
40工业级激光雷达，IMU采用ALUBI公司的IMU惯性测量模块LPMS
‑
IG1，计算平台采用英伟达Jetson AGX Xavier，采用手持方式采集环境点云数据。3.根据权利要求1所述的一种基于IEKF的固态激光雷达建图方法，其特征在于：步骤二包括：在提取线特征前，为保证特征点的正确提取，将入射偏转角、点云强度以及激光入射角大于一定阈值的点视为杂点进行剔除。4.根据权利要求1所述的一种基于IEKF的固态激光雷达建图方法，其特征在于：步骤三包括：提取线特征和面特征时，将不平滑的点划分为线特征点，平滑的点划分为面特征点，其点云平滑度表示为上式中，S表示参与计算的点集；
L
X
k，i
表示在雷达坐标系(L系)下第k帧、第i个点的深度。5.根据权利要求1所述的一种基于IEKF的固态激光雷达建图方法，其特征在于：步骤四包括：在特征子地图中，将在L系下，第k帧、第i个特征点
L
p
k，i
的映射点指向与其欧式距离最近的线或者平面的方向作为梯度下降的方向，点到线或者面的距离作为残差，即式中，G(
·
)为求残差...

【专利技术属性】
技术研发人员：刘今越，孙浩然，孙鑫豹，郑佳硕，杨旭钊，
申请(专利权)人：河北工业大学，
类型：发明
国别省市：