CN102402225A - 一种实现移动机器人同时定位与地图构建的方法 - Google Patents

Abstract

本发明公开了一种实现移动机器人同时定位与地图构建的方法。该方法针对移动机器人在未知环境中利用航位推测传感器数据以及路标观测数据，借助于改进的强跟踪滤波技术实现移动机器人的自主定位，并同时构建出环境地图。本发明利用“强跟踪滤波器”，自适应调整卡尔曼增益；引入一种新的多重渐消因子，保证协方差矩阵的对称性，降低求解卡尔曼增益的计算复杂度；针对同时地位与地图构建问题中的观测不连续问题，提出了一种新的算法流程，并给出了一种新的多重渐消因子的计算方法。与传统方法相比，本发明降低了线性化过程引入的误差，提高了机器人的定位精度以及构建地图的精度，同时也能将协方差抑制在一个较小的范围内，提高了所建地图的可信度。

Description

一种实现移动机器人同时定位与地图构建的方法

技术领域

本发明涉及移动机器人自主定位与环境感知技术领域，尤其涉及一种基于强跟踪滤波实现移动机器人同时定位与地图构建的方法，可用于根据航位推测传感器数据以及路标观测数据估计未知环境中移动机器人的位姿并构建环境地图。

背景技术

随着机器人技术的发展，具有移动行走功能、环境感知能力以及自主规划能力的智能移动机器人得到各国研究人员的普遍重视。实现自主定位与导航是智能机器人最基本的功能之一，也是其完成各种任务的重要前提。而移动机器人实现自主导航的关键是使机器人拥有完整的地图信息和具有良好的自主定位能力。

机器人同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)为实现自主定位与导航提供了一个新的解决思路：机器人利用自身配备的传感器得到环境的测量数据，在构建所在环境地图的同时，利用环境地图来计算自身在环境中的位姿。由于SLAM理论具有重要的应用价值，被很多学者认为是实现真正意义上全自主移动机器人的关键(参见“Dissanayake，M.W.M.G.，et al.，A solution to the simultaneous localizationand map building(SLAM)problem.IEEE Transactions on Robotics andAutomation，2001.17(3)：pp.229-241.”)。

SLAM常用的算法大致有以下几种：扩展卡尔曼滤波(ExtendedKalman Filter)、Rao-Blackwellised粒子滤波(Rao-Blackwellised ParticleFilter，也被称为FastSLAM)、稀疏扩展信息滤波(Sparse ExtendedInformation Filter，SEIF)以及基于最大似然估计的SLAM方法。

EKF-SLAM对运动模型和观测模型进行线性化近似处理，并假设其概率密度概述为高斯分布。采用增广的系统状态空间，运用卡尔曼滤波器对系统状态进行递归地估计，对增广的系统状态和增广的系统协方差矩阵进行递归地更新替换，从而解决非线性系统模型的估计问题。EKF是解决SLAM问题使用的最多的方法，也是现有多种SLAM算法的基础。但是，当估计值与真实值偏离较大时，EKF对非线性模型进行一阶线性近似会引入较大的线性化误差。

RBPF-SLAM采用粒子集来表示机器人的位姿，而环境特征的估计依然利用EKF解析计算。使用机器人的路径来估计SLAM的后验概率，根据条件无关特性将SLAM后验分解为一系列的低维的估计问题，使得RBPF-SLAM的计算复杂度为O(MN)，其中M，N分别为粒子数目和特征的个数。对于给定的粒子数目，RBPF-SLAM的计算复杂度与特征个数呈线性关系，因此RBPF-SLAM也被称为FastSLAM。(参见“Montemerlo，M.，etal.，FastSLAM 2.0：An Improved Particle Filtering Algorithm for SimultaneousLocalization and Mapping that Provably Converges.in Proceedings of theInternational Conference on Artificial Intelligence，2003，pp.1151-1156.”)。该算法在处理大环境地图建立问题比较有效，不过对于一些特征比较少的不是很大的环境来说，速度上的优势并不是很明显。

信息滤波IF是卡尔曼滤波的另一种表示形式，信息滤波器中使用信息矩阵和信息向量代替KF中的协方差矩阵和状态均值向量。稀疏扩展信息滤波(Sparse Extended Information Filter，SEIF)是信息滤波的一种扩展近似。SEIF的基本观点在于，即使协方差矩阵为稠密阵，但其逆矩阵可能是稀疏矩阵，或者逆矩阵的很多元素值非常小。于是在SEIF的每一步，有选择地将逆矩阵较小的元素置为零，从而达到恒定时间的近似滤波器更新(参见“Thrun，S.，et al.，Simultaneous Localization and Mapping with SparseExtended Information Filters.The International Journal of Robotics Research，2004.23(7-8)：pp.693-716.”)。该处理方法使得观测更新执行效率更高，同时精度损失很小。但其在实际应用中也存在着缺点：每次更新时都需要将信息向量和信息矩阵转换为状态向量的估计均值和方差，用于运动模型和观测模型的线性化。该转换过程计算复杂度与路标个数成立方关系、计算开销庞大。

基于最大似然估计(Maximum Likelihood Estimation)的SLAM方法首先利用运动模型估计在假定当前地图确定情况下，给定控制作用下机器人可能移动的新位置；然后计算机器人在此位置形成的环境地图中每个特征同以往地图相比出现的频率(作为最大相似性估计器)。经过计算，较准确存在的环境特征在地图中的概率值增加了，而较不准确存在的环境特征在地图中的概率值降低了。该方法充分利用过去若干时间步的数据，因而对观测值具有鲁棒性；观测的环境特征不用非常清晰，甚至可以是错误的。与EKF相比，该方法绕过了对观测值与地图中元素数据相关的准确性的依赖，提高了算法的鲁棒性。但是该方法处理的数据量过大，对运算速度和存储空间要求都非常高，制约了其应用于大规模环境。

对于实时SLAM算法来说，计算复杂度是算法使用的主要限制因素。

通常情况下，EKF-SLAM的算法复杂度是O(N²)，FastSLAM的算法复杂度是O(MN)，其中N是地图中路标的个数，M是使用的粒子个数。随着粒子个数的增加，FastSLAM的精度会逐渐接近EKF(参见“Thrun，S.，W.Burgard，and D.Fox，Probabilistic Robotics.Cambridge，Massachusetts：TheMIT Press，2005，pp.470-471”)。一般情况下，M的经验取值会在50～500左右，在特征个数较多的情况下，FastSLAM的计算效率方面的优势才能体现出来。稀疏扩展信息滤波(Sparse Extended Information Filter)在观测更新执行效率更高，同时精度损失很小。但是每次更新时都需要将信息向量和信息矩阵转换为状态向量的估计均值和方差，该转换过程计算复杂度与路标个数成立方关系、计算开销庞大。

一般基于拓扑结构的室内环境地图的节点数不会很多(100以内)，而EKF-SLAM采用状态增广(state augmentation)、分块更新等优化技巧后计算开销会明显降低，能够很好地满足室内环境SLAM的实时性要求。但是，当估计值与真实值偏离较大时，EKF对非线性模型进行一阶线性近似会引入较大的线性化误差。

