CN108519615A - 基于组合导航和特征点匹配的移动机器人自主导航方法 - Google Patents

Abstract

本发明公开了一种基于组合导航和特征点匹配的移动机器人自主导航方法，步骤如下：S1，建立空间地图数据库；S2，规划路径；S3，获取移动机器人的位置信息；S4，进行卡尔曼滤波迭代；S5，对移动机器人的惯性导航进行修正。基于组合导航+特征点匹配的自主导航，是利用激光雷达对周围环境的扫描建模，对周围特征点进行提取，确定距离特征点的位置，再根据特征点在已知地图中的位置来推算出机器人在地图中的当前位置，以此确定位置坐标，来弥补巡视机器人GPS信号受遮挡的问题。

Description

基于组合导航和特征点匹配的移动机器人自主导航方法

技术领域

本发明属于机器人的自定位与导航领域，特别是一种基于组合导航和特征点匹配的移动机器人自主导航方法。

背景技术

校园智能安防巡视机器人是针对于校园这个特定环境进行安全巡视的机器人，由于一般的校园情况比较复杂，存在有树木和高大建筑物等，而现今的巡视机器人更多的是采用GPS进行导航，但依赖GPS为主进行导航的机器人在导航的过程中，容易出现GPS信号受遮挡，定位不准，以及出现丢星的情况，从而使机器人无法确定自己的位置，不能完成导航任务。

此需要研究一种方法在GPS信号不准及丢失的情况下依然能够对子自己的位置进行定位。目前，通常的解决办法是GPS和惯性导航技术的融合，依靠惯导来对自己位置进行实时预测，GPS可以对惯导的的误差进行修正。但是在GPS长时间不能够精确定位的情况下，惯性导航的误差会随着时间的而增大。

发明内容

针对上述现有技术中描述的不足，本发明提供一种基于组合导航和特征点匹配的移动机器人自主导航方法。

为解决上述技术问题，本发明所采用的技术方案如下：

一种基于组合导航和特征点匹配的移动机器人自主导航方法，步骤如下：

S1，建立空间地图数据库；

S2，规划路径；

从空间地图数据库中获得所需的地图，确定已知特征点在地图中的具***置，并确定起点和终点，利用Dstar算法进行路径规划；

S3，获取移动机器人的位置信息：

S3.1，移动机器人获得GPS接收机的经纬度信息；

GPS接收机接收到四颗以上卫星信号，并通过GPS接收机到卫星的伪距，确定出GPS接收机所在的经度、纬度和高度；

S3.2，将经纬度信息转换为当地地理坐标系信息，转换公式为：

e²＝2f-f²；

式中，L为当前时刻的经度，B为当前时刻的纬度，H为当前时刻的高度，X、Y、Z为当前时刻的当地地理坐标系，N为目标点的卯酉圈曲率半径，a为地球的长轴长，f为地球的扁率；e为第一偏心率；

S3.3，获得移动机器人在地理坐标系中的位姿信息和位置信息；

S3.3.1，获得移动机器人的位姿信息；

加速度计测量移动机器人在惯性参考系的运动加速度，对时间进行一次积分，获得移动机器人相对于当地地理坐标系的即时速度信息，然后将即时速度信息对时间进行一次积分后得到的位移信息转换到当地地理坐标系中，得到移动机器人在当地地理坐标系中位置，并结合陀螺仪测得移动机器人的姿态信息；

S3.3.2，获得移动机器人的位置信息；

S3.3.2.1，移动机器人通过激光雷达扫描周围环境，获得激光雷达扫描中心到第n个目标物体的距离ρ_n，第n个目标物体的扫描方向的角度β_n，距离ρ_n和角度β_n构成点集(ρ_n，β_n)；

S3.3.2.2，对获得的点集进行分割、聚合，得到测量的特征点信息；

S3.3.2.3，将提取的测量特征点信息与地图中的已知特征点信息进行匹配，得到特征点在地图中的位置信息(x_p，y_p)：

S3.3.2.4，根据特征点的位置信息得到移动机器人的地理坐标系，公式为：

式中，x_r，y_r，θ_r表示移动机器人中心在世界坐标系中的位置；

整理得到：

S4，进行卡尔曼滤波迭代：

