基于直线特征地图的EKF‑SLAM算法的制作方法

本发明属于移动机器人技术领域，具体涉及一种基于直线特征地图的EKF-SLAM算法的设计。

背景技术：

随着机器人技术的日益发展，机器人的应用越来越广泛，几乎渗透到所有领域。移动机器人是机器人学中的一个重要分支，按结构分，一般可分为轮式、腿足式、履带式、吸盘式以及复合式机器人。其中，轮式移动机器人由于其控制简单，运动稳定和能源利用率高等特点应用较为广泛。

如今，移动机器人的地图建立与导航问题的该领域的一个热门研究课题，SLAM(Simultaneous Localization And Mapping)就是指移动机器人的同时定位与地图创建问题，也被称为CLM(Concurrent Mapping and Localization)。SLAM问题可以描述为：在一个二维平面内，已知运动模型的移动机器人从一个未知的初始点出发，在一个有若干环境特征的环境中移动，机器人通过传感器的数据确定环境特征的二维坐标，同时确定自身的三维坐标。

目前解决SLAM问题比较常用的一种方法是EKF-SLAM(Extended Kalman Filter-Simultaneous Localization And Mapping，基于扩展卡尔曼滤波器的移动机器人同时定位与地图创建)算法。现有的EKF-SLAM算法均是基于环境的空间点特征进行计算的，然而由于空间中点较多，会造成运算量较大，进而使得EKF-SLAM算法的实时性较差，无法满足应用需求。

技术实现要素：

本发明的目的是为了解决现有技术中EKF-SLAM算法基于环境的空间点特征进行计算，运算量较大，实时性较差的问题，提出了一种基于直线特征地图的EKF-SLAM算法。

本发明的技术方案为：基于直线特征地图的EKF-SLAM算法，包括以下步骤：

S1、直线特征提取：系统对激光雷达测量的数据进行区域分割与直线拟合，得到直线特征的参数方程；

S2、机器人控制量输入：输入移动机器人的控制量，并计算得到系统的状态预测量和协方差预测量；

S3、直线特征匹配：系统实时将激光雷达最新一帧测量数据中每条直线的特征参数与之前一帧每条直线的特征参数进行逐一对比，判断是否出现新的直线，若是则进入步骤S4，否则进入步骤S5；

S4、添加新的直线特征；

S5、对原有的直线特征进行观测与修正。

本发明的有益效果是：

(1)本发明在直线特征明显的实验环境中，非常容易提取环境的直线特征，算法的复杂程度低，降低了处理器运算开销，提高了系统实时性。

(2)本发明开创性地设计了以直线为特征地图的EKF-SLAM算法，丰富了SLAM算法处理环境的手段。

(3)本发明直线特征明显，特征提取精度高，因此定位精度高于基于环境的空间点特征的EKF-SLAM算法。

附图说明

图1为本发明提供的基于直线特征地图的EKF-SLAM算法流程图。

图2为本发明实施例的机器人运动姿态示意图。

图3为本发明实施例的直线特征观测示意图。

具体实施方式

下面结合附图对本发明的实施例作进一步的说明。

本发明提供了一种基于直线特征地图的EKF-SLAM算法，如图1所示，包括以下步骤：

S1、直线特征提取：系统对激光雷达测量的数据进行区域分割与直线拟合，得到直线特征的参数方程。

在本发明中，环境特征主要是由许多分割的线段所构成，因此区域的分割、特征的提取是我们在这样的结构化环境中的主要任务。本发明中的系统指移动机器人平台系统。

首先我们分析下从激光雷达扫描的数据中直接得到的信息，在激光雷达的数据中有这样的一些点会干扰我们的直线特征的提取，这样点主要有三类：

(1)表示地图中可以容易被移动的物体的点，例如人的腿、桌子和椅子的腿等；

(2)在两个物体之间的间隙点；

(3)超出量程的点，对于量程为4m的激光雷达，可能会返回超出4m的测量值，但是在超过4m后激光雷达测得的数据误差较大，因此不应该被使用。

在对激光雷达测量的数据进行简单的处理后，下一阶段就是区域分割问题。本发明实施例中，采取的是改进的基于激光雷达两点间距离的区域分割方法。基本原理是，计算连续两数据点之间的距离，当距离值小于距离阈值时或计算连续三个点之间构成的两条直线的夹角，如果夹角小于角度阈值时，则认为这些点在同一直线内。这种算法的关键之处在于对阈值的选取，阈值的选择直接影响着结果的好坏。

区域分割的具体步骤如下：

T1、对于所有的点i，i＝1,2,…,n-2，n为激光雷达扫描点数，计算连续两点间的欧式距离D_i，和连续三点构成直线的夹角θ_i：

式中x_i、y_i分别为点i的横纵坐标；

T2、确定合适距离阈值MAX_DIST和角度阈值MAX_TH，当D_i小于MAX_DIST或者θ_i小于MAX_TH时，则认为i点在直线集A_j之内，反之则不在，最后得到k个互不连通的区域{A₁,A₂,....,A_k}；

T3、判断每个区域A_i内的点数，i＝1,2,…,k，如果该区域内的点数小于点数阈值MIN_NOP，则认为该区域为噪声点而去除。

在经过了直线分割处理之后，得到了k个表示线段的点集{A₁,A₂,....,A_k}。由于这些点不是严格的直线关系，所以需要使用对其进行直线拟合。本发明实施例中，采用的是最小二乘直线拟合法，来拟合每条直线的参数方程。

对于给定的点集(x_i,y_i)，i＝0,1,2,…,m，找到合适的函数f(x)来使得误差r_i＝f(x_i)-y_i的平方和最小，即有：