周东华提出了“强跟踪滤波器”(Strong Tracking Filter)的概念(参见“周东华，席裕庚，一种带多重次优渐消因子的扩展卡尔曼滤波器.自动化学报，1991.17(6)：pp.689-695.”)。STF通过强迫所有的新息序列保持正交，自适应地改变Kalman增益，降低线性化过程引入的误差。

将STF应用到SLAM中，已经有若干相关的研究成果。Huiping Li提出了STF SLAM，采用多重次优渐消因子来在线调整Kalman增益(参见“Huiping，L.，et al.Strong Tracking Filter Simultaneous Localization andMapping Algorithm.in Proceedings of International Conference on ComputerScience and Software Engineering，2008，pp.1085-1088”)。但是该算法不适用于路标观测不连续的情况，而这在SLAM实际应用中会经常出现。在计算多重渐消因子以及路标初始化的过程值得推敲，也没有对于多重次优渐消因子对于协方差矩阵的对称性影响进行讨论，仅仅做了仿真实验，没有实际实验结果。Zeng Jing对于带单重次优渐消因子的STF在SLAM中进行了讨论(参见“Zeng，J.，et al.Study on Application of STF in Mobile RobotSLAM.in Proceedings of International Conference on InformationEngineering and Computer Science.2009，pp.1-4”)，但并没有对SLAM问题中路标观测不连续情况以及STF对于状态估计方差的影响进行探讨。

国内方面也有一些关于移动机器人同时定位与地图构建的相关专利，其中申请号为200610053690.2的中国发明专利提出了一种基于测距传感器的同时定位与地图构建方法，构建得到线段特征地图和占用栅格地图。该方法基于扩展卡尔曼滤波，对于线性化误差不够鲁棒。基于线段特征的匹配计算量较大，在对称环境容易造成错误定位，不能自动生成便于导航的拓扑地图。

发明内容

(一)要解决的技术问题

有鉴于此，本发明的主要目的在于提供一种基于强跟踪滤波实现移动机器人同时定位与地图构建的方法，以解决传统方法线性化误差较大、普通强跟踪滤波不能适用于观测不连续情况以及可信度较低等问题，达到提高机器人的定位精度以及构建地图的精度，并同时提高所建地图可信度的目的。

(二)技术方案

为了达成上述目的，本发明提供了一种实现移动机器人同时定位与地图构建的方法，包括：

步骤S1：确定全局坐标系以及该坐标系下的移动机器人的初始位姿值；

步骤S2：记录k-1时刻的状态估计量以及对应的协方差矩阵，其中状态估计量包括移动机器人的位姿以及路标位置；

步骤S3：判断在k时刻是否能够观测到路标，如果在k时刻不能观测到路标，则进行时间更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果在k时刻能够观测到路标，则进行数据关联，然后执行步骤S4；

步骤S4：判断观测到的路标是否为新的特征，如果观测到的路标是新的特征，则进行路标初始化，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果观测到的路标是已有的特征，则进行STF预测，然后执行步骤S5；

步骤S5：对STF预测的结果进行观测更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；

步骤S6：重复步骤S2～步骤S5，得到最后时刻的状态估计量，即移动机器人的位姿以及环境中路标的位置，实现同时定位与地图构建。

(三)有益效果

从上述技术方案可以看出，本发明具有以下有益效果：

1、本发明提供的这种基于强跟踪滤波实现移动机器人同时定位与地图构建的方法，是在STF-SLAM基础之上，充分考虑计算复杂度、实际应用中路标观测不连续的情况以及所构建地图的可信度，而提出的一种计算简单、精度和可信度高的实用的同时定位与地图构建算法。

2、本发明提供的基于强跟踪滤波实现移动机器人同时定位与地图构建的方法，针对SLAM问题中路标观测不连续的情况提出了一种新的算法流程，引入了新的多重次优渐消因子T(k)，实现对状态中不同变量以不同的速率进行渐消，保证协方差矩阵的对称性，降低求解卡尔曼增益时矩阵求逆过程的计算复杂度，抑制线性化过程引入的误差，提高了移动机器人的定位以及构建地图的精度，同时也能将协方差抑制在一个较小的范围内，提高了所建地图的可信度。

3、本发明提供的基于强跟踪滤波实现移动机器人同时定位与地图构建的方法，简单鲁棒，对于安装有路标识别装置的移动机器人，可以实现在线未知环境地图构建，所构建的全局拓扑地图可以方便地用于移动机器人路径规划和导航。

附图说明

图1为依照本发明实施例的基于强跟踪滤波实现移动机器人同时定位与地图构建的方法流程图；

图2为利用本发明实际绘制全局特征地图的一个实例；其中：

图2(a)为本实施例中使用的移动机器人平台；

图2(b)为本实施例中使用的移动机器人运动模型；

图2(c)为本实施例中使用的观测模型；

图2(d)为本实施例中实验场景示意图；

图2(e)为本实施例中实际场景图；

图2(f)为本实施例中机器人轨迹对比图；

图2(g)为本实施例中各估计值的标准误差对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。在该实施例中，机器人能够独立移动，所配备的路标识别装置为视觉传感器，所配备的航位推测传感器为里程计。

在本发明中，移动机器人从未知环境中的任意一个位置朝着任意一个方向开始行进，在初始时刻确定全局坐标系以及该坐标系下的移动机器人的初始位姿值，然后判断是否能够观测到路标(即MR码)，如果不能观测到路标，则根据里程计信息进行时间更新，进入下一个处理周期；如果能够观测到路标，判断是否是新的路标，如果是新的路标，则进行状态增广，将新观测到的路标添加到特征地图中，进入下一个处理周期；如果观测到的是已有的路标，首先进行STF预测，然后进行观测更新，进入下一个处理周期。当机器人不再移动时，退出循环。

图1为依照本发明实施例的基于强跟踪滤波实现移动机器人同时定位与地图构建的方法流程图，该方法包括以下步骤：

步骤S1：确定全局坐标系以及该坐标系下的移动机器人的初始位姿值；

步骤S2：记录k-1时刻的状态估计量以及对应的协方差矩阵，其中状态估计量包括移动机器人的状态以及路标的位置；

步骤S3：判断在k时刻是否能够观测到路标，如果在k时刻不能观测到路标，则进行时间更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果在k时刻能够观测到路标，则进行数据关联；

步骤S4：判断观测到的路标是否为新的特征，如果观测到的路标是新的特征，则进行路标初始化，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果观测到的路标是已有的特征，则进行STF预测；

步骤S5：对STF预测的结果进行观测更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；

步骤S6：重复步骤S2～步骤S5，得到最后时刻的状态估计量，即移动机器人的位姿以及环境中路标的位置，实现同时定位与地图构建。

