一种非理想环境下的激光/惯性里程计方法

本发明涉及机器人导航和定位，具体为一种非理想环境下的激光/惯性里程计方法。

背景技术：

1、3d激光雷达是一种能够测量物体距离和形状的传感器，其原理为通过发射激光束并接受反射回来的光信号来计算物体于传感器之间的距离。激光雷达可以提供高精度、高分辨的环境深度信息。惯性测量单元通常包括加速度计和陀螺仪，能够测量物体的线性加速度和角速度。imu通过积分加速度计和陀螺仪的输出可以估计物体的位置、速度和姿态。然而，imu存在误差累积的问题，长时间积分会引入位置漂移和姿态误差。激光/惯性里程计是一种融合激光雷达和imu数据从而估计机器人或无人车姿态和位置的方法。这种方法利用激光雷达提供的环境深度信息，通过特征提取和匹配来计算机器人的相对位姿变化。同时，通过imu测量的加速度和角速度数据，结合运动模型和滤波算法对机器人运动进行估计和预测。激光/惯性里程计融合了两种传感器的优点，能提供高精度、实时的定位和导航解决方案，广泛应用于自动驾驶、移动机器人和无人系统等领域。

2、尽管激光/惯性里程计具有广泛的应用和许多优点，但在实际使用中，传统的激光/惯性里程计常常面临一些限制和挑战。例如，在遇到几何上连续重复或结构特征稀疏的场景时，由于无法提取足够的特征，系统可能会出现精度退化甚至失效的情况；此外，点云特征匹配是基于静态环境的假设，而当系统运行在动态场景中时，动态物体会干扰传感器的观测数据，导致定位准确性下降，并在点云地图中留下残影。因此，如何提升激光/惯性里程计在非理想环境下的鲁棒性和准确性成为亟待解决的问题。

3、与专利cn114526745a“一种紧耦合激光雷达和惯性里程计的建图方法及系统”的技术对比中

4、1、专利cn114526745a中，使用因子图融合激光雷达和imu数据，其缺点是随着系统运行，因子图规模逐渐增大，内存消耗和计算时间也会逐渐增加。相比之下，本发明采用了迭代误差状态卡尔曼滤波的方法，在计算速度和内存消耗方面表现更加良好。

5、2、专利cn114526745a中，采用了角特征点和平面特征点构建点-线和点-面约束进行位姿优化。然而，这种仅使用几何特征点的方法在特征退化场景中容易出现由于无法提取足够的特征从而导致系统定位精度下降甚至无法正常运行的问题。而本发明在提取几何特征的同时引入了强度特征，这种综合特征的引入使得系统能够更好地适应各种场景，并提供更可靠的定位结果。

6、3、专利cn114526745a中，该方法在进行关键帧与局部地图特征匹配时基于静态环境的假设，但未考虑到实际应用场景中存在的动态物体会在点云特征提取和匹配的过程中造成特征点的不稳定且关联性差，进而降低系统精度并在点云地图中留下残影。而本发明在对点云数据处理时增加了动态物体检测与剔除步骤，有效地避免了这种情况的发生。

7、与专利cn114565726a“一种非结构化动态环境下的同时定位与建图方法”的技术对比中

8、1、专利cn114565726a中，通过对点云进行语义分割，将动态物体和静态墙壁进行有效分离，并在墙壁点云中提取特征进行匹配，以避免动态物体的干扰。然而，这种方法容易将半静态物体误判为动态物体，导致提取到的特征点不足，从而使定位精度下降。而本发明采用一种对比相邻两帧点云簇类包围框iou的方法检测动态物体，能有效保留半静态物体并将其用于特征匹配。

9、2、专利cn114565726a中，惯性数据仅用于去除点云畸变和提供优化的初始值，通过松耦合方式将惯性数据和激光雷达数据进行融合。与此不同的是，本发明提出了一种紧耦合的激光/惯性里程计方法，相较之下具备更好的鲁棒性和精度。