将惯性导航测定的位置和速度状态信息作为卡尔曼滤波器的状态信息，将GPS得到的位姿信息的观测值和特征点提取的位置信息的观测值作为卡尔曼滤波器的量测信息，进行迭代；

S4.1，构建移动机器人的***方程；

所述***方程包括状态方程，量测方程和预测方程；

S4.1.1，构建状态方程和量测方程，

状态方程的表达式为：

dx(t)＝F(t)×x(t)+G(t)+W(t)；

式中，x(t)为移动机器人***的n维状态向量；F(t)为状态转移矩阵n×n阶；G(t)为噪声矩阵；W(t)为过程噪声矢量；

量测方程的表达式为：

Z(t)＝H(t)×x(t)+V(t)；

式中，Z(t)为m维量测矢量；H(t)为量测矩阵m×n阶；V(t)为m维量测噪声矢量；

S4.1.2，将状态方程和量测方程分别进行离散化，得到：

式中，X_k维k时刻的n维状态向量；W_k为k时刻的过程噪声；Z_k为k时刻的量测矢量；H_k为k时刻的量测矩阵；V_k1为GPS接收的量测噪音矢量；V_k2为特征点提取的量测噪音矢量；

φ_k,k+1为k到k+1时刻的状态转移矩阵，计算公式为：

Γ_x为k时刻的过程噪声加权矩阵，计算公式为：

式中，T为迭代周期；

S4.2，构建预测方程

所述预测方程包括状态向量的预测值方程和状态向量预测值的协方差P_k,k+1方程，公式如下：

S4.3，构建修正方程

所述修正方程包括最优估计方程、滤波增益K_k+1方程以及滤波值协方差P_k+1方程，公式为：

S4.4，根据步骤S4.1-S4.3进行迭代，得到移动机器人精确的位置信息。

S5，对移动机器人的惯性导航进行修正。

根据惯导设备的精度，设定时间范围t,每隔t时间段，利用该时刻融合后的位置信息对惯性导航进行清零修正，惯导设备以此时刻为起点，重新进行测量。

而且，GPS接收机定位到的位置信息和特征点匹配定位得到的位置信息可能出现野值，对野值进行处理。

对采集得到的定位点采用多项式拟合法来推算出较为真实的定位点，将m个连续已知正常定位点坐标Pi，代入到拟合多项式中，根据最小二乘原理求出拟合系数，得到拟合多项式，利用该多项式外推下一点的定位坐标，根据拟合后的残差计算中误差σ，将外推得到的定位坐标和实际的观测坐标进行比较，当两者之差小于3σ时，认为当前的观测值不是野值，为正常观测值；当两者之差大于等于3σ时，认为当前的观测值是野值予以剔除，此时采用外推的拟合值取代当前定位点的观测值，完成一次检测与修正，然后去掉最早的一个观测值，加入下一个非野值的观测值，重复上述过程，检测和修正该定位观测值。

本发明基于组合导航+特征点匹配的自主导航，是利用激光雷达对周围环境的扫描建模，对周围特征点进行提取，确定距离特征点的位置，再根据特征点在已知地图中的位置来推算出机器人在地图中的当前位置，以此确定位置坐标，来弥补巡视机器人GPS信号受遮挡的问题。本发明采用多种定位方式相结合，可以大大提高安防巡视机器人的使用范围，使巡视机器人可在一些校园，社区等具有高大建筑物以及树木等遮挡GPS信号的环境中进行正常的自主巡视。本发明利用多项式野值剔除对定位信息进行优化，对一些定位误差大的位置信息进行剔除，最终利用卡尔曼滤波进行预测，提高的巡视机器人定位的精度，使机器人能够在场景复杂多变的地域进行安全工作，提高了巡视机器人的安全性和可靠性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的流程图。

图2为本发明卡尔曼滤波的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于组合导航和特征点匹配的移动机器人自主导航方法，如图1所示，步骤如下：

S1，建立空间地图数据库；

S2，规划路径；

从空间地图数据库中获得所需的地图，确定已知特征点在地图中的具***置，并确定起点和终点，利用Dstar算法进行路径规划；

S3，获取移动机器人的位置信息：

S3.1，移动机器人获得GPS接收机的经纬度信息；

GPS接收机接收到四颗以上卫星信号，并通过GPS接收机到卫星的伪距，确定出GPS接收机所在的经度、纬度和高度；

S3.2，将经纬度信息转换为当地地理坐标系信息，转换公式为：