在本实施例中，移动机器人平台采用中国科学院自动化研究所自主研制开发的一款通用性较强的自主移动机器人平台AIM(AdvancedIntelligent Mobile Robot)，如图2所示。该平台采用高性能的低功耗CPU单板计算机作为主控上位机，该单板机可以支持主频达1.6G HZ的CPU，系统上位机采用Windows操作系统，双轮差动驱动系统采用自行开发的伺服控制技术。摄像机采用SONY公司生产的EVI-D70摄像机，自带云台，能够进行宽范围、高速度地平移和倾斜；具有18倍光学变焦镜头，能够以极高的清晰度对微小或远距离物体加以放大；具有背景光补偿功能，可以有效地克服在光源附近路标偏暗的问题。

机器人位姿X_r＝[x_r，y_r，θ_r]^T、路标位置m_i＝(x_i，y_i)均采用笛卡尔坐标表示。在本实施例中因为视觉传感器位于机器人的中心位置，所以只采用了全局坐标系O_WX_WY_W和机器人坐标系O_RX_RY_R。全局坐标系采用以第一周期移动机器人位置为原点、以第一周期移动机器人正方向为X轴的坐标系。

机器人运动模型如图3所示。确定世界坐标系O_WX_WY_W后，机器人的位姿用一个三维状态向量X_r＝[x_r，y_r，θ_r]^T表示，其中θ表示机器人朝向偏离世界坐标系X_W轴的夹角，角度范围为(-π，π]。

当机器人从位姿X_r(k-1)＝[x_r(k-1)，y_r(k-1)，θ_r(k-1)]^T移动到X_r(k)＝[x_r(k)，y_r(k)，θ_r(k)]^T时，机器人的移动距离记为Δs，转过的角度记为Δθ。

Δx＝Δs cos(θ+Δθ/2)    公式1

Δy＝Δs sin(θ+Δθ/2)    公式2

$\mathrm{\Δ\θ}=\frac{{\mathrm{\Δs}}_r-{\mathrm{\Δs}}_l}{b}$ 公式3

$\mathrm{\Δs}=\frac{{\mathrm{\Δs}}_r+{\mathrm{\Δs}}_l}{2}$ 公式4

(Δx；Δy；Δθ)＝前一次采样周期走过的距离；

Δs_r；Δs_l＝左右两轮行走的距离；

b＝机器人两轮之间的距离。

以u(k)＝[Δs(k)，Δθ(k)]^T作为航位推测模型的输入，则移动机器人的航位推测模型为：

X_r(k)＝f(X_r(k-1)，u(k))+w(k)    公式5

其中w(k)为零均值高斯白噪声，其协方差矩阵为Q(k)。

通过以上分析得到：

$X_r\left(k\right)=X_r(k-1)+\left[\begin{array}{c}\mathrm{\Δ}s\cos (\mathrm{\θ}+\mathrm{\Δ\θ}/2)\\ \mathrm{\Δ}s\sin (\mathrm{\θ}+\mathrm{\Δ\θ}/2)\\ \mathrm{\Δ\θ}\end{array}\right]+w\left(k\right)$ 公式6

在本实施例中，机器人采用视觉传感器获取环境中路标的观测值，该视觉传感器获得的数据是路标相对于移动机器人的位姿信息，该路标含有方向信息。机器人的观测模型如图4所示，视觉传感器置于机器人的中心位置，对于第i个路标的观测值为：

z_i(k)＝[x_i|s(k)，y_i|s(k)，θ_i|s(k)]^T    公式7

其中θ_i|z为路标的正向与机器人正向的夹角值。

涉及到的状态量分成两部分：机器人的位姿和路标的位姿。令： $X_i\left(k\right)=\left[\begin{array}{c}{X}_r\left(k\right)\\ m_i\left(k\right)\end{array}\right],$ 则观测模型可以表述为：

z_i(k)＝h(X_i(k))+v(k)    公式8

在本实施例中，观测模型可以具体表述为：

$z_i\left(k\right)=\left[\begin{array}{c}{x}_{i|s}\left(k\right)\\ y_{i|s}\left(k\right)\\ {\mathrm{\θ}}_{i|s}\left(k\right)\end{array}\right]=h\left(X_i\left(k\right)\right)+v\left(k\right)$ 公式9

$=\left[\begin{array}{ccc}\cos \left({\mathrm{\θ}}_r\left(k\right)\right)& \sin \left({\mathrm{\θ}}_r\left(k\right)\right)& 0\\ -\sin \left({\mathrm{\θ}}_r\left(k\right)\right)& \cos \left({\mathrm{\θ}}_r\left(k\right)\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i-x_r\left(k\right)\\ y_i-y_r\left(k\right)\\ {\mathrm{\θ}}_i-{\mathrm{\θ}}_r\left(k\right)\end{array}\right]+v\left(k\right)$

其中的观测噪声v(k)假定为高斯噪声，其均值为零，而协方差矩阵R(k)为对角阵。

在步骤S3中，采用扩展卡尔曼滤波法，根据上一周期移动机器人的位姿估计和航位推测传感器数据，利用移动机器人运动模型，预估当前周期移动机器人的位姿，即时间更新，其计算公式如下：

$X\left(k\right)=\left[\begin{array}{c}{X}_r\left(k\right|k)\\ m\left(k\right)\end{array}\right]=g(X(k-1),u\left(k\right))+\mathrm{\ϵ}\left(k\right)$

公式10

$=\left[\begin{array}{c}f(X_r(k-1|k-1),u\left(k\right))\\ m(k-1)\end{array}\right]+\mathrm{\ϵ}\left(k\right)$

其中m(k)分别路标k时刻的位姿，ε(k)为零均值高斯白噪声，其协方差矩阵为

J_r将3维的状态向量映射为3N+3维的向量。

公式11

将运动方程g用一阶泰勒展开式近似：

$g(X(k-1),u\left(k\right))\≈g(\hat{X}(k-1),u\left(k\right))+G\left(k\right)\·(X(k-1)-\hat{X}(k-1))$ 公式12

其中：

$G\left(k\right)=\frac{\∂g(X\left(k\right),u\left(k\right))}{\∂X\left(k\right)}{|}_{\hat{X}\left(k\right|k-1),u\left(k\right)}$ 公式13

则k时刻的状态向量估计值为：

$\hat{X}\left(k\right)=g(\hat{X}(k-1),u\left(k\right))$

$=\left[\begin{array}{c}{\hat{X}}_r\left(k\right|k)\\ \hat{m}\left(k\right)\end{array}\right]=\left[\begin{array}{c}f({\hat{X}}_r(k-1|k-1),u\left(k\right))\\ \hat{m}(k-1)\end{array}\right]$ 公式14

k时刻的协方差矩阵P(k|k)为：

$P\left(k\right|k)=G\left(k\right)P(k-1|k-1)G^T\left(k\right)+J_r^{T}Q\left(k\right)J_r$ 公式15

在本实施例中，上述公式可以具体表述为：

$X\left(k\right)=g(X(k-1),u\left(k\right))+\mathrm{\ϵ}\left(k\right)$

$=X(k-1)+J_r^T\left[\begin{array}{c}\mathrm{\Δ}s\cos (\mathrm{\θ}+\mathrm{\Δ\θ}/2)\\ \mathrm{\Δ}s\sin (\mathrm{\θ}+\mathrm{\Δ\θ}/2)\\ \mathrm{\Δ\θ}\end{array}\right]+N(0,{J_r}^{T}Q\left(k\right)J_r)$ 公式16

则k时刻的状态向量估计值为：

$\hat{X}\left(k\right|k)=\hat{X}(k-1|k-1)+J_r^T\left[\begin{array}{c}\mathrm{\Δ}s\left(k\right)\cos ({\widehat{\mathrm{\θ}}}_r(k-1|k-1)+\mathrm{\Δ\θ}\left(k\right)/2)\\ \mathrm{\Δ}s\left(k\right)\sin ({\widehat{\mathrm{\θ}}}_r(k-1|k-1)+\mathrm{\Δ\θ}\left(k\right)/2)\\ \mathrm{\Δ\θ}\left(k\right)\end{array}\right]$ 公式17

$G\left(k\right)=I+J_r^T\·g_t\·J_r$ 公式18

$g_t=\left[\begin{array}{ccc}0& 0& -\mathrm{\Δs}\left(k\right)\sin ({\widehat{\mathrm{\θ}}}_r(k-1|k-1)+\mathrm{\Δ\θ}\left(k\right)/2)\\ 0& 0& \mathrm{\Δs}\left(k\right)\cos ({\widehat{\mathrm{\θ}}}_r(k-1|k-1)+\mathrm{\Δ\θ}\left(k\right)/2)\\ 0& 0& 0\end{array}\right]$ 公式19

在步骤S3中，当k时刻观测到路标时，为了保证算法的收敛性，需要对观测值进行预处理，解决观测值与估计值之间的数据关联问题。该数据关联采用延迟更新的方法提高路标识别的准确率，具体步骤如下：

1)有新的观测值时，不立刻进行观测更新，记录下当前移动机器人的位姿信息以及对应的观测值；

