一種基于圖像標(biāo)志物識(shí)別的移動(dòng)機(jī)器人slam方法
【專利摘要】本發(fā)明提出一種基于圖像標(biāo)志物識(shí)別的移動(dòng)機(jī)器人SLAM方法，采用EKF-SLAM模型進(jìn)行機(jī)器人狀態(tài)參數(shù)的時(shí)間更新和基于標(biāo)志物識(shí)別的測(cè)量更新。本方法可以從環(huán)境中獲取大量信息，從而使機(jī)器人能夠構(gòu)建出高精度的環(huán)境地圖。與現(xiàn)有方法相比具有對(duì)環(huán)境空間結(jié)構(gòu)理解更準(zhǔn)確，抗干擾能力強(qiáng)，構(gòu)建的地圖精度高的特點(diǎn)。
【專利說(shuō)明】-種基于圖像標(biāo)志物識(shí)別的移動(dòng)機(jī)器人SLAM方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人學(xué)領(lǐng)域，具體地說(shuō)是用基于擴(kuò)展卡爾曼濾波器的同步定位與地 圖構(gòu)建（EKF-SLAM)方法結(jié)合計(jì)算機(jī)圖像技術(shù)實(shí)現(xiàn)機(jī)器人對(duì)環(huán)境的探索，并構(gòu)建環(huán)境地圖。

