CN102706342A - 一种智能移动机器人的定位与环境建模方法 - Google Patents

Abstract

本发明公开了一种智能移动机器人的定位与环境建模方法，首先形成修正迭代扩展卡尔曼滤波算法并确定迭代次数，然后建立移动机器人的运动模型和观测模型，初始化移动机器人的状态，计算位置雅可比矩阵和控制输入雅可比矩阵计算、观测雅可比矩阵等；最后通过求解卡尔曼增益矩阵，更新状态估计方程和协方差矩阵，并重复以上部分步骤。本发明以在移动机器人的同时定位与环境建模领域中使用最为广泛的扩展卡尔曼滤波算法为核心，通过对该算法进行改进，使得算法的性能得到极大提高，能够更好的满足在SLAM中的应用。也为移动机器人在未知环境中的自主导航、完成复杂智能任务提供强大的技术支撑。

Description

一种智能移动机器人的定位与环境建模方法

技术领域

本发明涉及自动控制领域，特别涉及一种移动机器人的定位与环境建模方法。

背景技术

移动机器人是传感器技术、控制技术、信息处理技术、机械加工技术、电子技术、计算技术等多门技术相互结合而形成的。在这些技术的基础上，移动机器人主要研究以下内容：机器结构、多机器人系统、体系结构、路径规划、人工智能、导航与定位、多传感器信息融合技术、人机交互、反馈镇定以及跟踪控制。在以上的研究内容中，有一项技术是移动机器人的实际应用中不可或缺的，这项技术就是导航与定位的研究。移动机器人，顾名思义这种机器人不是固定在某一地方不动，而是根据任务的要求随时的采取相应措施从起点到达目标点。而在这个过程中，移动机器人需要知道自己该往哪儿走，需要知道自己在哪儿，需要知道自己是不是到了目的地。而对这些问题的回答，就形成了移动机器人的导航与定位研究内容。Leonard和Durrant-Whyte两位研究人员对这个问题做了一个具有纲领性的总结，也即移动机器人在具备以下几个条件的情况下能够自主的进行移动，避免时刻受到人为因素的控制：（1）Where am I?（我在哪里？）在移动机器人的导航过程中，移动机器人需要时刻的知道自己所处的位置，只有在知道了自己所处的位置，才能为下一步需要采取何种动作提高判断依据。这个问题就是移动机器人的定位问题，在很好的回答了这个问题的基础上，才有可能解决后面两个问题；（2）Where am I going?(我要去哪里?)在移动机器人获得任务后，它就得考虑在完成一步之后，下一步应该往哪里走，或者是它的目的地在什么地方。这个问题就需要移动机器人对所处环境进行建模，也就是环境建模或者说是地图构建的研究；(3)How should I get there?(我应该如何到那儿?)一旦机器人在知晓了自己的当前位置和下一步要去哪里之后，它就需要考虑自己在从当前位置到达下一步位置应该怎么走，也就是在这个过程中，需要走的路径尽可能的短并且不能碰到障碍物。这就是移动机器人的路径规划研究，是机器人的运动控制中的一部分研究内容。

因此，对于移动机器人的定位与环境建模进行研究，可以扩展移动机器人的运动范围、使用功能，能够对移动机器人的信息无障碍研究和自主式移动提供巨大的技术支撑。

在移动机器人的定位和环境建模研究方面，传统上的方法都是对这两个问题进行单独的分析。因此，没有考虑过将两者进行统一的研究。我们知道，在移动机器人的定位中，如果在有精确的地图的情况下，机器人的定位准确性就很高。但是，由于移动机器人本身的工作特性，我们不可能每次都能知道机器人工作环境的具体情况。在这种情况下，移动机器人对环境的认知就只有依靠自己携带的传感器来采集环境信息。因此，当机器人所处的环境比较陌生的情况下，机器人会面临这一个两难的问题：即为了构建环境地图，机器人需要清楚的知道自己在每个时刻所处的位置，为了精确的定位，移动机器人需要有一个高准确率的地图。而这个问题，可以用一个非常形象的比喻来描述，即“鸡与蛋”的问题。为了准确并且高效的感知环境，移动机器人需要将定位问题与环境建模问题同时考虑。在未知环境模型下移动机器人的定位导航需要构造环境的模型，这种在确定自身位置的同时并构造环境模型的过程，被称为SLAM(Simultaneous Localizationand Mapping)。移动机器人的同时定位与环境建模(SLAM)最早由Smith、Self和Cheeseman提出，由于其重要的理论与应用价值，被诸多研究人员认为是实现真正的全自主移动机器人的关键。