2)时间约束：在一定观测周期内，观测次数超过一定阈值的特征为稳定特征，去除其他非稳定观测值以及对应的移动机器人的位姿信息；

3)距离约束：对于满足时间约束的特征，利用观测模型计算出特征在全局坐标系下的估计值序列，去除相对距离较远的外点；

4)对于满足时间约束与距离约束的观测值进行批更新。

在上述数据关联中，当观测到第i个路标时，采用公式8中的观测模型，为了算法描述的简洁性，这里引入匹配变量

来表示观测到的特征

与地图中的路标mj之间的关系。其中

N表示地图中路标的个数。

如果

则在k时刻第i次观测到的特征对应于地图中的第j个路标。

如果

则表示当前的观测到的特征在地图中没有与之匹配的路标。此时有两种情况：其一，该次观测结果是一个“伪”路标；其二，该次观测结果是一个之前没有发现的新的路标。

假设在k时刻观测到的第i个特征对应于地图中的第j个路标，该观测模型可以近似为一阶线形模型：

$X_j^i\left(k\right)=\left[\begin{array}{c}{X}_r\left(k\right)\\ m_j\left(k\right)\end{array}\right]{=J}_{r,j}X\left(k\right)$ 公式20

J_r，j是一个6×(3N+3)的矩阵，如下所示：

公式21

$h\left(X_j^i\left(k\right|k-1)\right)\≈h\left({\hat{X}}_j^i\left(k\right|k-1)\right)+H(X_j^i\left(k\right|k-1)-{\hat{X}}_j^i\left(k\right|k-1))$ 公式22

H是

对于状态向量X_r(k|k-1)和m_j(k)在

处的雅各比矩阵，维数为3×6。

$H=\left[\begin{array}{cc}\frac{\∂h}{{\∂X}_r\left(k\right)}{|}_{X_r\left(k\right)={\hat{X}}_r\left(k\right|k-1)}& \frac{\∂h}{{\∂m}_j}{|}_{m_j={\hat{m}}_j(k-1)}\end{array}\right]$ 公式23

在本实施例中，上述公式可以具体表述为：

$z_i\left(k\right)=\left[\begin{array}{c}{x}_{i|s}\left(k\right)\\ y_{i|s}\left(k\right)\\ {\mathrm{\θ}}_{i|s}\left(k\right)\end{array}\right]=h\left(X_j^i\left(k\right)\right)+v\left(k\right)$ 公式24

$=\left[\begin{array}{ccc}\cos \left({\mathrm{\θ}}_r\left(k\right)\right)& \sin \left({\mathrm{\θ}}_r\left(k\right)\right)& 0\\ -\sin \left({\mathrm{\θ}}_r\left(k\right)\right)& \cos \left({\mathrm{\θ}}_r\left(k\right)\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_j-x_r\left(k\right)\\ y_j-y_r\left(k\right)\\ {\mathrm{\θ}}_j-{\mathrm{\θ}}_r\left(k\right)\end{array}\right]+N(0,R\left(k\right))$

$h_k^i=\left[\begin{array}{cc}\frac{\∂h}{{\∂X}_r\left(k\right)}{|}_{X_r\left(k\right)={\hat{X}}_r\left(k\right|k-1)}& \frac{\∂h}{{\∂m}_j}{|}_{m_j={\hat{m}}_j(k-1)}\end{array}\right]$

$=\left[\begin{array}{cccc}-\cos \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& -\sin \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& {\hat{y}}_{i|s}k& \\ \sin \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& -\cos \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& -{\hat{x}}_{i|s}\left(k\right)& \·\·\·\\ 0& 0& -1& \end{array}\right.$ 公式25

$\left.\begin{array}{cccc}& \cos \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& \sin \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& 0\\ \·\·\·& -\sin \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& \cos \left({\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\right)& 0\\ & 0& 0& 1\end{array}\right]$

在步骤S4中，当机器人观测到一个新的路标m_j时，需要将新的路标的状态加入到系统状态向量中。所述的路标初始化的步骤如下：

1)根据上一周期移动机器人的位姿估计值与当前的观测值，利用观测模型，逆运算得到当前路标的期望位姿；

2)将期望位姿作为该路标的初始值，加入到状态估计量中；

3)利用状态增广方法计算增广协方差矩阵。

如果路标的位置初始化为 ${\hat{m}}_j={\left(\begin{array}{ccc}0& 0& 0\end{array}\right)}^T,$ 将会产生与真实位置较大的偏差。这里采用期望位置作为该路标的初始状态，期望位置由机器人期望位姿和当前的观测值推出，其计算公式如下：

${\hat{X}}_{\mathrm{aug}}\left(k\right|k)=\left[\begin{array}{c}\hat{X}\left(k\right|k-1)\\ {\hat{m}}_j\left(k\right)\end{array}\right]$ 公式26

其中

由观测模型逆运算得出：

${\hat{m}}_j\left(k\right)=q({\hat{z}}_i\left(k\right),{\hat{X}}_r\left(k\right|k-1))$ 公式27

此时，

公式28

$P_{\mathrm{aug}}\left(k\right|k)=\left[\begin{array}{ccc}{P}_{\mathrm{rr}}\left(k\right|k)& P_{\mathrm{rm}}\left(k\right|k)& {\left(P_{\mathrm{ar}}\right)}^T\\ P_{\mathrm{mr}}\left(k\right|k)& P_{\mathrm{mm}}\left(k\right|k)& {\left(P_{\mathrm{am}}\right)}^T\\ P_{\mathrm{ar}}& P_{\mathrm{am}}& P_{\mathrm{aa}}\end{array}\right]$ 公式29

