一種基于Kinect的機(jī)器人自定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于Kinect的機(jī)器人自定位方法，屬于機(jī)器人自主定位導(dǎo)航領(lǐng) 域。
【背景技術(shù)】
[0002] 隨著計(jì)算機(jī)和機(jī)器人技術(shù)的發(fā)展，移動(dòng)機(jī)器人得到了快速發(fā)展和廣泛應(yīng)用，幾乎 滲透到了社會(huì)生活的各個(gè)領(lǐng)域。傳統(tǒng)的自主定位方法中，基于磁條、磁釘?shù)扔熊壍淖远ㄎ环?式涉及到磁條的鋪設(shè)，環(huán)境的改造和維護(hù)成本大，基于激光的無軌自定位方法，激光傳感器 通常十分昂貴，無法實(shí)現(xiàn)普遍應(yīng)用推廣。由于技術(shù)和成本限制，對(duì)機(jī)器人無軌自定位，目前 還沒有低成本的、穩(wěn)定的解決方案。
[0003] 近年來，低成本的Kinect等RGB-D相機(jī)，已經(jīng)越來越多的用于移動(dòng)機(jī)器人的定位 和導(dǎo)航。Kinect相機(jī)由一個(gè)普通相機(jī)和一個(gè)紅外相機(jī)組成，分別用于采集顏色（RGB)信 息和深度（Depth)信息，它的集成度高，價(jià)格低廉，因而更適和在一般的家庭和辦公服務(wù)等 場合應(yīng)用。然而，Kinect相機(jī)的精度較低，視野較小，可視范圍較近，噪聲較大，這些都為 Kinect相機(jī)的定位和導(dǎo)航，帶來了巨大的困難和挑戰(zhàn)。

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

