[發明專利]一種基于圖像標志物識別的移動機器人SLAM方法

有效

• 申請號: 201410283032.7
• 申請日: 2014-06-23
• 公開（公告）號: CN104062973A
• 公開（公告）日: 2014-09-24
• 發明（設計）人: 布樹輝;何畢;劉貞報
• 申請（專利權）人: 西北工業大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 陳星

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中，無法識別出環境中的標志物時，采用理想雅克比模型進行測量更新。