增广协方差矩阵P_aug(k|k)中新增元素的计算方法如下：

$P_{\mathrm{aa}}={\▿}_{X_r}q\·P_{\mathrm{rr}}\·{\left({\▿}_{X_r}q\right)}^T+{\▿}_{z_i}q\·R\left(k\right)\·{\left({\▿}_{z_i}q\right)}^T$ 公式30

$P_{\mathrm{ar}}={\▿}_{X_r}q\·P_{\mathrm{rr}}$ 公式31

$P_{\mathrm{am}}={\▿}_{X_r}q\·P_{\mathrm{rm}}$ 公式32

其中雅克比矩阵

和

的计算公式如下所示：

${\▿}_{X_r}q=\frac{\∂q(z_i,X_r)}{{\∂X}_r}{|}_{X_r={\hat{X}}_r\left(k\right|k)}$ 公式33

${\▿}_{z_i}g=\frac{\∂g}{{\∂z}_i}{|}_{Z_i={\hat{Z}}_i}$ 公式34

在本实施例中，上述公式可以具体表述为：

${\hat{m}}_j\left(k\right)=q({\hat{z}}_i\left(k\right),{\hat{X}}_r\left(k\right|k-1))$

$=\left[\begin{array}{c}{\hat{x}}_r\left(k\right|k-1)\\ {\hat{y}}_r\left(k\right|k-1)\\ {\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)\end{array}\right]+\left[\begin{array}{c}{\hat{x}}_{i|s}\left(k\right)\\ {\hat{y}}_{i|s}\left(k\right)\\ {\widehat{\mathrm{\θ}}}_{i|s}\left(k\right)\end{array}\right]$

公式35

$\·\left[\begin{array}{ccc}\cos {\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)& -\sin {\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)& 0\\ \sin {\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)& \cos {\widehat{\mathrm{\θ}}}_r\left(k\right|k-1)& 0\\ 0& 0& 1\end{array}\right]$

${\▿}_{X_r}q=\frac{\∂q(z_i,X_r)}{{\∂X}_r}{|}_{X_r={\hat{X}}_r\left(k\right|k)}$

$=\left[\begin{array}{ccc}1& 0& -{\hat{x}}_{a|s}\sin \left({\widehat{\mathrm{\θ}}}_r\left(k\right)\right)-{\hat{y}}_{a|s}\cos \left({\widehat{\mathrm{\θ}}}_r\left(k\right)\right)\\ 0& 1& {\hat{x}}_{a|s}\cos \left({\widehat{\mathrm{\θ}}}_r\left(k\right)\right)-{\hat{y}}_{a|s}\sin \left({\widehat{\mathrm{\θ}}}_r\left(k\right)\right)\\ 0& 0& 1\end{array}\right]$ 公式36

${\▿}_{z_i}g=\frac{\∂g}{{\∂z}_i}{|}_{Z_i={\hat{Z}}_i}$

$=\left[\begin{array}{ccc}\cos {\widehat{\mathrm{\θ}}}_r\left(k\right|k)& -\sin {\widehat{\mathrm{\θ}}}_r\left(k\right|k)& 0\\ \sin {\widehat{\mathrm{\θ}}}_r\left(k\right|k)& \cos {\widehat{\mathrm{\θ}}}_r\left(k\right|k)& 0\\ 0& 0& 1\end{array}\right]$ 公式37

在步骤S4中，当观测到的路标是已有的特征时，进行STF预测，所述的STF预测的步骤如下：

1)根据航位推测传感器数据和上一周期对移动机器人的位姿估计，估计当前周期移动机器人的位姿；

2)引入新的多重渐消因子T(k)，实现对状态中不同变量以不同的速率进行渐消，也能保证协方差矩阵的对称性；

3)采用强跟踪滤波方法中的一步近似算法求解T(k)的各个分量，针对观测不连续的情况，动态设定运算过程中的参数α_i；

4)α_i的设定采用如下方法：机器人发现路标时，当前的观测值只与机器人位姿与地图中路标的状态有关，其对应的α值应该增大，同时对机器人位姿估计、路标位姿估计中误差较大的分量“分配”更大的权值。

5)利用上一周期的协方差矩阵和当前的多重渐消因子T(k)，得到当前周期的协方差矩阵。

上述STF预测采用的计算公式如下：

$\hat{X}\left(k\right|k-1)=g(\hat{X}(k-1|k-1),u\left(k\right))$

$=\left[\begin{array}{c}{\hat{X}}_r\left(k\right|k)\\ \hat{m}\left(k\right)\end{array}\right]=\left[\begin{array}{c}f({\hat{X}}_r(k-1|k-1),u\left(k\right))\\ \hat{m}(k-1)\end{array}\right]$ 公式38

Huiping Li采用采用多重次优渐消因子在线调整Kalman增益的方法计算预测步骤的协方差矩阵(参见“Huiping，L.，et al.Strong Tracking FilterSimultaneous Localization and Mapping Algorithm.in Proceedings ofInternational Conference on Computer Science and Software Engineering，2008，pp.1085-1088”)，计算公式如下：

$P^{\′}\left(k\right|k-1)=\mathrm{\Λ}\left(k\right)\·G\left(k\right)\·P(k-1|k-1)\·G{\left(k\right)}^T+J_r^{T}Q\left(k\right)J_r$ 公式39

其中Λ(k)为多重次优渐消因子：

Λ(k)＝diag[λ₁(k)，λ₂(k)，…，λ_n(k)]；λ_i(k)≥1，i＝1，2，…，n    公式40

当λ₁，…λ_n不相等时，P′(k|k-1)为非对称阵，不满足协方差矩阵的定义，且求解Kalman增益时不能使用Cholesky分解来降低矩阵求逆的计算复杂度，而该求逆过程是EKF中计算开销最大的步骤之一。

这里引入新的多重渐消因子T(k)，在实现对状态中不同变量以不同的速率进行渐消，也能保证协方差矩阵P(k|k-1)的对称性：

T(k)＝diag[τ₁(k)，…，τ_n(k))； ${\mathrm{\τ}}_i\left(k\right)=\sqrt{{\mathrm{\λ}}_i\left(k\right)},i=\mathrm{1,2},\·\·\·,n$ 公式41

其中λ_i与公式39中相同。

$P\left(k\right|k-1)=T\left(k\right)\·G\left(k\right)\·P(k-1|k-1)\·G{\left(k\right)}^T\·T\left(k\right)+J_r^{T}Q\left(k\right)J_r$ 公式42

STF采用适合于在线运算的一步近似算法来求解Λ(k)：

令：

λ₁(k)∶λ₂(k)∶…∶λ_n(k)＝α₁∶α₂∶…∶α_n    公式43

其中α_i由先验知识确定，当无任何先验知识时，可取：α_i＝1，i＝1，2，…，n。

