【技术实现步骤摘要】
本专利技术属于激光slam，具体涉及一种环境平面信息有效合并的定位建图方法。
技术介绍
1、激光-(惯性)里程计得益于高鲁棒性和高定位精度在自动驾驶汽车、无人飞行器、移动机器人等领域获得广泛应用。其中，便于精准匹配和有效增长的历史地图表示是里程计算法设计的关键。
2、目前，先进的slam(simultaneous localization and mapping，同时定位与地图构建)算法主要采用kd-tree(k-dimensional tree，k维树)结构和空间哈希的体素结构进行历史地图的管理。在kd-tree结构中，拟合平面使用的k最近邻算法会消耗大量的算力，影响算法运行的实时性。且在kd-tree框架中进行点的协方差估计会占用大量的内存空间，在实践中无法实现。基于空间哈希结构设计的voxelmap框架在鲁棒性和定位精度上具有较高的性能。但六自由度的平面表示消耗大量不必要的算力和内存，同时对平面相互关系考量的缺失导致地图中相同平面的重复表示。针对上述定位建图算法的问题，为了提高slam框架的实时性和定位精度，本专利技术提出了一种环境平面信息有效合并的定位建图方法。通过重新定义平面的三自由度表示降低系统复杂度和算法的内存占用，利用并查集算法合并平面，降低平面表示的不确定度，提升平面表示的精度，实现算法定位精度和运行效率的双重提升。
技术实现思路
1、针对现有传统定位建图算法的上述不足，本专利技术提出的一种环境平面信息有效合并的定位建图算法，比传统的定位建图算法在效率
2、为了达到以上目的，本专利技术采用的技术方案为：
3、本方案提供一种环境平面信息有效合并的定位建图方法，其特征在于，包括以下步骤：
4、s1、获取激光雷达传感器原始数据，积累形成激光点云帧；
5、s2、通过imu(inertial measurement unit，惯性测量单元)数据反向传播去除点云畸变，前向传播(其中，前向传播是指基于积分imu测量的线性加速度和角速度，从而推断系统的运动轨迹和方向)推断系统当前时刻的初始位姿(其中，初始位姿是指系统在算法开始时的初始位置和朝向)；
6、s3、去畸变后的点云进行空间体素(其中，体素指三维空间中具有固定大小和位置的体积单元)划分，在各体素内拟合平面、估计不确定度，形成局部地图；
7、s4、利用imu前向传播获得的位姿将新到来激光点投影至局部地图，计算获得点到对应体素内平面的距离残差并添加至ieskf(迭代误差状态卡尔曼滤波器)中优化，获得最佳位姿；
8、s5、滤波处理后的激光点添加到对应体素中，在体素内完成平面参数和协方差的构造或更新；
9、s6、根据地图中相邻体素内平面间的共面关系，利用并查集算法(其中，并查集算法是一种用于解决集合操作问题的简单而有效的数据结构和算法，涉及元素之间的合并和查询操作等)实现相邻体素间的平面合并(其中，被合并的平面是由于空间体素划分导致原本整个平面被划分为若干共面的小平面)，并进一步完成合并后平面的参数更新，用于后续激光帧的位姿估计；
10、进一步地，所述步骤s1中获得的激光雷达传感器的原始数据如下：
11、在数据采集阶段，通过激光雷达传感器获取环境中的信息，激光雷达采集信息的数据格式为(x，y，z，intensity，timestamp)，其中，(x，y，z)是激光点在环境中的位置坐标，intensity是激光点处测得的反射强度，timestamp是采集激光点时的时间戳。
12、再进一步地，所述步骤s2包括以下步骤：
13、s201、为获得系统初始的先验位姿x，将imu数据带入模型，计算获得系统初始的先验位姿x如下式所示：
14、
15、其中，是全局坐标系下系统的姿态，是全局坐标系下系统的位置，是全局坐标系下系统的速度，是imu的角速度偏差，是imu的加速度偏差，是全局坐标系下的重力矢量。
16、s202、为去除点云帧中由于激光点采集时间不一致导致的点云畸变(其中，点云畸变是指使用激光雷达进行三维点云数据采集时，由于各种因素引起的数据失真或偏差)，利用imu测量数据计算各激光点(采集时间pj)和当前帧最后一个激光点(采集时间tk)间的相对位姿，将当前帧所有激光点投影到同一tk时刻，获得去畸变后的点云帧。
17、再进一步地，所述步骤s3包括以下步骤：
18、s301、建立激光点测量模型，利用激光点数据和imu的估计位姿计算获得世界坐标系下激光点的测量协方差，如下式所示：
19、
20、其中，是由激光雷达坐标系到世界坐标系的旋转变换，是激光雷达坐标系下激光点的协方差，是激光雷达坐标系到世界坐标系的平移变换的协方差，表示平移位姿估计所带来的不确定度，是激光雷达坐标系到世界坐标系的旋转变换的协方差，表示旋转位姿估计所带来的不确定度，表示将向量转换为用于叉积运算的斜对称矩阵。
21、s302、为进一步提升平面拟合和协方差估计的效率，减少该模块的内存占用，本专利技术抛弃原有的六自由度表示方法，利用最小二乘法根据激光点的位置坐标和协方差计算获得体素内平面的参数和协方差的三自由度表示(其中，三自由度表示是指在三维空间中用三个参数来描述一个对象的姿态)，如下式所示：
22、
23、其中，e＝[-∑xizi -∑yizi -∑zi]t
24、其中，n是拟合平面的法向量，(xi，yi，zi)是指第i个激光点的位置坐标，∑n是指平面三自由度表示的不确定度，是指世界坐标系下平面上激光点的协方差矩阵。
25、再进一步地，所述步骤s4包括以下步骤：
26、s401、利用imu前向传播获得的系统当前时刻的位姿和和对应的协方差将新到来的激光点投影到对应体素中，计算点到体素内平面的距离残差，构建最大后验估计。其中，第i次点到面距离形成的观测方程如下式所示：
27、
28、
29、其中，是第k次迭代中点到面的距离，由体素内平面的参数(其中，平面参数包括法向量参数，记作ω和中心点坐标，记作d)以及世界坐标系下激光点坐标计算获得，表示距离残差对位姿估计误差的雅克比行列式，表示位姿估计的误差(其中，xi表示当前的真实位姿，表示第k次迭代中的位姿估计，表示封装算子，vi是由激光点测量和平面拟合带来的观测噪声。
30、s402、结合系统先验位姿和系统传感器所有的有效测量信息，经迭代误差状态卡尔曼滤波器滤波后输出准确的系统位姿。其中，获得的最大后验估计如下式所示：
31、
32、其中，表示系统状态的先验位姿，是系统的测量观测(其中，定义运算符)。
33、再进一步地，所述步骤s5包括以下步骤：
34、s501、为保证系统的实时性能，采用空间哈希(其中，空间哈希方法是一种用于高效处理和查询空间中的对象的数据结构和算本文档来自技高网...
【技术保护点】
1.一种环境平面信息有效合并的定位建图方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤S1中采集的激光点的数据格式如下：
3.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤S2的步骤如下：
4.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤S3包括以下步骤：
5.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤S4包括以下步骤：
6.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤S5包括以下步骤：
7.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤S5包括以下步骤：
【技术特征摘要】
1.一种环境平面信息有效合并的定位建图方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤s1中采集的激光点的数据格式如下：
3.根据权利要求1所述的一种环境平面信息有效合并的定位建图方法，其特征在于，所述步骤s2的步骤如下：
4.根据权利要求1所述的一种环境平面信息有效合并的定位建图方...
【专利技术属性】
技术研发人员：武畅，李其炎，袁翼飞，杜小琦，夏玉林，
申请(专利权)人：电子科技大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。