技术实现思路

1、为解决上述技术问题，本发明提出了一种非理想环境下的激光/惯性里程计方法，，本发明具有精度高、鲁棒性强、计算效率高的优点，并能够在大多数非理想场景中提供精确可靠的位姿估计结果，具有重要的实际应用价值。

2、为实现上述目的，本发明采取的技术方案是：

3、一种非理想环境下的激光/惯性里程计方法，具体步骤为：

4、步骤s1：imu数据积分，imu数据到来时对名义状态进行积分，对误差状态和协方差进行预测；

5、步骤s2：点云数据预处理，激光雷达点云数据到来时，首先进行点云去畸变处理，然后将无序点云进行有序化处理，即将点云按照角度和线数投影到点云图像上，接着执行剔除地面点的操作，最后对点云进行欧式聚类；

6、步骤s3：动态物体剔除，对已分割的点云簇提取包围框，然后通过名义状态将包围框变换到初始坐标系下，最后计算初始坐标系下相邻两帧点云簇包围框的iou来检测动态物体；

7、步骤s4：点云特征提取，通过计算各点曲率提取线特征点和面特征点。对平面点按照入射角和入射距离进行点云强度补偿并提取强度边界特征；

8、步骤s5：构建观测方程并更新状态，以点-线、点-面残差构建观测方程，使用改进的迭代误差卡尔曼滤波状态更新公式得到后验误差状态从而得到更准确的载体位姿估计。

9、作为本发明进一步改进，步骤s2所述点云数据预处理方法包括以下步骤：

10、步骤s2.1：点云去畸变：

11、计算该帧起始点角度，根据各点角度确定点云实际采集时刻，结合已有imu积分数据将点云坐标插补至该帧结束时刻从而消除畸变带来的影响；

12、步骤s2.2：将无序点云进行有序化处理：

13、首先，遍历当前帧的每个点，并计算该点与坐标原点之间的射线相对于x轴正方向的角度；然后，将该角度除以激光雷达的角度分辨率，得到一个相对于角度分辨率的列数；

14、接着，根据该点所属的激光发射器的线束，将其作为点云图像的行数，使用计算得到的列数和行数作为索引，将当前点放入点云图像的相应位置，在点云图像中，每个像素代表着一个特定角度和线束的点，具有坐标信息和强度值；

15、步骤s2.3：剔除地面点：

16、按列遍历点云图像中的点，假设点云图像中同一列相邻行的两个点a、b，计算垂直高度差：h＝|z0-z1|和水平距离差：计算垂直高度差和水平高度差的角度此处θ的物理意义为连线ab与水平面的夹角，若ab两点均为地面点，理想情况下θ应该接近于0，将阈值设为10度，当计算得到的θ值小于10度时，a、b点将被标记为地面点，并剔除；

17、步骤s2.4：欧式聚类：

18、剔除地面点后将点云作欧式聚类，使得距离上相近的点被分割为同一个簇。

19、作为本发明进一步改进，步骤s3所述动态物体剔除步骤包括以下步骤:

20、步骤s3.1：三维点云经过步骤s2点云预处理后被分割为数个点云簇，对每个簇，计算长宽高，以确定一个能够包围该簇点云的三维包围框，同时计算其形心；

21、步骤s3.2：记录当前帧下各包围框的坐标，并结合imu数据积分得到的状态，将当前帧的各个簇的形心点和包围框坐标转换到初始坐标系下；

22、步骤s3.3：对于当前帧的每个簇，在初始坐标系下的上一帧点云中选择与之形心距离最近的簇，计算两个簇包围框之间的iou，并与阈值进行比较，如果iou小于阈值，则将当前帧中该簇点云标记为动态物体，并从包围框中剔除相应的点云；

23、作为本发明进一步改进，步骤s4点云特征提取同时提取几何特征和强度特征包括以下步骤：

24、s4.1几何特征：