${\mathrm{\λ}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\λ}}_{0i}\left(k\right),& {\mathrm{\λ}}_{0i}\left(k\right)>1\\ 1,& {\mathrm{\λ}}_{0i}\left(k\right)\≤1,i=\mathrm{1,2},\·\·\·,n.\end{array}\right.$ 公式44

其中λ_0i(k)由以下步骤确定：

λ_0i(k)＝α_i·c(k)    公式45

$c\left(k\right)=\frac{\mathrm{tr}\left[N\left(k\right)\right]}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i\·M_{\mathrm{ii}}\left(k\right)}$ 公式46

$H_k^i=h_k^i{J}_{r,j}$ 公式47

$N\left(k\right)=V_0\left(k\right)-R\left(k\right)-H_k^i\·J_r\·Q\left(k\right)\·J_r^T\·{\left(H_k^i\right)}^T$ 公式48

$M\left(k\right)=G\left(k\right)\·P(k-1|k-1)\·G^T\left(k\right)\·{\left(H_k^i\right)}^T\·H_k^i$

公式49

$=\left(M_{\mathrm{ii}}\left(k\right)\right)$

$V_0=\left\{\begin{array}{cc}\mathrm{\γ}\left(1\right)\·{\mathrm{\γ}}^T\left(1\right),& k=0\\ \frac{\mathrm{\ρ}\·V_0(k-1)+\mathrm{\γ}\left(k\right)\·{\mathrm{\γ}}^T\left(k\right)}{1+\mathrm{\ρ}},& k\≥1\end{array}\right.$ 公式50

其中0＜ρ≤1为遗忘因子。

$\mathrm{\γ}\left(k\right)=z_i\left(k\right)-h\left({\hat{X}}_j^i\left(k\right|k-1)\right)$ 公式51

其中γ(k)为残差向量。

STF的模型建立在观测连续的情况下，而在SLAM问题中观测不连续的情况非常常见，而且不同时段的观测值与状态中不同的变量有关，且状态的维数不同时间也不尽相同。因此，固定α_i并不适合SLAM实际情况，需要动态地设定α_i。

本发明中，α_i的设定采用如下方法：机器人发现路标i时，当前的观测值只与机器人位姿(x_r，y_r，θ_r)与地图中路标i的状态有关，其对应的α值应该增大，同时对机器人位姿估计、路标位姿估计中误差较大的分量“分配”更大的权值。

当机器人观测到路标i时，机器人位姿与路标状态各分量的α确定方法如下：

α^x∶α^y∶α^θ＝1∶1∶β    公式52

${\mathrm{\α}}_x^j={\mathrm{\α}}_y^j=1;{\mathrm{\α}}_{\mathrm{\θ}}^j=\mathrm{\β},(j=\mathrm{1,2},\·\·\·,N;j\≠i)$ 公式53

${\mathrm{\α}}_x^i+{\mathrm{\α}}_y^i+{\mathrm{\α}}_{\mathrm{\θ}}^i=k_i\·\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}({\mathrm{\α}}_x^j+{\mathrm{\α}}_y^j+{\mathrm{\α}}_{\mathrm{\θ}}^j)$ 公式54

${\mathrm{\α}}_x^r+{\mathrm{\α}}_y^r+{\mathrm{\α}}_{\mathrm{\θ}}^r=k_r\·\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}({\mathrm{\α}}_x^j+{\mathrm{\α}}_y^j+{\mathrm{\α}}_{\mathrm{\θ}}^j)$ 公式55

在本实施例中，选定参数为：β＝3；k_i＝1.25；k_r＝0.25。

其中公式52指定了机器人和所有路标的状态在各自三个分量上的比例。N表示当前的路标个数，

表示路标j在x方向上的α值。

在步骤S5中，对STF预测的结果进行观测更新，所述的观测更新的步骤为：

1)将预计当前可见的路标从全局坐标系转换到局部坐标系中；

2)将进行数据关联后的传感器数据与当前可见的路标进行比较，计算位姿校正偏移量，即：新息；

3)根据当前周期移动机器人的预估位姿和位姿校正偏移量，计算当前周期移动机器人的位姿估计；

4)根据观测模型，计算新息协方差矩阵，进而计算出当前周期的协方差矩阵。

上述观测更新采用的计算公式如下：

$\hat{X}\left(k\right|k)=\hat{X}\left(k\right|k-1)+W_i\left(k\right)\·\mathrm{\γ}\left(k\right)$ 公式56

P(k|k)＝P(k|k-1)-W_i(k)S_i(k)W_i(k)^T    公式57

新息协方差矩阵S_i(k)计算如下：

$S_i\left(k\right)=H_k^i\·P\left(k\right|k-1)\·{\left(H_k^i\right)}^T+R_i\left(k\right)$ 公式58

卡尔曼增益W_i(k)计算如下：

$W_i\left(k\right)=P\left(k\right|k-1)\·{\left(H_k^i\right)}^T\·S_i{\left(k\right)}^{-1}$ 公式59

图2示出了利用本发明实际绘制全局特征地图的一个实例。其中，图2(a)为本实施例中使用的移动机器人平台；图2(b)为本实施例中使用的移动机器人运动模型；图2(c)为本实施例中使用的观测模型；图2(d)为实验场景示意图。图中五边形表示张贴在天花板上的路标，实线表示机器人的行进路线。为了便于实验数据的对比，机器人起始位置定在路标T1的正下方，沿着既定轨迹逆时针行进，依次经过其他路标点，重新回到T1处为1周，重复3次。O_WX_WY_W为世界坐标系，各路标以及指定轨迹的关键点在该世界坐标系中的坐标也一并在图中标出；图2(e)为实际场景图。

图2(f)为机器人轨迹对比图。三圈的实验结果如图中所示，三条轨迹分别为真实轨迹(虚线)、里程计得到的轨迹(星号)和使用SLAM算法得到的轨迹(加号)。五角星表示路标所在的真实位置，椭圆表示使用SLAM算法得到的路标的估计值的置信度为95％(2σ)的区间。可以直观地得出如下结论：SF-STF在第3圈能保持很好的估计精度，而里程计积分轨迹的误差逐渐增大。路标的真实值(五角星)都在椭圆的中心位置附近，但是椭圆面积较大，表明路标的估计均值相对真实值偏差较小，但是方差较大，可信度不高；EMF-STFSLAM定位和地图的精度始终比较稳定。相对于SF-STF SLAM来说，椭圆的面积始终稳定在一个较小的范围内，说明估计值方差较小，构建地图可信度较高。

图2(g)为各估计值的标准误差对比图。在机器人定位精度以及构建地图精度上，SF-STFSLAM和EMF-STFSLAM都要优于EKF-SLAM，且在地图精度方面的优势更为明显。由(e)可知，EMF-STFSLAM、SF-STFSLAM对于路标角度的估计精度相对于EKF-SLAM都有较大幅度的提高，体现出渐消因子利用残差信息对于角度估计的修正作用。由(a～d)可知，EMF-STFSLAM在定位及构建地图精度上都要优于SF-STFSLAM。这是因为当渐消因子发生突变时，SF-STFSLAM增大了整个状态的协方差矩阵，导致后续所有状态分量都过度地依赖于观测更新；而EMF-STFSLAM能够较大幅度地增大与当前观测路标的状态量的预测方差，使其估计量能够快速地跟踪残差的变化，增大观测更新对其估计值的修正权重，而其它路标状态量的方差变化幅度则相对较小，从而抑制整体协方差的增大速度，一定程度上保持了里程计信息对于其它路标状态量的估计值的修正权重。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种实现移动机器人同时定位与地图构建的方法，其特征在于，包括：