e²＝2f-f²；

式中，L为当前时刻的经度，B为当前时刻的纬度，H为当前时刻的高度，X、Y、Z为当前时刻的当地地理坐标系，N为目标点的卯酉圈曲率半径，a为地球的长轴长，f为地球的扁率；e为第一偏心率。

对于GPS接收机定位到的位置信息可能出现野值，对野值进行处理。

对采集得到的定位点采用多项式拟合法来推算出较为真实的定位点，将m个连续已知正常定位点坐标Pi，代入到拟合多项式中，根据最小二乘原理求出拟合系数，得到拟合多项式，利用该多项式外推下一点的定位坐标，根据拟合后的残差计算中误差σ，将外推得到的定位坐标和实际的观测坐标进行比较，当两者之差小于3σ时，认为当前的观测值不是野值，为正常观测值；当两者之差大于等于3σ时，认为当前的观测值是野值予以剔除，此时采用外推的拟合值取代当前定位点的观测值，完成一次检测与修正，然后去掉最早的一个观测值，加入下一个非野值的观测值，重复上述过程，检测和修正该定位观测值。

S3.3，获得移动机器人在地理坐标系中的位姿信息和位置信息；

S3.3.1，获得移动机器人的位姿信息；

加速度计测量移动机器人在惯性参考系的运动加速度，对时间进行一次积分，获得移动机器人相对于当地地理坐标系的即时速度信息，然后将即时速度信息对时间进行一次积分后得到的位移信息转换到当地地理坐标系中，得到移动机器人在当地地理坐标系中位置，并结合陀螺仪测得移动机器人的姿态信息；

S3.3.2，获得移动机器人的位置信息；

S3.3.2.1，移动机器人通过激光雷达扫描周围环境，获得激光雷达扫描中心到第n个目标物体的距离ρ_n，第n个目标物体的扫描方向的角度β_n，距离ρ_n和角度β_n构成点集(ρ_n，β_n)；

S3.3.2.2，对获得的点集进行分割、聚合，得到测量的特征点信息；

S3.3.2.3，将提取的测量特征点信息与地图中的已知特征点信息进行匹配，得到特征点在地图中的位置信息(x_p，y_p)：

S3.3.2.4，根据特征点的位置信息得到移动机器人的地理坐标系，公式为：

式中，x_r，y_r，θ_r表示移动机器人中心在世界坐标系中的位置；

整理得到：

在特征点匹配定位得到的位置信息可能出现野值，对野值进行处理。对采集得到的定位点采用多项式拟合法来推算出较为真实的定位点，将m个连续已知正常定位点坐标Pi，代入到拟合多项式中，根据最小二乘原理求出拟合系数，得到拟合多项式，利用该多项式外推下一点的定位坐标，根据拟合后的残差计算中误差σ，将外推得到的定位坐标和实际的观测坐标进行比较，当两者之差小于3σ时，认为当前的观测值不是野值，为正常观测值；当两者之差大于等于3σ时，认为当前的观测值是野值予以剔除，此时采用外推的拟合值取代当前定位点的观测值，完成一次检测与修正，然后去掉最早的一个观测值，加入下一个非野值的观测值，重复上述过程，检测和修正该定位观测值。

S4，进行卡尔曼滤波迭代，如图2所示；

将惯性导航测定的位置和速度状态信息作为卡尔曼滤波器的状态信息，将GPS得到的位姿信息的观测值和特征点提取的位置信息的观测值作为卡尔曼滤波器的量测信息，进行迭代；

S4.1，构建移动机器人的***方程；

所述***方程包括状态方程，量测方程和预测方程；

S4.1.1，构建状态方程和量测方程，

状态方程的表达式为：

dx(t)＝F(t)×x(t)+G(t)+W(t)；

式中，x(t)为移动机器人***的n维状态向量；F(t)为状态转移矩阵n×n阶；G(t)为噪声矩阵；W(t)为过程噪声矢量；

量测方程的表达式为：

Z(t)＝H(t)×x(t)+V(t)；

式中，Z(t)为m维量测矢量；H(t)为量测矩阵m×n阶；V(t)为m维量测噪声矢量；

S4.1.2，将状态方程和量测方程分别进行离散化，得到：