25、由步骤s2点云数据预处理后，得到按角度和线束将点云进行投影的点云图像，每个点云的坐标值反应了点云的深度，以每行的顺序遍历点云图像中的点，计算每个点的曲率，将点云图像中的每一行各平均划分为6个区域，在各区域中选取曲率值最大的2个点作为线特征点，曲率最小的2个点作为面特征点，与此同时，将曲率小于阈值的点标记为平面点；

26、s4.2强度特征：

27、激光点云强度值由从反射信号接收的能量与发射的激光功率之比决定：

28、

29、其中i为点云强度，pr为接收功率，pe为发射功率，ρ为材料反射率，r为测量距离，α为物体表面和激光的入射角度，ηsys和ηatm为系统和大气传输因子，将ηsys和ηatm视为常数，因此i主要与入射角α、距离r和材料反射率ρ相关：

30、

31、基于上述原理，采用一种基于点云强度补偿的方法提取强度特征：

32、针对被标记为的平面点，其距离信息根据坐标值得到；而入射角则通过分析局部法向量来计算：选取平面点p的两个近邻点p1和p2计算局部法向量n：

33、

34、则:

35、

36、选定标准距离rs和标准入射角αs，将各平面点的强度值u补偿到标准距离和入射角下：

37、

38、补偿后的点云强度主要受材料反射率ρ影响。通过sobel算子可以计算点云图像中平面点上的强度边界，与几何特征选取方式相同，将点云图像中的每一行各平均划分为6个区域，并选取各线各区中强度梯度最大的2个点作为强度边缘特征点。

39、作为本发明进一步改进，步骤s5采用改进的迭代误差卡尔曼滤波器对状态进行估计和更新，其改进之处为对卡尔曼增益计算公式进行变换包括以下步骤：

40、原始的迭代误差状态卡尔曼滤波增益计算公式如下，在第k次迭代时：

41、

42、在本系统中，观测方程由步骤s4中提取到的特征按点-面、点-线残差的方式构建，设观测方程维度为m，所选取的状态为n维，此时hk应为m×n维，而卡尔曼增益式中需要求逆的计算变为m×m维矩阵的求逆，当m非常大时，对于该求逆是非常消耗计算资源的，这显然需要避免；

43、采用sherman-morrison-woodbury恒等式中的一组：

44、ab(d+cab)-1＝(a-1+bd-1c)-1bd-1

45、对卡尔曼增益公式进行变换，将a、b、c、d由pk、hk、v代替，得到：

46、

47、变换之后，该式内部的求逆变为n×n维，即仅与状态量的个数有关。

48、有益效果：

49、本发明提供了一种非理想环境下的激光/惯性里程计，能够更好地适应实际场景中的挑战。

50、针对动态场景中动态障碍物对特征点数据关联的影响导致系统定位精度下降的问题，采用了一种基于聚类的动态物体检测与剔除方法来消除动态点云的影响。该方法首先剔除地面点云，然后对剩余点云进行欧式聚类，对每个点云簇生成包围框坐标，并将其转换到初始坐标系下。通过计算连续两帧近邻聚类的包围框的交并比，从而能够检测并剔除动态物体，从而提高系统定位精度。

51、针对特征退化场景(如长走廊、隧道等结构单一、特征稀疏的环境)，本发明引入了强度特征。从点云强度产生的物理学原理出发，首先对点云强度进行补偿，并采用sobel算子提取强度边缘特征点。这使得系统在特征退化的场景中不仅可以提取几何特征，还能提取到不同材质物体交界处的边缘特征，从而提高了系统的鲁棒性及准确性。

52、针对激光雷达观测维度高，计算卡尔曼增益时求逆矩阵维度过大导致系统计算量大的问题，本发明采用了一种改进的卡尔曼增益计算公式。将原始的卡尔曼增益公式代入smw(sherman-morrison-woodbury)恒等式进行变换后使得计算卡尔曼增益时求逆矩阵的维度仅与状态量个数有关。这一改进提高了计算速度，同时减少了硬件资源的消耗。