步骤S1：确定全局坐标系以及该坐标系下的移动机器人的初始位姿值；

步骤S2：记录k-1时刻的状态估计量以及对应的协方差矩阵，其中状态估计量包括移动机器人的位姿以及路标位置；

步骤S3：判断在k时刻是否能够观测到路标，如果在k时刻不能观测到路标，则进行时间更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果在k时刻能够观测到路标，则进行数据关联，然后执行步骤S4；

步骤S4：判断观测到的路标是否为新的特征，如果观测到的路标是新的特征，则进行路标初始化，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果观测到的路标是已有的特征，则进行STF预测，然后执行步骤S5；

步骤S5：对STF预测的结果进行观测更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；

步骤S6：重复步骤S2～步骤S5，得到最后时刻的状态估计量，即移动机器人的位姿以及环境中路标的位置，实现同时定位与地图构建。

2.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，在步骤S3中，移动机器人采用传感器获取环境中路标观测数据，该路标观测数据是路标相对于移动机器人的位姿信息，且该路标观测数据含有方向信息。

3.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，步骤S3中所述时间更新，采用扩展卡尔曼滤波法，根据上一周期移动机器人的位姿估计和航位推测传感器数据，利用移动机器人运动模型，预估当前周期移动机器人的位姿。

4.根据权利要求3所述的实现移动机器人同时定位与地图构建的方法，其特征在于，所述时间更新采用的计算公式如下：

$X\left(k\right)=\left[\begin{array}{c}{X}_r\left(k\right|k)\\ m\left(k\right)\end{array}\right]=g(X(k-1),u\left(k\right))+\mathrm{\ϵ}\left(k\right)$

公式10

$=\left[\begin{array}{c}f(X_r(k-1|k-1),u\left(k\right))\\ m(k-1)\end{array}\right]+\mathrm{\ϵ}\left(k\right)$

其中m(k)为路标k时刻的位姿，ε(k)为零均值高斯白噪声，ε(k)的协方差矩阵为

J_r用于将3维的状态向量映射为3N+3维的向量；

公式11

将运动方程g用一阶泰勒展开式近似：

$g(X(k-1),u\left(k\right))\≈g(\hat{X}(k-1),u\left(k\right))+G\left(k\right)\·(X(k-1)-\hat{X}(k-1))$ 公式12

其中：

$G\left(k\right)=\frac{\∂g(X\left(k\right),u\left(k\right))}{\∂X\left(k\right)}{|}_{\hat{X}\left(k\right|k-1),u\left(k\right)}$ 公式13

则k时刻的状态向量估计值为：

$\hat{X}\left(k\right)=g(\hat{X}(k-1),u\left(k\right))$

$=\left[\begin{array}{c}{\hat{X}}_r\left(k\right|k)\\ \hat{m}\left(k\right)\end{array}\right]=\left[\begin{array}{c}f({\hat{X}}_r(k-1|k-1),u\left(k\right))\\ \hat{m}(k-1)\end{array}\right]$ 公式14

k时刻的协方差矩阵P(k|k)为：

$P\left(k\right|k)=G\left(k\right)P(k-1|k-1)G^T\left(k\right)+J_r^{T}Q\left(k\right)J_r$ 公式15。

5.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，步骤S3中所述数据关联采用延迟更新的方法，具体步骤如下：

有新的观测值时，不立刻进行观测更新，记录下当前移动机器人的位姿信息以及对应的观测值；

时间约束：在一定观测周期内，观测次数超过一定阈值的特征为稳定特征，去除其他非稳定观测值以及对应的移动机器人的位姿信息；

距离约束：对于满足时间约束的特征，利用观测模型计算出特征在全局坐标系下的估计值序列，去除相对距离较远的外点；

对于满足时间约束与距离约束的观测值进行批更新。

6.根据权利要求5所述的实现移动机器人同时定位与地图构建的方法，其特征在于，所述数据关联中，当观测到第i个路标时，采用公式z_i(k)＝h(X_i(k))+v(k)中的观测模型，引入匹配变量

表示观测到的特征

与地图中的路标m_j之间的关系，其中

N表示地图中路标的个数；

如果

则在k时刻第i次观测到的特征对应于地图中的第j个路标；

如果

则表示当前的观测到的特征在地图中没有与之匹配的路标；此时有两种情况：其一，该次观测结果是一个“伪”路标；其二，该次观测结果是一个之前没有发现的新的路标；

假设在k时刻观测到的第i个特征对应于地图中的第j个路标，该观测模型可近似为一阶线形模型：

$X_j^i\left(k\right)=\left[\begin{array}{c}{X}_r\left(k\right)\\ m_j\left(k\right)\end{array}\right]{=J}_{r,j}X\left(k\right)$ 公式20

J_r，j是一个6×(3N+3)的矩阵，如下所示：

公式21

$h\left(X_j^i\left(k\right|k-1)\right)\≈h\left({\hat{X}}_j^i\left(k\right|k-1)\right)+H(X_j^i\left(k\right|k-1)-{\hat{X}}_j^i\left(k\right|k-1))$ 公式22

H是

对于状态向量Xr(k|k-1)和mj(k)在

处的雅各比矩阵，维数为3×6；

$H=\left[\begin{array}{cc}\frac{\∂h}{{\∂X}_r\left(k\right)}{|}_{X_r\left(k\right)={\hat{X}}_r\left(k\right|k-1)}& \frac{\∂h}{{\∂m}_j}{|}_{m_j={\hat{m}}_j(k-1)}\end{array}\right]$ 公式23。

7.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，步骤S4中所述路标初始化，是当机器人观测到一个新的路标m_j时，将新的路标的状态加入到系统状态向量中；具体包括：

根据上一周期移动机器人的位姿估计值与当前的观测值，利用观测模型，逆运算得到当前路标的期望位姿：

${\hat{m}}_j\left(k\right)=q({\hat{z}}_i\left(k\right),{\hat{X}}_r\left(k\right|k-1))$ 公式27

将期望位姿作为该路标的初始值，加入到状态估计量中：

${\hat{X}}_{\mathrm{aug}}\left(k\right|k)=\left[\begin{array}{c}\hat{X}\left(k\right|k-1)\\ {\hat{m}}_j\left(k\right)\end{array}\right]$ 公式26

利用状态增广方法计算增广协方差矩阵：

$P_{\mathrm{aug}}\left(k\right|k)=\left[\begin{array}{ccc}{P}_{\mathrm{rr}}\left(k\right|k)& P_{\mathrm{rm}}\left(k\right|k)& {\left(P_{\mathrm{ar}}\right)}^T\\ P_{\mathrm{mr}}\left(k\right|k)& P_{\mathrm{mm}}\left(k\right|k)& {\left(P_{\mathrm{am}}\right)}^T\\ P_{\mathrm{ar}}& P_{\mathrm{am}}& P_{\mathrm{aa}}\end{array}\right]$ 公式29

