移動(dòng)機(jī)器人的位姿確定方法和設(shè)備的制造方法
【專利摘要】本發(fā)明的實(shí)施方式涉及移動(dòng)機(jī)器人的位姿確定方法和設(shè)備。該方法包括：使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的全局地圖；在移動(dòng)機(jī)器人上電時(shí)，使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建針對(duì)所述移動(dòng)機(jī)器人的上電時(shí)刻的環(huán)境的局部地圖；以及將局部地圖在述全局地圖中進(jìn)行圖像匹配，獲取移動(dòng)機(jī)器人在全局地圖中的初始位姿。根據(jù)本發(fā)明的實(shí)施方式，可以獲取移動(dòng)機(jī)器人上電啟動(dòng)時(shí)在全局地圖中的絕對(duì)位姿，實(shí)現(xiàn)移動(dòng)機(jī)器人位姿初始化。進(jìn)一步地，可以對(duì)確定的位姿進(jìn)行校正。
【專利說明】
移動(dòng)機(jī)器人的位姿確定方法和設(shè)備
技術(shù)領(lǐng)域
[0001] 本發(fā)明的實(shí)施方式涉及移動(dòng)機(jī)器人導(dǎo)航中的位姿確定方法，特別是一種基于激光 掃描儀的位姿初始化方法，以及位姿校正方法。
【背景技術(shù)】
[0002] 目前所有導(dǎo)航策略無論是室外還是室內(nèi)導(dǎo)航，都必須解決移動(dòng)機(jī)器人上電啟動(dòng)時(shí) 位姿初始化問題，初始位姿的準(zhǔn)確性影響后續(xù)整個(gè)導(dǎo)航的準(zhǔn)確性和穩(wěn)定性。由于目前全球 定位系統(tǒng)(GPS)的成熟，在室外可時(shí)刻獲得基于地球坐標(biāo)系的絕對(duì)位姿，故上電位姿初始化 在室外已經(jīng)得到很好解決。但在室內(nèi)導(dǎo)航領(lǐng)域，目前仍不成熟。
[0003]慣性導(dǎo)航定位技術(shù)和激光導(dǎo)航定位技術(shù)是目前較為成熟的幾種室內(nèi)導(dǎo)航定位手 段中的兩種手段。慣性導(dǎo)航定位精度高，但是存在累計(jì)誤差，上電初始時(shí)刻，需配合外部其 他設(shè)備提供地圖和初始位姿。基于激光掃描儀的即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)可在新環(huán) 境下同時(shí)建圖和定位，第一次創(chuàng)建地圖時(shí)，初始位姿可為固定值或由其他設(shè)備提供，但是在 完成創(chuàng)建地圖后，上電初始時(shí)刻無法確定移動(dòng)機(jī)器人在已有地圖中的初始位姿。對(duì)于此類 的導(dǎo)航定位技術(shù)，移動(dòng)機(jī)器人上電初期精準(zhǔn)的初始位姿定位是首要問題。

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