[0004] 本發(fā)明的目的是針對(duì)現(xiàn)有的激光定位的缺陷，提供一種基于Kinect相機(jī)的機(jī)器 人自定位方法。
[0005] 本發(fā)明的目的是通過以下技術(shù)方案來實(shí)現(xiàn)的：一種基于Kinect的機(jī)器人自定位 方法，包括以下步驟：
[0006] (1)將Kinect相機(jī)固定安裝在機(jī)器人上，通過Kinect相機(jī)采集環(huán)境的RGB圖像信 息和深度圖像信息；
[0007] (2)將深度圖像信息進(jìn)行三維恢復(fù)，得到三維點(diǎn)云數(shù)據(jù)：設(shè)Kinect深度相機(jī)的 焦距為亡和f y，光心為（cx, cy)，通過公式（1)恢復(fù)深度圖像上任一點(diǎn)（X，y)的三維坐標(biāo) (X, Y, Z)；
[0011] 其中z為深度值；
[0012] (3)利用平面提取算法在三維點(diǎn)云中提取出地面部分的點(diǎn)云，并將地面部分的 點(diǎn)云進(jìn)行平面擬合，得到地面方程；根據(jù)相機(jī)坐標(biāo)系下的地面方程，通過坐標(biāo)求解，計(jì)算 Kinect相機(jī)相對(duì)于地面的高度h和俯仰角貨；
[0013] (4)利用基于尺度不變、旋轉(zhuǎn)不變的特征提取算法，得到RGB圖像中的特征點(diǎn)，并 通過基于像素的特征描述子對(duì)其進(jìn)行描述，通過對(duì)相鄰時(shí)刻采集的兩幀RGB圖像中的特征 點(diǎn)進(jìn)行特征匹配，獲取相機(jī)在時(shí)間段內(nèi)的相對(duì)運(yùn)動(dòng)關(guān)系，用三維旋轉(zhuǎn)矩陣R和平移向量t來 描述，形式如下：
[0016] 下標(biāo)c表示相機(jī)坐標(biāo)系下的運(yùn)動(dòng)，相機(jī)坐標(biāo)系方向分別是：x軸指向相機(jī)正右方，y 軸指向相機(jī)正下方，z軸指向相機(jī)正前方；
[0017] (5)根據(jù)步驟（3)中得到的相機(jī)高度h和俯仰角^將步驟（4)中得到的相機(jī)運(yùn) 動(dòng)R和t，通過公式轉(zhuǎn)換到地面坐標(biāo)系，得到基于視覺的機(jī)器人在二維地面的相對(duì)運(yùn)動(dòng) (△χ ν，^yv, △ θν)，其中，下標(biāo)V表示該相對(duì)運(yùn)動(dòng)是基于視覺給出的結(jié)果，地面坐標(biāo)系方向 為軸為機(jī)器人正前方，y軸為機(jī)器人正右方；轉(zhuǎn)換過程中，先根據(jù)相機(jī)高度h和俯仰角F 得到相機(jī)相對(duì)于機(jī)器人的相對(duì)位姿關(guān)系，
[0018]
[0019] 7-(0,0,/?)
[0020] 左下標(biāo)c和左上標(biāo)r表示相機(jī)相對(duì)于機(jī)器人的位姿關(guān)系，然后再計(jì)算相機(jī)在機(jī)器 人坐標(biāo)系的運(yùn)動(dòng)氺和公式如下：
[0023] 最后，將其投影到二維地面，得到相對(duì)運(yùn)動(dòng)（Δ χν，Δ yv，Δ θ v)，其中Δ \和Δ y力 彳的前兩維，△ θ J相對(duì)于z軸的轉(zhuǎn)角；
[0024] (6)通過里程計(jì)獲取機(jī)器人的相對(duì)運(yùn)動(dòng)信息（Δ X。，Δ y。，Δ Θ。)，下標(biāo)〇表示里程 計(jì)給出的結(jié)果；
[0025] (7)利用在線信息融合算法，將視覺和里程計(jì)給出的兩種機(jī)器人相對(duì)運(yùn)動(dòng)量，即 (Δ Χν，Δ yv, Δ θ v)和（△ X。，Δ y。，Δ Θ。）進(jìn)行融合，準(zhǔn)確估計(jì)機(jī)器人在t時(shí)刻的相對(duì)運(yùn)動(dòng)量 (Axt，^yt, Δ Θ t);信息融合公式為：
[0029] 其中，《。和Wv分別是里程計(jì)信息和視覺信息的權(quán)值，表示兩種信息的可信程度；
[0030] (8)根據(jù)機(jī)器人上一時(shí)刻位姿（xt D yt η Θ t D和相對(duì)運(yùn)動(dòng)信息 (AXt，Ayt, △ 0t)，估計(jì)機(jī)器人當(dāng)前時(shí)刻的位姿（Xt，yt，0t)，從而實(shí)現(xiàn)機(jī)器人位姿的跟蹤； 計(jì)算公式為：
[0034] (9)根據(jù)步驟（3)中提取的地面，將三維點(diǎn)云數(shù)據(jù)投影到地面，得到類似于激光數(shù) 據(jù)的二維點(diǎn)云；
[0035] (10)利用二維地圖構(gòu)建的SLAM算法，通過激光或Kinect傳感器構(gòu)建出二維環(huán)境 地圖；
[0036] (11)通過地圖匹配算法，將投影后的二維點(diǎn)云與步驟（10)進(jìn)行匹配，從而修正機(jī) 器人跟蹤過程中的累積誤差，準(zhǔn)確的估計(jì)機(jī)器人在二維地圖中的位置和朝向角，實(shí)現(xiàn)實(shí)時(shí) 自定位。
[0037] 進(jìn)一步地，所述步驟（3)具體包括以下子步驟：
[0038] (3. 1)地面點(diǎn)云分割階段：給定點(diǎn)云（Pi= (X ;，y;，ZiMi = 1...η，η為點(diǎn)的個(gè)數(shù)，通 過法向量特征和距離特征來判斷任意兩點(diǎn)是否屬于同一個(gè)平面，并以此作為擴(kuò)展規(guī)則對(duì)所 有點(diǎn)進(jìn)行擴(kuò)展，將同一個(gè)平面的點(diǎn)拓展到一起，從而將點(diǎn)云分割成一系列的平面{ Π 1 = {pu，pi2. · · pik}} i = L..m，m 為平面的個(gè)數(shù)；
[0039] (3. 2)地面方程擬合階段：先粗略的給出相機(jī)初始高度瓦和初始俯仰角度I，在 所有平面中，搜索出與初始假設(shè)最接近的平面；搜索方法是，步驟（3. 1)得到的平面點(diǎn)的法 向量和距離十分接近，以其平均值作為該平面在相機(jī)坐標(biāo)系中的法向量和原點(diǎn)到平面的距 離，從而搜索出地面Π = {Pl，p2... pk}，利用最小二乘擬合求出地面的平面方程nT (X-p)= 〇,其中η為地面的法向量，ρ為平面上一點(diǎn)，X = (X，y, ζ)τ為廣義坐標(biāo)；
[0040] (3. 3)相機(jī)位姿參數(shù)求解階段：給定地面的平面方程ητ(X-p) = 0,求相機(jī)原點(diǎn)到 平面的距離d，得到當(dāng)前相機(jī)的高度h，求相機(jī)正前方的單位方向向量與平面法向量η之間 的夾角，得到當(dāng)前Kinect相機(jī)的俯仰角度爐_ &
[0041] 本發(fā)明的有益效果是：
[0042] 1.本發(fā)明采用廉價(jià)的Kinect相機(jī)替代昂貴的激光傳感器，實(shí)現(xiàn)機(jī)器人的自主定 位，極大的降低了成本。同時(shí)，針對(duì)Kinect相機(jī)感知視野小、噪聲大的缺點(diǎn)，通過融合視覺 特征和物理里程計(jì)，準(zhǔn)確的估計(jì)機(jī)器人的運(yùn)動(dòng)并進(jìn)行位姿跟蹤，并通過深度信息恢復(fù)得到 的點(diǎn)云數(shù)據(jù)與地圖進(jìn)行匹配，進(jìn)一步修正機(jī)器人的定位誤差，可以實(shí)現(xiàn)實(shí)時(shí)的、高精度的、 穩(wěn)定的機(jī)器人自定位。
[0043] 2.本發(fā)明通過觀測到的地面，自動(dòng)矯正相機(jī)的實(shí)時(shí)高度和俯仰角。不需要實(shí)現(xiàn)對(duì) 高度和俯仰角等參數(shù)進(jìn)行標(biāo)定，并且能夠應(yīng)付機(jī)器人顛簸、搖擺導(dǎo)致的相機(jī)高度和俯仰角 變化的問題。
[0044] 3.本發(fā)明雖然采用Kinect相機(jī)，但是同樣可以兼容使用激光采集的地圖進(jìn)行自 定位。使用Kinect替代激光以后，不需要通過Kinect再次掃描地圖，便可以進(jìn)行機(jī)器人實(shí) 時(shí)自定位。
【附圖說明】
[0045] 圖1為地面平面方程與Kinect相機(jī)高度和俯仰角的坐標(biāo)轉(zhuǎn)換關(guān)系。
【具體實(shí)施方式】
[0046] 下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)說明。
[0047] 本發(fā)明一種基于Kinect的機(jī)器人自定位方法，包括以下步驟：
[0048] (1)將Kinect相機(jī)固定安裝在機(jī)器人上，通過Kinect相機(jī)采集環(huán)境的RGB圖像信 息和深度圖像信息；
[0049] (2)將深度圖像信息進(jìn)行三維恢復(fù)，得到三維點(diǎn)云數(shù)據(jù)：設(shè)Kinect深度相機(jī)的 焦距為亡和f y，光心為（cx, cy)，通過公式（1)恢復(fù)深度圖像上任一點(diǎn)（X，y)的三維坐標(biāo) (X, Y, Z)；
[0053] 其中z為深度值；
[0054] (3)利用平面提取算法在三維點(diǎn)云中提取出地面部分的點(diǎn)云，并將地面部分的 點(diǎn)云進(jìn)行平面擬合，得到地面方程；根據(jù)相機(jī)坐標(biāo)系下的地面方程，通過坐標(biāo)求解，計(jì)算 Kinect相機(jī)相對(duì)于地面的高度h和俯仰角如圖1所示，具體包括以下子步驟：
[0055] (3. 1)地面點(diǎn)云分割階段：給定點(diǎn)云（Pi= (X ;，y;，ZiMi = 1...η，η為點(diǎn)的個(gè)數(shù)，通 過法向量特征和距離特征來判斷任意兩點(diǎn)是否屬于同一個(gè)平面，并以此作為擴(kuò)展規(guī)則對(duì)所 有點(diǎn)進(jìn)行擴(kuò)展，將同一個(gè)平面的點(diǎn)拓展到一起，從而將點(diǎn)云分割成一系列的平面{ Π 1 = {pu，pi2. · · pik}} i = L..m，m 為平面的個(gè)數(shù)；
[0056] (3. 2)地面方程擬合階段：先粗略的給出相機(jī)初始高度I和初始俯仰角度沒,在 所有平面中，搜索出與初始假設(shè)最接近的平面；搜索方法是，步驟