CN114355908A - 一种基于特征识别的导航优化方法 - Google Patents

Abstract

本发明公开了一种基于特征识别的导航优化方法，本发明基于激光特征导航，通过优化特征提取，以及在特征匹配的基础上，再加入图优化进行精匹配，从而有效提升了导航的精度和稳定性；在特征提取前，加入了预处理，有效去除移动机器人在运动过程中，尤其是高速运动过程中，因无效噪点和数据畸变，导致的特征失真，为后续特征匹配提供了较接近真值的特征；在完成特征匹配后，通过构建特征图优化，并设计带参损失函数，增强对离群特征的鲁棒性，有效解决了初步特征拟合因特征不稳定带来的明显匹配位姿跳动问题，大大提升了定位的稳定性与精度。

Description

一种基于特征识别的导航优化方法

技术领域

本发明涉及移动机器人感知与标定技术，具体是一种基于特征识别的导航优化方法。

背景技术

物流行业的飞速发展既体现在物流产业规模不断壮大也体现在物流管理上。物流产业规模的不断壮大导致劳动力的需求量越来越大。而物流管理主要体现在如何提高物流效率上。但是劳动力成本越来越高，而且人力搬运的效率天花板较低，不利于提高物流效率。此外，疫情的出现也促使了自动化配送的发展。但是现有用于物流中的自主移动机器人在配送过程中的应用缺点是导航不够精确，进而导致移动机器人的移动稳定性不足的情况发生。

现有市场上自主移动机器人的导航技术主要包括磁导航传感器技术、色带导航传感器技术、二维码导航技术、激光导航技术和视觉导航技术。磁导航传感器技术、色带导航传感器技术在实施过程中均需要在地面上布置磁条，二维码导航技术在实施过程中均需要在地面上布置二维码标识。磁条和二维码标识长期使用过程中均会因损坏而需要持续的维护而且在路线调整时也需要重新布置，因此磁导航传感器技术、色带导航传感器技术和二维码导航技术的柔性化程序不高。

视觉导航技术受光线和环境特征等不确定性因素的影响较大，并且视觉导航技术的导航算法计算量相对于磁导航传感器技术、色带导航传感器技术和二维码导航技术明显更大，所以视觉导航技术稳定性有待进一步验证，目前实际落地使用比较少。

激光导航是目前市场上比较主流的导航方法，其中包括激光特征导航和激光自然导航。激光特征导航为通过算法提取自然环境中的点线特征或者语义特征来实现导航。激光自然导航为通过采集自然环境轮廓与预先存储的地图进行匹配实现导航。激光特征导航在可准确提取特征时，可获得相较激光自然导航更高的精度。但是在激光特征导航中，由于一帧完整激光点云中的每个点是在不同时间采集的；如果移动机器人一边移动一边采集数据，最终处理该帧数据时刻，机器人的当前位姿与采集各激光点时的位姿已经不同；即各激光点的坐标原点不重合，因此如果直接在原始数据的基础上提取特征，特征将会是畸变的，从而影响最终的定位精度。

发明内容

本发明的目的在于提供一种基于特征识别的导航优化方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：一种基于特征识别的导航优化方法，所述优化方法包括以下步骤：

步骤一、导航程序对接收到的激光原始数据进行预处理；

步骤二、从预处理后的激光点云数据中提取出点特征、线特征和高反光强度特征；

步骤三、融合移动机器人累计的行走里程以及估计的朝向，融合得到移动机器人的预估位姿作为特征匹配的初始位姿；

步骤四、根据初始位姿，将点线特征和高反光强度特征变换到地图坐标系中，再与预先存储的特征地图执行初步匹配，得到准确位姿；

步骤五、根据初匹配位姿，更新点线特征和高反光强度特征在地图坐标系中位姿，并将各特征构建成地图优化的顶点；各个特征分别与初匹配位姿的相对位姿关系构建成图优化的边，并设计带参损失函数；增强平方误差损失函数对离群点的鲁棒性，有效抑制离群特征对最终匹配结果的影响；最后执行图优化，得到最优位姿。

作为本发明中的一种优选技术方案，上述步骤一中先进行噪声过滤，再执行畸变校正。

作为本发明中的一种优选技术方案，上述噪点过滤具体操作为通过滑窗对相邻数据进行三线性滤波。噪声过滤主要滤除传感器原始数据中的明显奇异点和较大离群点，保证后续处理的数据均是正常可用的数据，提高定位精确度。

作为本发明中的一种优选技术方案，上述畸变校正具体过程如下：先对每个数据点采集时的位姿进行插值计算，然后通过该插值位姿与最终位姿的偏差反向补偿传感器数据，具体计算方法如下：

设机器人上一时刻的位姿为(x₀ y₀ th₀)，更新时间为t₀，速度为v，要做位姿插值的激光点的采集时间为t₁，则插值位姿(x y th)为

