CN104062973B - 一种基于图像标志物识别的移动机器人slam方法 - Google Patents

Abstract

本发明提出一种基于图像标志物识别的移动机器人SLAM方法，采用EKF‑SLAM模型进行机器人状态参数的时间更新和基于标志物识别的测量更新。本方法可以从环境中获取大量信息，从而使机器人能够构建出高精度的环境地图。与现有方法相比具有对环境空间结构理解更准确，抗干扰能力强，构建的地图精度高的特点。

Description

一种基于图像标志物识别的移动机器人SLAM方法

技术领域

本发明涉及机器人学领域，具体地说是用基于扩展卡尔曼滤波器的同步定位与地图构建(EKF-SLAM)方法结合计算机图像技术实现机器人对环境的探索，并构建环境地图。

背景技术

同步定位与地图构建(Simultaneous Localization and Mapping,简称SLAM)这一概念最早由Smith、Self和Cheeseman于1988年提出。这种方法描述了机器人从未知环境的未知地点出发，然后对未知环境进行探索的情境：机器人在运动过程中重复地观测环境，然后根据传感器感知的环境特征定位自身位置和姿态，再根据自身位置增量式的构建地图。这就提出了SLAM包含的两个目标：同时定位和地图构建。其中地图构建是把从传感器收集到的一系列信息，集成到一个一致性的模型(即地图)上，得到的地图被用来描述当前环境以及给机器人定位；而定位则是在地图上估测机器人的坐标和姿势形态。

实际中的SLAM应用一般有两种典型情境：追踪——这种情况下通常机器人的初始位置已知；全局定位——这种情况下通常只给出很少，甚至不给出有关于机器人起始位置或者环境特征的先验信息。因此，SLAM的核心被定义为一个关于机器人运动路径和所处环境特征的后验估计。要解决SLAM问题，就必须建立适当的模型，解出这个后验概率。

近十几年来，机器人学界涌现出了各种各样的针对SLAM问题的解决方案。其中，基于扩展卡尔曼滤波器(EKF)的EKF-SLAM方法获得了广泛的认可并被普遍应用。这种方法首先估计运动过程中某一时刻的状态，然后以(含噪声的)测量变量的方式获得反馈，最后根据反馈修正估计值。这样，EKF-SLAM就能在不需了解机器人详细性质的前提下，高效地对运动的过去、当前、甚至将来的状态进行估计。

在目前国内外公开的文献中，Durrant-Whyte H,Bailey T.Simultaneous localizationand mapping:part I[J].Robotics&Automation Magazine,IEEE,2006,13(2):99-110.讨论了EKF-SLAM的模型建立。该模型基于对机器人运动进行描述，然后用EKF的状态更新及测量更新对机器人运动进行重复估计，从而得到SLAM问题的解。该模型不关注机器人机体特性，具有很强的平台迁移性。Thrun S,Burgard W,Fox D.Probabilisticrobotics[M].Cambridge:MIT press,2006,309-336,149-187.详细讨论了将激光传感器、超声波传感器在SLAM中的应用。这种应用将传感器的测量值用作EKF-SLAM中测量方程的输入，用真实的环境特征修正状态方程的估计。同时，文章还详细分析了这类传感器的原理，讨论了传感器测量值与实际值的偏差特性，并且在隐马尔科夫链的基础上，提出了一个概率密度函数来衡量测量值准确度。这种应用方法对传感器的特点的深入分析使其具有很强的实用性，因此其模拟仿真和实验中对几种常见的环境噪声都表现出了较强的鲁棒性，能得到令人满意的结果。

但上述的EKF-SLAM方法有几点不足：

(1)机器人只能识别局部空间障碍物的有无，并不能感知环境的空间结构。如果环境结构高度对称(常见于人工建筑，如走廊、大厅等)，则机器人会受这一对称性的影响，使运动范围局限于环境中极小的空间内，无法构建全局地图，SLAM结果出错。

(2)测量方式单一。上述方法中机器人仅以一种类型的测量数据进行测量更新，尽管对常见的几种噪声进行了建模，但当环境中出现未知干扰时，测量值的错误依然很难被修正。考虑到EKF-SLAM方法完全依赖测量值对估计进行修正，测量值的错误会给计算结果带来严重影响。

发明内容

近年来，随着计算机技术、数字图像处理技术及图像处理硬件的进一步发展，计算机视觉在机器人领域中开始得到广泛关注。相比其他方法，基于计算机视觉的方法能够在同等条件下获取环境中的更大量的信息，同时还具备了精度高、不受电子干扰等优点。同时，开源的OpenCV(Open Computer Vision)函数库让主流的计算机视觉方法能被有效地实现。这样，如果在机器人上装载摄像头来拍摄环境，就可以捕获丰富的环境特征，从而机器人感知能力有限给SLAM带来的问题。