$P_{\mathrm{aa}}={\▿}_{X_r}q\·P_{\mathrm{rr}}\·{\left({\▿}_{X_r}q\right)}^T+{\▿}_{z_i}q\·R\left(k\right)\·{\left({\▿}_{z_i}q\right)}^T$ 公式30

$P_{\mathrm{ar}}={\▿}_{X_r}q\·P_{\mathrm{rr}}$ 公式31

$P_{\mathrm{am}}={\▿}_{X_r}q\·P_{\mathrm{rm}}$ 公式32

其中雅克比矩阵

和的计算公式如下所示：

${\▿}_{X_r}q=\frac{\∂q(z_i,X_r)}{{\∂X}_r}{|}_{X_r={\hat{X}}_r\left(k\right|k)}$ 公式33

${\▿}_{z_i}g=\frac{\∂g}{{\∂z}_i}{|}_{Z_i={\hat{Z}}_i}$ 公式34

此时，

公式28。

8.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，步骤S4中所述STF预测，具体包括：

根据航位推测传感器数据和上一周期对移动机器人的位姿估计，估计当前周期移动机器人的位姿；

$\hat{X}\left(k\right|k-1)=g(\hat{X}(k-1|k-1),u\left(k\right))$

$=\left[\begin{array}{c}{\hat{X}}_r\left(k\right|k)\\ \hat{m}\left(k\right)\end{array}\right]=\left[\begin{array}{c}f({\hat{X}}_r(k-1|k-1),u\left(k\right))\\ \hat{m}(k-1)\end{array}\right]$ 公式38

引入新的多重渐消因子T(k)，实现对状态中不同变量以不同的速率进行渐消，也能保证协方差矩阵的对称性；

T(k)＝diag[τ₁(k)，…，τ_n(k))； ${\mathrm{\τ}}_i\left(k\right)=\sqrt{{\mathrm{\λ}}_i\left(k\right)},i=\mathrm{1,2},\·\·\·,n$ 公式41

采用强跟踪滤波方法中的一步近似算法求解T(k)的各个分量，针对观测不连续的情况，动态设定运算过程中的参数α_i；

令：

λ₁(k)∶λ₂(k)∶…∶λ_n(k)＝α₁∶α₂∶…∶α_n    公式43

其中α_i由先验知识确定，当无任何先验知识时，可取：α_i＝1，i＝1，2，…，n；

${\mathrm{\λ}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\λ}}_{0i}\left(k\right),& {\mathrm{\λ}}_{0i}\left(k\right)>1\\ 1,& {\mathrm{\λ}}_{0i}\left(k\right)\≤1,i=\mathrm{1,2},\·\·\·,n.\end{array}\right.$ 公式44

其中λ_0i(k)由以下步骤确定：

λ_0i(k)＝α_i·c(k)    公式45

$c\left(k\right)=\frac{\mathrm{tr}\left[N\left(k\right)\right]}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i\·M_{\mathrm{ii}}\left(k\right)}$ 公式46

$H_k^i=h_k^i{J}_{r,j}$ 公式47

$N\left(k\right)=V_0\left(k\right)-R\left(k\right)-H_k^i\·J_r\·Q\left(k\right)\·J_r^T\·{\left(H_k^i\right)}^T$ 公式48

$M\left(k\right)=G\left(k\right)\·P(k-1|k-1)\·G^T\left(k\right)\·{\left(H_k^i\right)}^T\·H_k^i$

公式49

$=\left(M_{\mathrm{ii}}\left(k\right)\right)$

$V_0=\left\{\begin{array}{cc}\mathrm{\γ}\left(1\right)\·{\mathrm{\γ}}^T\left(1\right),& k=0\\ \frac{\mathrm{\ρ}\·V_0(k-1)+\mathrm{\γ}\left(k\right)\·{\mathrm{\γ}}^T\left(k\right)}{1+\mathrm{\ρ}},& k\≥1\end{array}\right.$ 公式50

其中0＜ρ≤1为遗忘因子；

$\mathrm{\γ}\left(k\right)=z_i\left(k\right)-h\left({\hat{X}}_j^i\left(k\right|k-1)\right)$ 公式51

其中γ(k)为残差向量；

α_i的设定采用如下方法：机器人发现路标时，当前的观测值只与机器人位姿与地图中路标的状态有关，其对应的α值应该增大，同时对机器人位姿估计、路标位姿估计中误差较大的分量“分配”更大的权值；

α^x∶α^y∶α^θ＝1∶1∶β    公式52

${\mathrm{\α}}_x^j={\mathrm{\α}}_y^j=1;{\mathrm{\α}}_{\mathrm{\θ}}^j=\mathrm{\β},(j=\mathrm{1,2},\·\·\·,N;j\≠i)$ 公式53

${\mathrm{\α}}_x^i+{\mathrm{\α}}_y^i+{\mathrm{\α}}_{\mathrm{\θ}}^i=k_i\·\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}({\mathrm{\α}}_x^j+{\mathrm{\α}}_y^j+{\mathrm{\α}}_{\mathrm{\θ}}^j)$ 公式54

${\mathrm{\α}}_x^r+{\mathrm{\α}}_y^r+{\mathrm{\α}}_{\mathrm{\θ}}^r=k_r\·\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}({\mathrm{\α}}_x^j+{\mathrm{\α}}_y^j+{\mathrm{\α}}_{\mathrm{\θ}}^j)$ 公式55

利用上一周期的协方差矩阵和当前的多重渐消因子T(k)，得到当前周期的协方差矩阵；

$P\left(k\right|k-1)=T\left(k\right)\·G\left(k\right)\·P(k-1|k-1)\·G{\left(k\right)}^T\·T\left(k\right)+J_r^{T}Q\left(k\right)J_r$ 公式42。

9.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，步骤S5中所述观测更新，具体包括：

将预计当前可见的路标从全局坐标系转换到局部坐标系中；

将进行数据关联后的传感器数据与当前可见的路标进行比较，计算位姿校正偏移量，即新息；

根据当前周期移动机器人的预估位姿和位姿校正偏移量，计算当前周期移动机器人的位姿估计：

$W_i\left(k\right)=P\left(k\right|k-1)\·{\left(H_k^i\right)}^T\·S_i{\left(k\right)}^{-1}$ 公式59

$\hat{X}\left(k\right|k)=\hat{X}\left(k\right|k-1)+W_i\left(k\right)\·\mathrm{\γ}\left(k\right)$ 公式56

根据观测模型，计算新息协方差矩阵，进而计算出当前周期的协方差矩阵：

$S_i\left(k\right)=H_k^i\·P\left(k\right|k-1)\·{\left(H_k^i\right)}^T+R_i\left(k\right)$ 公式58

P(k|k)＝P(k|k-1)-W_i(k)S_i(k)W_i(k)^T    公式57。

Images