CN110866927A - 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 - Google Patents

Abstract

本发明公开了一种基于垂足点线特征结合的EKF‑SLAM算法的机器人定位与构图方法。步骤1：将激光雷达获取的点云数据进行区域划分，使得激光雷达的点云能够分割开；步骤2：对分割开的点云进行线段特征的提取，作为EKF算法的观测值；步骤3：将观测值进行数据融合处理，进行一个特征的匹配；步骤4：里程计的信息输入作为EKF的预测阶段，而匹配之后的数据信息作为EKF的更新阶段，完成一次完整的预测更新；步骤5：输出机器人的位姿和地图的信息。本发明采用线段特征作为环境特征，提出了端点迭代的算法进行线段的拟合，基于EKF算法进行优化，利用几何地图作为地图表示，得到基于线段特征的激光雷达SLAM算法，经验证，该算法在实际运用中价值高。

Description

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

技术领域：

本发明涉及一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法。

背景技术：

随着自动化和智能化时代的到来，移动机器人作为一个日益普遍的智能体，已经在不断充斥在人们的科技和生活领域，小到家庭扫地机，大到无人驾驶和星球表面探测，移动机器人都扮演着十分重要的作用；不论处于什么样的环境中，定位与构图是自主移动机器人相对导航的两个重要问题，能够在各种位置的复杂环境中进行自主定位和导航是完成任务的前提，在某些封闭场景中，环境相对单一化，结构化，视觉特征点少，信息不充分，而且在黑暗环境下无法正常工作。

传统的激光雷达SLAM方法有：基于卡尔曼滤波器的KF-SLAM、基于扩展卡尔曼滤波的EKF-SLAM算法、基于粒子滤波的PF-SLAM算法，以及基于图优化的graph-SLAM算法等；卡尔曼滤波(Kalman Filter，KF)的导航算法，该方法是一种线性离散状态估计滤波器，通过***的观测数据实现对机器人位姿的最优估计，该方法假设***状态模型噪声、控制输入的噪声以及传感器的噪声均满足高斯分布，但是实际的过程中，大多数机器人的运动都是非线性的，因此，卡尔曼滤波存在着一定的局限性，故而推出了扩展卡尔曼滤波器的算法，这样会比KF得到更加精确的位姿估计；基于粒子滤波器(Particle Filter，PF)算法的导航方式，这种方法则基于贝叶斯估计的后验概率估计，使用非高斯分布的随机样本来近似表示状态，之后通过加权来确定最终的状态向量的概率分布；这种方法在上述噪声非高斯分布的非线性***定位估计中得到大量的使用；图优化的SLAM算法通过位姿节点和节点之间的约束来构建一个位姿图，最后采用最小二乘的优化方法来优化整个图结构，进而完成对位姿和地图的优化；在以上方法中，KF-SLAM和EKF-SLAM多采用点特征作为地图表示，而PF-SLAM和graph-SLAM多采用占据栅格地图作为地图标识，点特征的地图只有稀疏的路标点，不能作为导航的依据，而栅格地图需要维护庞大的网格，除此之外，这两种SLAM方法都需要对点云数据进行匹配处理，点云数据相对巨大，而且粒子滤波需要维护大量的粒子。

发明内容：

本发明的目的是提供一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法，采用线段特征作为环境特征，提出了端点迭代的算法进行线段的拟合，基于EKF算法进行优化，利用几何地图作为地图表示，得到基于线段特征的激光雷达SLAM算法，经验证，该算法在实际运用中价值高。

上述的目的通过以下的技术方案实现：

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

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

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

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

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

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

进一步的，所述步骤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}。

进一步的，所述步骤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，该坐标系的原点与机器人坐标系的原点重合，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)参数在各自坐标系下的表达式：

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

进一步的，所述步骤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：更新***的状态和协方差的初始值。

有益效果：

利用本算法，对在结构化环境中的移动机器人定位精度在5％以内。

附图说明：

附图1是本发明的算法整体结构示意图。

附图2是本发明(a)激光雷达扫描点云分布图，(b)区域分割示意图。

附图3是本发明(a)A₁区域示意图，(b)A₁区域分割示意图。

附图4是本发明的迭代端点算法原理图及数据仿真验证图。

附图5是本发明线段地标在机器人坐标系下和世界坐标系下的表示图。

附图6是本发明两线段间的匹配图。

附图7是本发明(a)观测线段与匹配地标线段不重叠，(b)观测线段与匹配地标线段部分重叠图，(c)观测线段包含于匹配地标线段，(d)观测线段包含匹配地标线段。

附图8是本发明扩充***协方差矩阵图。

附图9是本发明(a)环境地图无拐角时的线段提取图，(b)机器人的运动图。