【背景技術(shù)】
[0002] 同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping,簡(jiǎn)稱 SLAM)這 一概念最早由Smith、Self和Cheeseman于1988年提出。這種方法描述了機(jī)器人從未知環(huán) 境的未知地點(diǎn)出發(fā)，然后對(duì)未知環(huán)境進(jìn)行探索的情境：機(jī)器人在運(yùn)動(dòng)過程中重復(fù)地觀測(cè)環(huán) 境，然后根據(jù)傳感器感知的環(huán)境特征定位自身位置和姿態(tài)，再根據(jù)自身位置增量式的構(gòu)建 地圖。這就提出了 SLAM包含的兩個(gè)目標(biāo)：同時(shí)定位和地圖構(gòu)建。其中地圖構(gòu)建是把從傳感 器收集到的一系列信息，集成到一個(gè)一致性的模型（即地圖）上，得到的地圖被用來(lái)描述當(dāng) 前環(huán)境以及給機(jī)器人定位；而定位則是在地圖上估測(cè)機(jī)器人的坐標(biāo)和姿勢(shì)形態(tài)。
[0003] 實(shí)際中的SLAM應(yīng)用一般有兩種典型情境：追蹤--這種情況下通常機(jī)器人的初 始位置已知；全局定位--這種情況下通常只給出很少，甚至不給出有關(guān)于機(jī)器人起始位 置或者環(huán)境特征的先驗(yàn)信息。因此，SLAM的核心被定義為一個(gè)關(guān)于機(jī)器人運(yùn)動(dòng)路徑和所處 環(huán)境特征的后驗(yàn)估計(jì)。要解決SLAM問題，就必須建立適當(dāng)?shù)哪Ｐ停獬鲞@個(gè)后驗(yàn)概率。
[0004] 近十幾年來(lái)，機(jī)器人學(xué)界涌現(xiàn)出了各種各樣的針對(duì)SLAM問題的解決方案。其中， 基于擴(kuò)展卡爾曼濾波器（EKF)的EKF-SLAM方法獲得了廣泛的認(rèn)可并被普遍應(yīng)用。這種方 法首先估計(jì)運(yùn)動(dòng)過程中某一時(shí)刻的狀態(tài)，然后以（含噪聲的）測(cè)量變量的方式獲得反饋，最 后根據(jù)反饋修正估計(jì)值。這樣，EKF-SLAM就能在不需了解機(jī)器人詳細(xì)性質(zhì)的前提下，高效 地對(duì)運(yùn)動(dòng)的過去、當(dāng)前、甚至將來(lái)的狀態(tài)進(jìn)行估計(jì)。
[0005] 在目前國(guó)內(nèi)夕卜公開的文獻(xiàn)中，Durrant-Whyte H, Bailey T. Simultaneous localization and mapping:part I[J]. Robotics&Automation Magazine, IEEE, 2006, 13(2) :99-110.討論了 EKF-SLAM的模型建立。該模型基于對(duì)機(jī)器人 運(yùn)動(dòng)進(jìn)行描述，然后用EKF的狀態(tài)更新及測(cè)量更新對(duì)機(jī)器人運(yùn)動(dòng)進(jìn)行重復(fù)估計(jì)，從而得到 SLAM問題的解。該模型不關(guān)注機(jī)器人機(jī)體特性，具有很強(qiáng)的平臺(tái)遷移性。ThrUnS，BUrgard W，F(xiàn)ox D. Probabilistic robotics[M].Cambridge:MIT press, 2006,309-336, 149-187.詳 細(xì)討論了將激光傳感器、超聲波傳感器在SLAM中的應(yīng)用。這種應(yīng)用將傳感器的測(cè)量值用作 EKF-SLAM中測(cè)量方程的輸入，用真實(shí)的環(huán)境特征修正狀態(tài)方程的估計(jì)。同時(shí)，文章還詳細(xì)分 析了這類傳感器的原理，討論了傳感器測(cè)量值與實(shí)際值的偏差特性，并且在隱馬爾科夫鏈 的基礎(chǔ)上，提出了一個(gè)概率密度函數(shù)來(lái)衡量測(cè)量值準(zhǔn)確度。這種應(yīng)用方法對(duì)傳感器的特點(diǎn) 的深入分析使其具有很強(qiáng)的實(shí)用性，因此其模擬仿真和實(shí)驗(yàn)中對(duì)幾種常見的環(huán)境噪聲都表 現(xiàn)出了較強(qiáng)的魯棒性，能得到令人滿意的結(jié)果。
[0006] 但上述的EKF-SLAM方法有幾點(diǎn)不足：
[0007] (1)機(jī)器人只能識(shí)別局部空間障礙物的有無(wú)，并不能感知環(huán)境的空間結(jié)構(gòu)。如果環(huán) 境結(jié)構(gòu)高度對(duì)稱（常見于人工建筑，如走廊、大廳等），則機(jī)器人會(huì)受這一對(duì)稱性的影響，使 運(yùn)動(dòng)范圍局限于環(huán)境中極小的空間內(nèi)，無(wú)法構(gòu)建全局地圖，SLAM結(jié)果出錯(cuò)。
[0008] (2)測(cè)量方式單一。上述方法中機(jī)器人僅以一種類型的測(cè)量數(shù)據(jù)進(jìn)行測(cè)量更新，盡 管對(duì)常見的幾種噪聲進(jìn)行了建模，但當(dāng)環(huán)境中出現(xiàn)未知干擾時(shí)，測(cè)量值的錯(cuò)誤依然很難被 修正。考慮到EKF-SLAM方法完全依賴測(cè)量值對(duì)估計(jì)進(jìn)行修正，測(cè)量值的錯(cuò)誤會(huì)給計(jì)算結(jié)果 帶來(lái)嚴(yán)重影響。


【發(fā)明內(nèi)容】

[0009] 近年來(lái)，隨著計(jì)算機(jī)技術(shù)、數(shù)字圖像處理技術(shù)及圖像處理硬件的進(jìn)一步發(fā)展，計(jì)算 機(jī)視覺在機(jī)器人領(lǐng)域中開始得到廣泛關(guān)注。相比其他方法，基于計(jì)算機(jī)視覺的方法能夠在 同等條件下獲取環(huán)境中的更大量的信息，同時(shí)還具備了精度高、不受電子干擾等優(yōu)點(diǎn)。同 時(shí)，開源的OpenCV (Open Computer Vision)函數(shù)庫(kù)讓主流的計(jì)算機(jī)視覺方法能被有效地實(shí) 現(xiàn)。這樣，如果在機(jī)器人上裝載攝像頭來(lái)拍攝環(huán)境，就可以捕獲豐富的環(huán)境特征，從而機(jī)器 人感知能力有限給SLAM帶來(lái)的問題。
[0010] 本發(fā)明的技術(shù)方案為：
[0011] 所述一種基于圖像標(biāo)志物識(shí)別的移動(dòng)機(jī)器人SLAM方法，其特征在于：采用以下步 驟：
[0012] 步驟1 :機(jī)器人開始運(yùn)動(dòng)后，由實(shí)際中機(jī)器人對(duì)應(yīng)的數(shù)學(xué)模型和運(yùn)動(dòng)的初始條件， 機(jī)器人使用上一時(shí)刻的狀態(tài)信息，根據(jù)EKF-SLAM模型的時(shí)間更新方程對(duì)此時(shí)的狀態(tài)進(jìn)行 預(yù)測(cè)估計(jì)，完成先驗(yàn)估計(jì)的構(gòu)造，同時(shí)構(gòu)建地圖：
[0013] 由機(jī)器人運(yùn)動(dòng)過程第k-Ι時(shí)刻的狀態(tài)信息，按照時(shí)間更新方程：
[0014]