假设直线方程为y＝f(x)＝a₁x+a₀，根据最小二乘法拟合直线的公式：

最后求得k条直线的特征参数为

S2、机器人控制量输入：输入移动机器人的控制量，并计算得到系统的状态预测量和协方差预测量。

如图2所示，假设机器人的速度方向与机器人坐标系X_L轴方向相同，机器人在世界坐标系的姿态为X_R(k-1)＝[x_R,y_R,θ_R]^T，则机器人对应的在世界坐标系中的速度为：

假设激光雷达在机器人坐标系中为位置为机器人的控制量表示为u(k)＝[v,w]，v表示机器人的线速度，w表示机器人的自旋角速度，通过齐次变换矩阵，把激光雷达转换到世界坐标系下：

通过对时间求导，计算出激光雷达在世界坐标系下的速度：

从k-1时刻到k时刻，在dt的控制时间内，利用机器人的直线运动模型，预测激光雷达在k时刻时在世界坐标系中的位姿：

假设表示第j条直线的特征参数，j＝1,2,…,k，由于在机器人的运动过程中环境的陆标特征是不变的，所以有：l_i(k)＝l_i(k-1)；

系统的状态量表示为X(k)＝[x_L(k),y_L(k),θ_L(k),l₁(k),...,l_N(k)]^T，N表示直线特征的个数，状态量是3+2N维向量，所以系统状态的预测量表示为：

计算状态转变方程的雅克比矩阵：

式中：

I_2N×2N表示2N*2N的单位矩阵，计算系统各协方差矩阵的预测量为：

式中W(k)表示系统状态转换误差的协方差矩阵，F(k)表示系统状态转换的雅克比矩阵。

S3、直线特征匹配：系统实时将激光雷达最新一帧测量数据中每条直线的特征参数与之前一帧每条直线的特征参数进行逐一对比，判断是否出现新的直线，若是则进入步骤S4，否则进入步骤S5。

在步骤S1中，我们得到激光雷达一帧扫描数据中k条直线特征的参数方程，我们把这个过程用函数extract_line(laser_scan)来表示，函数返回k条直线的参数

假设机器人刚刚启动，在激光雷达扫描一圈过后，我们获得了一帧数据。通过extract_line算法，我们得到了k条直线的参数，我们把这k条直线放入机器人的特征地图中。然后，当激光雷达第二次扫描结束后，假设这次我们通过extract_line算法得出了m条直线，很明显，这m条直线有可能包含之前k条直线中的一部分，同时可能观察到了一些新的直线。所以这时候我们就需要对这新的m条直线进行匹配，通过每条直线对之前k条直线进行对比，如果这两条直线相似，则认为两条直线是同一条直线；如果这两条直线不同，则认为这条直线是新的直线，把它的参数方程添加到状态量中，特征地图中就多了一条直线。具体过程如下：

系统实时从激光雷达最新一帧数据中提取m条直线，由于之前一帧数据求得k条直线的特征参数为则对最新一帧数据中的直线i，i＝1,2,…,m与之前一帧数据中的直线j，j＝1,2,…,k进行如下处理：

计算两条直线到y轴的距离差值以及两条直线之间的夹角

假设LINE_DA0为直线的距离阈值，LINE_DA1为直线的角度阈值，如果有dl_ij<LINE_DA0并且dθ_ij<LINE_DA1，则认为直线i对应于直线j，进入步骤S5；否则直线i为新的直线，计算直线i在世界坐标系下的参数进入步骤S4。

S4、添加新的直线特征。

由于在激光雷达中观测到的直线是在雷达坐标系中的，而系统中的直线参数是在世界坐标系下的，因此需要选取合适的参数，使得在雷达坐标系内的特征能和世界坐标系中的特征了联系起来。假设世界坐标系下，直线的特征为直线的倾斜角β和原点到直线的距离dist，则通过直线特征的提取，直线i，i＝1,2,…,m在雷达坐标系中的特征参数方程表示为：

在雷达坐标系中，机器人对直线i的观测量表示为：

式中z_L表示雷达坐标系原点到直线的距离(假设雷达在直线的下方)，zα表示直线与雷达坐标系X轴正方向的夹角，本发明实施例中，为方便计算，令α∈(-0.5π，0.5π)。

如图3所示，根据图中几何关系可知：

其中d_L＝z_L，α＝z_α。

则对于新观测的一条特征直线N+1，直线的状态量为：

式中α＝z_α，θ表示世界坐标系下特征直线相对机器人的夹角，a表示世界坐标系下特征直线相对机器人的距离，则EKF-SLAM的系统空间扩充为：

系统的状态量扩充为3+2(N+1)维，表示为：

系统的协方差矩阵扩充为：

式中R(k)是机器人在雷达坐标系下，观测直线提取观测参数[z_L,z_α]时产生的误差，本发明实施例中通过实验计算得

状态转换误差的协方差矩阵扩充为：

式中Q(k)表示移动平台运动误差，本发明实施例中经过实验估计

观测误差的协方差矩阵扩充为：

S5、对原有的直线特征进行观测与修正。

对于激光雷达观测到的直线i(观测量为z_i(k)＝[z_L,z_α])，若通过直线匹配计算出其对应于地图中的直线j(状态量为l_j＝[dist,β])，则可以用观测到的参数，对状态量进行修正。

计算出激光雷达对直线j的观测值的预测量：

对预测量求偏导数，计算出系统观测函数的雅克比矩阵为：

式中：

更新系统的状态量和协方差：

其中公式(24)为计算卡尔曼增益，公式(25)为用系统观测量修正均值，即更新均值，公式(26)为更新系统状态转换协方差。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。