SLAM问题归纳起来是一个“预测—观测—更新”的过程，主要包括如下迭代步骤：(1)根据当前的机器人位姿估计和地图信息对下一步的机器人位姿和路标位置进行预测；（2）在下一步中通过外部传感器观测环境中的路标；（3）利用观测结果校正第一步预测的定位和建模结果。上述步骤迭代进行，在此过程中机器人对自身位姿和路标位置的估计逐渐准确。

SLAM中使用得最广泛的扩展卡尔曼滤波算法（Extended Kalman Filter，EKF）在线性化过程中对非线性函数进行Taylor级数展开，忽略二次及其以上阶次得到近似的线性模型，但是该算法在对于Taylor展开式的高阶次项的忽略，线性化所产生的误差就会比较大。所以，此时应用于SLAM的EKF就会增大误差。需要对EKF进行相应的改进。

因此急需一种适用于移动机器人同时进行定位与环境建模的高效率的计算方法。

发明内容

有鉴于此，本发明所要解决的技术问题是提供一种适用于移动机器人同时进行定位与环境建模的高效率的计算方法。该算法以常规的EKF算法为基础，对EKF在数学上的缺陷进行优化、修改，使之能够很适用于移动机器人的同时定位与环境建模中，提高算法在SLAM中的估计效果。

本发明的目的是这样实现的：

本发明提供的一种智能移动机器人的定位与环境建模方法，包括以下步骤：

S1：形成修正迭代扩展卡尔曼滤波算法，并确定迭代次数；

S2：建立移动机器人的运动模型x_k；

S3：建立移动机器人的观测模型z_k；

S4：初始化移动机器人的先验状态变量

先验协方差矩阵

系统的过程噪声协方差Q_k-1及观测噪声协方差R_k-1；

S5：计算移动机器人运动模型的位置雅可比矩阵

S6：计算移动机器人控制输入雅可比矩阵

S7：计算观测模型的观测雅可比矩阵J_h；

S8：计算卡尔曼增益K_k；

S9：更新后验状态估计

和后验状态估计协方差矩阵P_k；

S10：重复循环步骤S5至S9。

进一步，所述修正迭代扩展卡尔曼滤波算法，具体如下：

时间更新（预测）：

计算先验状态变量：

计算先验状态估计协方差矩阵：

测量更新（校正）：

计算卡尔曼增益： $K_k=P_k^{-}{J}_h^T{(J_h{P}_h^{-}{J}_h^T+V_k{R}_k{V}_k^T)}^{-1},$

由观测变量z_k更新后验状态估计：

后验状态估计协方差矩阵更新：

其中，

表示x_k在k时刻的先验估计，

是x_k在k时刻的后验估计；u_k-1是系统从k-1时刻的状态向量变化到k时刻的控制输入，z_k是k时刻的观测向量，f(·)是系统的状态转移函数，

表示k时刻的协方差估计，A_k和W_k分别是f(·)对x_k和对W的偏导雅可比矩阵，

和

分别是它们的转置矩阵，Q_k-1表示过程激励噪声协方差，K_k表示卡尔曼增益，J_h表示状态向量x_k对测量向量z_k的增益，V_K是f(·)对V的偏导雅可比矩阵，R_K表示k时刻的噪声协方差，()^-1表示矩阵的逆，z_k表示测量向量，H_K表示非线性函数h对x的偏导雅可比矩阵，I表示单位矩阵；

进一步，所述建立移动机器人的运动学模型具体如下：

$x_k=f(x_{k-1},u_k)=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\mathrm{\φ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{v}_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L\end{array}\right]+w_k,$

其中，v_k是机器人的移动速度，L是机器人前后轴之间的轴距，θ_k是机器人的前轮偏角，Δt为传感器的采样周期，w_k是系统噪声。

进一步，所述建立的移动机器人观测模型具体如下：

z_k＝h(x_k,v_k)，

$z_{\mathrm{ik}}=\left[\begin{array}{c}{r}_{\mathrm{ik}}\\ {\mathrm{\θ}}_{\mathrm{ik}}\end{array}\right]h_i\left(x_k\right)=\left[\begin{array}{c}\sqrt{{(x_i-x_k)}^2+{(y_i-y_k)}^2}\\ \arctan \frac{y_i-y_k}{x_i-x_k}-{\mathrm{\φ}}_k\end{array}\right],$

其中，z_k是机器人在k时刻的观测量，h(·)为测量函数，v_k为观测噪声，由于激光传感器的光束发散角小，所以大多数情况下都是利用激光传感器进行测量，以此得到某个环境特征到机器人的距离以和前进方向的夹角，假定第i个点特征的位置坐标为(x_i,y_i)。

进一步，所述机器人位置雅可比矩阵

计算如下：

${\▿f}_{x_k}=\frac{\∂f}{{\∂x}_k}=\left[\begin{array}{ccc}\frac{{\∂f}_x}{\∂x}& \frac{{\∂f}_x}{\∂y}& \frac{{\∂f}_x}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_y}{\∂x}& \frac{{\∂f}_y}{\∂y}& \frac{{\∂f}_y}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{\θ}}}{\∂x}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂y}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 1& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 0& 1\end{array}\right],$