式中，X_k维k时刻的n维状态向量；W_k为k时刻的过程噪声；Z_k为k时刻的量测矢量；H_k为k时刻的量测矩阵；V_k1为GPS接收的量测噪音矢量；V_k2为特征点提取的量测噪音矢量；

φ_k,k+1为k到k+1时刻的状态转移矩阵，计算公式为：

Γ_x为k时刻的过程噪声加权矩阵，计算公式为：

式中，T为迭代周期；

S4.2，构建预测方程

所述预测方程包括状态向量的预测值方程和状态向量预测值的协方差P_k,k+1方程，公式如下：

S4.3，构建修正方程

所述修正方程包括最优估计方程、滤波增益K_k+1方程以及滤波值协方差P_k+1方程，公式为：

S4.4，根据步骤S4.1-S4.3进行迭代，得到移动机器人精确的位置信息。

S5，对移动机器人的惯性导航进行修正。

根据惯导设备的精度，设定时间范围t,每隔t时间段，利用该时刻融合后的位置信息对惯性导航进行清零修正，惯导设备以此时刻为起点，重新进行测量。

下面对本发明的思路进行解释说明

在已知的环境中，确定起点和终点，然后利用Dstar算法进行路径规划，根据规划后的路径进行自主导航。运行过程中遇到障碍物时，通过自身携带的激光雷达和超声波设备进行动态避障，避障失败则以此点为起点重新进行路径规划。

自主导航过程中，对GPS接收机接收到的经纬度信息进行数据处理，坐标转换，转换为大地坐标系；对惯导***中的加速度计，陀螺仪等传感器设备进行融合，得出该时刻的位置信息和姿态信息；通过激光雷达对周围环境的扫描，特征点提取，与空间数据库进行匹配，确定出位置信息。

GPS和特征点提取在导航过程中，GPS出现受干扰的情况时会出现测量数据误差波动非常大，特征点提取因传感器噪声或者特征点数目不足导致位置信息提取失败的情况，为防止相关问题对最终的定位结果造成干扰，影响定位精度，设置一个阈值，对于超出阈值的测量信息进行剔除。

分别建立GPS，INS，特征点匹配的***方程，状态方程，量测方程以及误差模型，利用惯性导航设备得出的状态信息，对机器人下一时刻状态进行预测，对GPS和特征点提取作为量测值对惯性导航预测进行修正，二者输入卡尔曼滤波器，经过迭代，得出精确的位置信息。

根据惯性导航的误差范围，每隔一定的时间段，利用滤波融合过的位置信息对惯性导航进行修正。

在具体实施时，需要的前期预备工作：

建立空间地图数据库：根据已知地图信息，确定出地图中已知特征点在地图中的具体在位置，以此来校准机器人在地图中的位置信息。

利用GPS接收机得到当前位置的经纬度坐标，然后将经纬度坐标转换为当地地理坐标系。给出起末点位置信息，利用Dstar采用后向搜索的办法，从目标点向起点规划出一条路径，完成路径规划工作。

正常启动后，所需的步骤为：

第一步：GPS，惯性导航，特征点匹配的数据预处理。

1，GPS为全球定位***，通过GPS接收机接收到四颗以上卫星信号，并通过接收机到卫星的伪距，确定出接收机所在的经度，纬度和高度。通过坐标系转换，将给定的经纬度信息转化为当地地理坐标系。

GPS的坐标转换为经纬度信息转变为当地地理坐标系信息：

e²＝2f-f²；

L、B、H为该时刻的经度、纬度、高度。X、Y、Z为该时刻的当地地理坐标系。N为目标点的卯酉圈曲率半径；a地球的长轴长；f为地球的扁率；e第一偏心率。

2，惯性导航***中存在陀螺仪和加速度计，过加速度计测量载体在惯性参考系的运动加速度，对时间进行一次积分，获得载体相对于导航坐标系的即时速度信息，然后再对时间进行一次积分，并转换到导航坐标系中，就能够得到在导航坐标系中载体的位置，通过陀螺仪可以测得载体的姿态信息。

3，特征点匹配是利用激光雷达对周围环境进行扫描，扫描到距离特征点的距离和角度，表示为(ρ_n，β_n)，ρ_n为激光雷达扫描中心到第n个目标物体的距离；β_n为第n个目标物体的扫描方向的角度，n为扫描点的个数。然后进行点集分割，聚合，提取出线性参数，从而提取出线性特征，提取出特征点信息，得出特征点在机器人坐标系中的位置(x_p，y_p)。然后用测得的特征点与地图中的已知特征点信息进行匹配，确定出特征点在地图中的位置信息，从而通过坐标系转换，得出机器人在地图中的位置信息(x_r,y_r)。