附图10是本发明(a)无拐角位置激光点在机器人坐标系下的分布图，(b)全局坐标系下的机器人运动图。

附图11是本发明(a)拐角处激光点在机器人坐标系下的分布图，(b)全局坐标系下的机器人运动图。

附图12是本发明(a)初始时刻的位姿估计图，(b)t＝10s时刻的位姿估计图，(c)t＝20s时刻的位姿估计图，(d)t＝35s时刻的位姿估计图，(e)t＝50s时刻的位姿估计图，(f)t＝65s时刻的位姿估计。

附图13是本发明(a)t＝0s时刻EKF-SLAM绘制的地图，(b)t＝10s时刻EKF-SLAM绘制的地图，(c)t＝20s时刻EKF-SLAM绘制的地图，(d)t＝45s时刻EKF-SLAM绘制的地图，(e)t＝65s时刻EKF-SLAM绘制的地图。

附图14是本发明(a)机器人全局行走运动图，(b)对环境的绘制地图

附图15是本发明轨迹图及机器人各参数的误差曲线。

具体实施方式：

下述说明并非是对本发明的限制，本发明也并不仅限于下述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

实施例1

相比于传统的SLAM，该算法采用区域分割和迭代端点的方法进行折线拟合，代替传统的最小二乘，能够实现折角处的线段分割和提取，除此之外，利用垂足点，结合线段的长度和夹角信息作为匹配依据，完成EKF-SLAM的更新。

本算法采用SICK激光雷达，可以获取18米内，分辨率为0.5的180°范围内的激光点云；首先将激光雷达获取的点云数据输入到区域分割模块进行区域划分，使得激光雷达的点云能够分割开来；然后对分割开的区域里的点云进行线段特征的提取，作为EKF算法的观测值；将观测值进行数据融合处理，进行一个特征的匹配；里程计的信息输入作为EKF的预测阶段，而匹配之后的数据信息作为EKF的更新阶段，完成一次完整的预测更新，最后输出机器人的位姿和地图的信息。

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

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

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

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

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

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

进一步的，所述步骤1点云数据输入到区域分割模块进行区域划分具体为：由于结构化环境中存在较多的直线和线段，线段的特征通常是通过激光雷达获取的数据点来表示，而激光雷达根据自己的扫描范围，在此之内被照射到的物体都会返回给雷达，得到一个扫描点；激光雷达在每次扫描的过程中会提供一系列关于障碍物的距离信息d_s＝[d_s-90°,...,d_s90°]和角度信息θ_s＝[-90°,...,90°]；如图2(a)所示的是激光雷达的可视化范围；但是由于激光雷达获得的点都是在极坐标系下表示的，为了能够描绘出环境的位置，需要将扫描的点转换到世界坐标系下进行拟合；对于每个扫描点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}。

进一步的，所述步骤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)}，如此循环下去，直至细分的所有区域都满足能够拟合成直线的要求。

以A₁{(x₁,y₁),...,(x_N,y_N)}区域为例，为了拟合出折现，首先选择区域的起点(x₁,y₁)和终点(x_N,y_N)，连成一条直线，在这两点间，判断每一个点(x_i,y_i)|i＝2,...,N-1到该直线的距离，找出距离的最大值所对应的那个点(x_l,y_l)，如果这个最大值小于一个让A1区域是直线的阈值的时候，直接对这个区域进行拟合得到直线，否则，就从这一点处将这个区域再一次分割，这样A1就被分开成A₁₁{(x₁,y₁),...,(x_l,y_l)}和A₁₂{(x_l+1,y_l+1),...,(x_N,y_N)}两个部分，对每个部分再进行上述的距离判断，如此循环下去，直至细分的所有区域都满足能够拟合成直线的要求，这样就把A1这个大区域分段拟合成折线了。

以区域A₁为例：

步骤201：设定判断是否是直线的阈值δ；

步骤202：连接该区域的起点和终点形成一条直线L；

步骤203：计算该区域起点和终点之间的每一个点到直线L的距离d；

步骤204：寻找所有计算的距离中的最大值dmax及其对应的点，判断这个距离是否大于阈值δ，如果d_max大于，那么从这点开始将这个区域分成A₁₁和A₁₂；

步骤205：每个新生成的区域，重复计算步骤201～步骤204，直到所有的d都小于设定的阈值δ为止。

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

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

其中，X_Wl,Y_Wl为线段垂足点在世界系下的表示，βst(X_stl,Y_stl),βet(X_etl,Y_etl)为线段起始点和终点，

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

其中，x_rl,y_rl为线段垂足点在机器人坐标系下的表示，

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

