[发明专利]一种基于图像标志物识别的移动机器人SLAM方法

摘要

本发明提出一种基于图像标志物识别的移动机器人SLAM方法，采用EKF-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：地图扩建：每次观测到标志物后，机器人会将该标志物的位置扩充到构建的地图中。