特征点匹配过程中，利用激光雷达对周围环境进行扫描，扫描到距离特征点的距离和角度，表示为(ρ_n，β_n)，ρ_n为激光雷达扫描中心到第n个目标物体的距离；β_n为第n个目标物体的扫描方向的角度，n为扫描点的个数。然后进行点集分割，聚合，提取出线性参数，从而提取出线性特征，提取出环境信息，然后对地图中的特征点信息进行匹配，确定出特征点在机器人坐标系中的位置信息，记(x_p，y_p),则：

将机器人坐标系转化为地理坐标系：

(x_r,y_r)和θ_r分别表示为机器人中心在世界坐标系中的位置；

该特征点在地理坐标系位置表示为：

第二步：野值的处理

对于GPS接收机和特征点提取出现的野值问题，采用多项式拟合法来推算位置信息。将m个连续已知正常定位点坐标(Pi，i＝1，2，3，…，5)代入到拟合多项式中，根据最小二乘原理求出拟合系数，得到拟合多项式，利用该多项式外推下一点的定位坐标，根据拟合后的残差计算中误差σ，将外推得到的定位坐标和实际的观测坐标进行比较，当两者之差小于3σ时，认为当前的观测值不是野值，为正常观测值；当两者之差大于等于3σ时，认为当前的观测值是野值予以剔除，此时采用外推的拟合值取代当前定位点的观测值，完成一次检测与修正。然后去掉最早的一个观测值，加入下一个非野值的观测值，重复上述过程，检测和修正该定位观测值。

第三步：卡尔曼滤波迭代

将惯性导航测定的位置，和速度状态信息作为滤波的状态信息。将GPS和特征点提取的观测值作为滤波的量测信息，进行迭代。

卡尔曼滤波融合的过程为：

在已知连续***的状态方程和量测方程：

式中，x(t)为***的n维状态向量；F(t)为状态转移矩阵(n×n)阶；G(t)为噪声矩阵；W(t)为过程噪声矢量；Z(t)为m维量测矢量；H(t)为量测矩阵(m×n)阶；V(t)为m维量测噪声矢量。

连续***离散化:

将上式的状态方程和量测方程离散化可得：

式中，X_k维k时刻的n维状态向量；φ_k,k+1为k到k+1时刻的状态转移矩阵；Γ_x为k时刻的过程噪声加权矩阵；W_k为k时刻的过程噪声；Z_k为k时刻的量测矢量；H_k为k时刻的量测矩阵；V_k1V_k2分别为GPS和特征点提取的量测噪声矢量；

φ_k,k+1和Γ_k有下式计算得到；

式中，T为迭代周期，在实际计算中，两式仅取有限项即可。

时间更新(即预测方程)

时间更新部分包括状态向量的预测值方程和状态向量预测值的协方差P_k,k+1方程，称为预测方程。它根据所求参数X的动态变化规律，从k时刻的参数值X_k预测k+1时刻的参数值X_k+1。具体如下式所示：

量测更新(即修正方程)

通过外部观测信息，可以在状态向量预测值的基础上得到向量的最优滤波值上式滤波方程中，最优估计方程、滤波增益K_k+1方程以及滤波值协方差P_k+1方程统称为修正方程。它是基于对某些参数和观测值Z_k+1，对预测值进行修正，从而给出最优滤波值，并估计相应协方差阵。

第四部：对惯性导航修正

根据惯导设备的精度x米，设定时间范围t,每隔t时间段，利用该时刻融合后的位置信息惯性导航进行清零修正，惯性设备以此时刻为起点，重新进行测量。以此来确保惯性导航的精度，防止随时间推移，出现误差累计的情况。

上面所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于组合导航和特征点匹配的移动机器人自主导航方法，其特征在于，步骤如下：

S1，建立空间地图数据库；

S2，规划路径；

从空间地图数据库中获得所需的地图，确定已知特征点在地图中的具***置，并确定起点和终点，利用Dstar算法进行路径规划；

S3，获取移动机器人的位置信息；