[0004] 有鑒于此，本發(fā)明實(shí)施方式的所要解決的技術(shù)問題之一是為移動(dòng)機(jī)器人確定初始 位姿，即移動(dòng)機(jī)器人的位姿初始化。
[0005] 根據(jù)本發(fā)明的一個(gè)方面，提供一種移動(dòng)機(jī)器人的位姿初始化方法，所述移動(dòng)機(jī)器 人包括激光掃描儀。該方法可以包括：使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建 (SLAM)技術(shù)創(chuàng)建所述移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的全局地圖；在所述移動(dòng)機(jī)器人上電 時(shí)，使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建針對(duì)所述移動(dòng)機(jī)器人 的上電時(shí)刻的環(huán)境的局部地圖；以及將所述局部地圖在所述全局地圖中進(jìn)行圖像匹配，獲 取所述移動(dòng)機(jī)器人在全局地圖中的初始位姿。
[0006] 進(jìn)一步地，所述移動(dòng)機(jī)器人還可以包括慣性導(dǎo)航傳感器，所述方法還包括:在所述 移動(dòng)機(jī)器人的位姿發(fā)生改變時(shí)，利用所述慣性導(dǎo)航傳感器計(jì)算所述移動(dòng)機(jī)器人在所述全局 地圖中的第一位姿;使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建關(guān)于 所述移動(dòng)機(jī)器人的周圍環(huán)境的局部地圖；將所述局部地圖在全局地圖中進(jìn)行圖像匹配，獲 取所述移動(dòng)機(jī)器人在所述全局地圖中的第二位姿和與所述第二位姿對(duì)應(yīng)的匹配可信度，其 中所述第二位姿為圖形匹配得出的所述移動(dòng)機(jī)器人在所述全局地圖中的最佳位姿；以及根 據(jù)所述匹配可信度，確定所述移動(dòng)機(jī)器人的位姿為所述第一位姿或者所述第二位姿。
[0007] 進(jìn)一步地，所述匹配可信度為使用所述激光掃描儀在所述局部地圖中進(jìn)行激光掃 描獲得的障礙輪廓在所述全局地圖中的映射輪廓的坐標(biāo)的像素和、并且對(duì)所述像素和進(jìn)行 歸一化的結(jié)果。
[0008] 進(jìn)一步地，所述圖像匹配可以采用SIFT算法或模板匹配算法。
[0009] 進(jìn)一步地，在創(chuàng)建所述全局地圖和所述局部地圖時(shí)可以都依賴于所述移動(dòng)機(jī)器人 所包括的電子指南針的航向角來確定移動(dòng)機(jī)器人的初始位姿。
[0010] 進(jìn)一步地，所述方法還可以包括:在移動(dòng)機(jī)器人位姿初始化或者位姿發(fā)生改變時(shí)， 根據(jù)移動(dòng)機(jī)器人室內(nèi)定位技術(shù)估計(jì)移動(dòng)機(jī)器人的當(dāng)前位姿;訪問參考物數(shù)據(jù)庫，所述參考 物數(shù)據(jù)庫包括所述移動(dòng)機(jī)器人配備的攝像頭拍攝的、關(guān)于至少一個(gè)參考物中的每個(gè)參考物 的一幅參考圖像，以及與所述參考圖像關(guān)聯(lián)的、所述攝像頭拍攝所述參考圖像時(shí)所述移動(dòng) 機(jī)器人的相應(yīng)真實(shí)位姿，并且從所述參考物數(shù)據(jù)庫中選擇一個(gè)真實(shí)位姿和相關(guān)聯(lián)的參考圖 像，其中所述參考物是室內(nèi)環(huán)境中的靜止物體;將所述移動(dòng)機(jī)器人導(dǎo)航至目標(biāo)位姿，所述目 標(biāo)位姿的數(shù)值是根據(jù)移動(dòng)機(jī)器人室內(nèi)定位技術(shù)對(duì)于所選擇的真實(shí)位姿的估計(jì)值;利用所述 移動(dòng)機(jī)器人配備的所述攝像頭進(jìn)行圖像拍攝，獲得拍攝圖像;將所述拍攝圖像中包括的參 考物圖像、即實(shí)際參考物圖像與所選擇的相關(guān)聯(lián)的參考圖像中包括的參考物圖像、即數(shù)據(jù) 庫參考物圖像進(jìn)行對(duì)比;根據(jù)對(duì)比的結(jié)果來確定所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿 誤差；以及利用所確定的位姿誤差對(duì)根據(jù)所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)所估計(jì)的移動(dòng)機(jī)器 人的位姿進(jìn)行誤差校正。
[0011] 根據(jù)本發(fā)明的一個(gè)方面，提供一種移動(dòng)機(jī)器人的位姿初始化設(shè)備，所述移動(dòng)機(jī)器 人包括激光掃描儀。該設(shè)備可以包括:全局地圖創(chuàng)建裝置，用于使用所述激光掃描儀，利用 即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建所述移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的全局地圖；局部 地圖創(chuàng)建裝置，用于在所述移動(dòng)機(jī)器人上電時(shí)，使用所述激光掃描儀，利用即時(shí)定位與地圖 構(gòu)建(SLAM)技術(shù)創(chuàng)建針對(duì)所述移動(dòng)機(jī)器人的上電時(shí)刻的環(huán)境的局部地圖；以及初始位姿獲 取裝置，用于將所述局部地圖在所述全局地圖中進(jìn)行圖像匹配，獲取所述移動(dòng)機(jī)器人在全 局地圖中的初始位姿。
[0012] 根據(jù)本發(fā)明的一個(gè)方面，提供一種在計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中實(shí)現(xiàn)的計(jì)算機(jī)程序產(chǎn) 品，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)具有存儲(chǔ)于其中的計(jì)算機(jī)可讀程序代碼部分，所述計(jì)算機(jī)可 讀程序代碼部分被配置為執(zhí)行根據(jù)前述的方法。
[0013] 根據(jù)本發(fā)明實(shí)施方式的技術(shù)方案，可以獲取移動(dòng)機(jī)器人上電啟動(dòng)時(shí)在全局地圖中 的絕對(duì)位姿，實(shí)現(xiàn)移動(dòng)機(jī)器人位姿初始化?？梢宰龅綑C(jī)器人放在任意有效位置點(diǎn)，上電啟動(dòng) 時(shí)均可確定其準(zhǔn)確的初始位姿，即坐標(biāo)和航向角。進(jìn)一步可選地，能夠及時(shí)發(fā)現(xiàn)移動(dòng)機(jī)器人 在室內(nèi)導(dǎo)航過程中所產(chǎn)生的位姿誤差，從而在必要時(shí)可以對(duì)該位姿誤差進(jìn)行校正。
[0014] 從下文結(jié)合附圖所做出的詳細(xì)描述中，本發(fā)明的這些和其他優(yōu)點(diǎn)和特征將變得更 加明顯，其中在整個(gè)下文描述的若干附圖中，類似的元件將具有類似的編號(hào)。
【附圖說明】
[0015] 圖1圖示根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿初始化方法的流程圖；
[0016] 圖2圖示根據(jù)本發(fā)明一個(gè)實(shí)施例所創(chuàng)建的全局地圖；
[0017] 圖3圖示根據(jù)本發(fā)明一個(gè)實(shí)施例所創(chuàng)建的局部地圖；
[0018] 圖4圖示如圖3所示的局部地圖在如圖2所示的全局地圖中的匹配示意圖；
[0019] 圖5圖示根據(jù)本發(fā)明實(shí)施方式的一種模塊匹配方法的示意圖；
[0020] 圖6圖示如圖3所示的局部地圖中的最小有效地圖圖像的示意圖；
[0021 ]圖7圖示移動(dòng)機(jī)器人所包括的激光掃描儀的一種可能布置的示意圖；
[0022] 圖8圖示移動(dòng)機(jī)器人所包括的激光掃描儀的另一種可能布置的示意圖；
[0023] 圖9圖示根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿初始化設(shè)備的框圖；
[0024] 圖10圖示現(xiàn)有技術(shù)的室內(nèi)定位和導(dǎo)航技術(shù)的方法的流程圖；
[0025]圖11圖不了基于超寬帶(UWB)的定位和導(dǎo)航技術(shù)的不意圖；
[0026] 圖12圖示根據(jù)本發(fā)明的實(shí)施方式的用于移動(dòng)機(jī)器人定位的方法的流程圖；
[0027] 圖13圖示根據(jù)本發(fā)明的實(shí)施方式的用于移動(dòng)機(jī)器人定位和導(dǎo)航的方法的流程圖；
[0028] 圖14A、圖14B和圖14C圖示實(shí)際參考物圖像相對(duì)于數(shù)據(jù)庫參考物圖像所可能發(fā)生 變化的不意圖；
[0029] 圖15圖示實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像匹配的示意圖；
[0030] 圖16圖示了移動(dòng)機(jī)器人拍攝畫面中的數(shù)據(jù)庫參考物圖像和實(shí)際參考物圖像的可 能的偏差；
[0031] 圖17圖示根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿確定方法的流程圖；
[0032] 圖18圖示根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿確定方法的流程圖；
[0033] 圖19圖示根據(jù)本發(fā)明實(shí)施方式的一種激光導(dǎo)航測(cè)量原理結(jié)構(gòu)圖；以及 [0034]圖20圖示根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿確定方法的流程圖。
【具體實(shí)施方式】
[0035] 下文將參考附圖更完整地描述本公開內(nèi)容，其中在附圖中顯示了本公開內(nèi)容的實(shí) 施方式。但是這些實(shí)施方式可以用許多不同形式來實(shí)現(xiàn)并且不應(yīng)該被解釋為限于本文所述 的實(shí)施方式。相反地，提供這些實(shí)例以使得本公開內(nèi)容將是透徹和完整的，并且將全面地向 本領(lǐng)域的熟練技術(shù)人員表達(dá)本公開內(nèi)容的范圍。
[0036] 參考圖1，其圖示了根據(jù)本發(fā)明的實(shí)施方式的移動(dòng)機(jī)器人的位姿初始化方法100的 流程圖。移動(dòng)機(jī)器人包括激光掃描儀。
[0037] 在步驟S110,使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)(下文也稱為 激光SLAM技術(shù))創(chuàng)建移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的全局地圖。移動(dòng)機(jī)器人在導(dǎo)航環(huán)境中 的位姿包括坐標(biāo)和航向角，用(x，y，0)來表示。
[0038] 在一個(gè)未知環(huán)境(比如室內(nèi)環(huán)境)下，采用激光SLAM技術(shù)對(duì)當(dāng)前環(huán)境進(jìn)行第一次建 圖，可以設(shè)定地圖大小(地圖大小應(yīng)比實(shí)際的室內(nèi)范圍大)為2048*2048。建圖時(shí)移動(dòng)機(jī)器人 的假定初始位姿比如可固定為（1024,1024，0)。可以自主地導(dǎo)航移動(dòng)機(jī)器人使其走遍室內(nèi) 范圍，也可以人工引導(dǎo)機(jī)器人走遍整個(gè)室內(nèi)范圍，創(chuàng)建全局地圖。創(chuàng)建的地圖可以為柵格地 圖，如圖2所示，其示出了作為一個(gè)實(shí)施例的所創(chuàng)建的全局地圖，其中障礙的像素值為0,無 障礙的像素值為255,未觀測(cè)到的像素值為127?？梢詫?chuàng)建好的地圖進(jìn)行保存在移動(dòng)機(jī)器 人的存儲(chǔ)器中，或者移動(dòng)機(jī)器人外部的存儲(chǔ)設(shè)備中、在后續(xù)的步驟中移動(dòng)機(jī)器人可以與該 外部存儲(chǔ)設(shè)備中進(jìn)行無線或有線通信來獲取所保存的全局地圖。
[0039] 在建立了上述地圖的情況下，則此后移動(dòng)機(jī)器人的定位、導(dǎo)航、路徑規(guī)劃等均可以 以此地圖為全局地圖，定位點(diǎn)以此圖的坐標(biāo)為絕對(duì)位姿。
[0040] 在步驟S120,在移動(dòng)機(jī)器人上電時(shí)，使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建 (SLAM)技術(shù)創(chuàng)建針對(duì)移動(dòng)機(jī)器人的上電時(shí)刻的環(huán)境的局部地圖。
[0041] 在移動(dòng)機(jī)器人上電重新啟動(dòng)時(shí)，可以設(shè)置移動(dòng)機(jī)器人的假定初始位姿為（1024， 1024,0)，開啟激光SLAM建圖功能，使得移動(dòng)機(jī)器人在原位置自行旋轉(zhuǎn)一周，形成較大的、詳 細(xì)的、360度的局部地圖，如圖3所示，其示出了作為一個(gè)實(shí)施例的所創(chuàng)建的局部地圖。由于 上電時(shí)刻移動(dòng)機(jī)器人位姿可能為任意值，故匹配時(shí)需考慮360度范圍的航向角情況下在地 圖中的位置。應(yīng)當(dāng)理解，要?jiǎng)?chuàng)建360度的局部地圖，需要的移動(dòng)機(jī)器人旋轉(zhuǎn)的角度和移動(dòng)機(jī) 器人上布置的激光掃描儀數(shù)量以及激光掃描儀視野范圍有關(guān)。
[0042]在步驟S130,將局部地圖在全局地圖中進(jìn)行圖像匹配，獲取移動(dòng)機(jī)器人在全局地 圖中的初始位姿?？梢詫⒁苿?dòng)機(jī)器人上電重新啟動(dòng)時(shí)所創(chuàng)建的360度的局部地圖與全局地 圖采用圖像處理方法進(jìn)行圖像匹配，得到其在全局地圖中的位置和方向，如圖4所示，其示 出了如圖3所示的局部地圖在如圖2所示的全局地圖中的匹配示意圖。從而，根據(jù)圖像匹配 的結(jié)果，可以獲取移動(dòng)機(jī)器人的在全局地圖中的初始位姿，其為絕對(duì)位姿。
[0043] 激光SLAM所創(chuàng)建的地圖可以為二維柵格灰度圖像，可如圖2、圖3所示，地圖存在復(fù) 雜性、毛刺、粗糙、不工整等特點(diǎn)，尤其此類二維圖像的特征點(diǎn)不是十分明顯。同時(shí)在創(chuàng)建局 部底部的初始時(shí)刻，激光所測(cè)局部距離信息點(diǎn)為離散的、局部性強(qiáng)、特征點(diǎn)殘缺等特點(diǎn)，直 接與原始的全局地圖進(jìn)行匹配易出現(xiàn)匹配錯(cuò)誤或無法得到正確的匹配。進(jìn)一步地，為了實(shí) 現(xiàn)局部地圖與全局地圖的精準(zhǔn)匹配，可以采用的具體圖像匹配方式包括但不限于尺度不變 特征轉(zhuǎn)換（SIFT，Scale-invariant feature transform)算法和模板匹配算法，還可以使用 直方圖匹配、形狀匹配、蒙特卡洛等匹配算法。
[0044] SIFT算法是用于圖像匹配的經(jīng)典算法之一，用來偵測(cè)與描述影像中的局部性特 征，它在空間尺度中尋找極值點(diǎn)，并提取出其位置、尺度、旋轉(zhuǎn)不變量。此算法由David Lowe 在1999年所發(fā)表，2004年完善總結(jié)。根據(jù)本發(fā)明的實(shí)施方式，局部地圖和全局地圖采用完全 相同的SLAM算法繪制而成，從圖2、圖3、圖4可以看出，圖2所示局部地圖是圖1所示的全局地 圖中的一部分，但是位置和朝向隨機(jī)（由于全局地圖是掃描儀的n次掃描結(jié)果相疊加的結(jié) 果，而局部地圖是1次掃描的結(jié)果，所以肉眼未必能夠發(fā)現(xiàn)局部地圖在全局地圖中的位置和 朝向，但計(jì)算機(jī)視覺算法SIFT可以實(shí)現(xiàn)）。故圖像處理中SIFT算法非常適用于旋轉(zhuǎn)、平移的 兩幅圖像匹配，即本發(fā)明實(shí)施方式的場(chǎng)景。圖3相對(duì)于圖2進(jìn)行SIFT特征提取、并進(jìn)行匹配， 得出旋轉(zhuǎn)角度為9t、平移向量為(xt，yt)時(shí)為最佳匹配，如圖4所示。由于兩幅圖建圖時(shí)的移 動(dòng)機(jī)器人假定初始位姿設(shè)置為相同，比如均為（1024，1024,0)，而其中圖2作為絕對(duì)坐標(biāo)系， 圖3中的假定初始位姿（1024,1024,0)實(shí)際為圖2中當(dāng)前位姿，經(jīng)旋轉(zhuǎn)與平移即可得到在圖2 中的絕對(duì)坐標(biāo)。從而，獲取移動(dòng)機(jī)器人在圖2的全局地圖中的初始位姿POSE為：
[0045] POSE=(lO24，lO24,O) + (xt，yt，0t)
[0046] =(l〇24+xt，l〇24+yt，0t)。
[0047] 模板匹配也是用于圖像匹配的經(jīng)典算法之一，用于判斷數(shù)字圖像的模板是否匹 配，即模板與原圖像進(jìn)行比較，以確定在原圖像中是否存在與該模板相同或相似的區(qū)域，若 該區(qū)域存在，還可確定其位置，認(rèn)為模板與原圖像匹配。根據(jù)本發(fā)明的實(shí)施方式，局部地圖 和全局地圖采用完全相同的SLAM算法繪制而成，從圖2、圖3、圖4可以看出，圖2所示局部地 圖是圖1所示的全局地圖中的一部分，但是位置和朝向隨機(jī)。模板匹配常用的方法有平方差 匹配法、相關(guān)匹配法、相關(guān)系數(shù)匹配法、歸一化平方差法、歸一化相關(guān)匹配法模板等等。
[0048] 在一個(gè)實(shí)施例中，可以采用一種相關(guān)匹配法與歸一化相關(guān)匹配法相結(jié)合，匹配相 似度則為兩幅圖像的相關(guān)數(shù)值。相關(guān)數(shù)值越大，則越相似。匹配原理示意圖如圖5所示。圖5 從左到右依次圖示了將局部地圖旋轉(zhuǎn)n度，找出最小有效地圖，獲取偏移坐標(biāo)(x_off set_n， y_of f set_n);提取最小有效地圖，作為模板;獲得最佳匹配位置(local_x_n，local_y_n)。 參考圖5,一種示例性匹配算法的具體步驟可以包括:1.提取圖3中最小有效地圖圖像，如圖 6,即將圖3進(jìn)行裁剪后的地圖作為模板;2.裁剪后的地圖模板（圖6)與原局部地圖圖像（圖 3)的水平偏移為(xjffsetJdjffsetJ)』.采用歸一化模板匹配算法，找到圖2中與圖6 最佳匹配位置，獲得在圖2中圖6左上角的坐標(biāo)（l 〇Cal_X_l，l〇Cal_y_l)，同時(shí)獲得最佳匹配 位置的未歸一化相似度result」。4.將圖3順時(shí)針旋轉(zhuǎn)一度，重復(fù)前述步驟1、步驟2、步驟3， 直到遍歷完360度為止。5.獲得360個(gè)最佳匹配位置，即局部地圖遍歷360度，每一度均存在 一個(gè)最佳匹配位置，提取有效地圖的水平偏移為(x_off set_n，y_off set_n)，對(duì)應(yīng)的匹配位 置的模板的左上角坐標(biāo)為（l〇cal_x_n，local_y_n)和匹配相似度result_n，其中n表示旋轉(zhuǎn) 的角度。6.比較360個(gè)相似度result_n，找出其最大值(記為result_R)對(duì)應(yīng)的水平偏移(x_ offset_R，y_offset_R)和左上角坐標(biāo)(local_x_R，local_y_R)。其中則R表明當(dāng)航向角順時(shí) 針旋轉(zhuǎn)R度為最佳匹配角度，從而獲得最佳匹配位姿。7.圖3中（1024,1024,0)位姿為上電初 始位姿，則根據(jù)其在圖2中的映射關(guān)系可以獲取移動(dòng)機(jī)器人在圖2的全局地圖中的初始位姿 POSE 為：
[0049] P〇SE=( 1024,1024,0)-(x_ofTset_R，y_ofTset_R) + (0,0，R) + (l〇cal_x_R，local_ y_R,0)
[0050] =(1024-x_offset_R+local_x_R,1024-y_)〇
[0051] 在獲取了移動(dòng)機(jī)器人在全局地圖中的初始位姿后，便可利用慣性導(dǎo)航定位技術(shù)、 激光導(dǎo)航定位技術(shù)或者其他適當(dāng)?shù)膶?dǎo)航定位技術(shù)或它們的組合對(duì)移動(dòng)機(jī)器人在已有全局 地圖中進(jìn)行精確定位，實(shí)現(xiàn)自動(dòng)導(dǎo)航功能。
[0052]在一個(gè)實(shí)施例中，移動(dòng)機(jī)器人可包括2個(gè)激光雷達(dá)作為激光掃描儀，用于創(chuàng)建全局 地圖和360度局部地圖。圖7和圖8圖示移動(dòng)機(jī)器人所包括的激光掃描儀的可能的布置的示 意圖。如圖7所示，2個(gè)激光雷達(dá)分別安裝在移動(dòng)機(jī)器人的前方和后方，從而可直接觀測(cè)正前 方180度和正后方180度范圍的距離信息。從而，在創(chuàng)建360局部地圖時(shí)，無需移動(dòng)機(jī)器人進(jìn) 行360度的旋轉(zhuǎn)，只需旋轉(zhuǎn)90度。如圖8所示，2個(gè)激光雷達(dá)選用可視角為270度的激光雷達(dá)， 分別被安裝在移動(dòng)機(jī)器人的前方左角和后方右角，分別可以觀測(cè)正前方和左側(cè)以及正后方 和右側(cè)。從而，可直接觀測(cè)機(jī)器人周圍360度范圍距離信息，在創(chuàng)建360局部地圖時(shí)，無需移 動(dòng)機(jī)器人進(jìn)行旋轉(zhuǎn)。
[0053] 在一個(gè)實(shí)施例中，移動(dòng)機(jī)器人可包括電子指南針用于全局地圖和局部地圖的創(chuàng) 建。在未知環(huán)境下，采用激光SLAM技術(shù)對(duì)當(dāng)前環(huán)境進(jìn)行第一次建圖，可以設(shè)定地圖大小(地 圖大小應(yīng)比實(shí)際的室內(nèi)范圍大)為2048*2048。建圖時(shí)移動(dòng)機(jī)器人的初始位置比如可固定為 (1024,1024)，可按照電子指南針的航向角0〇作為初始航向角，故初始位姿為（1024,1024， 9〇)。人工引導(dǎo)走遍整個(gè)室內(nèi)范圍，創(chuàng)建的完整的柵格地圖并保存。在移動(dòng)機(jī)器人上電重新 啟動(dòng)而創(chuàng)建局部地圖時(shí)，也可以采用電子指南針航向角作為初始航向角。
[0054]由于每次建圖都是以電子指南針航向角為移動(dòng)機(jī)器人的初始航向角，但由于電子 指南針自身誤差，同時(shí)不同位置處可能受外界磁場(chǎng)變化也會(huì)引起一定誤差，故上電重啟時(shí) 電子指南針的航向角不能直接作為局部地圖中的初始航向角，但一定在電子指南針航向角 附近處。設(shè)電子指南針最大誤差為正負(fù)15度，則匹配時(shí)僅需考慮30度范圍內(nèi)航向角的地圖 匹配，大大提高匹配速度和準(zhǔn)確率。將局部地圖與全局地圖采用圖像處理方案進(jìn)行圖像匹 配，得到當(dāng)前在原始地圖中的初始位姿(絕對(duì)位姿）。由于全局地圖和局部地圖的創(chuàng)建，都依 靠電子指南針的航向角確定移動(dòng)機(jī)器人的初始航向角，從而，在采用諸如SIFT和模板匹配 兩種算法進(jìn)行地圖匹配時(shí)，航向角可以約束在正負(fù)15度范圍內(nèi)進(jìn)行匹配，因此可以極大地 降低局部地圖和全部地圖匹配的復(fù)雜度。
[0055] 圖9示意性圖示根據(jù)本發(fā)明實(shí)施方式的移動(dòng)機(jī)器人的位姿初始化設(shè)備900的框圖。 設(shè)備900可以包括:全局地圖創(chuàng)建裝置910,用于使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建 (SLAM)技術(shù)創(chuàng)建移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的全局地圖；局部地圖創(chuàng)建裝置920,用于在 移動(dòng)機(jī)器人上電時(shí)，使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建針對(duì)移動(dòng) 機(jī)器人的上電時(shí)刻的環(huán)境的局部地圖；以及初始位姿獲取裝置930,用于將局部地圖在全局 地圖中進(jìn)行圖像匹配，獲取移動(dòng)機(jī)器人的初始位姿。
[0056] 應(yīng)當(dāng)理解，設(shè)備900中記載的各個(gè)裝置與參考圖1描述的方法100中的各個(gè)步驟相 對(duì)應(yīng)。由此，上文針對(duì)方法100描述的操作和特征同樣適用于設(shè)備900及其中包含的裝置，重 復(fù)的內(nèi)容在此不再贅述。
[0057] 根據(jù)本發(fā)明的實(shí)施方式的移動(dòng)機(jī)器人的位姿初始化方法和設(shè)備，可以獲取移動(dòng)機(jī) 器人上電啟動(dòng)時(shí)在全局地圖中的絕對(duì)位姿，實(shí)現(xiàn)移動(dòng)機(jī)器人位姿初始化?？梢宰龅綗o論機(jī) 器人放在任意位置點(diǎn)，上電啟動(dòng)時(shí)均可確定其準(zhǔn)確的初始位姿。
[0058] 另一方面，隨著移動(dòng)機(jī)器人技術(shù)的快速發(fā)展和日趨成熟，移動(dòng)機(jī)器人在家庭和公 共場(chǎng)所等室內(nèi)環(huán)境的應(yīng)用也越來越廣泛?；谝苿?dòng)機(jī)器人的各種服務(wù)和應(yīng)用要求移動(dòng)機(jī)器 人能夠?qū)崟r(shí)了解自身的位姿信息，包括自身位置和航向，然后可以按照規(guī)劃路徑準(zhǔn)確到達(dá) 目的地完成指定任務(wù)。因此，室內(nèi)定位是移動(dòng)機(jī)器人領(lǐng)域的關(guān)鍵技術(shù)之一。
[0059] 室內(nèi)定位目前出現(xiàn)了各種技術(shù)，如超寬帶技術(shù)、基于無線局為(WLAN)定位技術(shù)、激 光測(cè)距定位技術(shù)、基于慣性導(dǎo)航的定位技術(shù)、基于里程計(jì)的定位技術(shù)等。而每種定位技術(shù)都 存在各自的缺點(diǎn)和局限性，大多數(shù)方案采用多種定位技術(shù)的組合導(dǎo)航。但是由于每種定位 技術(shù)所依賴的傳感器本身精度限制和周圍環(huán)境干擾，導(dǎo)致室內(nèi)定位不可避免地存在位置和 航向誤差。
[0060] 在進(jìn)一步的實(shí)施方式中，可以進(jìn)一步對(duì)移動(dòng)機(jī)器人初始化后的位姿或者位姿發(fā)生 改變后的位姿進(jìn)行位姿誤差校正。下面結(jié)合圖10-圖16來描述具體的誤差校正過程。
[0061] 圖10圖示了現(xiàn)有技術(shù)的室內(nèi)定位和導(dǎo)航技術(shù)的方法1000的流程圖。
[0062]步驟S1010,室內(nèi)地圖創(chuàng)建。室內(nèi)地圖的創(chuàng)建可以采用卷尺和激光測(cè)距儀等方法， 測(cè)量墻壁的長度和厚度以及相對(duì)坐標(biāo)原點(diǎn)的位置，基于直角坐標(biāo)系建立室內(nèi)地圖數(shù)據(jù)庫。 [0063]步驟S1020,路徑規(guī)劃。路徑規(guī)劃一般是指機(jī)器人在有障礙物的環(huán)境中，按照一定 的評(píng)價(jià)標(biāo)準(zhǔn)，尋找一條從當(dāng)前位置到目標(biāo)位置的無碰撞路徑。移動(dòng)機(jī)器人導(dǎo)航引擎在得到 目標(biāo)位置與當(dāng)前位置信息后，就需要根據(jù)地圖，計(jì)算出最優(yōu)的路徑以及可能的備選路徑，將 移動(dòng)機(jī)器人導(dǎo)航到目的地。常見的路徑規(guī)劃算法例如包括A*和Dijkstra算法。
[0064] 步驟S1030,室內(nèi)導(dǎo)航。移動(dòng)機(jī)器人導(dǎo)航引擎使用所規(guī)劃的路徑將移動(dòng)機(jī)器人導(dǎo)航 到目標(biāo)位置。
[0065] 移動(dòng)機(jī)器人導(dǎo)航引擎可以采用的定位和導(dǎo)航技術(shù)例如可以是基于超寬帶(UWB)的 定位和導(dǎo)航技術(shù)、基于慣性的定位和導(dǎo)航技術(shù)、基于無線局域網(wǎng)(WLAN)的定位和導(dǎo)航技術(shù)、 激光測(cè)距定位和導(dǎo)航技術(shù)、基于視覺導(dǎo)航的定位和導(dǎo)航技術(shù)、基于超聲波測(cè)距的定位和導(dǎo) 航技術(shù)、或者它們中的一項(xiàng)或多項(xiàng)的結(jié)合。本領(lǐng)域技術(shù)人員應(yīng)當(dāng)能夠理解這些定位和導(dǎo)航 技術(shù)的基本原理和位姿誤差產(chǎn)生的原因，然而考慮到本文的完整性，下面僅以超寬帶(UWB) 技術(shù)和慣性定位和導(dǎo)航技術(shù)為例做個(gè)簡單說明。
[0066]超寬帶可以測(cè)量基站和標(biāo)簽之間的距離信息。將標(biāo)簽安裝到移動(dòng)機(jī)器人上，當(dāng)標(biāo) 簽同時(shí)測(cè)量到三個(gè)及以上基站的距離信息時(shí)，采用三邊測(cè)量法就可以計(jì)算出移動(dòng)機(jī)器人的 位置和航向。圖11圖示了基于超寬帶(UWB)的定位和導(dǎo)航技術(shù)的示意圖。如圖11所示，具體 計(jì)算步驟可以如下。
[0067] 1)基站六、8、(：的位置為已知，分別為(13,73)、（11 )，71))、（1。，7。），利用基站發(fā)射的超 寬帶信號(hào)測(cè)量的其距離標(biāo)簽T的距離分別為ra、rb和r c;
[0068] 2)標(biāo)簽T的位置為(Xt，yt)。則根據(jù)步驟1)的信息，可得出以下公式：
[0069] (xf - xa) + [yt - yj = i；2
[0070] + (yt - = l；2
[0071] (A _ 義)」+ k = J:,」
[0072 ] 3)由步驟2)中公式，可計(jì)算出標(biāo)簽T的位置(xt，yt)為：
[0075] 4)根據(jù)當(dāng)前時(shí)刻標(biāo)簽T的位置(Xt，yt)和上一時(shí)刻標(biāo)簽T的位置( XtQ，ytQ)，可計(jì)算標(biāo) 簽T的航向W為：
[0077] 可見，在超寬帶室內(nèi)定位中，引起位姿誤差的因素有兩個(gè)，一是UWB信號(hào)的傳播效 應(yīng)，包括非視距傳播效應(yīng)、多徑傳播效應(yīng)及遠(yuǎn)近效應(yīng)等，二是由于當(dāng)前時(shí)刻標(biāo)簽位置的計(jì)算 依賴于前一時(shí)刻標(biāo)簽位置所帶來的累積誤差。
[0078] 慣性定位和導(dǎo)航技術(shù)是通過陀螺儀、加速計(jì)、里程計(jì)等傳感器來進(jìn)行移動(dòng)機(jī)器人 的定位和導(dǎo)航。陀螺儀測(cè)量移動(dòng)機(jī)器人的角速度，里程計(jì)測(cè)量移動(dòng)機(jī)器人的移動(dòng)距離，陀螺 儀/里程計(jì)可以計(jì)算移動(dòng)機(jī)器人的相對(duì)變化位置。具體步驟如下：
[0079] 1)在室內(nèi)布置多個(gè)超寬帶基站，并記錄基站在室內(nèi)地圖下的位置信息；
[0080] 2)當(dāng)組合導(dǎo)航未初始化位置和航向時(shí)，如果標(biāo)簽測(cè)量到的基站距離信息小于三 個(gè)，則移動(dòng)機(jī)器人自由行走，直到測(cè)量到的基站距離信息不少于三個(gè)；
[0081] 3)當(dāng)標(biāo)簽測(cè)量到三個(gè)及以上基站數(shù)據(jù)時(shí)，移動(dòng)機(jī)器人根據(jù)標(biāo)簽和基站的距離信息 計(jì)算移動(dòng)機(jī)器人的位置和航向，完成組合導(dǎo)航的位置和航向初始化；
[0082] 4)移動(dòng)機(jī)器人根據(jù)當(dāng)前時(shí)刻的位置和航向，基于陀螺儀的角速度數(shù)據(jù)和里程計(jì)的 距離數(shù)據(jù)計(jì)算下一時(shí)刻的位置和航向；
[0083] 5)當(dāng)標(biāo)簽測(cè)量到三個(gè)及以上基站數(shù)據(jù)時(shí)，計(jì)算出移動(dòng)機(jī)器人的位置和航向，并與 步驟4)所得位置和航向按照設(shè)置的比例參數(shù)進(jìn)行融合，并作為移動(dòng)機(jī)器人的真實(shí)位置和航 向；
[0084] 6)當(dāng)標(biāo)簽未測(cè)量到三個(gè)基站數(shù)據(jù)時(shí)，步驟4)所得位置和航向即為移動(dòng)機(jī)器人的真 實(shí)位置和航向。
[0085] 可見，慣性定位和導(dǎo)航技術(shù)所使用的傳感器的精度會(huì)產(chǎn)生即時(shí)誤差，依賴于前一 時(shí)刻位姿來計(jì)算當(dāng)前時(shí)刻位姿會(huì)帶來累積誤差。
[0086] 參考圖12,其圖示根據(jù)本發(fā)明的實(shí)施方式的用于移動(dòng)機(jī)器人定位的方法300的流 程圖。方法300可以用于確定移動(dòng)機(jī)器人室內(nèi)定位方法的位姿誤差，并且可選地，可以進(jìn)行 位姿誤差校正。位姿（也稱為定位)可以包括移動(dòng)機(jī)器人的位置和航向，位姿誤差可以包括 位置誤差和航向誤差。應(yīng)當(dāng)理解，方法300可以結(jié)合在方法100的描述所提及的任何一種定 位和導(dǎo)航技術(shù)而使用。
[0087] 步驟S310,根據(jù)移動(dòng)機(jī)器人室內(nèi)定位技術(shù)估計(jì)移動(dòng)機(jī)器人的當(dāng)前位姿。
[0088] 步驟S320,訪問參考物數(shù)據(jù)庫，所述參考物數(shù)據(jù)庫包括移動(dòng)機(jī)器人配備的攝像頭 拍攝的、關(guān)于至少一個(gè)參考物中的每個(gè)參考物的一幅或多幅參考圖像，以及與所述參考圖 像關(guān)聯(lián)的、所述攝像頭拍攝參考圖像時(shí)移動(dòng)機(jī)器人的真實(shí)位姿，其中所述參考物是用來在 其中定位移動(dòng)機(jī)器人的環(huán)境中的靜止物體，并且從所述參考物數(shù)據(jù)庫中選擇一個(gè)真實(shí)位姿 和相關(guān)聯(lián)的參考圖像。
[0089] 在一個(gè)實(shí)施例中，可以從參考物數(shù)據(jù)庫中選擇與所選擇的當(dāng)前位姿所需距離最短 的真實(shí)位姿。在一個(gè)實(shí)施例中，可以從參考物數(shù)據(jù)庫中選擇從當(dāng)前位姿導(dǎo)航到所需時(shí)間最 短的真實(shí)位姿。在一個(gè)實(shí)施例中，可以選擇在所需距離和所需時(shí)間兩個(gè)方面較為折中的真 實(shí)位姿。
[0090] 步驟S330，將移動(dòng)機(jī)器人導(dǎo)航至所選擇的真實(shí)位姿，即目標(biāo)位姿，所述目標(biāo)位姿是 根據(jù)所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)所估計(jì)出的。
[0091] 步驟S340,利用移動(dòng)機(jī)器人配備的攝像頭進(jìn)行圖像拍攝，獲得拍攝圖像。應(yīng)當(dāng)注 意，該拍攝圖像中需要包括參考物圖像，該參考物也出現(xiàn)在所選擇的相關(guān)聯(lián)的參考圖像中； 否則，表明移動(dòng)機(jī)器人室內(nèi)定位技術(shù)的累積誤差過大了，使得參考物不能出現(xiàn)在移動(dòng)機(jī)器 人配備的攝像頭的視野中了，至少應(yīng)當(dāng)在累積誤差積累到出現(xiàn)這種情況之前進(jìn)行誤差校 正。在一個(gè)實(shí)施例中，可以據(jù)出現(xiàn)這種情況來定義誤差校正閾值。
[0092] 步驟S350,將所述拍攝圖像中包括的參考物圖像(也稱為實(shí)際參考物圖像)與所選 擇的相關(guān)聯(lián)的參考圖像中包括的參考物圖像(也稱為數(shù)據(jù)庫參考物圖像)進(jìn)行對(duì)比。對(duì)比可 以包括形狀和/或尺寸方面的對(duì)比。形狀對(duì)比是指判斷實(shí)際參考物圖像相對(duì)于數(shù)據(jù)庫參考 物圖像是否存在變形，比如向左傾斜、向右傾斜，這是由于拍攝時(shí)的攝像頭的方向不同（表 明機(jī)器人的航向不同）所引起的。尺寸對(duì)比是指判斷實(shí)際參考物圖像相對(duì)于數(shù)據(jù)庫參考物 圖像在形狀相同的前提下是否存在大小變化，比如尺寸偏小、尺寸偏大，這是由于拍攝時(shí)的 攝像頭距離參考物的距離不同所引起的。
[0093] 步驟S360,根據(jù)對(duì)比的結(jié)果來確定移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿誤差。
[0094] 在可選的步驟S370,必要時(shí)，可以利用所確定的位姿誤差對(duì)步驟S310中所估計(jì)的 移動(dòng)機(jī)器人的當(dāng)前位姿進(jìn)行誤差校正?？梢灾辉诖_定的位姿誤差超過預(yù)設(shè)的閾值時(shí)，才執(zhí) 行此校正步驟。例如，該閾值可以是進(jìn)行了預(yù)設(shè)次數(shù)的定位和導(dǎo)航，比如執(zhí)行了 3次慣性導(dǎo) 航后執(zhí)行一次位姿誤差校正。
[0095] 進(jìn)一步地，在步驟S320之前，方法300還可以包括步驟S315,確定是否需要對(duì)步驟 S310中所估計(jì)的移動(dòng)機(jī)器人的當(dāng)前位姿進(jìn)行誤差確定(未示出）。只有在估計(jì)當(dāng)前位姿的誤 差可能超過誤差閾值時(shí)，才執(zhí)行步驟S320及后續(xù)進(jìn)行，進(jìn)行位姿誤差確認(rèn)。例如，該誤差閾 值可以是進(jìn)行了預(yù)設(shè)次數(shù)的定位和導(dǎo)航，比如執(zhí)行了 3次慣性導(dǎo)航后執(zhí)行一次位姿誤差確 定，還可以是航向誤差超過了預(yù)設(shè)的航向誤差閾值、和/或位置誤差超過了預(yù)設(shè)的位置誤差 閾值;或者，該誤差閾值可以依據(jù)所采用移動(dòng)機(jī)器人室內(nèi)定位技術(shù)而定;或者，該誤差閾值 可以依據(jù)定位所使用傳感器的精度而定。
[0096]可選地，方法300還可以包括建立參考物數(shù)據(jù)庫的步驟。應(yīng)當(dāng)理解，無需針對(duì)每次 誤差確定或者誤差校正的方法300的流程都執(zhí)行建立參考物數(shù)據(jù)庫的步驟。預(yù)先建立的參 考物數(shù)據(jù)庫也可以為家庭內(nèi)的不同的移動(dòng)機(jī)器人所共用?？梢詫⒖嘉飻?shù)據(jù)庫存儲(chǔ)在移動(dòng) 機(jī)器人中，也可以將參考物數(shù)據(jù)庫存儲(chǔ)在移動(dòng)機(jī)器人之外的服務(wù)器中，在步驟S320中，移動(dòng) 機(jī)器人需要與該服務(wù)器通信來獲取從所述參考物數(shù)據(jù)庫中選擇的真實(shí)位姿和相關(guān)聯(lián)的參 考圖像。
[0097]參考圖13,其圖示了根據(jù)本發(fā)明的實(shí)施方式的用于移動(dòng)機(jī)器人定位和導(dǎo)航的方法 400的流程圖。方法400可以理解為是將圖11所示的位姿誤差確定和校正方法應(yīng)用于圖10所 示的移動(dòng)機(jī)器人定位和導(dǎo)航方法的一個(gè)具體實(shí)例。
[0098] 步驟S402,室內(nèi)地圖創(chuàng)建。
[0099]步驟S404,建立參考物數(shù)據(jù)庫，具體建立步驟可以如下進(jìn)行。
[0100] 1)可以選擇固定在墻壁上的電燈開關(guān)、消防警鐘、電視等作為參考物。所選參考物 不局限于以上物體，所有固定在墻壁上的易識(shí)別物體和標(biāo)記均在所述范圍之內(nèi)；或者，還例 如可以是固定擺放的家具（比如電視、飲水機(jī)等)或者家具上的標(biāo)記(比如Logo、電子家具的 開關(guān)按鍵)等。
[0101] 2)將移動(dòng)機(jī)器人調(diào)整到處于參考物前方，開啟攝像頭拍攝參考物，存儲(chǔ)參考物圖 像至參考物數(shù)據(jù)庫，并用卷尺或激光測(cè)距儀等準(zhǔn)確測(cè)量此時(shí)(進(jìn)行拍攝時(shí))移動(dòng)機(jī)器人的位 姿，包括位置Pt rue和航向Atrue，建立參考物圖像與拍攝時(shí)的機(jī)器人位姿Ptrue和航向A true-一 對(duì)應(yīng)的參考物數(shù)據(jù)庫記錄。參考物數(shù)據(jù)庫不局限某一個(gè)參考物，可建立多個(gè)參考物圖像與 拍攝位置和航向的數(shù)據(jù)庫，且同一幅圖像中可同時(shí)包含多個(gè)參考物。
[0102] 3)以多個(gè)角度拍攝同一參考物，和/或以一個(gè)或多個(gè)角度拍攝不同的參考物，按照 步驟2)建立參考物數(shù)據(jù)庫。
[0103] 步驟S410，根據(jù)移動(dòng)機(jī)器人室內(nèi)定位技術(shù)估計(jì)移動(dòng)機(jī)器人的當(dāng)前位姿。
[01 04]步驟S420，訪問參考物數(shù)據(jù)庫，并且從所述參考物數(shù)據(jù)庫中選擇一個(gè)真實(shí)位姿和 相關(guān)聯(lián)的參考圖像。
[0105]步驟S422,路徑規(guī)劃。比如，當(dāng)監(jiān)測(cè)到定位傳感器出現(xiàn)故障、定位系統(tǒng)受到環(huán)境干 擾等情況時(shí)，這可能會(huì)導(dǎo)致定位或定向誤差超過閾值，移動(dòng)機(jī)器人需要利用參考物進(jìn)行定 位和定向誤差確定和修正。移動(dòng)機(jī)器人可以根據(jù)當(dāng)前位姿從參考物數(shù)據(jù)庫中選擇距離最近 的參考物圖像，并將所對(duì)應(yīng)的拍攝位置和航向作為目標(biāo)定向，根據(jù)室內(nèi)地圖計(jì)算出從當(dāng)前 位置到目標(biāo)點(diǎn)的最佳路徑，避開墻壁等各種障礙物，指導(dǎo)移動(dòng)機(jī)器人按照規(guī)劃路徑行駛。
[0106] 步驟S424,室內(nèi)導(dǎo)航。移動(dòng)機(jī)器人導(dǎo)航引擎使用步驟S422中所計(jì)算的路徑將移動(dòng) 機(jī)器人導(dǎo)航到目標(biāo)定向。
[0107] 步驟S430，將移動(dòng)機(jī)器人導(dǎo)航至所選擇的真實(shí)位姿，即目標(biāo)位姿，所述目標(biāo)位姿是 根據(jù)所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)所估計(jì)出的。
[0108] 步驟S440,利用移動(dòng)機(jī)器人配備的攝像頭進(jìn)行圖像拍攝，獲得拍攝圖像。
[0109] 移動(dòng)機(jī)器人到達(dá)目的定向后，可以打開攝像頭進(jìn)行數(shù)據(jù)采集，將攝像畫面作為圖 像存儲(chǔ)。由于室內(nèi)導(dǎo)航存在誤差，移動(dòng)機(jī)器人的位置和航向與預(yù)期存在差距，導(dǎo)致拍攝的實(shí) 際參考物與數(shù)據(jù)庫參考物相比，可能存在變形、尺寸偏小和尺寸偏大等問題。
[0110] 步驟S450,將所述拍攝圖像中包括的參考物圖像(也稱為實(shí)際參考物圖像)與所選 擇的相關(guān)聯(lián)的參考圖像中包括的參考物圖像(也稱為數(shù)據(jù)庫參考物圖像)進(jìn)行對(duì)比。圖14A、 圖14B和圖14C圖示攝像頭拍攝的實(shí)際參考物圖像相對(duì)于數(shù)據(jù)庫參考物圖像所可能發(fā)生的 變化的示意圖，分別圖示了實(shí)際參考物圖像發(fā)生變形、尺寸偏大、尺寸偏小的情形。
[0111]步驟S460,根據(jù)對(duì)比的結(jié)果來確定移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿誤差，具 體可以包括步驟S4602和S4604。
[0112] 在步驟S4602,根據(jù)對(duì)比的結(jié)果調(diào)整所述移動(dòng)機(jī)器人的航向，使得移動(dòng)機(jī)器人配備 的攝像頭所拍攝的圖像中的參考物圖像與所選擇的相關(guān)聯(lián)的參考圖像中包括的參考物圖 像趨于一致。
[0113] 根據(jù)攝像頭采集的實(shí)際參考物圖像和數(shù)據(jù)庫參考物圖像進(jìn)行對(duì)比，分析兩者的匹 配情況，判斷實(shí)際參考物的狀態(tài)，包括變形、尺寸偏小和尺寸偏大三種。當(dāng)移動(dòng)機(jī)器人位置 偏左、偏右或拍攝航向與數(shù)據(jù)庫的移動(dòng)人定向不一致時(shí)，會(huì)導(dǎo)致參考物變形；當(dāng)移動(dòng)機(jī)器人 位置偏前或偏后時(shí)，會(huì)導(dǎo)致參考物尺寸偏大或偏小。
[0114] 可以通過調(diào)整移動(dòng)機(jī)器人的位置和航向，校正參考物的拍攝效果，實(shí)現(xiàn)參考物的 拍攝效果與數(shù)據(jù)庫參考物的某一個(gè)圖像一致。
[0115] 在步驟S4604,確定所述移動(dòng)機(jī)器人所調(diào)整的位姿變化，即為所述移動(dòng)機(jī)器人室內(nèi) 定位技術(shù)產(chǎn)生的當(dāng)前位姿誤差，包括位置誤差Pm和航向誤差A(yù)m。
[0116] 步驟S470,利用所確定的位姿誤差對(duì)步驟S410中所估計(jì)的移動(dòng)機(jī)器人的當(dāng)前位姿 進(jìn)行誤差校正。在步驟S460中計(jì)算出位置誤差P err和航向誤差A(yù)err后，可以對(duì)步驟S410中所 估計(jì)的位姿，即位置P1和航向A1，進(jìn)行校正，得到移動(dòng)機(jī)器人的校正后的位姿，位置Pi'和 Ai'：
[0117] Pl^Pl-Perr
[0118] A'i = Ai-Aeir
[0119] 在步驟S360和步驟S460中，可以有多種方式確定移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的 位姿誤差。
[0120] 在第一實(shí)施方式中，可以只采用參考物數(shù)據(jù)庫中的參考物的一幅圖像，通過調(diào)整 移動(dòng)機(jī)器人的位置和航向，使拍攝的實(shí)際參考物與數(shù)據(jù)庫參考物達(dá)到最佳匹配度來進(jìn)行誤 差確定，可以執(zhí)行如下步驟。
[0121 ]步驟Sla，設(shè)置移動(dòng)機(jī)器人導(dǎo)航引擎使得移動(dòng)機(jī)器人左右移動(dòng)的最小距離為d，左 右旋轉(zhuǎn)最小角度為9,預(yù)先設(shè)定在進(jìn)行位姿誤差確定時(shí)要進(jìn)行的移動(dòng)次數(shù)為左右各為m，要 進(jìn)行的旋轉(zhuǎn)次數(shù)左右各為n。移動(dòng)機(jī)器人未進(jìn)行調(diào)整前的位置為Po，航向?yàn)锳o。
[0122] 步驟Sib，在移動(dòng)機(jī)器人的當(dāng)前位姿(Po，Ao)，保持移動(dòng)機(jī)器人位置不動(dòng)，將航向依 次向左旋轉(zhuǎn)0*9、1*9、…、n*9拍攝參考物，并相應(yīng)計(jì)算實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像 的匹配度So、Si、…、S n。然后，向右旋轉(zhuǎn)n*0，回到航向Ao。接著，將機(jī)器人的航向依次向右旋 轉(zhuǎn)1*0、2*0、…、n*0拍攝參考物，并相應(yīng)計(jì)算實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像的匹配度 Sn+1、Sn+2、…、S2n。然后，向左旋轉(zhuǎn)n*0，回到航向Ao。則，該位姿(Po，Ao)所拍攝的實(shí)際參考物圖 像與數(shù)據(jù)庫參考物圖像的最高匹配度為DoiMAXtS^Si，…，S 2n}，記錄該位置的最高匹配度 Do、和與Do對(duì)應(yīng)的左右旋轉(zhuǎn)的航向Yo (Yo = j*0，j屬于{-n，…，n}中的某個(gè)值），其中j表示取 得最高匹配度Do的航向左右旋轉(zhuǎn)的次數(shù)。還同時(shí)記錄移動(dòng)機(jī)器人左右移動(dòng)的距離Lo(L() = i* (14屬于{-111，一，111}中的某個(gè)值）。其中1表示移動(dòng)機(jī)器人相對(duì)于位姿(？()4())進(jìn)行左右移動(dòng)的 次數(shù)，此時(shí)，由于移動(dòng)機(jī)器人未進(jìn)行左右移動(dòng)，所以i = 〇。
[0123] 步驟Sic，移動(dòng)機(jī)器人依次向左移動(dòng)l*d、2*d、…、m*d，并在每個(gè)位置執(zhí)行步驟Sib， 計(jì)算并記錄相應(yīng)的最高匹配度Di以及對(duì)應(yīng)的左右移動(dòng)距離和航向，從而，有池山，YU，{D 2， L2，Y2}，'??，{ Dm，Lin，Yin } 〇
[0124] 步驟Sld，移動(dòng)機(jī)器人回到當(dāng)前位姿(Po，AQ)。然后，移動(dòng)機(jī)器人依次向右移動(dòng)l*d、 2*d、…、m*d，并在每個(gè)位置執(zhí)行步驟Sib，計(jì)算并記錄相應(yīng)的最高匹配度Di以及對(duì)應(yīng)的左右 移動(dòng)距離和航向，從而，有{D-hL-hY-U，{D- 2，L-2，Y-2}，…，{D-m，L-m，Y- m}。
[0125] 步驟Sle，在不考慮尺寸問題的情況下，實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像的最 佳匹配度為D=MAX{D-m，…，，…，D m}。根據(jù)D所對(duì)應(yīng)的左右移動(dòng)距離L和左右旋轉(zhuǎn)航 向Y調(diào)整移動(dòng)機(jī)器人，該狀態(tài)下所拍攝的實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像只存在尺寸 不一致問題，不存在變形。
[0126] 步驟Slf，若此時(shí)實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像尺寸也相等，則參考物圖像 的特征匹配完成，如圖15所示，其示出了實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像尺寸剛好匹 配的情形。記錄移動(dòng)機(jī)器人此時(shí)的位姿為(丹，心），為室內(nèi)定位系統(tǒng)的位置和航向輸出。則此 時(shí)，位置誤差I(lǐng)W = Pl-PtrUe，航向誤差和Atrue為與數(shù)據(jù)庫參考物圖像對(duì)應(yīng) 的參考物數(shù)據(jù)庫中的移動(dòng)機(jī)器人位姿，包括位置和航向。
[0127] 若此時(shí)實(shí)際參考物圖像比數(shù)據(jù)庫參考物圖像尺寸小，則調(diào)整移動(dòng)機(jī)器人前進(jìn)，直 到兩者尺寸相等，參考物特征匹配完成，如圖15所示。記錄移動(dòng)機(jī)器人此時(shí)的位姿為(Pi， Ai )，為室內(nèi)定位系統(tǒng)的位置和航向輸出。則此時(shí)，位置誤差Perr = Pi-Ptrue，航向誤差A(yù)err = 和Atrue5為與數(shù)據(jù)庫參考物圖像對(duì)應(yīng)的參考物數(shù)據(jù)庫中的移動(dòng)機(jī)器人位姿，包 括位置和航向。
[0128] 若此時(shí)實(shí)際參考物圖像比數(shù)據(jù)庫參考物圖像尺寸大，則調(diào)整移動(dòng)機(jī)器人后退，直 到兩者尺寸相等，參考物特征匹配完成，如圖15所示。記錄移動(dòng)機(jī)器人此時(shí)的位姿為(Pi， Ai )，為室內(nèi)定位系統(tǒng)的位置和航向輸出。則此時(shí)，位置誤差Perr = Pi-Ptrue，航向誤差A(yù)err = 和Atrue5為與數(shù)據(jù)庫參考物圖像對(duì)應(yīng)的參考物數(shù)據(jù)庫中的移動(dòng)機(jī)器人位姿，包 括位置和航向。
[0129] 在第一實(shí)施方式中，需多次調(diào)整移動(dòng)機(jī)器人，比較耗時(shí)，且對(duì)移動(dòng)機(jī)器人進(jìn)行左右 移動(dòng)的調(diào)整比較麻煩。為此，在第二實(shí)施方式中，可采用數(shù)據(jù)庫中一個(gè)參考物的多幅圖像進(jìn) 行特征匹配，避免左右移動(dòng)。
[0130] 在第二實(shí)施方式中，參考物數(shù)據(jù)庫包括一個(gè)參考物的多幅圖像，該多幅圖像包括 移動(dòng)機(jī)器人在一個(gè)拍攝位置拍攝的一幅圖像、和在該位置左右各移動(dòng)m次拍攝的2m幅圖像。 機(jī)器人每次能夠左右移動(dòng)的距離是確定的，例如為d。在第二實(shí)施方式中，采用數(shù)據(jù)庫的多 幅參考物圖像進(jìn)行匹配，通過調(diào)整移動(dòng)機(jī)器人左右旋轉(zhuǎn)(調(diào)整航向）和前后移動(dòng)實(shí)現(xiàn)參考物 特征匹配，使拍攝的實(shí)際參考物與數(shù)據(jù)庫參考物達(dá)到最佳匹配度來進(jìn)行誤差確定，可以執(zhí) 行如下步驟。
[0131] 步驟S2a，保持移動(dòng)機(jī)器人位置不動(dòng)，調(diào)整移動(dòng)機(jī)器人左右旋轉(zhuǎn)（依次改變航向 角），使實(shí)際參考物圖像與多幅數(shù)據(jù)庫參考物圖像中的某幅圖像的匹配度最高，該狀態(tài)下所 拍攝的實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像只存在尺寸不一致問題，不存在變形。
[0132] 步驟S2b，當(dāng)實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像尺寸相等時(shí)，參考物特征匹配完 成；當(dāng)實(shí)際參考物圖像尺寸比數(shù)據(jù)庫參考物圖像尺寸小時(shí)，調(diào)整移動(dòng)機(jī)器人前進(jìn)，直到兩者 尺寸相等，參考物特征匹配完成；當(dāng)實(shí)際參考物圖像尺寸比數(shù)據(jù)庫參考物圖像尺寸大時(shí)，調(diào) 整移動(dòng)機(jī)器人后退，直到兩者尺寸相等，參考物特征匹配完成。在后兩種情況下，將移動(dòng)機(jī) 器人移動(dòng)到參考物數(shù)據(jù)庫中的與最匹配的數(shù)據(jù)庫參考物關(guān)聯(lián)的移動(dòng)機(jī)器人拍攝該圖像時(shí) 的位置。記錄移動(dòng)機(jī)器人最后的位姿為(P^AO,為室內(nèi)定位系統(tǒng)的位置和航向輸出。則此 時(shí)，位置誤差I(lǐng)W = Pl-PtrUe，航向誤差和Atrue為與數(shù)據(jù)庫參考物圖像對(duì)應(yīng) 的參考物數(shù)據(jù)庫中的移動(dòng)機(jī)器人位姿，包括位置和航向。
[0133] 在第三實(shí)施方式中，由于室內(nèi)地面通常是平坦的，可以假設(shè)移動(dòng)機(jī)器人攝像頭的 高度是固定不變的，則參考物在攝像畫面中的相對(duì)高度是不變的，只存在左右偏差。圖16圖 示了移動(dòng)機(jī)器人拍攝畫面(也稱為攝影畫面）中的數(shù)據(jù)庫參考物圖像和實(shí)際參考物圖像的 可能的偏差。因此，有可能通過偏差距離來反推航向誤差，避免移動(dòng)機(jī)器人旋轉(zhuǎn)，減少調(diào)整 過程，具體步驟如下。
[0134] 步驟S3a，數(shù)據(jù)庫參考物圖像和實(shí)際參考物圖像距離攝影畫面左邊緣的距離分別 為dl和d2。所述距離可以不局限于左邊緣距離，還可以是右邊緣距離、或者左邊緣和右邊緣 的結(jié)合。
[0135] 步驟S3b，假設(shè)移動(dòng)機(jī)器人旋轉(zhuǎn)角度與攝影畫面中的距離系數(shù)為f，系數(shù)f可以用移 動(dòng)機(jī)器人拍攝數(shù)據(jù)庫參考物圖像時(shí)距離參考物的距離和航向來表示，可以將其與數(shù)據(jù)庫參 考物圖像相關(guān)聯(lián)的存儲(chǔ)在參考物數(shù)據(jù)庫中，則移動(dòng)機(jī)器人的航向誤差為：
[0137] 經(jīng)過航向誤差A(yù)err校正后所拍攝的實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像只存在尺 寸不一致問題，不存在變形。
[0138] 步驟S3c，當(dāng)實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像尺寸相等時(shí)，參考物特征匹配完 成；當(dāng)實(shí)際參考物圖像尺寸比數(shù)據(jù)庫參考物圖像尺寸小時(shí)，調(diào)整移動(dòng)機(jī)器人前進(jìn)，直到兩者 尺寸相等，參考物特征匹配完成；當(dāng)實(shí)際參考物圖像尺寸比數(shù)據(jù)庫參考物圖像尺寸大時(shí)，調(diào) 整移動(dòng)機(jī)器人后退，直到兩者尺寸相等，參考物特征匹配完成。位置誤差P err可以采用第一 或者第二實(shí)施方式的確定方式。
[0139] 在第三實(shí)施方式中，可以通過實(shí)際參考物圖像和數(shù)據(jù)庫參考物圖像距離攝影畫面 邊緣的距離差來計(jì)算航向誤差，移動(dòng)機(jī)器人無需左右旋轉(zhuǎn)，但仍需前后調(diào)整來計(jì)算位置誤 差。
[0140]在第四實(shí)施方式中，由于參考物的尺寸與攝像頭和參考物的距離有關(guān)，因此可根 據(jù)參考物尺寸差來計(jì)算位置誤差，具體步驟如下。
[0141]步驟S4a，數(shù)據(jù)庫參考物圖像和實(shí)際參考物圖像距離攝影畫面左邊緣的距離分別 為dl和d2。所述距離可以不局限于左邊緣距離，還可以是右邊緣距離、或者左邊緣和右邊緣 的結(jié)合。
[0142]步驟S4b，假設(shè)移動(dòng)機(jī)器人旋轉(zhuǎn)角度與攝影畫面中的距離系數(shù)為f，則移動(dòng)機(jī)器人 的航向誤差為：
[0144] 經(jīng)過航向誤差A(yù)err校正后所拍攝的實(shí)際參考物圖像與數(shù)據(jù)庫參考物圖像只存在尺 寸不一致問題，不存在變形。
[0145] 步驟S4c，參考物數(shù)據(jù)庫中參考物尺寸（比如高度)為hi，實(shí)際參考物尺寸（比如高 度)為h2,攝像頭與參考物的距離和尺寸的系數(shù)為V，系數(shù)v可以用移動(dòng)機(jī)器人拍攝數(shù)據(jù)庫參 考物圖像時(shí)距離參考物的距離來表示，可以將其與數(shù)據(jù)庫參考物圖像相關(guān)聯(lián)的存儲(chǔ)在參考 物數(shù)據(jù)庫中，則移動(dòng)機(jī)器人的位置誤差為：
[0147] 參考圖17,其圖示了圖根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿確定方法 1700的流程圖。移動(dòng)機(jī)器人包括激光掃描儀和慣性導(dǎo)航傳感器。
[0148] 在步驟S1710,在移動(dòng)機(jī)器人的位姿發(fā)生改變時(shí)，利用慣性導(dǎo)航傳感器計(jì)算移動(dòng)機(jī) 器人在全局地圖中的第一位姿。全局地圖可以是針對(duì)移動(dòng)機(jī)器人在其中移動(dòng)的環(huán)境而預(yù)先 創(chuàng)建的。使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)、針對(duì)移動(dòng)機(jī)器人在其 中導(dǎo)航的環(huán)境而預(yù)先創(chuàng)建的。
[0149] 慣性導(dǎo)航傳感器可以包括陀螺儀、電子指南針、加速計(jì)和里程計(jì)中一種或多種。陀 螺儀獲得機(jī)器人可以旋轉(zhuǎn)角度，加速計(jì)或里程計(jì)可以獲取行走距離，電子指南針可以獲取 機(jī)器人絕對(duì)航向角。慣性導(dǎo)航的基本原理為:設(shè)移動(dòng)機(jī)器人t時(shí)刻位姿為( Xt，yt，0t)，例如采 用陀螺儀和里程計(jì)傳感器獲得角度變化信息、前行距離，可以推出t + 1時(shí)刻的測(cè)量位姿 (xt+i，yt+i，Qt+i) 〇
[0150] 在步驟S1720,使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建關(guān)于所 述移動(dòng)機(jī)器人的周圍環(huán)境的局部地圖。
[0151] 在步驟S1730,將局部地圖在全局地圖中進(jìn)行圖像匹配，獲取移動(dòng)機(jī)器人在全局地 圖中的第二位姿、和與第二位姿對(duì)應(yīng)的匹配可信度。第二位姿為圖形匹配得出的移動(dòng)機(jī)器 人在全局地圖中的最佳位姿?？梢圆捎玫木唧w圖像匹配算法可以包括但不限于，尺度不變 特征轉(zhuǎn)換（SIFT，Scale-invariant feature transform)算法和模板匹配算法，還可以使用 直方圖匹配、形狀匹配、蒙特卡洛等匹配算法。
[0152] 在步驟S1740,根據(jù)所獲取的匹配可信度，確定移動(dòng)機(jī)器人的位姿為第一位姿或者 第二位姿。匹配可信度表示第二位姿的準(zhǔn)確度，并且其值越大，表明第二位姿越精確，從而 有，當(dāng)所獲取的匹配可信度大于第一閾值時(shí)，確定移動(dòng)機(jī)器人的位姿為第二位姿，并且當(dāng)所 獲取的匹配可信度小于或者等于第一閾值時(shí)，確定移動(dòng)機(jī)器人的位姿為第一位姿。
[0153] 參考圖18,其圖示了根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿確定方法1800 的流程圖。移動(dòng)機(jī)器人包括激光掃描儀和慣性導(dǎo)航傳感器。
[0154] 在步驟S1810,判斷移動(dòng)機(jī)器人是否首次出現(xiàn)在新環(huán)境中，若判斷結(jié)果為是，則方 法1800前進(jìn)至步驟S1820,否則，方法1800前進(jìn)至步驟S1830。
[0155] 在步驟S1820,使用激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建全局地 圖，并且在移動(dòng)機(jī)器人上電時(shí)對(duì)移動(dòng)機(jī)器人在該全局地圖中的位姿進(jìn)行初始化。
[0156] 后續(xù)步驟31830、31840、31850和31860涉及在移動(dòng)機(jī)器人的位姿發(fā)生改變時(shí)，確定 移動(dòng)機(jī)器人在全局地圖中的精確位姿，其分別與參考方法1700所描述的步驟S1710、S1720、 S1730 和 S1740 相同。
[0157] 慣性導(dǎo)航定位技術(shù)和激光導(dǎo)航定位技術(shù)是目前較為成熟的幾種室內(nèi)導(dǎo)航定位手 段中的兩種手段，二者都需要依賴于預(yù)先設(shè)定的關(guān)于環(huán)境的全局地圖。另外，慣性導(dǎo)航定位 精度高，但是存在累計(jì)誤差，上電初始時(shí)刻，需配合外部其他設(shè)備提供地圖和初始位姿?；?于激光掃描儀的即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)可在新環(huán)境下同時(shí)建圖和定位，第一次創(chuàng) 建地圖時(shí)，初始位姿可為固定值或由其他設(shè)備提供，但是在完成創(chuàng)建地圖后，上電初始時(shí)刻 無法確定移動(dòng)機(jī)器人在已有地圖中的初始位姿。
[0158] 在步驟S1720和步驟S1840中創(chuàng)建關(guān)于移動(dòng)機(jī)器人的位姿發(fā)生變化后的周圍環(huán)境 的局部地圖的方法，可以采用與步驟S120中創(chuàng)建局部地圖的相同的方法。在移動(dòng)機(jī)器人的 位姿發(fā)生改變并且在步驟S1710或者S1840中、利用慣性導(dǎo)航傳感器計(jì)算移動(dòng)機(jī)器人在全局 地圖中的第一位姿之后，可以設(shè)置移動(dòng)機(jī)器人的假定位姿為該第一位姿，開啟激光SLAM建 圖功能，使得移動(dòng)機(jī)器人在原位置自行旋轉(zhuǎn)一周，形成較大的、詳細(xì)的、360度的局部地圖。 應(yīng)當(dāng)理解，要?jiǎng)?chuàng)建360度的局部地圖，需要的移動(dòng)機(jī)器人旋轉(zhuǎn)的角度和移動(dòng)機(jī)器人上布置的 激光掃描儀數(shù)量以及激光掃描儀視野范圍有關(guān)。
[0159] 在步驟S1730和步驟S1850中將局部地圖在全局地圖中進(jìn)行圖像匹配，獲取移動(dòng)機(jī) 器人在全局地圖中的第二位姿、和與第二位姿對(duì)應(yīng)的匹配可信度的方法，可以采用與步驟 S130中進(jìn)行局部地圖和全局地圖匹配的相類似的方法，從而可以獲得在激光SLAM技術(shù)下的 關(guān)于慣性導(dǎo)航傳感器計(jì)算的第一位姿的第二位姿表示、以及匹配可信度(或稱為匹配相似 度)。
[0160] 在用于進(jìn)行圖像匹配的一個(gè)實(shí)施例中，依據(jù)當(dāng)前慣性導(dǎo)航測(cè)量元件定義最大誤差 范圍，在根據(jù)慣性導(dǎo)航測(cè)量的第一位姿(xl，yl，91)最大誤差范圍內(nèi)，可以用當(dāng)前激光掃描 的局部地圖數(shù)據(jù)與全局柵格地圖基于蒙特卡洛方法進(jìn)行圖像匹配，即采用粒子濾波獲取最 佳位姿、即第二位姿為(x2，y2，02)。附加地，還可以計(jì)算最佳匹配的匹配可信度。
[0161] 關(guān)于匹配可信度的計(jì)算，在獲取最佳位姿后，移動(dòng)機(jī)器人可以獲得激光掃描范圍 (在一定角度范圍內(nèi)按一定頻率進(jìn)行掃描）內(nèi)的激光掃描距離信息，并且將獲得的激光掃描 距離信息在原有全局柵格地圖上進(jìn)行映射來計(jì)算匹配可信度。
[0162] 移動(dòng)機(jī)器人在導(dǎo)航時(shí)有兩個(gè)坐標(biāo)系，一是機(jī)器人坐標(biāo)系（即以激光器為坐標(biāo)原點(diǎn)， 激光器前后方向?yàn)閅軸，左右方向?yàn)閄軸）；二是已建立好的全局地圖坐標(biāo)系，即所謂的世界 坐標(biāo)系。全局地圖表現(xiàn)的是實(shí)際所在環(huán)境所有障礙位置的表示，假設(shè)在激光觀察點(diǎn)在全局 地圖中（x，y)坐標(biāo)位置，并且航向角為9。則從此處向9角度觀察可以獲取掃描范圍內(nèi)的障礙 物距離信息Dl(直接可以從全局地圖中獲得）。移動(dòng)機(jī)器人配備的激光雷達(dá)可以在現(xiàn)實(shí)環(huán)境 中獲得掃描范圍內(nèi)的障礙物距離信息D2A1和D2兩個(gè)掃描距離信息越相近，則便是可信度 越高。這便是激光掃描距離信息與原始地圖結(jié)合可獲得可信度的原理。
[0163] 實(shí)際操作時(shí)如下，利用激光雷達(dá)在現(xiàn)實(shí)中獲取的距離信息，映射到全局地圖中表 示實(shí)際障礙所在的坐標(biāo)，然后與原來全局地圖中障礙物做比較，具體做法如下。
[0164] 在匹配可信度計(jì)算時(shí)，例如通過慣性導(dǎo)航方式可以獲得當(dāng)前位置和姿態(tài)(XC，yc，0 c)，其并不精確，表明當(dāng)前準(zhǔn)確的位姿就在其附近。假設(shè)獲得當(dāng)前的位姿(XC，yC，9 C)，在此 位姿激光雷達(dá)可掃描出N個(gè)距離(障礙)信息，轉(zhuǎn)換成直角坐標(biāo)且在機(jī)器人坐標(biāo)系(激光器為 坐標(biāo)原點(diǎn)(〇,〇)，航向角為〇)中可以看到激光掃描出的障礙輪廓。而機(jī)器人實(shí)際在全局地圖 中的位姿為(xd，yd，0d)，依據(jù)映射原理可以算出激光掃描出的障礙輪廓在全局地圖中的坐 標(biāo)，即N個(gè)坐標(biāo)。N是激光雷達(dá)在一定角度范圍內(nèi)按一定頻率進(jìn)行掃描（即在激光掃描范圍內(nèi) 進(jìn)行掃描)獲得的對(duì)于障礙物在地圖中的像素?cái)?shù)量。
[0165] 在一個(gè)實(shí)施例中，匹配可信度可以根據(jù)激光掃描獲得的障礙輪廓在全局地圖中的 映射輪廓(輪廓形狀匹配)的坐標(biāo)的像素和并且歸一化來計(jì)算。匹配可信度可以選擇全局地 圖中這 N個(gè)坐標(biāo)的像素和并歸一化。歸一化原因是確保每次獲得的可信度范圍為一定值，由 于激光掃描距離范圍限制，假設(shè)激光掃描可以掃描點(diǎn)數(shù)為180個(gè)，但由于距離限制實(shí)際上測(cè) 量到有效距離信息的個(gè)數(shù)小于或等于180個(gè)。因此所有有效掃描點(diǎn)像素和不能直接作為可 信度，必須除以有效點(diǎn)個(gè)數(shù)，則可保證可信度dis_temp-定在0~255間。
[0166] 進(jìn)一步可選地，由于在二維柵格地圖中，障礙像素值為0，故di s_temp越小表明第 二位姿越精確。為適應(yīng)匹配可信度意義，匹配可信度reliability可以為:reliability = 255-dis_temp。故匹配可信度越大，表明位姿越精確。激光導(dǎo)航測(cè)量原理結(jié)構(gòu)圖如圖19所 不。
[0167] 參考圖20,其圖示了根據(jù)本發(fā)明的一個(gè)實(shí)施方式的移動(dòng)機(jī)器人位姿確定方法2000 的流程圖。方法2000在步驟S2060之前的步驟與方法1800中的對(duì)應(yīng)步驟相同。方法2000進(jìn)一 步包括步驟S2070-S2078，用于根據(jù)匹配可信度，確定移動(dòng)機(jī)器人的位姿應(yīng)當(dāng)為慣性導(dǎo)航計(jì) 算的第一位姿，還是激光導(dǎo)航計(jì)算的第二位姿。
[0168] 在圖20所示的實(shí)施方式中，匹配可信度越大，表明位姿越精確。所示意的激光導(dǎo)航 和慣性導(dǎo)航選擇策略為：當(dāng)激光導(dǎo)航策略中匹配可信度高于一定閾值時(shí)，表明激光導(dǎo)航確 定的第二位姿有效，移動(dòng)機(jī)器人的位姿為第二位姿;否則激光導(dǎo)航確定的第一位姿無效，移 動(dòng)機(jī)器人的位姿為慣性導(dǎo)航確定的第一位姿。如此，可消除由于外界環(huán)境臨時(shí)變動(dòng)而導(dǎo)致 激光導(dǎo)航策略獲得的錯(cuò)誤絕對(duì)位姿。
[0169] 在圖20所示的實(shí)施方式中還能夠?qū)崿F(xiàn)慣性導(dǎo)航的累計(jì)誤差消除，其原理可以為： 通過每時(shí)刻激光導(dǎo)航策略均對(duì)慣性導(dǎo)航策略的位姿進(jìn)行修正，故避免了慣性導(dǎo)航累計(jì)誤差 的出現(xiàn);然而，當(dāng)環(huán)境臨時(shí)變動(dòng)時(shí)，可導(dǎo)致當(dāng)前位姿一定時(shí)間內(nèi)連續(xù)使用慣性導(dǎo)航數(shù)據(jù)，即 使環(huán)境恢復(fù)，由于累計(jì)誤差原因，使得當(dāng)前位姿與實(shí)際位姿存在誤差，可導(dǎo)致激光導(dǎo)航策略 中匹配可信度永遠(yuǎn)不滿足閾值，從而僅有慣性導(dǎo)航。故當(dāng)檢測(cè)到激光導(dǎo)航可信度連續(xù)不滿 足閾值一定時(shí)，需要重新進(jìn)行初始位姿定位，以此循環(huán)。
[0170] 在進(jìn)一步的實(shí)施方式中，可以進(jìn)一步確定移動(dòng)機(jī)器人慣性導(dǎo)航技術(shù)計(jì)算的第一位 姿的位姿誤差，并且對(duì)該位姿誤差進(jìn)行誤差校正。
[0171] 應(yīng)當(dāng)理解，附圖中所示流程圖圖示了按照本發(fā)明各種實(shí)施例的方法可能實(shí)現(xiàn)的功 能和操作，在有些作為替換的實(shí)現(xiàn)中，方法流程的執(zhí)行順序可以以不同于附圖中所標(biāo)注的 順序發(fā)生。
[0172] 在另一個(gè)方面，應(yīng)當(dāng)理解，本發(fā)明實(shí)施方式所提出的各種方法可以被實(shí)現(xiàn)為軟件、 硬件或者軟件和軟件的組合。硬件可通過被設(shè)計(jì)來執(zhí)行上述功能的轉(zhuǎn)用集成電路(ASIC)、 數(shù)字信號(hào)處理器(DSP)、可編程邏輯器件(PLD)、現(xiàn)場(chǎng)可編程門陣列(FPGA)、處理器、控制器、 微處理器、其他電子單元或其組合來實(shí)現(xiàn)。軟件可通過執(zhí)行上述功能的模塊來實(shí)現(xiàn)。軟件可 被存儲(chǔ)在存儲(chǔ)單元中并由處理器運(yùn)行。作為存儲(chǔ)單元或處理器，可采用本領(lǐng)域技術(shù)人員公 知的各種單元。因此，本公開內(nèi)容也意圖涵蓋能夠?qū)崿F(xiàn)所有這些方法的功能模塊架構(gòu)的裝 置，這些裝置可以被依軟件、硬件、或者軟件和硬件的結(jié)合來實(shí)現(xiàn)。
[0173] 舉例而言，本發(fā)明實(shí)施方式提供了一種移動(dòng)機(jī)器人的室內(nèi)定位設(shè)備。所述移動(dòng)機(jī) 器人包括激光掃描儀和慣性導(dǎo)航傳感器。該設(shè)備可以包括:第一位姿確定裝置，用于在所述 移動(dòng)機(jī)器人的位姿發(fā)生改變時(shí)，利用所述慣性導(dǎo)航傳感器計(jì)算所述移動(dòng)機(jī)器人在全局地圖 中的第一位姿；局部地圖創(chuàng)建裝置，用于使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建 (SLAM)技術(shù)創(chuàng)建關(guān)于所述移動(dòng)機(jī)器人的周圍環(huán)境的局部地圖；第二位姿確定裝置，用于將 所述局部地圖在全局地圖中進(jìn)行圖像匹配，獲取所述移動(dòng)機(jī)器人在所述全局地圖中的第二 位姿、和與所述第二位姿對(duì)應(yīng)的匹配可信度，其中所述第二位姿為圖形匹配得出的所述移 動(dòng)機(jī)器人在所述全局地圖中的最佳位姿，并且其中所述全局地圖是使用所述激光掃描儀， 利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建的關(guān)于所述移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的地 圖，以及最終位姿確定裝置，用于根據(jù)所述匹配可信度，確定所述移動(dòng)機(jī)器人的位姿為所述 第一位姿或者所述第二位姿。
[0174] 進(jìn)一步地，所述設(shè)備還可以包括:估計(jì)裝置，用于根據(jù)移動(dòng)機(jī)器人室內(nèi)定位技術(shù)估 計(jì)移動(dòng)機(jī)器人的當(dāng)前位姿;訪問裝置，用于訪問參考物數(shù)據(jù)庫，所述參考物數(shù)據(jù)庫包括所述 移動(dòng)機(jī)器人配備的攝像頭拍攝的、關(guān)于至少一個(gè)參考物中的每個(gè)參考物的一幅參考圖像， 以及與所述參考圖像關(guān)聯(lián)的、所述攝像頭拍攝所述參考圖像時(shí)所述移動(dòng)機(jī)器人的相應(yīng)真實(shí) 位姿，并且從所述參考物數(shù)據(jù)庫中選擇一個(gè)真實(shí)位姿和相關(guān)聯(lián)的參考圖像，其中所述參考 物是室內(nèi)環(huán)境中的靜止物體;導(dǎo)航裝置，用于將所述移動(dòng)機(jī)器人導(dǎo)航至目標(biāo)位姿，所述目標(biāo) 位姿的數(shù)值是根據(jù)所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)對(duì)于所選擇的真實(shí)位姿的估計(jì)值;拍攝裝 置，用于利用所述移動(dòng)機(jī)器人配備的所述攝像頭進(jìn)行圖像拍攝，獲得拍攝圖像;對(duì)比裝置， 用于將所述拍攝圖像中包括的參考物圖像、即實(shí)際參考物圖像與所選擇的相關(guān)聯(lián)的參考圖 像中包括的參考物圖像、即數(shù)據(jù)庫參考物圖像進(jìn)行對(duì)比；以及確定裝置，用于根據(jù)對(duì)比的結(jié) 果來確定所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿誤差。
[0175] 進(jìn)一步地，所述確定裝置可以進(jìn)一步包括:調(diào)整和特征匹配裝置，用于根據(jù)對(duì)比的 結(jié)果調(diào)整所述移動(dòng)機(jī)器人的位姿，并且對(duì)所述移動(dòng)機(jī)器人配備的所述攝像頭所拍攝的拍攝 圖像中包括的實(shí)際參考物圖像與所述數(shù)據(jù)庫參考物圖像進(jìn)行特征匹配，使得達(dá)到最佳匹配 度；以及位姿誤差確定裝置，用于根據(jù)特征匹配的結(jié)果來確定所述移動(dòng)機(jī)器人室內(nèi)定位技 術(shù)產(chǎn)生的位姿誤差。
[0176] 進(jìn)一步地，所述調(diào)整和特征匹配裝置可以進(jìn)一步包括:第一調(diào)整和特征匹配裝置， 用于通過對(duì)移動(dòng)機(jī)器人進(jìn)行以下各項(xiàng)操作中的一項(xiàng)或多項(xiàng)：向左和/或向右移動(dòng)、向左和/ 或向右旋轉(zhuǎn)、以及向前和/或向后調(diào)整，使得所述移動(dòng)機(jī)器人配備的所述攝像頭所拍攝的拍 攝圖像中包括的實(shí)際參考物圖像與所述數(shù)據(jù)庫參考物圖像在形狀方面和尺寸方面達(dá)到最 佳匹配度;第二調(diào)整和特征匹配裝置，用于通過向左和/或向右旋轉(zhuǎn)移動(dòng)機(jī)器人，使得所述 攝像頭所拍攝的拍攝圖像中包括的實(shí)際參考物圖像與所述數(shù)據(jù)庫參考物圖像在形狀方面 達(dá)到最佳匹配度；或者第三調(diào)整和特征匹配裝置，用于通過向前和/或向后調(diào)整移動(dòng)機(jī)器 人，使得所述攝像頭所拍攝的拍攝圖像中包括的實(shí)際參考物圖像與所述數(shù)據(jù)庫參考物圖像 在尺寸方面達(dá)到最佳匹配度。
[0177] 進(jìn)一步地，所述參考物數(shù)據(jù)庫還可以包括與所述移動(dòng)機(jī)器人的一個(gè)真實(shí)位姿相關(guān) 聯(lián)的多幅參考圖像，所述多幅參考圖像包括所述移動(dòng)機(jī)器人配備的攝像頭在所述移動(dòng)機(jī)器 人的如下位姿拍攝的關(guān)于參考物的參考圖像集合、以及拍攝每幅參考圖像時(shí)的所述移動(dòng)機(jī) 器人的相應(yīng)真實(shí)位姿:所述移動(dòng)機(jī)器人的所述真實(shí)位姿，在所述移動(dòng)機(jī)器人的真實(shí)位姿處 向左和向右各旋轉(zhuǎn)n次所述移動(dòng)機(jī)器人的最小旋轉(zhuǎn)角度，在所述移動(dòng)機(jī)器人的真實(shí)位姿處 向左和向右各移動(dòng)m次所述移動(dòng)機(jī)器人的最小移動(dòng)距離，以及在所述移動(dòng)機(jī)器人的真實(shí)位 姿處向左和向右各移動(dòng)m次所述移動(dòng)機(jī)器人的最小移動(dòng)距離并且在每個(gè)移動(dòng)后的位置處向 左和向右各旋轉(zhuǎn)n次所述移動(dòng)機(jī)器人的最小旋轉(zhuǎn)角度，其中m和n為自然數(shù)。所述調(diào)整和特征 匹配裝置可以進(jìn)一步包括:第四調(diào)整和特征匹配裝置，用于通過向左和/或向右旋轉(zhuǎn)移動(dòng)機(jī) 器人，使得所述攝像頭所拍攝的拍攝圖像中包括的實(shí)際參考物圖像與所述多幅參考圖像中 包括的數(shù)據(jù)庫參考物圖像在形狀方面達(dá)到最佳匹配度；以及第五調(diào)整和特征匹配裝置，用 于通過向前和/或向后調(diào)整移動(dòng)機(jī)器人，使得所述攝像頭所拍攝的拍攝圖像中包括的實(shí)際 參考物圖像與所述數(shù)據(jù)庫參考物圖像在尺寸方面達(dá)到最佳匹配度。
[0178] 進(jìn)一步地，所述確定裝置可以進(jìn)一步包括:第一確定裝置，用于根據(jù)所述移動(dòng)機(jī)器 人室內(nèi)定位技術(shù)估計(jì)特征匹配后的所述移動(dòng)機(jī)器人的位姿(P1，A1)，并且根據(jù)所述特征匹 配后的位姿(P1，A1)和所述參考物數(shù)據(jù)庫中存儲(chǔ)的、與拍攝所述達(dá)到最佳匹配度的參考圖 像時(shí)的所述移動(dòng)機(jī)器人的相應(yīng)真實(shí)位姿來確定所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿 誤差。
[0179] 進(jìn)一步地，所述確定裝置可以進(jìn)一步包括:第二確定裝置，用于通過所述實(shí)際參考 物圖像和所述數(shù)據(jù)庫參考物圖像距離各自拍攝圖像左邊緣或者右邊緣的距離差來確定航 向誤差;或者第三確定裝置，用于通過移動(dòng)機(jī)器人拍攝所述實(shí)際參考物圖像時(shí)距離參考物 的距離、和參考物數(shù)據(jù)庫中存儲(chǔ)的移動(dòng)機(jī)器人拍攝數(shù)據(jù)庫參考物圖像時(shí)距離參考物的距離 來確位姿誤差。
[0180] 本發(fā)明實(shí)施方式的教導(dǎo)還可以實(shí)現(xiàn)為一種可以在計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中實(shí)現(xiàn)的 計(jì)算機(jī)程序產(chǎn)品，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)具有存儲(chǔ)于其中的計(jì)算機(jī)可讀程序代碼部分， 所述計(jì)算機(jī)可讀程序代碼部分被配置為執(zhí)行如前所述的根據(jù)本發(fā)明實(shí)施方式的方法或者 所述方法的各個(gè)步驟。
[0181] 根據(jù)本發(fā)明進(jìn)一步實(shí)施方式的用于移動(dòng)機(jī)器人室內(nèi)定位的方案，包括前述方法、 設(shè)備和計(jì)算機(jī)程序產(chǎn)品，可以及時(shí)發(fā)現(xiàn)移動(dòng)機(jī)器人在室內(nèi)導(dǎo)航過程中所產(chǎn)生的位姿誤差， 并且進(jìn)一步地，在必要時(shí)可以對(duì)該位姿誤差進(jìn)行校正。
[0182] 應(yīng)當(dāng)理解，本發(fā)明的實(shí)施方式的方法或設(shè)備可以被以軟件、硬件、或者軟件和硬件 的結(jié)合來實(shí)現(xiàn)。盡管在上文的詳細(xì)描述中提及了系統(tǒng)的若干裝置或子裝置，但是這種劃分 僅僅是示例性而非強(qiáng)制性的。實(shí)際上，根據(jù)本發(fā)明的示例性實(shí)施方式，上文描述的兩個(gè)或更 多裝置的特征和功能可以在一個(gè)裝置中實(shí)現(xiàn)。反之，上文描述的一個(gè)裝置的特征和功能可 以進(jìn)一步劃分為由多個(gè)裝置來實(shí)現(xiàn)。
[0183]以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精 神和原則之內(nèi)，所作的任何修改、等同替換等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種移動(dòng)機(jī)器人的位姿確定方法，所述移動(dòng)機(jī)器人包括激光掃描儀，其特征在于，所 述方法包括： 使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建所述移動(dòng)機(jī)器人在其 中導(dǎo)航的環(huán)境的全局地圖； 在所述移動(dòng)機(jī)器人上電時(shí)，使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技 術(shù)創(chuàng)建針對(duì)所述移動(dòng)機(jī)器人的上電時(shí)刻的環(huán)境的局部地圖；以及 將所述局部地圖在所述全局地圖中進(jìn)行圖像匹配，獲取所述移動(dòng)機(jī)器人在全局地圖中 的初始位姿。2. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述移動(dòng)機(jī)器人還包括慣性導(dǎo)航傳感器， 所述方法還包括： 在所述移動(dòng)機(jī)器人的位姿發(fā)生改變時(shí)，利用所述慣性導(dǎo)航傳感器計(jì)算所述移動(dòng)機(jī)器人 在所述全局地圖中的第一位姿； 使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建關(guān)于所述移動(dòng)機(jī)器人 的周圍環(huán)境的局部地圖； 將所述局部地圖在全局地圖中進(jìn)行圖像匹配，獲取所述移動(dòng)機(jī)器人在所述全局地圖中 的第二位姿和與所述第二位姿對(duì)應(yīng)的匹配可信度，其中所述第二位姿為圖形匹配得出的所 述移動(dòng)機(jī)器人在所述全局地圖中的最佳位姿；以及 根據(jù)所述匹配可信度，確定所述移動(dòng)機(jī)器人的位姿為所述第一位姿或者所述第二位 姿。3. 根據(jù)權(quán)利要求2所述的方法，其特征在于，所述匹配可信度為使用所述激光掃描儀在 所述局部地圖中進(jìn)行激光掃描獲得的障礙輪廓在所述全局地圖中的映射輪廓的坐標(biāo)的像 素和、并且對(duì)所述像素和進(jìn)行歸一化的結(jié)果。4. 根據(jù)權(quán)利要求1所述的方法，其特征在于，還包括： 在移動(dòng)機(jī)器人位姿初始化或者位姿發(fā)生改變時(shí)，根據(jù)移動(dòng)機(jī)器人室內(nèi)定位技術(shù)估計(jì)移 動(dòng)機(jī)器人的當(dāng)前位姿； 訪問參考物數(shù)據(jù)庫，所述參考物數(shù)據(jù)庫包括所述移動(dòng)機(jī)器人配備的攝像頭拍攝的、關(guān) 于至少一個(gè)參考物中的每個(gè)參考物的一幅參考圖像，以及與所述參考圖像關(guān)聯(lián)的、所述攝 像頭拍攝所述參考圖像時(shí)所述移動(dòng)機(jī)器人的相應(yīng)真實(shí)位姿，并且從所述參考物數(shù)據(jù)庫中選 擇一個(gè)真實(shí)位姿和相關(guān)聯(lián)的參考圖像，其中所述參考物是室內(nèi)環(huán)境中的靜止物體； 將所述移動(dòng)機(jī)器人導(dǎo)航至目標(biāo)位姿，所述目標(biāo)位姿的數(shù)值是根據(jù)移動(dòng)機(jī)器人室內(nèi)定位 技術(shù)對(duì)于所選擇的真實(shí)位姿的估計(jì)值； 利用所述移動(dòng)機(jī)器人配備的所述攝像頭進(jìn)行圖像拍攝，獲得拍攝圖像； 將所述拍攝圖像中包括的參考物圖像、即實(shí)際參考物圖像與所選擇的相關(guān)聯(lián)的參考圖 像中包括的參考物圖像、即數(shù)據(jù)庫參考物圖像進(jìn)行對(duì)比； 根據(jù)對(duì)比的結(jié)果來確定所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿誤差；以及 利用所確定的位姿誤差對(duì)根據(jù)所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)所估計(jì)的移動(dòng)機(jī)器人的 位姿進(jìn)行誤差校正。5. 根據(jù)權(quán)利要求4所述的方法，其特征在于，根據(jù)對(duì)比的結(jié)果來確定所述移動(dòng)機(jī)器人室 內(nèi)定位技術(shù)產(chǎn)生的位姿誤差進(jìn)一步包括： 根據(jù)對(duì)比的結(jié)果調(diào)整所述移動(dòng)機(jī)器人的位姿，并且對(duì)所述移動(dòng)機(jī)器人配備的所述攝像 頭所拍攝的拍攝圖像中包括的實(shí)際參考物圖像與所述數(shù)據(jù)庫參考物圖像進(jìn)行特征匹配，使 得達(dá)到最佳匹配度；以及 根據(jù)特征匹配的結(jié)果來確定所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)產(chǎn)生的位姿誤差。6. 根據(jù)權(quán)利要求5所述的方法，其特征在于，所述根據(jù)對(duì)比的結(jié)果調(diào)整所述移動(dòng)機(jī)器人 的位姿，并且對(duì)所述移動(dòng)機(jī)器人配備的所述攝像頭所拍攝的拍攝圖像中包括的實(shí)際參考物 圖像與所述數(shù)據(jù)庫參考物圖像進(jìn)行特征匹配，使得達(dá)到最佳匹配度包括： 通過對(duì)移動(dòng)機(jī)器人進(jìn)行以下各項(xiàng)操作中的一項(xiàng)或多項(xiàng)：向左和/或向右移動(dòng)、向左和/ 或向右旋轉(zhuǎn)、以及向前和/或向后調(diào)整，使得所述移動(dòng)機(jī)器人配備的所述攝像頭所拍攝的拍 攝圖像中包括的實(shí)際參考物圖像與所述數(shù)據(jù)庫參考物圖像在形狀方面和尺寸方面達(dá)到最 佳匹配度； 通過向左和/或向右旋轉(zhuǎn)移動(dòng)機(jī)器人，使得所述攝像頭所拍攝的拍攝圖像中包括的實(shí) 際參考物圖像與所述數(shù)據(jù)庫參考物圖像在形狀方面達(dá)到最佳匹配度;或者 通過向前和/或向后調(diào)整移動(dòng)機(jī)器人，使得所述攝像頭所拍攝的拍攝圖像中包括的實(shí) 際參考物圖像與所述數(shù)據(jù)庫參考物圖像在尺寸方面達(dá)到最佳匹配度。7. 根據(jù)權(quán)利要求1-6中任一項(xiàng)所述的方法，其特征在于，所述根據(jù)特征匹配的結(jié)果來確 定所述移動(dòng)機(jī)器人室內(nèi)技術(shù)產(chǎn)生的位姿誤差包括： 根據(jù)所述移動(dòng)機(jī)器人室內(nèi)定位技術(shù)估計(jì)特征匹配后的所述移動(dòng)機(jī)器人的位姿（Pl， Al)，并且根據(jù)所述特征匹配后的位姿(PI，A1)和所述參考物數(shù)據(jù)庫中存儲(chǔ)的、與拍攝所述 達(dá)到最佳匹配度的參考圖像時(shí)的所述移動(dòng)機(jī)器人的相應(yīng)真實(shí)位姿來確定所述移動(dòng)機(jī)器人 室內(nèi)定位技術(shù)產(chǎn)生的位姿誤差。8. 根據(jù)權(quán)利要求4所述的方法，其特征在于，根據(jù)對(duì)比的結(jié)果來確定所述移動(dòng)機(jī)器人室 內(nèi)定位技術(shù)產(chǎn)生的位姿誤差進(jìn)一步包括： 通過所述實(shí)際參考物圖像和所述數(shù)據(jù)庫參考物圖像距離各自拍攝圖像左邊緣或者右 邊緣的距離差來確定航向誤差;或者 通過移動(dòng)機(jī)器人拍攝所述實(shí)際參考物圖像時(shí)距離參考物的距離和參考物數(shù)據(jù)庫中存 儲(chǔ)的移動(dòng)機(jī)器人拍攝數(shù)據(jù)庫參考物圖像時(shí)距離參考物的距離來確位姿誤差。9. 一種移動(dòng)機(jī)器人的位姿確定設(shè)備，所述移動(dòng)機(jī)器人包括激光掃描儀，其特征在于，所 述設(shè)備包括： 全局地圖創(chuàng)建裝置，用于使用所述激光掃描儀，利用即時(shí)定位與地圖構(gòu)建（SLAM)技術(shù) 創(chuàng)建所述移動(dòng)機(jī)器人在其中導(dǎo)航的環(huán)境的全局地圖； 局部地圖創(chuàng)建裝置，用于在所述移動(dòng)機(jī)器人上電時(shí)，使用所述激光掃描儀，利用即時(shí)定 位與地圖構(gòu)建(SLAM)技術(shù)創(chuàng)建針對(duì)所述移動(dòng)機(jī)器人的上電時(shí)刻的環(huán)境的局部地圖；以及 初始位姿獲取裝置，用于將所述局部地圖在所述全局地圖中進(jìn)行圖像匹配，獲取所述 移動(dòng)機(jī)器人在全局地圖中的初始位姿。10. -種在計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中實(shí)現(xiàn)的計(jì)算機(jī)程序產(chǎn)品，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì) 具有存儲(chǔ)于其中的計(jì)算機(jī)可讀程序代碼部分，所述計(jì)算機(jī)可讀程序代碼部分被配置為執(zhí)行 根據(jù)權(quán)利要求1-8所述的方法。
【文檔編號(hào)】G01C11/06GK105928505SQ201610244325
【公開日】2016年9月7日
【申請(qǐng)日】2016年4月19日
【發(fā)明人】王加加, 劉彪, 王可可, 劉英英
【申請(qǐng)人】深圳市神州云海智能科技有限公司