通过畸变校能够校正因运动导致的原始数据畸变，提高数据的数据的准确率。

作为本发明中的一种优选技术方案，上述步骤二中点特征通过检测传感器数据出现明显梯度变化得到的，将得到的点特征与预先存储的点特征进行匹配；

线特征通过拟合环境中数据点云，得到存在的直线段，并计算直线段的长度和朝向，并与预先存储的线特征进行匹配；

高反光强度特征是预先在运行场景中布置高强度反光膜，通过检测反光强度的大小，可以准确提取出高反光强度物体的尺寸，并与预先存储的高反光强度特征进行匹配。通过点特征、线特征和高反光强度特征的匹配，能够实现移动机器人的实时定位。

作为本发明中的一种优选技术方案，上述步骤三中将步骤二得到的点特征、线特征和高反光强度特征通过扩展卡尔曼滤波算法，进行融合滤波，最优位姿。通过融合定位的优势在于，不会因为某种定位结果跳动，而出现移动机器人定位的最优位姿出现明显跳动，导致运动姿态出现明显摆动，提高定位精确度。

作为本发明中的一种优选技术方案，上述步骤四中具体操作为:设点特征、线特征和高反光强度特征在检测传感器坐标系下的2D位姿为(x_l y_l θ_l)，其中，x₁表示点特征，y₁表示线特征，θ₁表示线特征；检测传感器在地图坐标系中2D位姿为(x y β)，根据以下公式可将特征从检测传感器的局部坐标系变换到地图的全局坐标系下，

所以点特征、线特征和高反光强度特征在地图的全坐标系下的位姿为

与现有技术相比，本发明的有益效果是：

本发明基于激光特征导航，通过优化特征提取，以及在特征匹配的基础上，再加入图优化进行精匹配，从而有效提升了导航的精度和稳定性。在特征提取前，加入了预处理，有效去除移动机器人在运动过程中，尤其是高速运动过程中，因无效噪点和数据畸变，导致的特征失真，为后续特征匹配提供了较接近真值的特征；在完成特征匹配后，通过构建特征图优化，并设计带参损失函数，增强对离群特征的鲁棒性，有效解决了初步特征拟合因特征不稳定带来的明显匹配位姿跳动问题，大大提升了定位的稳定性与精度。

附图说明

附图1是一种基于特征识别的导航优化方法的流程图。

具体实施方式

为了使本领域的技术人员更好地理解本申请方案，下面将结合本申请实施例中的附图对本申请实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动成果前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合，下面将参考附图1，并结合实施例来详细说明本申请。

请参阅图1，一种基于特征识别的导航优化方法，优化方法包括以下步骤：

8.步骤一、导航程序对接收到的激光原始数据进行预处理；预处理先进行噪声过滤，再执行畸变校正，噪点过滤具体操作为通过滑窗对相邻数据进行三线性滤波。噪声过滤主要滤除传感器原始数据中的明显奇异点和较大离群点，保证后续处理的数据均是正常可用的数据，提高定位精确度；畸变校正过程为先对每个数据点采集时的位姿进行插值计算，然后通过该插值位姿与最终位姿的偏差反向补偿传感器数据。具体计算方法如下：

设机器人上一时刻的位姿为(x₀ y₀ th₀)，更新时间为t₀，速度为v，要做位姿插值的激光点的采集时间为t₁，则插值位姿(x y th)为

通过畸变校能够校正因运动导致的原始数据畸变，提高数据的数据的准确率。

步骤二、从预处理后的激光点云数据中提取出点特征、线特征和高反光强度特征；

点特征即角点特征，点特征通过检测传感器数据出现明显梯度变化得到的，将得到的点特征与预先存储的点特征进行匹配；

线特征通过拟合环境中数据点云，得到存在的直线段，并计算直线段的长度和朝向，并与预先存储的线特征进行匹配；

高反光强度特征是预先在运行场景中布置高强度反光膜，通过检测反光强度的大小，可以准确提取出高反光强度物体的尺寸，并与预先存储的高反光强度特征进行匹配。通过点特征、线特征和高反光强度特征的匹配，能够实现移动机器人的实时定位

步骤三、融合移动机器人累计的行走里程以及估计的朝向，融合得到移动机器人的预估位姿作为特征匹配的初始位姿；步骤二所得的点特征、线特征和高反光强度特征通过扩展卡尔曼滤波算法，进行融合滤波，最优位姿。通过融合定位的优势在于，不会因为某种定位结果跳动，而出现移动机器人定位的最优位姿出现明显跳动，导致运动姿态出现明显摆动，提高定位精确度；