S4，进行卡尔曼滤波迭代：

将惯性导航测定的位置和速度状态信息作为卡尔曼滤波器的状态信息，将GPS得到的位姿信息的观测值和特征点提取的位置信息的观测值作为卡尔曼滤波器的量测信息，进行迭代；

S5，对移动机器人的惯性导航进行修正；

根据惯导设备的精度，设定时间范围t，每隔t时间段，利用该时刻融合后的位置信息对惯性导航进行清零修正，惯导设备以此时刻为起点，重新进行测量。

2.根据权利要求1所述的基于组合导航和特征点匹配的移动机器人自主导航方法，其特征在于，在步骤S3中，具体步骤为：

S3.1，移动机器人获得GPS接收机的经纬度信息；

GPS接收机接收到四颗以上卫星信号，并通过GPS接收机到卫星的伪距，确定出GPS接收机所在的经度、纬度和高度；

S3.2，将经纬度信息转换为当地地理坐标系信息，转换公式为：

e²＝2f-f²；

式中，L为当前时刻的经度，B为当前时刻的纬度，H为当前时刻的高度，X、Y、Z为当前时刻的当地地理坐标系，N为目标点的卯酉圈曲率半径，a为地球的长轴长，f为地球的扁率；e为第一偏心率；

S3.3，获得移动机器人在地理坐标系中的位姿信息和位置信息。

3.根据权利要求2所述的基于组合导航和特征点匹配的移动机器人自主导航方法，其特征在于，在步骤S3.3中，具体步骤为：S3.3.1，获得移动机器人的位姿信息；

加速度计测量移动机器人在惯性参考系的运动加速度，对时间进行一次积分，获得移动机器人相对于当地地理坐标系的即时速度信息，然后将即时速度信息对时间进行一次积分后得到的位移信息转换到当地地理坐标系中，得到移动机器人在当地地理坐标系中位置，并结合陀螺仪测得移动机器人的姿态信息；

S3.3.2，获得移动机器人的位置信息；

S3.3.2.1，移动机器人通过激光雷达扫描周围环境，获得激光雷达扫描中心到第n个目标物体的距离ρ_n，第n个目标物体的扫描方向的角度β_n，距离ρ_n和角度β_n构成点集(ρ_n，β_n)；

S3.3.2.2，对获得的点集进行分割、聚合，得到测量的特征点信息；

S3.3.2.3，将提取的测量特征点信息与地图中的已知特征点信息进行匹配，得到特征点在地图中的位置信息(x_p,y_p)：

S3.3.2.4，根据特征点的位置信息得到移动机器人的地理坐标系，公式为：

式中，x_r，y_r，θ_r表示移动机器人中心在世界坐标系中的位置；

整理得到：

4.根据权利要求1所述的基于组合导航和特征点匹配的移动机器人自主导航方法，其特征在于，在步骤S4中，具体步骤为：S4.1，构建移动机器人的状态方程和量测方程，

状态方程的表达式为：

dx(t)＝F(t)×x(t)+G(t)+W(t)；

式中，x(t)为移动机器人***的n维状态向量；F(t)为状态转移矩阵n×n阶；G(t)为噪声矩阵；W(t)为过程噪声矢量；

量测方程的表达式为：

Z(t)＝H(t)×x(t)+V(t)；

式中，Z(t)为m维量测矢量；H(t)为量测矩阵m×n阶；V(t)为m维量测噪声矢量；

S4.2，将状态方程和量测方程分别进行离散化，得到：

式中，X_k维k时刻的n维状态向量；W_k为k时刻的过程噪声；Z_k为k时刻的量测矢量；H_k为k时刻的量测矩阵；V_k1为GPS接收的量测噪音矢量；V_k2为特征点提取的量测噪音矢量；

φ_k,k+1为k到k+1时刻的状态转移矩阵，计算公式为：

Γ_x为k时刻的过程噪声加权矩阵，计算公式为：

式中，T为迭代周期；

S4.3，构建预测方程

所述预测方程包括状态向量的预测值方程和状态向量预测值的协方差P_k,k+1方程，公式如下：

S4.4，构建修正方程

所述修正方程包括最优估计方程、滤波增益K_k+1方程以及滤波值协方差P_k+1方程，公式为：

S4.5，根据步骤S4.2-S4.4进行迭代，得到移动机器人精确的位置信息。