本发明的技术方案为：

所述一种基于图像标志物识别的移动机器人SLAM方法，其特征在于：采用以下步骤：

步骤1：机器人开始运动后，由实际中机器人对应的数学模型和运动的初始条件，机器人使用上一时刻的状态信息，根据EKF-SLAM模型的时间更新方程对此时的状态进行预测估计，完成先验估计的构造，同时构建地图：

由机器人运动过程第k-1时刻的状态信息按照时间更新方程：

$\left\{\begin{array}{c}{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}\\ {P_k}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q\end{array}\right.$

推算第k时刻状态估计的先验值其中P_k ^-为第k时刻协方差的先验值，u_k-1为第k-1时刻对机器人施加的控制信号，A、B分别表示机器人运动模型对u_k-1的雅克比矩阵；Q为先验估计误差；P_k-1为第k-1时刻协方差的后验值；机器人运动过程的状态信息由机器人的位置和姿态信息组成；

地图由m＝(m₁,m₂，...)^T表示，由到第k时刻为止机器人存储的所有标识物坐标构成；

步骤2：机器人实时拍摄环境，使用计算机视觉方法的处理拍摄到的图像，找到标志物在图像中的位置，然后用机器视觉测量方法计算出机器人与标志物的相对位置关系；得到相对位置关系作为EKF-SLAM模型的测量更新方程的输入，根据测量更新方程对机器人位置状态的先验估计进行修正：

步骤2.1：识别标志物：所述标志物由7x7的黑色和白色小方格组成，最外围的小方格全为黑色，内部的5x5的小方格中，每一行均包括3个奇偶校验位和2个数据位，其中第2位和第4位为数据位；奇偶校验位确定出标志物的方向，数据位表示标志物唯一的ID号；

步骤2.1.1：将机器人拍摄的图像转化至灰度空间；

步骤2.1.2：对已转化至灰度空间的图像进行二值化操作；

步骤2.1.3：在二值化的图像中检测轮廓线，并查找候选的标志物对象；

步骤2.1.4：对候选标志物进行检测并确定标志物的ID号；

步骤2.2：估计标志物相对于机器人的位置和姿态：

步骤2.2.1：对于识别出的标志物，取其轮廓上不共线的三点A`、B`、C`的图像坐标(u<sub>i</sub>,v<sub>i</sub>)，分别算出它们在机器人图像采集装置成像平面上的成像点坐标i＝A`、B`、C`：

$\left[\begin{array}{c}{x}_{c_i}\\ y_{c_i}\\ 1\end{array}\right]=M^{-1}\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]$