进一步，所述机器人控制输入的雅可比矩阵

计算如下：

${\▿f}_u=\left[\begin{array}{cc}\frac{{\∂f}_{\mathrm{ux}}}{\∂v}& \frac{{\∂f}_{\mathrm{ux}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{uy}}}{\∂v}& \frac{{\∂f}_{\mathrm{uy}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{u\θ}}}{\∂v}& \frac{{\∂f}_{\mathrm{u\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L& v_k\mathrm{\Δ}t\tan {{\mathrm{\θ}}_k}^2/L\end{array}\right],$

进一步，所述观测模型实际观测数据通过移动机器人上的传感器获得，所述观测模型在第i个坐标下的雅可比矩阵J_h的计算具体如下：

$J_h=\left[\begin{array}{ccc}\frac{{\∂h}_1}{\∂x}& \frac{{\∂h}_1}{\∂y}& \frac{{\∂h}_1}{\∂\mathrm{\φ}}\\ \frac{{\∂h}_2}{\∂x}& \frac{{\∂h}_2}{\∂y}& \frac{{\∂h}_2}{\∂\mathrm{\φ}}\end{array}\right]=\left[\begin{array}{ccc}\frac{x_k-x_i}{r}& \frac{y_k-y_i}{r}& 0\\ \frac{y_k-y_i}{r^2}& \frac{x_k-x_i}{r^2}& 1\end{array}\right],$

其中， $r=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2},$

$V_k=\left[\begin{array}{cc}\frac{{\∂h}_1}{{\∂v}_r}& \frac{{\∂h}_1}{{\∂v}_{\mathrm{\θ}}}\\ \frac{{\∂h}_2}{{\∂v}_r}& \frac{{\∂h}_2}{{\∂v}_{\mathrm{\θ}}}\end{array}\right]=E,$

进一步，所述迭代处理次数根据优化效果和计算速度确定。

本发明的有益效果：本发明是对SLAM中使用得最为广泛的EKF算法为基础，通过对常规EKF进行迭代和修正处理后得到的一种更优的算法。迭代处理能够有效减少EKF的线性化误差，修正处理能够忽视迭代处理要求的较高的观测精度带来的影响。通过迭代、修正处理后的EKF算法能够有更好的估计精度，对SLAM的应用研究有更好的推动作用，能为移动机器人的自主式移动、未知环境中的智能任务的实施提供更多的选择。

本发明以在移动机器人的同时定位与环境建模领域中使用最为广泛的扩展卡尔曼滤波算法为核心，对该算法进行改进，使得算法的性能得到极大提高，能够更好的满足在SLAM中的应用。也为移动机器人在未知环境中的自主导航、完成复杂智能任务提供强大的技术支撑。

本发明的其它优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其它优点可以通过下面的说明书，权利要求书，以及附图中所特别指出的结构来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：

图1为MIEKF算法原理图；

图2为EKF/IEKF/MIEKF在X轴上的估计误差效果；

图3为EKF/IEKF/MIEKF在Y轴上的估计误差效果；

图4为EKF/IEKF/MIEKF在方向角上的估计误差效果；

图5为EKF循环更新图；

图6为SLAM示意图；

图7为机器人运动模型；

图8为MIEKF算法流程图。

具体实施方式

以下将结合附图，对本发明的优选实施例进行详细的描述；应当理解，优选实施例仅为了说明本发明，而不是为了限制本发明的保护范围。

实施例1

图1为MIEKF算法原理图；图2为EKF/IEKF/MIEKF在X轴上的估计误差效果；图3为EKF/IEKF/MIEKF在Y轴上的估计误差效果；图4为EKF/IEKF/MIEKF在方向角上的估计误差效果；图5为EKF循环更新图；图6为SLAM示意图；图7为机器人运动模型，图8为MIEKF算法流程图，如图所示：本发明实施例提供的一种智能移动机器人的定位与环境建模方法，包括以下步骤：

S1：形成修正迭代扩展卡尔曼滤波算法，并确定迭代次数；

S2：建立移动机器人的运动模型x_k；

S3：建立移动机器人的观测模型z_k；

S4：初始化移动机器人的先验状态变量先验协方差矩阵

系统的过程噪声协方差Q_k-1及观测噪声协方差R_k-1；

S5：计算移动机器人运动模型的位置雅可比矩阵

S6：计算移动机器人控制输入雅可比矩阵

S7：计算观测模型的观测雅可比矩阵J_h；

S8：计算卡尔曼增益K_k；

S9：更新后验状态估计

和后验状态估计协方差矩阵P_k；

S10：重复循环步骤S5至S9。

所述修正迭代扩展卡尔曼滤波算法，具体如下：

时间更新（预测）：

计算先验状态变量：

计算先验状态估计协方差矩阵：

测量更新（校正）：

计算卡尔曼增益： $K_k=P_k^{-}{J}_h^T{(J_h{P}_h^{-}{J}_h^T+V_k{R}_k{V}_k^T)}^{-1},$

由观测变量z_k更新后验状态估计：

后验状态估计协方差矩阵更新：

其中，

表示x_k在k时刻的先验估计，是x_k在k时刻的后验估计；u_k-1是系统从k-1时刻的状态向量变化到k时刻的控制输入，z_k是k时刻的观测向量，f(·)是系统的状态转移函数，

表示k时刻的协方差估计，A_k和W_k分别是f(·)对x_k和对W的偏导雅可比矩阵，

和

分别是它们的转置矩阵，Q_k-1表示过程激励噪声协方差，K_k表示卡尔曼增益，J_h表示状态向量x_k对测量向量z_k的增益，V_K是f(·)对V的偏导雅可比矩阵，R_K表示k时刻的噪声协方差，()^-1表示矩阵的逆，z_k表示测量向量，H_K表示非线性函数h对x的偏导雅可比矩阵，I表示单位矩阵；

所述建立移动机器人的运动学模型具体如下：

$x_k=f(x_{k-1},u_k)=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\mathrm{\φ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{v}_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L\end{array}\right]+w_k,$

其中，v_k是机器人的移动速度，L是机器人前后轴之间的轴距，θ_k是机器人的前轮偏角，Δt为传感器的采样周期，w_k是系统噪声。

所述建立的移动机器人观测模型具体如下：

z_k＝h(x_k,v_k)，

$z_{\mathrm{ik}}=\left[\begin{array}{c}{r}_{\mathrm{ik}}\\ {\mathrm{\θ}}_{\mathrm{ik}}\end{array}\right]h_i\left(x_k\right)=\left[\begin{array}{c}\sqrt{{(x_i-x_k)}^2+{(y_i-y_k)}^2}\\ \arctan \frac{y_i-y_k}{x_i-x_k}-{\mathrm{\φ}}_k\end{array}\right],$

其中，z_k是机器人在k时刻的观测量，h(·)为测量函数，v_k为观测噪声，由于激光传感器的光束发散角小，所以大多数情况下都是利用激光传感器进行测量，以此得到某个环境特征到机器人的距离以和前进方向的夹角，假定第i个点特征的位置坐标为(x_i,y_i)。

所述机器人位置雅可比矩阵

计算如下：

${\▿f}_{x_k}=\frac{\∂f}{{\∂x}_k}=\left[\begin{array}{ccc}\frac{{\∂f}_x}{\∂x}& \frac{{\∂f}_x}{\∂y}& \frac{{\∂f}_x}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_y}{\∂x}& \frac{{\∂f}_y}{\∂y}& \frac{{\∂f}_y}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{\θ}}}{\∂x}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂y}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 1& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 0& 1\end{array}\right],$

