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

权利要求书

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

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

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

x^k-=Ax^k-1+Buk-1Pk-=APk-1AT+Q]]>

推算第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`：

xciyci1=M-1uivi1]]>

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

ei=1xci2+yci2+1xciyci1]]>

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

步骤2.2.2：根据以下公式：

DAB=λ(xcA′-xcB′)2+(ycA′-ycB′)2DBC=λ(xcB′-xcC′)2+(ycB′-ycC′)2DAC=λ(xcA′-xcC′)2+(ycA′-ycC′)2cosθAB=eB′TeC′cosθAC=eA′TeC′cosθBC=eA′TeB′]]>

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

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

dA2+dB2-2dAdBcosθAB=DAB2dA2+dC2-2dAdCcosθAC=DAC2dB2+dC2-2dBdCcosθBC=DBC2]]>

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

Pj=djej=xjyjzj]]>

得到点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模型的测量更新方程中

Kk=Pk-HT(HPk-HT+R)-1x^k=x^k-+Kk(zk-Hx^k-)Pk=(I-KkH)Pk-]]>

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

步骤2.4：地图扩建：

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

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