[发明专利]一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法

权利要求书

1.一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法，其特征在于，所述算法包括以下步骤：

步骤1：将激光雷达获取的点云数据输入到区域分割模块进行区域划分，使得激光雷达的点云能够分割开；

步骤2：对分割开的区域里的点云进行线段特征的提取，作为EKF算法的观测值；

所述步骤2线段特征的提取，在区域分割的基础之上使用迭代端点的拟合算法，具体为：对于每一次激光雷达扫描获得的激光点云，通过阈值的设定对得到的扫描点进行区域A₁{(x₁,y₁),...,(x_N,y_N)}的分割，将这个区域分解成区域A₁₁{(x₁,y₁),...,(x_l,y_l)}和区域A₁₂{(x_l+1,y_l+1),...,(x_N,y_N)}，如此循环下去，直至细分的所有区域都满足能够拟合成直线的要求；

所述线段表示选择机器人坐标系的原点到线段所在直线的垂足点作为参考，并将这个点坐标转换为世界坐标系下的原点到线段所在直线的垂足点坐标，用这两个参数来当作线段的地标特征点，由于线段是有限的，所以另外四个参数分别表示的是线段的起点和终点在机器人坐标系下的数值，并将其转换到世界坐标系下：βst(X_stl,Y_stl),βet(X_etl,Y_etl)，另外由于机器人在始终的移动，而垂足点在机器人坐标系下的坐标(x_rl,y_rl)经过机器人坐标系到世界坐标系之间的转换后的点并不是世界系下的原点到线段所在直线的投影坐标(X_Wl,Y_Wl)，故而，这里建立一个中间坐标系O^Rxy，坐标系O^Rxy的原点与机器人坐标系的原点重合，x轴y轴指向分别与世界坐标系下的X，Y方向重合，同时令移动机器人到线段所在的投影点在中间坐标系下的坐标为(xl,yl)，最后使用以下参数来表示线段：

[X_Wl,Y_Wl,βst(X_stl,Y_stl),βet(X_etl,Y_etl)] (7)

求解出(x_rl,y_rl)、(xl,yl)参数在各自坐标系下的表达式：

将中间坐标系下的移动机器人投影点坐标转换至世界坐标系下可以求解出世界坐标系原点到线段所在直线的投影点坐标：

步骤3：将观测值进行数据融合处理，进行一个特征的匹配；

步骤4：里程计的信息输入作为EKF的预测阶段，而匹配之后的数据信息作为EKF的更新阶段，完成一次完整的预测更新；

步骤5：输出机器人的位姿和地图的信息；

所述步骤4具体为：

步骤4.1：假设地标的位置是不发生变化的，根据移动机器人的运动方程，利用运动模型对由t-1时刻的状态和控制量预测t时刻的运动状态，通过前面的模型可以得到：

步骤4.2：地标环境预测：

[X_Wl_i(k+1),Y_Wl_i(k+1)]＝X_i(k+1)＝X_i(k)＝[X_Wl_i(k),Y_Wl_i(k)] (11)

故而系统的状态预测方程可以写为：

步骤4.3：量测更新：

每次扫描激光雷达都可以获得一系列连续的点，利用前文提到的数据处理算法可以将这些连续的点拟合成多条直线，进而在利用上一节提到的方法提取出线段的地标，状态中的地标位置用原点到线段所在直线的投影来表示；

步骤4.4：状态增广：

当检测到状态中不曾出现过的路标的时候，就认为出现了新的地标，这时需把新的地标加入到状态中，根据量测方程得到新地标的位置；

步骤4.5：更新系统的状态和协方差的初始值。

2.根据权利要求1所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法，其特征在于，所述步骤1点云数据输入到区域分割模块进行区域划分具体为：激光雷达在每次扫描的过程中会提供关于障碍物的距离信息d_s＝[d_s-90°,...,d_s90°]和角度信息θ_s＝[-90°,...,90°]；但是由于激光雷达获得的点都是在极坐标系下表示的，为了能够描绘出环境的位置，需要将扫描的点转换到世界坐标系下进行拟合；对于每个扫描点m_i(ds(i),θs(i))，将其映射到世界坐标系下可表示为：

X(i)＝ds(i)*cos(θs(i)+θ_r)+x_r (1)

Y(i)＝ds(i)*sin(θs(i)+θ_r)+y_r (2)

假设激光雷达每次可以获得到的数据点的个数为n，并以这n个点作为一个初始区域，对这个区域的数据进行处理，采用距离分割的方法，从第一个点开始，首先通过欧式距离判断相邻两个点的距离D_j，再设定一个阈值δ，判断D_j和δ的大小，确定是否从这一点分开；当D_j大于δ时，确定点(X_j，Y_j)是区域的一个分割点，以此点将数据划分成两个部分，可以得到第一个区域A₁{(X_i,Y_i)|i＝1,2,...,j}和剩下的待分割区域，以此类推，假设得到N个互相不连通的区域{A₁,A₂,...,A_N}。