所述机器人控制输入的雅可比矩阵

计算如下：

${\▿f}_u=\left[\begin{array}{cc}\frac{{\∂f}_{\mathrm{ux}}}{\∂v}& \frac{{\∂f}_{\mathrm{ux}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{uy}}}{\∂v}& \frac{{\∂f}_{\mathrm{uy}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{u\θ}}}{\∂v}& \frac{{\∂f}_{\mathrm{u\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L& v_k\mathrm{\Δ}t\tan {{\mathrm{\θ}}_k}^2/L\end{array}\right],$

所述观测模型实际观测数据通过移动机器人上的传感器获得，所述观测模型在第i个坐标下的雅可比矩阵J_h的计算具体如下：

$J_h=\left[\begin{array}{ccc}\frac{{\∂h}_1}{\∂x}& \frac{{\∂h}_1}{\∂y}& \frac{{\∂h}_1}{\∂\mathrm{\φ}}\\ \frac{{\∂h}_2}{\∂x}& \frac{{\∂h}_2}{\∂y}& \frac{{\∂h}_2}{\∂\mathrm{\φ}}\end{array}\right]=\left[\begin{array}{ccc}\frac{x_k-x_i}{r}& \frac{y_k-y_i}{r}& 0\\ \frac{y_k-y_i}{r^2}& \frac{x_k-x_i}{r^2}& 1\end{array}\right],$

其中， $r=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2},$

$V_k=\left[\begin{array}{cc}\frac{{\∂h}_1}{{\∂v}_r}& \frac{{\∂h}_1}{{\∂v}_{\mathrm{\θ}}}\\ \frac{{\∂h}_2}{{\∂v}_r}& \frac{{\∂h}_2}{{\∂v}_{\mathrm{\θ}}}\end{array}\right]=E,$

所述迭代处理次数根据优化效果和计算速度确定。

实施例2

本发明提供的智能移动机器人的定位与环境建模方法，是以在移动机器人的同时定位与环境建模领域中使用最为广泛的扩展卡尔曼滤波算法为核心，对该算法进行改进，使得算法的性能得到极大提高，能够更好的满足在SLAM中的应用。也为移动机器人在未知环境中的自主导航、完成复杂智能任务提供强大的技术支撑。

下面详细描述智能移动机器人的定位与环境建模方法的原理：

首先，扩展卡尔曼滤波算法就是将非线性函数进行Taylor级数展开，并且对二次及其以上阶次做忽略处理，以此得到近似的线性模型，从而跳过线性化的影响。得到近似的线性模型后，再利用标准的卡尔曼滤波进行最优估计。具体思想如下所示：

第一步，定义如下的非线性模型：

x_k＝f(x_k-1,u_k-1,w_k-1)

z_k＝h(x_k,v_k)    （1）

其中，x_k是系统的k时刻的状态向量，u_k是系统从k-1时刻的状态向量变化到k时刻的控制输入，z_k是k时刻的观测向量，f(·)是系统的状态转移函数，h(·)是系统的观测函数，w_k是过程噪声，而v_k是测量噪声。

实际上，w_k和v_k的值并不是在每一步都能知道的。基于此，状态估计向量和观测向量可以做如下假设：

${\tilde{x}}_k=f({\tilde{x}}_{k-1},u_k,0)$

${\tilde{z}}_k=h({\tilde{x}}_k,0)---\left(2\right)$

已知k-1时刻的状态估计值

将方程进行关于

的Taylor展开，忽略二项式及其以上，可以得到：

$x_k=f({\tilde{x}}_{k-1},u_k,0)+A_k(x_{k-1}-{\tilde{x}}_{k-1})+W_k{w}_{k-1}---\left(3\right)$

同理，对观测向量也进行Taylor展开，也忽略二次项及其以上阶次，可以得到如下的线性化估计：

$z_k=h({\tilde{x}}_k,0)+J_h(x_k-{\tilde{x}}_k)+V_k{v}_k---\left(4\right)$

其中，x_k和z_k是实际的状态向量和观测向量；和

是根据方程得到的状态向量和观测向量的估计值；是状态向量在k时刻的后验估计；A_k和W_k分别是f(·)对x_k和对w的偏导矩阵，J_k和W_k分别是h(·)对x_k和v的偏导矩阵。

基于以上论述，将线性化的系统方程和观测方程与卡尔曼滤波算法进行结合，可以得到扩展卡尔曼滤波的方程如下所示：

（1）状态预测

计算先验状态估计 ${\hat{x}}_k^{-}=f({\hat{x}}_{k-1},u_{k-1},0)---\left(5\right)$

状态估计协方差矩阵 $P_k^{-}=A_k{P}_{k-1}{A}_k^T+W_k{Q}_{k-1}{W}_k^T---\left(6\right)$

（2）环境观测

观测新息 ${\tilde{y}}_k=z_k-H_k{\hat{x}}_k^{-}---\left(7\right)$

新息协方差矩阵 $S_k=J_h{P}_k^{-}{J}_h^T+V_k{R}_k{V}_k^T---\left(8\right)$

（3）更新

计算卡尔曼增益 $K_k=P_k^{-}{J}_h^T{S}_k^{-}---\left(9\right)$

状态更新 ${\hat{x}}_k={\hat{x}}_k^{-}+K_k{\tilde{y}}_k---\left(10\right)$

状态更新协方差矩阵 $P_k=(I-K_k{J}_h)P_k^{-}---\left(11\right)$

在有了上述的标准EKF算法后，对EKF做迭代处理能够有效的减小线性化误差可能带来的影响。因此，将迭代扩展卡尔曼滤波(Iterated Extended Kaman Filter，IEKF)应用在SLAM上面，对EKF中的更新方程进行迭代处理，能够减小误差。对于IEKF，得到状态预测

及协方差矩阵P_k|k-1后，按照下面的公式进行迭代，其中i为迭代次数，更新公式如下：

$K_{k,i}=P_{k|k-1}{\left(H_{k,i}\right)}^T{(H_{k,i}{P}_{k|k-1}{H_{k,i}}^T+R_k)}^{-1}---\left(12\right)$

$H_{k,i}=\frac{\∂h\left(x\right)}{\∂x}|x={\hat{x}}_{k|i+1}---\left(13\right)$

${\hat{x}}_{k|k,i+1}={\hat{x}}_{k|k-1}+K_{k,i}(z_k-h\left({\hat{x}}_{k|k,i}\right))-H_{k,i}({\hat{x}}_{k|k-1}-{\hat{x}}_{k|k,i})---\left(14\right)$

P_k|k，i+1＝(I-K_k，iH_k，i)P_k|k-1    （15）

理论上，IEKF的估计效果要优于EKF，但这有一个前提条件，就是状态估计得非常接近于真实值，这在很多时候并不是能够成立的。需要对IEKF的某些参量进行修正，期望得到更好的估计效果，经过修正之后可以得到对于MIEKF状态估计的一个更新方程式

x_i+1＝x_i+K_k，i(z-h(x_i))    （16）

卡尔曼滤波增益与协方差矩阵采用卡尔曼滤波算法进行更新，通过这种更新之后就可以得到一组新的状态估计及协方差方程，具体新方程如下所示：

$H_{k,i}=\frac{\∂h\left(x\right)}{\∂x}|x={\hat{x}}_{k|k,i}---\left(17\right)$

$K_{k,i}={({\left(H_{k,i}\right)}^T{R}_k^{-1}{H}_{k,i}+{\left(P_{k,i}\right)}^{-1})}^{-1}{\left(H_{k,i}\right)}^T{R}_k^{-1}---\left(18\right)$

${\hat{x}}_{k|k,i+1}={\hat{x}}_{k|k,i}+K_{k,i}(z_k-h\left({\hat{x}}_{k|k,i}\right))---\left(19\right)$

P_k|k，i+1＝(I-K_k,iH_k,i)P_k|k，i    （20）

将方程(17)~(18)与(5)~(11)中相关方程替换，就得到了改进的EKF算法。

然后，建立移动机器人的运动模型和观测模型，具体如下：

①移动机器人运动模型

移动机器人的运动模型是对机器人位姿进行预估的前提条件。机器人的位姿通常表述为一个三维向量X＝[x,y,θ]^T,因此用状态向量x的一个矢量函数就可以表示机器人的运动模型。机器人的运动模型描述了机器人在控制输入u_k和噪声干扰w_k作用下其状态x_k的变化，移动机器人的运动模型的离散时间差方程表示：x_k＝f(x_k-1,u_k)，其中，x_k＝f(x_k-1,u_k)是系统的状态转换函数，是非线性的。x_k是k时刻机器人的状态，u_k是使机器人从x_k-1时刻位置移动到x_k时刻位置的控制输入。在控制输入u_k＝(v_k,θ_k)的作用下，机器人的Bicycle运动学模型[40]就可以表示如下：

$x_k=f(x_{k-1},u_k)=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\mathrm{\φ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{v}_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L\end{array}\right]+w_k---\left(21\right)$

其中，v_k是机器人的移动速度，L是机器人前后轴之间的轴距，θ_k是机器人的前轮偏角，Δt为传感器的采样周期。

②移动机器人观测模型

观测模型是描述传感器所观测到的环境特征与机器人当前时刻之间的相互位置关系，对于观测量采用不同的表达形式，就可以得到不同的观测方程：

z_k＝h(x_k,v_k)，其中，z_k是机器人在k时刻的观测量，h(·)为测量函数，v_k为观测噪声。由于激光传感器的光束发散角小，所以大多数情况下都是利用激光传感器进行测量，以此得到某个环境特征到机器人的距离以和前进方向的夹角，假定第i个点特征的位置坐标为(x_i,y_i)，则此时机器人的观测模型可以做如下表示：

$z_{\mathrm{ik}}=\left[\begin{array}{c}{r}_{\mathrm{ik}}\\ {\mathrm{\θ}}_{\mathrm{ik}}\end{array}\right]h_i\left(x_k\right)=\left[\begin{array}{c}\sqrt{{(x_i-x_k)}^2+{(y_i-y_k)}^2}\\ \arctan \frac{y_i-y_k}{x_i-x_k}-{\mathrm{\φ}}_k\end{array}\right]---\left(22\right)$

③基于EKF的SLAM实现步骤：

（1）系统初始化

在这一步中，主要是对移动机器人的状态向量

和协方差矩阵P₀系统的过程噪声Q₀以及观测噪声R₀以及移动机器人本身和实验环境相关参数做一个初始化定义。

（2）状态预测

主要是针对移动机器人运动模型的离散化方程的位姿和控制输入的雅可比矩阵进行计算，获得相应的雅可比矩阵，便于以后的计算。

其中，对于机器人位姿的雅可比矩阵计算如下：

${\▿f}_{x_k}=\frac{\∂f}{{\∂x}_k}=\left[\begin{array}{ccc}\frac{{\∂f}_x}{\∂x}& \frac{{\∂f}_x}{\∂y}& \frac{{\∂f}_x}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_y}{\∂x}& \frac{{\∂f}_y}{\∂y}& \frac{{\∂f}_y}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{\θ}}}{\∂x}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂y}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 1& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 0& 1\end{array}\right]---\left(23\right)$