其中，M为机器人图像采集装置的内参数矩阵，得到了点A、B、C在机器人图像采集装置坐标系中投影方向上的单位向量e_i，其中点A、B、C为点A`、B`、C`对应的现实世界中的点：

$e_i=\frac{1}{\sqrt{x_{c_i}^2+y_{c_i}^2+1}}\left[\begin{array}{c}{x}_{c_i}\\ y_{c_i}\\ 1\end{array}\right]$

e_A'、e_B'、e_C'分别为点A、B、C在机器人图像采集装置坐标系中投影方向上的单位向量；

步骤2.2.2：根据以下公式：

$\left\{\begin{array}{c}{D}_{\mathrm{AB}}=\mathrm{\&lambda;}\sqrt{{(x_{c_{A^{\text{\&prime;}}}}-x_{c_{B^{\&prime;}}})}^2+{(y_{c_{A^{\&prime;}}}-y_{c_{B^{\&prime;}}})}^2}\\ D_{\mathrm{BC}}=\mathrm{\&lambda;}\sqrt{{(x_{c_{B^{\text{\&prime;}}}}-x_{c_{C^{\&prime;}}})}^2+{(y_{c_{B^{\&prime;}}}-y_{c_{C^{\&prime;}}})}^2}\\ D_{\mathrm{AC}}=\mathrm{\&lambda;}\sqrt{{(x_{c_{A^{\text{\&prime;}}}}-x_{c_{C^{\&prime;}}})}^2+{(y_{c_{A^{\&prime;}}}-y_{c_{C^{\&prime;}}})}^2}\end{array}\right.\left\{\begin{array}{c}\cos {\mathrm{\&theta;}}_{\mathrm{AB}}=e_{B^{\&prime;}}^T{e}_{C^{\&prime;}}\\ \cos {\mathrm{\&theta;}}_{\mathrm{AC}}=e_{A^{\&prime;}}^T{e}_{C^{\&prime;}}\\ \cos {\mathrm{\&theta;}}_{\mathrm{BC}}=e_{A^{\&prime;}}^T{e}_{B^{\&prime;}}\end{array}\right.$

计算空间点A、B、C间的距离D_AB，D_BC，D_AC以及相应角度余弦值cosθ_AB，cosθ_AC，cosθ_BC；

其中λ为比例系数，λ＝1/L，L为标志物中单个小方格的边长；并根据方程

$\left\{\begin{array}{c}{d}_A^2+d_B^2-2d_A{d}_B\cos {\mathrm{\&theta;}}_{\mathrm{AB}}=D_{\mathrm{AB}}^2\\ d_A^2+d_C^2-2d_A{d}_C\cos {\mathrm{\&theta;}}_{\mathrm{AC}}=D_{\mathrm{AC}}^2\\ d_B^2+d_C^2-2d_B{d}_C\cos {\mathrm{\&theta;}}_{\mathrm{BC}}=D_{\mathrm{BC}}^2\end{array}\right.$

得到机器人图像采集装置光心到点A、B、C的距离d_A，d_B，d_c；并由公式

$P_j=d_j{e}_j=\left[\begin{array}{c}{x}_j\\ y_j\\ z_j\end{array}\right]$

得到点A、B、C在机器人图像采集装置坐标系下的坐标P_j，j＝A,B,C；e_A＝e_A'，e_B＝e_B'，e_C＝e_C'；

步骤2.2.3：在点A、B、C中任意取一点，进行如下运算：

其中，D_j为机器人与标志物的距离，为标志物与机器人前进方向的夹角；x_j、z_j为点A、B、C中任意取的一点的坐标值；η为观测误差；z_k为第k时刻的测量值；

步骤2.3：根据步骤2.2得到的第k时刻的测量值，在EKF-SLAM模型的测量更新方程中

$\left\{\begin{array}{c}{K}_k={P_k}^{-}{H}^T{({{\mathrm{HP}}_k}^{-}{H}^T+R)}^{-1}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\\ P_k=(I-K_{k}H){P_k}^{-}\end{array}\right.$

对步骤1得到的第k时刻状态估计的先验值以及第k时刻协方差的先验值P_k ^-进行修正，得到第k时刻机器人状态估计的后验值和协方差的后验值P_k；其中H表示机器人观测模型对的雅克比矩阵；R为测量误差；

步骤2.4：地图扩建：

每次观测到标志物后，机器人会将该标志物的位置扩充到构建的地图中。

进一步的优选方案，所述一种基于图像标志物识别的移动机器人SLAM方法，其特征在于：当步骤2中，无法识别出环境中的标志物时，采用理想雅克比模型进行测量更新。

有益效果

本发明的有益效果是：

实现了一种机器人同步定位与地图构建的方法。该方法可以从环境中获取大量信息，从而使机器人能够构建出高精度的环境地图。与现有方法相比具有对环境空间结构理解更准确，抗干扰能力强，构建的地图精度高的特点。

本发明之所以具有上述的有益效果其原因在于：摄像机获取的大量环境特征使得机器人对环境空间结构的感知不再以简单的“有”还是“无”的结果表示。室内环境空间结构往往具有高度对称性，如果只是简单地感知障碍物的有无，则机器人极易被“相似的”空间结构误导，构建出错误的地图。而本发明中，机器人能够准确地判断自己是否探索过该区域，然后再对当前区域进行地图构建，这样就把环境的各个区域区分开来，不受对称空间结构的影响，构建的地图就能精确记录机器人的实际探索过程，也就能准确反映真实环境。另外，当环境中出现光照变化噪声时，机器人通过切换观测方式进行观测更新保证SLAM过程可以连续进行，因此对机器人运动过程的记录不会中断，构建的地图也一定是连续的。这样就消除了光照变化的干扰，增强了方法的鲁棒性，提高了地图的准确度。

附图说明

图1：本发明的流程图；

图2：实施例中汽车式移动机器人运动模型各变量的几何关系；

图3：机器人与观测到的标志物的几何关系；

图4：标志物示意图；

图5：标志物二维码示意图；

图6：将输入图像转化至灰度空间示意图；

图7：二值化操作示意图；

图8：检测轮廓线示意图；

图9：对候选标志物进行检测并确定标志物的ID号示意图；

图10：摄像机的拍摄角度的影响示意图；

图11：标志物相对于摄像机的位置关系图。

具体实施方式

下面结合具体实施例描述本发明：

附图1展示了本发明实现的机器人SLAM方法的总流程。图中包含了机器人一次SLAM过程的全部步骤。本发明目的为使机器人得到更好的SLAM结果。标志物测量数据对摄像头获取的图像进行处理得到，理想雅克比模型的观测数据由机载高精度惯性传感器得到，整个SLAM过程涉及的模型参数计算和计算机图像处理均由程序实现。得到的地图由记录的一系列“路标”点坐标表示。

下面是具体的实现步骤：

步骤1：机器人状态参数的时间更新

机器人运动过程第k时刻的状态向量为x_k＝(x_k,m)^T。其中，x_k＝(x,y,θ)^T描述机器人的位置和姿态状态，机器人的位置和姿态变化通过x_k中三个分量的不同取值来体现；

m＝(m₁,m₂，...)^T为机器人绘制的环境地图，由到第k时刻为止机器人存储的所有目标点(标识物坐标)构成，其中，每个地图点(如m₁)又用坐标来表示(如m₁＝(m_1,x,m_2,y))。这样m＝(m₁,m₂，...)^T分量就可视为是机器人对环境后的记录(即环境地图)。由于x_k＝(x,y,θ)^T反映了机器人此刻的位置信息，并且(x,y)的位置坐标与m＝(m₁,m₂，...)^T的地图坐标处于同一坐标系中，这样机器人就能够在自己绘制的地图中定出自身的位置。因此就实现了SLAM同步定位与绘图的目的。

机器人开始运动后，由实际中机器人对应的数学模型和运动的初始条件，机器人使用上一时刻的状态信息，根据EKF-SLAM模型的时间更新方程对此时的状态进行预测估计，完成先验估计的构造，同时构建地图：

由机器人运动过程第k-1时刻的状态信息按照时间更新方程：

$\left\{\begin{array}{c}{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}\\ {P_k}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q\end{array}\right.$

推算第k时刻状态估计的先验值其中P_k ^-为第k时刻协方差的先验值，u_k-1为第k-1时刻对机器人施加的控制信号，A、B分别表示机器人运动模型对u_k-1的雅克比矩阵；Q为先验估计误差；P_k-1为第k-1时刻协方差的后验值。

在EKF-SLAM模型中，时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计，是预估方程；而模型中的时间更新方程由机器人运动的数学模型给出，输入输出均为机器人的位置状态信息。以汽车式移动机器人为例，使用上述状态向量给定的变量，机器人运动引起的位置坐标变化可建模如下：

${\hat{x}}_k^{-}=\left[\begin{array}{c}{x}_k^{-}\\ y_k^{-}\\ {\mathrm{\&theta;}}_k^{-}\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}+\mathrm{V\&Delta;}T\cos ({\mathrm{\&theta;}}_{k-1}+\mathrm{\&gamma;})\\ y_{k-1}+\mathrm{V\&Delta;}T\sin ({\mathrm{\&theta;}}_{k-1}+\mathrm{\&gamma;})\\ {\mathrm{\&theta;}}_{k-1}+(\mathrm{V\&Delta;T}/B)\sin \mathrm{\&gamma;}\end{array}\right]+\mathrm{\&xi;}$

各变量的几何关系如附图2所示。其中，V为机器人驱动前进的速度，ΔT和ξ分别为时间间隔和噪声。这个模型对应EKF-SLAM模型的时间更新方程，由第k-1时刻的状态x_k-1、y_k-1、θ_k-1推算第k时刻状态估计的先验值雅克比矩阵A、B也由上式求导得到。

步骤2：基于标志物识别的测量更新

机器人在环境中开始运动。当机体对环境进行探索时，SLAM过程也随之展开。本发明在环境中放置特殊的标志物，机器人对环境的探索过程即可转换为对标志物的识别过程。通过从标志物中提取的信息，完成同步式地图构建与定位。

机器人实时拍摄环境，使用计算机视觉方法的处理拍摄到的图像，找到标志物在图像中的位置，然后用机器视觉测量方法计算出机器人与标志物的相对位置关系；得到相对位置关系作为EKF-SLAM模型的测量更新方程的输入，根据测量更新方程对机器人位置状态的先验估计进行修正：

步骤2.1：识别标志物：

在环境中放置标志物(如附图4所示)作为“路标”。本发明中，机器人通过识别这些标志物实现测量。标志物结构类似二维码，由7x7的黑色和白色小方格组成，最外围的小方格全为黑色，内部的5x5的小方格中，每一行均包括3个奇偶校验位和2个数据位，其中第2位和第4位为数据位，奇偶校验位确定出标志物的方向，数据位表示标志物唯一的ID号。3个奇偶校验位可以唯一确定出标志物的方向，每一行的两个数据位总共可以得出如附图5所示的4种可能组合。每个标志物总共有10个数据位，因此标志物ID号的取值范围为0～1023(附图4为其中之一)。

步骤2.1.1：将机器人拍摄的图像转化至灰度空间；由于标志物只含有黑白两种颜色的小方格，因此将输入图像转化至灰度空间后，可方便进行标志物的检测工作，而且也可减弱光照对标志物检测的影响。结果如图6所示。

步骤2.1.2：对已转化至灰度空间的图像进行二值化操作；

二值化操作会使图像中每一个像素点的像素值变为0(黑)或255(白)。同样地，考虑到环境中光照的影响，本发明选取了最大类间方差法进行图像的自适应二值化。如果将步骤2.1.1处理过的图像按灰度特性分为背景和目标两部分，那么这两部分之间的类间方差越大，说明该图像中这两部分的差别越大，而背景部分与目标部分的错误分类都会得到小的类间方差。因此使用最大类间方差法可以最大化背景与目标间的区别，从而减小错分概率。该方法可表达如下：

以背景较暗且大小为M×N的图像为例，设目标与背景部分的分割阈值为T。记图像中灰度值小于T的像素个数为N₀，灰度值大于T的像素个数为N₁，则有：

N₀+N₁＝M×N

ω₀＝N₀/M×N，ω₁＝N₁/M×N

ω₀、ω₁分别为属于目标和背景的像素点占整幅图像的比例，显然，ω₀+ω₁＝1。

图像中所有像素点分成两部分后，我们可以得到目标与背景部分的平均灰度，分别记为μ₀和μ₁。于是，整幅图像的平均灰度μ为：

μ＝ω₀μ₀+ω₁μ₁

为了使目标和背景分类错误的概率最小，就要使类间方差g最大。目标与背景的类间方差g表达式如下：

g＝ω₀(μ₀-μ)²+ω₁(μ₁-μ)²

进一步可得到：

g＝ω₀ω₁(μ₀-μ₁)²

可以通过选择最佳分割阈值T，使得类间方差g最大。这样二值化得到的图像目标与背景部分区别明显，便于提取目标部分包含的标志物。结果如附图7所示。

步骤2.1.3：在二值化的图像中检测轮廓线，并查找候选的标志物对象；

经过前两步处理后，图像中的标志物已经变成黑白两种颜色组成的方形。由于构成标志物的都是规则的方格，具有闭合的轮廓和特定的大小(此处大小用周长衡量)，因此我们通过检测图像中的轮廓线来查找候选的标志物对象。

本发明使用OpenCV库中的cvFindContours函数进行图像轮廓的提取，这个函数用存储序列记录一列构成轮廓的连续点来表示轮廓。因为包含标志物的不可能是小轮廓，所以我们要对得到的轮廓进行筛选。轮廓的大小可以用其周长来衡量，这里我们通过数各个序列中包含的像素点数得到周长值，当周长小于136时，舍弃该轮廓。

另一方面，标志物是由方格构成的，因此轮廓也应该是方形。而经过之前处理保留下来的轮廓中往往有并非方形轮廓存在。在这里我们使用多边形去近似这些轮廓，然后记录这些近似多边形的顶点。如果近似的多边形顶点数大于4，那么这个多边形就不是我们要检测的标志物，应该舍弃。经过以上处理保留下来的四边形就组成了候选标志物对象。结果如附图8所示。

步骤2.1.4：对候选标志物进行检测并确定标志物的ID号；

摄像机观测物体时，由于距离及方位不同，得到的图像会产生相应的变形。因此要把图像映射到与摄像头垂直的平面上，这样才能得到图像中物体的真实形状。

利用OpenCV函数库中的cvGetPerspectiveTransform和cvWarpPerspective函数，我们将步骤2.1.3得到的图像转换到与摄像头垂直的平面上。这时，图像中保留的四边形就能够与环境中的真实形状对应。如附图9所示，对这些四边形包含的5×5个方格中黑白格的个数及排列方式进行计算，就得到了图像中标志物的ID号。

由于四边形物体在室内环境中比较常见(如墙面瓷砖、地板砖等)，而这些四边形是无法提取ID号的。因此根据有无ID号可以使机器人自动忽略掉除标志物以外的其他四边形，从而提高了观测精度。

摄像机的拍摄角度会导致检测到的标志物有四种可能的摆放方式(如附图10所示)。由于我们选取的标志物中，每行都有三个专门用来确定标志物的正确朝向的奇偶校验位，这里只须检测奇偶校验位即可。

步骤2.2：估计标志物相对于机器人的位置和姿态：

为了得到标志物与机器人的相对位姿关系，我们用标志物在图像平面上的坐标，确定出其在摄像机坐标系下的三维空间位置，也就是从二维到三维的姿态估计。本发明使用PnP(Perspective-n-Point)问题的求解方法来解决这一姿态估计问题。

PnP问题又称作给定点的姿态估计问题。其定义为：在目标坐标系中，给定一系列点的坐标及其在图像平面上的投影，并假定摄像机的内参数已知，求取目标坐标系与摄像机坐标系之间的变换矩阵，即包含三个旋转参数和三个平移参数的摄像机外参数矩阵。本发明中n的值取3(即P3P)，用三个点完成问题的求解。

步骤2.2.1：如附图11所示，对于识别出的标志物，取其轮廓上不共线的三点A`、B`、C`的图像坐标(u<sub>i</sub>,v<sub>i</sub>)，分别算出它们在机器人图像采集装置成像平面上的成像点坐标，i＝A`、B`、C`：

$\left[\begin{array}{c}{x}_{c_i}\\ y_{c_i}\\ 1\end{array}\right]=M^{-1}\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]$

其中，M为机器人图像采集装置的内参数矩阵，得到了点A、B、C在机器人图像采集装置坐标系中投影方向上的单位向量e_i，其中点A、B、C为点A`、B`、C`对应的现实世界中的点：