步骤四、根据初始位姿，将点线特征和高反光强度特征变换到地图坐标系中，再与预先存储的特征地图执行初步匹配，得到准确位姿；具体过程如下：设点特征、线特征和高反光强度特征在检测传感器坐标系下的2D位姿为(x_l y_l θ_l)，其中，x₁表示点特征，y₁表示线特征，θ₁表示线特征；检测传感器在地图坐标系中2D位姿为(x y β)，根据以下公式可将特征从检测传感器的局部坐标系变换到地图的全局坐标系下，

所以点特征、线特征和高反光强度特征在地图的全坐标系下的位姿为

步骤五、根据初匹配位姿，更新点线特征和高反光强度特征在地图坐标系中位姿，并将各特征构建成地图优化的顶点；各个特征分别与初匹配位姿的相对位姿关系构建成图优化的边，采用Huber Loss函数作为带参损失函数；增强平方误差损失函数对离群点的鲁棒性，有效抑制离群特征对最终匹配结果的影响；最后执行图优化，得到最优位姿。

本实施例基于激光特征导航，通过优化特征提取，以及在特征匹配的基础上，再加入图优化进行精匹配，从而有效提升了导航的精度和稳定性。在特征提取前，加入了噪点过滤算法和畸变校正算法，有效去除移动机器人在运动过程中，尤其是高速运动过程中，因无效噪点和数据畸变，导致的特征失真，为后续特征匹配提供了较接近真值的特征；在完成特征匹配后，通过构建特征图优化，并设计带参损失函数，比如Huber Loss，增强对离群特征的鲁棒性，有效解决了初步特征拟合因特征不稳定带来的明显匹配位姿跳动问题，大大提升了定位的稳定性与精度。

以上的，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (7)

1.一种基于特征识别的导航优化方法，其特征在于，所述优化方法包括以下步骤：

步骤一、导航程序对接收到的激光原始数据进行预处理；

步骤二、从预处理后的激光点云数据中提取出点特征、线特征和高反光强度特征；

步骤三、融合移动机器人累计的行走里程以及估计的朝向，融合得到移动机器人的预估位姿作为特征匹配的初始位姿；

步骤四、根据初始位姿，将点线特征和高反光强度特征变换到地图坐标系中，再与预先存储的特征地图执行初步匹配，得到准确位姿；

步骤五、根据初匹配位姿，更新点线特征和高反光强度特征在地图坐标系中位姿，并将各特征构建成地图优化的顶点；各个特征分别与初匹配位姿的相对位姿关系构建成图优化的边，并设计带参损失函数；增强平方误差损失函数对离群点的鲁棒性，有效抑制离群特征对最终匹配结果的影响；最后执行图优化，得到最优位姿。

2.根据权利要求1所述的一种基于特征识别的导航优化方法，其特征在于，所述步骤一中先进行噪声过滤，再执行畸变校正。

3.根据权利要求2所述的一种基于特征识别的导航优化方法，其特征在于，所述噪点过滤具体操作为通过滑窗对相邻数据进行三线性滤波。

4.根据权利要求2所述的一种基于特征识别的导航优化方法，其特征在于，所述畸变校正具体过程如下：先对每个数据点采集时的位姿进行插值计算，然后通过该插值位姿与最终位姿的偏差反向补偿传感器数据，具体计算方法如下：

设机器人上一时刻的位姿为(x₀ y₀ th₀)，更新时间为t₀，速度为v，要做位姿插值的激光点的采集时间为t₁，则插值位姿(x y th)为

5.根据权利要求1所述的一种基于特征识别的导航优化方法，其特征在于，所述步骤二中点特征通过检测传感器数据出现明显梯度变化得到的，将得到的点特征与预先存储的点特征进行匹配；

线特征通过拟合环境中数据点云，得到存在的直线段，并计算直线段的长度和朝向，并与预先存储的线特征进行匹配；

高反光强度特征是预先在运行场景中布置高强度反光膜，通过检测反光强度的大小，可以准确提取出高反光强度物体的尺寸，并与预先存储的高反光强度特征进行匹配。

6.根据权利要求1所述的一种基于特征识别的导航优化方法，其特征在于，所述步骤三中将步骤二得到的点特征、线特征和高反光强度特征通过扩展卡尔曼滤波算法，进行融合滤波，最优位姿。

7.根据权利要求1所述的一种基于特征识别的导航优化方法，其特征在于，所述步骤四中具体操作为:设点特征、线特征和高反光强度特征在检测传感器坐标系下的2D位姿为(x_ly_l θ_l)，其中，x₁表示点特征，y₁表示线特征，θ₁表示线特征；检测传感器在地图坐标系中2D位姿为(x y β)，根据以下公式可将特征从检测传感器的局部坐标系变换到地图的全局坐标系下，

所以点特征、线特征和高反光强度特征在地图的全坐标系下的位姿为

Images