对于控制输入的雅可比矩阵

计算如下：

${\▿f}_u=\left[\begin{array}{cc}\frac{{\∂f}_{\mathrm{ux}}}{\∂v}& \frac{{\∂f}_{\mathrm{ux}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{uy}}}{\∂v}& \frac{{\∂f}_{\mathrm{uy}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{u\θ}}}{\∂v}& \frac{{\∂f}_{\mathrm{u\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L& v_k\mathrm{\Δ}t\tan {{\mathrm{\θ}}_k}^2/L\end{array}\right]---\left(24\right)$

（3）观测及获取协方差矩阵

这一部分的内容就是运用传感器以及观测模型获得实际观测数据和估计数据。其中，根据观测模型可以求得在第i个坐标下的雅可比矩阵J_h的计算如下：

$J_h=\left[\begin{array}{ccc}\frac{{\∂h}_1}{\∂x}& \frac{{\∂h}_1}{\∂y}& \frac{{\∂h}_1}{\∂\mathrm{\φ}}\\ \frac{{\∂h}_2}{\∂x}& \frac{{\∂h}_2}{\∂y}& \frac{{\∂h}_2}{\∂\mathrm{\φ}}\end{array}\right]=\left[\begin{array}{ccc}\frac{x_k-x_i}{r}& \frac{y_k-y_i}{r}& 0\\ \frac{y_k-y_i}{r^2}& \frac{x_k-x_i}{r^2}& 1\end{array}\right]---\left(25\right)$

其中， $r=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2}---\left(26\right)$

另外， $V_k=\left[\begin{array}{cc}\frac{{\∂h}_1}{{\∂v}_r}& \frac{{\∂h}_1}{{\∂v}_{\mathrm{\θ}}}\\ \frac{{\∂h}_2}{{\∂v}_r}& \frac{{\∂h}_2}{{\∂v}_{\mathrm{\θ}}}\end{array}\right]=E---\left(27\right)$

同时，利用求得的雅可比矩阵带入环境观测方程里面就可以求得对应的值。

（4）状态更新

利用前面几步求得的数据，就可以对算法方程进行更新

卡尔曼增益的更新

$K_k=P_k^{-}{J}_h^T{(J_h{P}_k^{-}{J}_h^T+V_k{R}_k{V}_k^T)}^{-}---\left(28\right)$

状态估计的更新

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k)---\left(29\right)$

协方差矩阵更新

$P_k=(I-P_k^{-}{H}_k^T{(H_k{P}_k^{-}{H}_k^T+V_k{R}_k{V}_k^T)}^{-}{H}_k)P_k^{-}---\left(30\right)$

以上所述仅为本发明的优选实施例，并不用于限制本发明，显然，本领域的技术人员可对本发明进行各种改动和变型而不脱离本发明的精神和范围。倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (8)

1.一种智能移动机器人的定位与环境建模方法，其特征在于：它包括以下步骤：

S1：形成修正迭代扩展卡尔曼滤波算法，并确定迭代次数；

S2：建立移动机器人的运动模型x_k；

S3：建立移动机器人的观测模型z_k；

S4：初始化移动机器人的先验状态变量

先验协方差矩阵

系统的过程噪声协方差Q_k-1及观测噪声协方差R_k-1；

S5：计算移动机器人运动模型的位置雅可比矩阵

S6：计算移动机器人控制输入雅可比矩阵

S7：计算观测模型的观测雅可比矩阵J_h；

S8：计算卡尔曼增益K_k；

S9：更新后验状态估计

和后验状态估计协方差矩阵P_k；

S10：重复循环步骤S5至S9。

2.根据权利要求1所述的智能移动机器人的定位与环境建模方法，其特征在于：形成修正迭代扩展卡尔曼滤波算法，具体如下：

时间更新-预测：

计算先验状态变量：

计算先验状态估计协方差矩阵：

测量更新-校正：

计算卡尔曼增益： $K_k=P_k^{-}{J}_h^T{(J_h{P}_h^{-}{J}_h^T+V_k{R}_k{V}_k^T)}^{-1},$

由观测变量z_k更新后验状态估计：

后验状态估计协方差矩阵更新：

其中，

表示x_k在k时刻的先验估计，

是x_k在k时刻的后验估计；u_k-1是系统从k-1时刻的状态向量变化到k时刻的控制输入，z_k是k时刻的观测向量，f(·)是系统的状态转移函数，

表示k时刻的协方差估计，A_k和W_k分别是f(·)对x_k和对W的偏导雅可比矩阵，

和

分别是它们的转置矩阵，Q_k-1表示过程激励噪声协方差，K_k表示卡尔曼增益，J_h表示状态向量x_k对测量向量z_k的增益，V_K是f(·)对V的偏导雅可比矩阵，R_K表示k时刻的噪声协方差，()^-1表示矩阵的逆，z_k表示测量向量，H_K表示非线性函数h对x的偏导雅可比矩阵，I表示单位矩阵。

3.根据权利要求2所述的智能移动机器人的定位与环境建模方法，其特征在于：建立移动机器人的运动学模型具体如下：

$x_k=f(x_{k-1},u_k)=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\mathrm{\φ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{v}_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ v_k\mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L\end{array}\right]+w_k,$

其中，v_k是机器人的移动速度，L是机器人前后轴之间的轴距，θ_k是机器人的前轮偏角，Δt为传感器的采样周期，w_k是系统噪声。

4.根据权利要求3所述的智能移动机器人的定位与环境建模方法，其特征在于：建立移动机器人观测模型具体如下：

z_k＝h(x_k,v_k)

$z_{\mathrm{ik}}=\left[\begin{array}{c}{r}_{\mathrm{ik}}\\ {\mathrm{\θ}}_{\mathrm{ik}}\end{array}\right]h_i\left(x_k\right)=\left[\begin{array}{c}\sqrt{{(x_i-x_k)}^2+{(y_i-y_k)}^2}\\ \arctan \frac{y_i-y_k}{x_i-x_k}-{\mathrm{\φ}}_k\end{array}\right],$

其中，z_k是机器人在k时刻的观测量，h(·)为测量函数，v_k为观测噪声，由于激光传感器的光束发散角小，所以大多数情况下都是利用激光传感器进行测量，以此得到某个环境特征到机器人的距离以和前进方向的夹角，第i个点特征的位置坐标为(x_i,y_i)。

5.根据权利要求4所述的智能移动机器人的定位与环境建模方法，其特征在于：计算移动机器人的位置雅可比矩阵如下：

${\▿f}_{x_k}=\frac{\∂f}{{\∂x}_k}=\left[\begin{array}{ccc}\frac{{\∂f}_x}{\∂x}& \frac{{\∂f}_x}{\∂y}& \frac{{\∂f}_x}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_y}{\∂x}& \frac{{\∂f}_y}{\∂y}& \frac{{\∂f}_y}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{\θ}}}{\∂x}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂y}& \frac{{\∂f}_{\mathrm{\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 1& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ 0& 0& 1\end{array}\right].$

6.根据权利要求5所述的智能移动机器人的定位与环境建模方法，其特征在于：计算移动机器人的控制输入的雅可比矩阵如下：

${\▿f}_u=\left[\begin{array}{cc}\frac{{\∂f}_{\mathrm{ux}}}{\∂v}& \frac{{\∂f}_{\mathrm{ux}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{uy}}}{\∂v}& \frac{{\∂f}_{\mathrm{uy}}}{\∂\mathrm{\θ}}\\ \frac{{\∂f}_{\mathrm{u\θ}}}{\∂v}& \frac{{\∂f}_{\mathrm{u\θ}}}{\∂\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& -v_k\mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\sin ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)& v_k\mathrm{\Δ}t\cos ({\mathrm{\φ}}_{k-1}+{\mathrm{\θ}}_k)\\ \mathrm{\Δ}t\tan {\mathrm{\θ}}_k/L& v_k\mathrm{\Δ}t\tan {{\mathrm{\θ}}_k}^2/L\end{array}\right].$

7.根据权利要求6所述的智能移动机器人的定位与环境建模方法，其特征在于：观测模型实际观测数据通过移动机器人上的传感器获得，所述观测模型在第i个坐标下的雅可比矩阵J_h的计算具体如下：

$J_h=\left[\begin{array}{ccc}\frac{{\∂h}_1}{\∂x}& \frac{{\∂h}_1}{\∂y}& \frac{{\∂h}_1}{\∂\mathrm{\φ}}\\ \frac{{\∂h}_2}{\∂x}& \frac{{\∂h}_2}{\∂y}& \frac{{\∂h}_2}{\∂\mathrm{\φ}}\end{array}\right]=\left[\begin{array}{ccc}\frac{x_k-x_i}{r}& \frac{y_k-y_i}{r}& 0\\ \frac{y_k-y_i}{r^2}& \frac{x_k-x_i}{r^2}& 1\end{array}\right],$

其中， $r=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2},$

$V_k=\left[\begin{array}{cc}\frac{{\∂h}_1}{{\∂v}_r}& \frac{{\∂h}_1}{{\∂v}_{\mathrm{\θ}}}\\ \frac{{\∂h}_2}{{\∂v}_r}& \frac{{\∂h}_2}{{\∂v}_{\mathrm{\θ}}}\end{array}\right]=E.$

8.根据权利要求1所述的智能移动机器人的定位与环境建模方法，其特征在于：所述迭代处理次数根据优化效果和计算速度确定。

Images