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

权利要求书

1.一种基于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方法对传感器和特征点进行状态估计

LMB滤波器可以实现特征点检测标记的要求，首先在特征点状态集中加入标签，X＝{(x_i,l_i)}(i＝1,2,…,|X|),x_i∈X,l_i∈L，X为特征点状态空间，L为标签空间，|·|表示集合的势。LMB RFS可以用参数{(r^(l),p^(l)(·),l∈L}表示，这里的r^(l)∈[0,1]是第l个特征点标签假设位置是真实位置的概率。LMB RFS密度函数如下：

其中，π(X)是特征点状态集的密度函数，p表示新生特征点的概率密度。X是标签集合，Δ(X)是离散标签指示器；是标签集，r是新生特征点的出生概率，是加权系数并依赖于状态标签1_Y(X)是指示函数，当时等于1，其它等于0。

使用LMB滤波算法对特征点进行检测估计，LMB滤波主要分为两步：预测步和更新步，预测步是根据现有信息预测下一时刻特征点状态。更新步是指结合下一时刻的特征点信息更新到目前为止特征点的状态，特征点状态(x,l)，特征点被检测到的概率为p_D(x,l)，未被检测到的概率为q_D(x,l)＝1-p_D(x,l)。

预测：在这里考虑特征点的存活和新生两种，它们分别表示从上一时刻到下一时刻一直存在和新出现的特征点，所以LMB预测多目标密度的参数可以表示为：

其中，

预测特征点的LMB参数为其中L₊＝L∪B，B为新生特征点的标签空间；S为存活特征点的标签空间，p_S是特征点存活概率，f(x|·,l)为存活特征点的概率密度，η_S(l)为归一化因子，·，·是内积函数，定义式为

更新：假定多特征点预测记为定义在状态空间X₊和标签空间L₊上的LMB RFS，由参数集表示为：

则多特征点后验密度可表示为[25]：

其中，r是新生特征点的出生概率。p表示新生特征点的概率密度。w表示加权系数，Θ为参数空间，g(z|x,l)为量测似然函数，K(·)为杂波密度函数，q_D(x,l)是标签为l的特征点x被检测到的概率，q_D(x,l)＝1-p_D(x,l)是标签为l的特征点x未被检测到的概率，g(z|x,l)是标签为l的特征点x关于量测的z的单目标似然函数，θ表示从标签集合到量测集的映射，K(·)表示杂波函数；

步骤四：采用B样条曲线对传感器避障路径进行平滑处理

初始的直线路径与传感器避障圆弧路径直接相连，会导致传感器在两端路径衔接处行驶方向瞬间变化90，行驶中的传感器并不能做到这一点，采用B样条曲线拟合方法来对路径进行平滑处理，B样条曲线用如下公式来描述。

u₀≤u₁≤…≤u_m-1≤u_m (22)

其中，C(u)为平滑后曲线，k是样条曲线的阶数，u_i是拟合后曲线上的节点，X(i)是待拟合的路径点，N_i,k(u)是基函数。