【權(quán)利要求】
1. 一種基于圖像標(biāo)志物識(shí)別的移動(dòng)機(jī)器人SLAM方法，其特征在于：采用以下步驟： 步驟1 :機(jī)器人開始運(yùn)動(dòng)后，由實(shí)際中機(jī)器人對(duì)應(yīng)的數(shù)學(xué)模型和運(yùn)動(dòng)的初始條件，機(jī)器 人使用上一時(shí)刻的狀態(tài)信息，根據(jù)EKF-SLAM模型的時(shí)間更新方程對(duì)此時(shí)的狀態(tài)進(jìn)行預(yù)測(cè) 估計(jì)，完成先驗(yàn)估計(jì)的構(gòu)造，同時(shí)構(gòu)建地圖： 由機(jī)器人運(yùn)動(dòng)過程第k-Ι時(shí)刻的狀態(tài)信息，按照時(shí)間更新方程：
推算第k時(shí)刻狀態(tài)估計(jì)的先驗(yàn)值巧，其中P1:為第k時(shí)刻協(xié)方差的先驗(yàn)值，Uh為第k-1 時(shí)刻對(duì)機(jī)器人施加的控制信號(hào)，A、B分別表不機(jī)器人運(yùn)動(dòng)模型對(duì)毛、Ug的雅克比矩陣；Q為 先驗(yàn)估計(jì)誤差；Ph為第k-Ι時(shí)刻協(xié)方差的后驗(yàn)值；機(jī)器人運(yùn)動(dòng)過程的狀態(tài)信息由機(jī)器人的 位置和姿態(tài)信息組成； 地圖由m = (mu m2,...）τ表示，由到第k時(shí)刻為止機(jī)器人存儲(chǔ)的所有標(biāo)識(shí)物坐標(biāo)構(gòu)成； 步驟2 :機(jī)器人實(shí)時(shí)拍攝環(huán)境，使用計(jì)算機(jī)視覺方法的處理拍攝到的圖像，找到標(biāo)志物 在圖像中的位置，然后用機(jī)器視覺測(cè)量方法計(jì)算出機(jī)器人與標(biāo)志物的相對(duì)位置關(guān)系；得到 相對(duì)位置關(guān)系作為EKF-SLAM模型的測(cè)量更新方程的輸入，根據(jù)測(cè)量更新方程對(duì)機(jī)器人位 置狀態(tài)的先驗(yàn)估計(jì)進(jìn)行修正： 步驟2. 1 :識(shí)別標(biāo)志物：所述標(biāo)志物由7x7的黑色和白色小方格組成，最外圍的小方格 全為黑色，內(nèi)部的5x5的小方格中，每一行均包括3個(gè)奇偶校驗(yàn)位和2個(gè)數(shù)據(jù)位，其中第2 位和第4位為數(shù)據(jù)位；奇偶校驗(yàn)位確定出標(biāo)志物的方向，數(shù)據(jù)位表示標(biāo)志物唯一的ID號(hào)； 步驟2. 1. 1 :將機(jī)器人拍攝的圖像轉(zhuǎn)化至灰度空間； 步驟2. 1. 2 :對(duì)已轉(zhuǎn)化至灰度空間的圖像進(jìn)行二值化操作； 步驟2. 1. 3 :在二值化的圖像中檢測(cè)輪廓線，并查找候選的標(biāo)志物對(duì)象； 步驟2. 1. 4 :對(duì)候選標(biāo)志物進(jìn)行檢測(cè)并確定標(biāo)志物的ID號(hào)； 步驟2. 2 :估計(jì)標(biāo)志物相對(duì)于機(jī)器人的位置和姿態(tài)： 步驟2. 2. 1 :對(duì)于識(shí)別出的標(biāo)志物，取其輪廓上不共線的三點(diǎn)A'、B'、C'的圖像坐標(biāo) (? Vi)，分別算出它們?cè)跈C(jī)器人圖像采集裝置成像平面上的成像點(diǎn)坐標(biāo)(UV )，i = A'、 B'、C' :
其中，Μ為機(jī)器人圖像采集裝置的內(nèi)參數(shù)矩陣，得到了點(diǎn)A、B、C在機(jī)器人圖像采集裝置 坐標(biāo)系中投影方向上的單位向量ei，其中點(diǎn)A、B、C為點(diǎn)A'、B'、C'對(duì)應(yīng)的現(xiàn)實(shí)世界中的點(diǎn)：
eA'、eB'、ec'分別為點(diǎn)A、B、C在機(jī)器人圖像采集裝置坐標(biāo)系中投影方向上的單位向量； 步驟2. 2. 2:根據(jù)以下公式：
計(jì)算空間點(diǎn)A、B、C間的距離DAB,DBC,DAC；以及相應(yīng)角度余弦值cos Θ ^cos Θ AC，cos Θ BC ; 其中λ為比例系數(shù)，λ = 1/L，L為標(biāo)志物中單個(gè)小方格的邊長(zhǎng)；并根據(jù)方程
得到機(jī)器人圖像采集裝置光心到點(diǎn)A、B、C的距離dA，dB，d。；并由公式
得到點(diǎn)A、B、C在機(jī)器人圖像采集裝置坐標(biāo)系下的坐標(biāo)Ρ」，j = A, B, C ;eA = eA，，eB = eB" eC = eC，； 步驟2. 2. 3 :在點(diǎn)A、B、C中任意取一點(diǎn)，進(jìn)行如下運(yùn)算：
其中，h為機(jī)器人與標(biāo)志物的距離，％為標(biāo)志物與機(jī)器人前進(jìn)方向的夾角；Xp 為點(diǎn) A、B、C中任意取的一點(diǎn)的坐標(biāo)值；η為觀測(cè)誤差；zk為第k時(shí)刻的測(cè)量值； 步驟2. 3 :根據(jù)步驟2. 2得到的第k時(shí)刻的測(cè)量值，在EKF-SLAM模型的測(cè)量更新方程 中
對(duì)步驟1得到的第k時(shí)刻狀態(tài)估計(jì)的先驗(yàn)值&以及第k時(shí)刻協(xié)方差的先驗(yàn)值p1:進(jìn)行 修正，得到第k時(shí)刻機(jī)器人狀態(tài)估計(jì)的后驗(yàn)值Λ和協(xié)方差的后驗(yàn)值Pk ;其中Η表示機(jī)器人觀 測(cè)模型對(duì)4的雅克比矩陣；R為測(cè)量誤差； 步驟2. 4 :地圖擴(kuò)建： 每次觀測(cè)到標(biāo)志物后，機(jī)器人會(huì)將該標(biāo)志物的位置擴(kuò)充到構(gòu)建的地圖中。
2.根據(jù)權(quán)利要求1所述一種基于圖像標(biāo)志物識(shí)別的移動(dòng)機(jī)器人SLAM方法，其特征在
【文檔編號(hào)】G05D1/02GK104062973SQ201410283032
【公開日】2014年9月24日 申請(qǐng)日期:2014年6月23日 優(yōu)先權(quán)日:2014年6月23日
【發(fā)明者】布樹輝, 何畢, 劉貞報(bào) 申請(qǐng)人:西北工業(yè)大學(xué)