[发明专利]一种基于LMB的两阶段SLAM特征点检测标记方法

说明书

本发明公开了一种基于LMB的两阶段SLAM特征点检测标记方法，包括以下步骤，根据给定场景信息进行传感器两阶段路径规划、移动传感器建模、根据步骤一所规划的传感器路径以及步骤二对移动传感器的建模，使得传感器在场景内行驶的同时产生量测信息，并使用LMB方法对传感器和特征点进行状态估计、采用B样条曲线对传感器避障路径进行平滑处理，本发明能够对特征点进行有效的检测和标记。

技术领域

本发明涉及传感器避障路径技术领域，特别涉及一种基于LMB的两阶段SLAM特征点检测标记方法。

背景技术

同时定位与建图(SLAM)是根据移动传感器探测数据(特征点位置、数量等)逐步构建整体环境地图的过程，并在此过程中估计传感器自身位置。定位、建图是与采矿、无人驾驶、自主导航等许多领域相关的状态估计问题。其中，定位是对传感器自身位置的估计，而建图的首要前提是对场景中特征点状态估计。

在自主导航、无人驾驶领域中至关重要的是估计周围环境中特征点位置。传统的SLAM问题使用RV来描述，其中传感器运动状态采用状态空间形式表示，传感器的位置、特征点位置真实值和量测值都用RV表示。在应用贝叶斯递归或优化之前，将特征点表示为RV，但由于向量内元素顺序改变，向量也会随之改变，这就要求在描述SLAM问题的同时解决一些并行问题，如数据关联、特征提取、杂波过滤以及地标或地图管理等。现存的RV表示方法并不能很好地解决这些问题。

发明内容

为了克服上述现有技术的不足，本发明的目的在于提供一种基于LMB的两阶段SLAM特征点检测标记方法，能够对特征点进行有效的检测和标记。

为了实现上述目的，本发明采用的技术方案是：

一种基于LMB的两阶段SLAM特征点检测标记方法，包括以下步骤；

步骤一：根据给定场景信息进行传感器两阶段路径规划

第一阶段为初始路径规划，着眼于全局，此时传感器为行驶模式，按照给定路线正常行驶，对于初始路径规划来说，所有关于场景的信息都是预先知道的,以这些信息为基础进行路径规划，在任意指定的起点和终点之间寻找一条优化路径，使得在传感器行驶过程中，它的探测范围最大限度覆盖整个场景，路径规划的精度取决于环境信息获取的准确度，虽然规划结果是全局的、较优的，但是不够精细，若传感器在运动过程中检测到障碍物，就触发第二阶段局部路径规划，传感器进入避障模式，在此过程中，传感器以初始路径为基础，利用量测信息,对原有路径进行局部更新和调整，实时避开障碍物。成功避开障碍物后，传感器又回到行驶模式，按照初始路径继续行驶；

步骤二：移动传感器建模

将传感器建模为匀速直线模型(CV)和匀速转弯模型(CT)，传感器在行驶过程中遵循以下动态模型：

当传感器沿直线运动时

当传感器沿圆弧运动时，

观测模型为：

其中，x_k+1表示k+1时刻传感器状态，F_k、G_k、L_k均为增益矩阵，m_k表示场景中特征点位置，u_k表示控制量，v_k、w_k为k时刻噪声误差，V_v,k为传感器控制量误差，是位置函数，S表示传感器探测区域，表示以传感器位置为圆心，r_k为半径的圆形区域，H为量测信息的转移矩阵。

步骤三：根据步骤一所规划的传感器路径以及步骤二对移动传感器的建模，使得传感器在场景内行驶的同时产生量测信息，并使用LMB方法对传感器和特征点进行状态估计