$e_i=\frac{1}{\sqrt{x_{c_i}^2+y_{c_i}^2+1}}\left[\begin{array}{c}{x}_{c_i}\\ y_{c_i}\\ 1\end{array}\right]$

e_A'、e_B'、e_C'分别为点A、B、C在机器人图像采集装置坐标系中投影方向上的单位向量；

步骤2.2.2：根据以下公式：

$\left\{\begin{array}{c}{D}_{\mathrm{AB}}=\mathrm{\&lambda;}\sqrt{{(x_{c_{A^{\text{\&prime;}}}}-x_{c_{B^{\&prime;}}})}^2+{(y_{c_{A^{\&prime;}}}-y_{c_{B^{\&prime;}}})}^2}\\ D_{\mathrm{BC}}=\mathrm{\&lambda;}\sqrt{{(x_{c_{B^{\text{\&prime;}}}}-x_{c_{C^{\&prime;}}})}^2+{(y_{c_{B^{\&prime;}}}-y_{c_{C^{\&prime;}}})}^2}\\ D_{\mathrm{AC}}=\mathrm{\&lambda;}\sqrt{{(x_{c_{A^{\text{\&prime;}}}}-x_{c_{C^{\&prime;}}})}^2+{(y_{c_{A^{\&prime;}}}-y_{c_{C^{\&prime;}}})}^2}\end{array}\right.\left\{\begin{array}{c}\cos {\mathrm{\&theta;}}_{\mathrm{AB}}=e_{B^{\&prime;}}^T{e}_{C^{\&prime;}}\\ \cos {\mathrm{\&theta;}}_{\mathrm{AC}}=e_{A^{\&prime;}}^T{e}_{C^{\&prime;}}\\ \cos {\mathrm{\&theta;}}_{\mathrm{BC}}=e_{A^{\&prime;}}^T{e}_{B^{\&prime;}}\end{array}\right.$

计算空间点A、B、C间的距离D_AB，D_BC，D_AC以及相应角度余弦值cosθ_AB，cosθ_AC，cosθ_BC；

其中λ为比例系数，λ＝1/L，L为标志物中单个小方格的边长；并根据方程

$\left\{\begin{array}{c}{d}_A^2+d_B^2-2d_A{d}_B\cos {\mathrm{\&theta;}}_{\mathrm{AB}}=D_{\mathrm{AB}}^2\\ d_A^2+d_C^2-2d_A{d}_C\cos {\mathrm{\&theta;}}_{\mathrm{AC}}=D_{\mathrm{AC}}^2\\ d_B^2+d_C^2-2d_B{d}_C\cos {\mathrm{\&theta;}}_{\mathrm{BC}}=D_{\mathrm{BC}}^2\end{array}\right.$

得到机器人图像采集装置光心到点A、B、C的距离d_A，d_B，d_c；并由公式

$P_j=d_j{e}_j=\left[\begin{array}{c}{x}_j\\ y_j\\ z_j\end{array}\right]$

得到点A、B、C在机器人图像采集装置坐标系下的坐标P_j，j＝A,B,C；e_A＝e_A'，e_B＝e_B'，e_C＝e_C'；

步骤2.2.3：在点A、B、C中任意取一点，进行如下运算：

其中，D_j为机器人与标志物的距离，为标志物与机器人前进方向的夹角；x_j、z_j为点A、B、C中任意取的一点的坐标值；η为观测误差；z_k为第k时刻的测量值；

根据附图3的几何关系，可以得到：

其中，分别为第k时刻机器人与观测到的标志物的相对距离和偏角。

步骤2.3：根据步骤2.2得到的第k时刻的测量值，在EKF-SLAM模型的测量更新方程中

$\left\{\begin{array}{c}{K}_k={P_k}^{-}{H}^T{({{\mathrm{HP}}_k}^{-}{H}^T+R)}^{-1}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\\ P_k=(I-K_{k}H){P_k}^{-}\end{array}\right.$

对步骤1得到的第k时刻状态估计的先验值以及第k时刻协方差的先验值P_k ^-进行修正，得到第k时刻机器人状态估计的后验值和协方差的后验值P_k；其中H表示机器人观测模型对的雅克比矩阵；R为测量误差；

在EKF-SLAM模型中，测量更新方程负责反馈，将先验估计和新的测量变量结合以构造改进的后验估计，测量更新方程由对机载传感器和环境相对位置的几何关系给出，输入为机器视觉测量方法得到的测量值，输出为修正后的机器人位置状态信息。

步骤2.4：地图扩建：

每次观测到标志物后，且修正位置坐标后，根据测量得到的相对位置关系计算出此时标志物在环境中的位置坐标(该坐标与机器人位置坐标使用同一坐标系)，记录这个坐标值。这样就实现了机器人对当前环境的地图构建。随着机器人不断运动，观测到的一系列标志物的坐标集合就得到了整个机器人探索区域的环境地图。其中，新观测的标志物坐标为：

其中，x,y由状态向量x_k＝(x,y,θ)^T分量获得。通过ID号判断标志物是否曾经被观测到，对于属于重复观测的标志物不进行位置计算与保存。被记录的坐标将作为环境地图中的一个点。当机器人记录下运动中观测到的所有坐标点后，就得到了完整的地图。

考虑到计算机图像方法受光照影响较大，在光照条件发生剧烈变化时(如机器人在走廊拐弯时，两侧的照明充足的走廊与阴暗的拐角的环境变化)，摄像机在短时间内不能识别出环境中的标志物，会丢失图像中的标志物，无法得到测量更新方程中的z_k。因此本发明还使用了理想雅克比模型进行测量更新。具体过程为：在对摄像头获取的图像处理提取不到标志物时，测量数据变为此刻机器人前进方向与坐标系的夹角；由EKF-SLAM的测量更新过程，使用理想雅克比模型及测量数据对第一步中机器人位置坐标的估值进行修正。其余步骤不变。

理想雅克比模型：

在理想雅克比模型中，机器人不对环境探索，而是对自身的实际前进方向，也就是状态向量x_k＝(x,y,θ)^T中的θ进行观测(见附图2)。此时的观测数据由高精度的陀螺仪给出。这时，测量值变为：z_k＝θ_k+ξ，其中，ξ为观测噪声。由于陀螺仪是惯性传感器，因此其系统误差会随时间逐渐累积，并且难以修正。当误差累积到一定程度时将，计算值与实际值将出现严重偏差。

基于理想雅克比模型的测量更新：

为了减少累积误差的影响，我们仅在当机器人受环境光照影响太大，以致无法从摄像头获取正常图像的短暂时间内使用理想雅克比模型。

与基于标志物识别的观测更新不同的是，此时的测量更新方程也使用z_k＝θ_k+ξ。也就是说，机器人使用陀螺仪测得θ_k值，得到观测值z_k，然后在测量更新方程中仅对机器人前进方向的先验值进行修正。

由于z_k＝θ_k+ξ是线性方程，并且其他值并没有被改变，因而此时的计算结果可以与之前的数据匹配，绘制的运动轨迹和地图也是连续的，没有跳变出现。

考虑到摄像机往往不用花费太长时间就能适应环境的光照条件，可以知道，在较短的时间内，理想雅克比模型不会引起机器人位置的较大偏差(无跳变)，而且可以是的由惯性原件引入的累积误差被控制在一个很小的范围。这样我们就提高了机器人对环境变化的适应性。

本发明效果可以通过以下实验进一步说明：

实验条件：实验模拟了机器人在室内环境下的同步定位与地图构建。室内环境选择了建筑物中常见的长走廊。实验中，在走廊两侧墙壁放置标志物，机器人沿走廊向前移动，通过摄像头拍摄的图像观测标志物，实时构建环境地图以及定位。

实验结果：我们通过考察机器人实时构建的地图能否准确反映环境的特征来评价SLAM方法的优劣。表1中的实验数据为构建的地图中各个标志物的位置坐标与其实际位置坐标偏差距离的平均值(单位为米)，‘RBS’代表使用激光或声纳传感器观测数据的EKF-SLAM方法的数据，‘本发明’是本发明的方法的数据。

表1.机器人同步定位与地图构建实验数据表

表1中，最小的平均偏差距离加粗表示。由实验数据可以看到，本发明采用的基于标志物识别和理想雅克比共同观测的机器人同步定位与地图构建方法在结果上比传统方法具有更高的精度。特别是在走廊“T”形路口的实验中，由于实验环境的墙壁在通往两侧方向过道上有延伸，可通行空间变窄，使用RBS方法的机器人因而未能识别到两侧的通道，构建的地图错误，而本发明的方法却完全没有受到这一环境特点的影响。

Claims (1)

1.一种基于图像标志物识别的移动机器人SLAM方法，其特征在于：采用以下步骤：

步骤1：机器人开始运动后，由实际中机器人对应的数学模型和运动的初始条件，机器人使用上一时刻的状态信息，根据EKF-SLAM模型的时间更新方程对此时的状态进行预测估计，完成先验估计的构造，同时构建地图：

由机器人运动过程第k-1时刻的状态信息按照时间更新方程：

推算第k时刻状态估计的先验值其中为第k时刻协方差的先验值，u_k-1为第k-1时刻对机器人施加的控制信号，A、B分别表示机器人运动模型对u_k-1的雅克比矩阵；Q为先验估计误差；P_k-1为第k-1时刻协方差的后验值；机器人运动过程的状态信息由机器人的位置和姿态信息组成；

地图由m＝(m₁,m₂，…)^T表示，由到第k时刻为止机器人存储的所有标志物坐标构成；

步骤2：机器人实时拍摄环境，使用计算机视觉方法的处理拍摄到的图像，找到标志物在图像中的位置，然后用机器视觉测量方法计算出机器人与标志物的相对位置关系；得到相对位置关系作为EKF-SLAM模型的测量更新方程的输入，根据测量更新方程对机器人位置状态的先验估计进行修正：

步骤2.1：识别标志物：所述标志物由7x7的黑色和白色小方格组成，最外围的小方格全为黑色，内部的5x5的小方格中，每一行均包括3个奇偶校验位和2个数据位，其中第2位和第4位为数据位；奇偶校验位确定出标志物的方向，数据位表示标志物唯一的ID号；

步骤2.1.1：将机器人拍摄的图像转化至灰度空间；

步骤2.1.2：对已转化至灰度空间的图像进行二值化操作；

步骤2.1.3：在二值化的图像中检测轮廓线，并查找候选的标志物对象；

步骤2.1.4：对候选标志物进行检测并确定标志物的ID号；

步骤2.2：估计标志物相对于机器人的位置和姿态：

步骤2.2.1：对于识别出的标志物，取其轮廓上不共线的三点A`、B`、C`的图像坐标(u<sub>i</sub>,v<sub>i</sub>)，分别算出它们在机器人图像采集装置成像平面上的成像点坐标 i＝A`、B`、C`：

其中，M为机器人图像采集装置的内参数矩阵，得到了点A、B、C在机器人图像采集装置坐标系中投影方向上的单位向量e_i，其中点A、B、C为点A`、B`、C`对应的现实世界中的点：

e_A'、e_B'、e_C'分别为点A、B、C在机器人图像采集装置坐标系中投影方向上的单位向量；

步骤2.2.2：根据以下公式：

计算空间点A、B、C间的距离D_AB，D_BC，D_AC以及相应角度余弦值cosθ_AB，cosθ_AC，cosθ_BC；其中λ为比例系数，λ＝1/L，L为标志物中单个小方格的边长；并根据方程

得到机器人图像采集装置光心到点A、B、C的距离d_A，d_B，d_c；并由公式

得到点A、B、C在机器人图像采集装置坐标系下的坐标P_j，j＝A,B,C；e_A＝e_A'，e_B＝e_B'，e_C＝e_C'；

步骤2.2.3：在点A、B、C中任意取一点，进行如下运算：

其中，D_j为机器人与标志物的距离，为标志物与机器人前进方向的夹角；x_j、z_j为点A、B、C中任意取的一点的坐标值；η为观测误差；z_k为第k时刻的测量值；

步骤2.3：根据步骤2.2得到的第k时刻的测量值，在EKF-SLAM模型的测量更新方程中

对步骤1得到的第k时刻状态估计的先验值以及第k时刻协方差的先验值进行修正，得到第k时刻机器人状态估计的后验值和协方差的后验值P_k；其中H表示机器人观测模型对的雅克比矩阵；R为测量误差；

步骤2.4：地图扩建：

每次观测到标志物后，机器人会将该标志物的位置扩充到构建的地图中。