进一步的，所述步骤4具体为：

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

其中，(x,y,θ_r)机器人位姿信息中的θ_r和上式中的θ_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)

其中，X_Wl_i,Y_Wl_i为第i个线段垂足点在世界系下的表示，

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

其中，f(X,u)为状态方程；有t-1时刻的状态量和控制量，通过状态方程f，来求t时刻的状态，X是状态量，u是控制量；

步骤4.3：量测更新：

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

其中，h_i(X)为观测方程，

通过计算该方程的雅克比矩阵就可以求得方程对于地标i的测量矩阵：

通过数据匹配的方法去判断观测的地标数据是否是之前已经观测到的，如果说观测的数据在之前都被观测过，那么，这些地标就可以当作观测量去更新状态。可以利用EKF的量测更新过程去估计机器人的位姿和周围环境的地标位置；

步骤4.4：状态增广：

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

利用上式分别计算新地标位置对机器人运动状态和测量值的雅克比矩阵如下：

其中，J_R为新观测对机器人位姿的雅可比矩阵，J_z为新观测对测量的雅可比矩阵；

同时，状态增广不仅仅只是增加状态量的维度，还要对协方差矩阵进行增广，就是在原有的协方差基础上增加图中阴影部分的内容：如图8所示，根据状态变量的协方差和测量误差矩阵可以利用刚刚求得的雅克比矩阵更新协方差：

其中，P_RN为机器人和扩充新地标协方差矩阵，P_NN为扩充地标协方差矩阵，P_NL为扩充新地标和之前地标协方差矩阵，P_LN为之前地标和扩充地标方差矩阵，J_z为新观测对测量的雅可比矩阵，P_RR为机器人协方差矩阵，P_LN与P_NL互为逆运算；

步骤4.5：更新***的状态和协方差的初始值，为步骤5的基础。

实施例2

对在结构化环境中的移动机器人完成实验，设置机器人的运动速度为0.3米每秒。

实验中让机器人运行在一个矩形框架中，框架里头带有另一个斜矩形，构成一个结构化的环境。实验中激光雷达的扫描距离范围是30米，扫描的角度范围是180°，分辨率是0.5°，那么激光雷达每次扫描能够得到361个点，实验中以三角形表示机器人的位置，三角形的指向表示机器人的方位，因为激光雷达的坐标中心与机器人坐标系中心重合，所以激光雷达测出的数据就可以直接反映出机器人的坐标中心到探测点的距离和角度，如图11所示。

SLAM的问题关键在于能否精确定位，机器人利用自身传感器对自己的位置进行准确定位，所以有必要对定位进行模拟，实验中为了能够很好地形成对比，同时绘制出了真实的运动情况和经过EKF-SLAM算法后估计的运动情况，进行实时的比对，如图12所示。

SLAM的另一个重要过程就是绘制地图，因为本次实验是基于线段特征进来进行的，所以以实时绘制出线段地图为目的，进行程序的设计，并考察最终的线段匹配的可靠性。为了能够更好的看出实验效果，选择将绘制地图与真实地图进行实时比对，并选取其中某些时间作为比较，如图13所示。

如图15所示，可以看到机器人的定位精度在5％以内。

O^WX^WY^W为世界坐标系、O^Wx^Ry^R为机器人坐标系、d_s/ρ为激光雷达点云距离信息、θ_s/α为激光雷达点云角度信息、X(i),Y(i)为激光点在世界系坐标的表示。

Claims (5)

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

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

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

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

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

步骤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}。

3.根据权利要求1所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法，其特征在于，所述步骤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)}，如此循环下去，直至细分的所有区域都满足能够拟合成直线的要求。

4.根据权利要求3所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法，其特征在于，所述线段表示选择机器人坐标系的原点到线段所在直线的垂足点作为参考，并将这个点坐标转换为世界坐标系下的原点到线段所在直线的垂足点坐标，用这两个参数来当作线段的地标特征点，由于线段是有限的，所以另外四个参数分别表示的是线段的起点和终点在机器人坐标系下的数值，并将其转换到世界坐标系下：βst(X_stl,Y_stl),βet(X_etl,Y_etl)另外由于机器人在始终的移动，而垂足点在机器人坐标系下的坐标(x_rl,y_rl)经过机器人坐标系到世界坐标系之间的转换后的点并不是世界系下的原点到线段所在直线的投影坐标(X_Wl,Y_Wl)，故而，这里建立一个中间坐标系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)参数在各自坐标系下的表达式：

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

5.根据权利要求1所述的一种基于垂足点线特征结合的EKF-SLAM算法的机器人定位与构图方法，其特征在于，所述步骤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：更新***的状态和协方差的初始值。

Images