本發(fā)明涉及機(jī)器人自主導(dǎo)航，具體涉及一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖系統(tǒng)。
背景技術(shù)：
：伴隨著信號處理、人工智能、機(jī)械制造等技術(shù)的進(jìn)步，擁有環(huán)境感知、自主導(dǎo)航以及人機(jī)交互的機(jī)器人的需求也越來越強(qiáng)烈。具有自主意識的機(jī)器人要求機(jī)器人對于環(huán)境有很好的感知、判斷和適應(yīng)能力，從而可以完成很多人類希望它們?nèi)ネ瓿傻娜蝿?wù)。機(jī)器人的自主性就是在未知的環(huán)境中通過機(jī)器人的自主決策來完成一些任務(wù)。而在未知的環(huán)境中，機(jī)器人的自主導(dǎo)航是研究中的一個難點(diǎn)。對于家庭服務(wù)式機(jī)器人來說，大部分的運(yùn)行環(huán)境都是在室內(nèi)，而室內(nèi)環(huán)境一般比較復(fù)雜，所以室內(nèi)導(dǎo)航的好壞直接影響家庭服務(wù)機(jī)器人的各種功能。要完成地圖的創(chuàng)建，首先要獲取周圍環(huán)境的信息，這就要用到傳感器來完成?，F(xiàn)在機(jī)器人的制圖研究熱點(diǎn)大體上集中在兩種傳感器上，一種是激光傳感器，這種傳感器有很好的快速性，并且可以得到很準(zhǔn)確地距離等信息；另一種比較常用的傳感器就是視覺傳感器，包括單目、雙目以及深度相機(jī)等。這兩種常用的傳感器各自有自己的優(yōu)點(diǎn)以及缺點(diǎn)，適用于不同的環(huán)境。對于機(jī)器人來說，制圖不僅要有一定的精度，并且對于快速性也有很高的要求，否則無法完成同步定位與制圖。在小型環(huán)境中，激光傳感器擁有很高的制圖精度，而到了大型環(huán)境中，機(jī)器人運(yùn)行一段時間后，由于累積誤差的存在地圖會有較大程度的形變?，F(xiàn)有的激光SLAM算法中，對于這個問題的解決，一般是引進(jìn)閉環(huán)檢測來消除累積誤差。但是由于激光傳感器所采集的信息量較少，所以在判斷閉環(huán)時從實(shí)際閉環(huán)點(diǎn)到判斷出來的閉環(huán)點(diǎn)之間有一個較大的誤差。而且當(dāng)機(jī)器人處在幾何環(huán)境特征重復(fù)率很高的環(huán)境中時，單獨(dú)根據(jù)激光傳感器所得的信息來判斷閉環(huán)有一定概率出現(xiàn)錯誤匹配。技術(shù)實(shí)現(xiàn)要素：本發(fā)明目的在于克服現(xiàn)有技術(shù)的不足，尤其解決現(xiàn)有的基于激光傳感器的機(jī)器人室內(nèi)定位方法累積誤差高、采集的信息量較少、匹配精度不高的問題。本發(fā)明方案用單目視覺信息來輔助進(jìn)行激光SLAM室內(nèi)定位，利用單目視覺方法來檢測閉環(huán)并給出一個閉環(huán)檢測信號幫助解決激光閉環(huán)檢測成功率過低的問題，提出使用對極幾何約束過濾錯誤匹配的視覺局部特征，提高了視覺局部特征的匹配準(zhǔn)確率，保證了視覺匹配的準(zhǔn)確性；同時，本方案中的視覺信息僅用于建立視覺特征而無需建立視覺地圖，省去了建立視覺地圖、更新視覺地圖耗時的步驟，利用了激光SLAM快速定位的同時，引入準(zhǔn)確的視覺閉環(huán)檢測。為解決上述技術(shù)問題，本發(fā)明采用如下的技術(shù)方案：一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖系統(tǒng)，該系統(tǒng)包括激光定位、視覺閉環(huán)檢測和地圖優(yōu)化模塊三個部分，其特征在于，其中：所述激光定位由激光幀間匹配和激光閉環(huán)檢測兩部分組成，其中：激光幀間檢測用于獲取激光傳感器以實(shí)現(xiàn)匹配，并將匹配結(jié)果用于激光閉環(huán)檢測；激光閉環(huán)檢測用于使用當(dāng)前點(diǎn)和以往走過的點(diǎn)的激光信息進(jìn)行比較，判斷機(jī)器人是否已經(jīng)走過該點(diǎn)，并以此作為地圖是否需要更新的依據(jù)。所述視覺閉環(huán)檢測用于使用當(dāng)前點(diǎn)和以往走過的點(diǎn)的視覺信息進(jìn)行比較，判斷機(jī)器人是否已經(jīng)走過該點(diǎn)，并以此作為是否需進(jìn)行激光閉環(huán)檢測的依據(jù)。所述地圖優(yōu)化模塊用于計(jì)算預(yù)計(jì)觀測值與通過傳感器采集到的實(shí)際觀測值的偏差，然后對激光閉環(huán)檢測進(jìn)行校正，并且更新已有地圖。一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖系統(tǒng)所實(shí)現(xiàn)的一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖方法，其過程包括以下步驟：S1：激光幀間匹配，根據(jù)激光傳感器獲取到的兩幀數(shù)據(jù)，建立幀間的對應(yīng)關(guān)系以及轉(zhuǎn)換坐標(biāo)；S2：視覺閉環(huán)檢測，視覺閉環(huán)檢測使用視覺信息對機(jī)器人當(dāng)前位置進(jìn)行閉環(huán)檢測，根據(jù)視覺閉環(huán)檢測的結(jié)果決定是否執(zhí)行激光閉環(huán)檢測；S3：激光閉環(huán)檢測，根據(jù)一個檢測到的地圖碎片以及已有地圖集合，計(jì)算機(jī)器人當(dāng)前處于位置，并以此判斷當(dāng)前是否處于閉環(huán)；S4：地圖優(yōu)化，根據(jù)閉環(huán)檢測的結(jié)果迭代調(diào)整特征點(diǎn)和攝像頭位姿。進(jìn)一步地，本發(fā)明的步驟S1，具體過程描述如下：(1)對于當(dāng)前機(jī)器人位姿圖像中的每一點(diǎn)，都找出離他最近的線段，稱為目標(biāo)線段，并對應(yīng)保存起來；(2)找到一個當(dāng)前最優(yōu)的變換。這個最優(yōu)的變換可以使當(dāng)前圖像中的所有點(diǎn)到它的目標(biāo)線段的距離的平方和最??；(3)根據(jù)第二步中找出的最優(yōu)變換來變換當(dāng)前圖像中的所有點(diǎn)，以及旋轉(zhuǎn)當(dāng)前地圖；(4)重復(fù)上面(1)到(3)的步驟，直到第二步中的結(jié)果小于一個閾值。附圖說明圖1是本發(fā)明的一個具體實(shí)施例的結(jié)構(gòu)示意圖。圖2為本發(fā)明的一個具體實(shí)施方式特征精確匹配的方法流程圖。具體實(shí)施方式下面結(jié)合附圖及具體實(shí)施例對本發(fā)明進(jìn)行更加詳細(xì)與完整的說明?？梢岳斫獾氖?，此處所描述的具體實(shí)施例僅用于解釋本發(fā)明，而非對本發(fā)明的限定。參見圖1，一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖系統(tǒng)，該系統(tǒng)包括激光定位1、視覺閉環(huán)檢測2和地圖優(yōu)化模塊3，其中：所述激光定位1由激光幀間匹配1-1和激光閉環(huán)檢測1-2兩部分組成，其中：激光幀間檢測1-1用于獲取激光傳感器4以實(shí)現(xiàn)匹配，并將匹配結(jié)果用于激光閉環(huán)檢測1-2；激光閉環(huán)檢測1-2用于使用當(dāng)前點(diǎn)和以往走過的點(diǎn)的激光信息進(jìn)行比較，判斷機(jī)器人是否已經(jīng)走過該點(diǎn)，并以此作為地圖是否需要更新的依據(jù)。所述視覺閉環(huán)檢測2使用單目攝像頭5采集當(dāng)前點(diǎn)和以往走過的點(diǎn)的視覺信息進(jìn)行比較，判斷機(jī)器人是否已經(jīng)走過該點(diǎn)，并以此作為是否需進(jìn)行激光閉環(huán)檢測1-2的依據(jù)。所述地圖優(yōu)化模塊3接收來自激光定位1的計(jì)算觀測值與通過激光傳感器4采集到的實(shí)際觀測值的偏差，然后對激光閉環(huán)檢測1-2進(jìn)行校正，并且更新已有地圖。一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖系統(tǒng)所實(shí)現(xiàn)的一種基于多傳感器的機(jī)器人室內(nèi)定位及制圖方法，其過程包括以下步驟：S1：激光幀間匹配，根據(jù)激光傳感器獲取到的兩幀數(shù)據(jù)，建立幀間的對應(yīng)關(guān)系以及轉(zhuǎn)換坐標(biāo)，具體描述如下：計(jì)算出當(dāng)前幀中每一個變換后的目標(biāo)點(diǎn)到變換后的目標(biāo)線段的距離：當(dāng)前最優(yōu)變換用平移向量t和轉(zhuǎn)角θ來表示，當(dāng)前幀中第i個目標(biāo)點(diǎn)為vi，為所有目標(biāo)點(diǎn)的重心，第i條目標(biāo)線段的正交單位向量為ui。在目標(biāo)線段上任取一點(diǎn)z，將z圍繞按以下變換來旋轉(zhuǎn)θ角度，則z如公式1所示：z=cosθ-sinθsinθcosθ(z-v‾)+v‾---(1)]]>當(dāng)機(jī)器人的激光傳感器采樣頻率較高時，轉(zhuǎn)角θ可以近似為極小，則z可以變換如公式2所示：z=1-θθ1(z-v‾)+v‾=θ0-110(z-v‾)+z---(2)]]>對于目標(biāo)點(diǎn)vi同時如果考慮加入平移因子t，則變換后的目標(biāo)點(diǎn)vi如公式3如下：vi=t+θ0-110(vi-v‾)+vi---(3)]]>則變換后的目標(biāo)點(diǎn)到變換后的目標(biāo)線段的距離d如公式4所示：d＝|ui·(vi-z)|2(4)計(jì)算出當(dāng)前幀中的所有目標(biāo)點(diǎn)到目標(biāo)線段的距離，尋找使得累加距離最小的變換，即是滿足激光幀間匹配的最優(yōu)變換。S2：視覺閉環(huán)檢測，視覺閉環(huán)檢測使用視覺信息對機(jī)器人當(dāng)前位置進(jìn)行閉環(huán)檢測，根據(jù)視覺閉環(huán)檢測的結(jié)果決定是否執(zhí)行激光閉環(huán)檢測，具體步驟如下：(1)參見圖2，根據(jù)激光幀間檢測的匹配結(jié)果進(jìn)行特征精確匹配，從匹配的局部特征集合中選取任意一對局部特征，用于計(jì)算要匹配的圖像與目標(biāo)圖像的基礎(chǔ)矩陣F，具體做法如下：設(shè)M為三維空間中一點(diǎn)，在左右兩幅圖像上的投影分別為m1和m2，C1和C2為兩相機(jī)的光心，C1C2連線與左右兩幅圖像分別交于點(diǎn)e1和e2，稱為對極點(diǎn)。其中投影點(diǎn)m1和m2與基礎(chǔ)矩陣F滿足下面的關(guān)系。在代數(shù)上，這一幾何關(guān)系可以表示為公式5所示：m2TFm1=0Fe=0---(5)]]>根據(jù)上述描述，兩個匹配的局部特征相當(dāng)于m1和m2，則可以根據(jù)公式5計(jì)算出基礎(chǔ)矩陣F。(2)將集合中剩余的局部特征對用于校驗(yàn)基礎(chǔ)矩陣，如能滿足的將該局部特征對作為內(nèi)點(diǎn)保留，同時計(jì)算要匹配的圖像與目標(biāo)圖像的能量函數(shù)，具體做法如下：如果剩余的局部特征對與所述基礎(chǔ)矩陣相匹配，則滿足對極幾何約束關(guān)系，約束關(guān)系描述為：一幅圖像上的點(diǎn)m1，在另一幅圖像上的匹配點(diǎn)m2位于該點(diǎn)的對極線Fm1上；相反的，一幅圖像上的點(diǎn)m2，在另一幅圖像上的匹配點(diǎn)m1位于該點(diǎn)的對極線FTm2上。這個描述可以形式化為公式6，其中d2表示空間中點(diǎn)到直線的歐式距離。d2(m1,Fm2)=0d2(m2,FTm1)=0---(6)]]>根據(jù)上述描述，將剩余的局部特征對代入公式6，如能滿足則將該局部特征對作為內(nèi)點(diǎn)保留。過濾完所有的局部特征對后，將所有的內(nèi)點(diǎn)用于計(jì)算要匹配的圖像與目標(biāo)圖像的能量函數(shù)，如公式7所示：E(I1,I2)=112KΣi=1K(d2(m1i,Fm2i)+d2(m2i,FTm1i))+β---(7)]]>參數(shù)β是歸一化能量函數(shù)的值，能量函數(shù)E(I1，I2)表示兩幅圖像I1和I2的興趣點(diǎn)對應(yīng)集合與估計(jì)的基礎(chǔ)矩陣F之間的一致程度。當(dāng)能量函數(shù)最大時，表示兩幅圖像的興趣點(diǎn)對應(yīng)的幾何約束程度最高。(3)從匹配的局部特征集合中重新選取任意多對局部特征，重復(fù)上述流程(1)和(2)并計(jì)算基礎(chǔ)矩陣；(4)多次循環(huán)后，比較每次循環(huán)的基礎(chǔ)矩陣能量函數(shù)和內(nèi)點(diǎn)個數(shù)，保留最優(yōu)基礎(chǔ)矩陣F，并用該最優(yōu)基礎(chǔ)矩陣過濾匹配的局部特征集合，實(shí)現(xiàn)局部特征的精確匹配。根據(jù)視覺精確匹配的結(jié)果，判斷當(dāng)前機(jī)器人位置是否處于之前經(jīng)過的位置，如果是，則輸出閉環(huán)結(jié)果，否則該位置并非閉環(huán)位置，則無需進(jìn)行下一步激光閉環(huán)檢測操作。S3：激光閉環(huán)檢測，根據(jù)一個檢測到的地圖碎片r以及已有地圖集合mi∈m，計(jì)算機(jī)器人當(dāng)前處于位置l的先驗(yàn)概率p(l|r，m)，選取出先驗(yàn)概率最大的位置作為機(jī)器人當(dāng)前位置，并依此判斷機(jī)器人是否實(shí)現(xiàn)閉環(huán)，具體步驟如下：根據(jù)貝葉斯法則，先驗(yàn)概率p(l|r，m)表示如公式8所示：p(l|r,m)=p(r|l,m)p(l,m)p(r,m)---(8)]]>將地圖分割為多個單元格，對于每個單元格mi∈m計(jì)算機(jī)器人出現(xiàn)在當(dāng)前單元格的概率p(mi)，則p(r|l，m)可以近似的表示為公式9：p(r|l,m)=Σip(r)·p(mi)---(9)]]>由于地圖m是以前已經(jīng)構(gòu)建好的，p(l，m)可以簡化為p(l)，在匹配區(qū)域上是均勻分布的，可以用均勻分布概率來表示；p(r，m)則表示為檢測到的地圖碎片r以及已有地圖m的概率，則p(r，m)＝1；則先驗(yàn)概率p(l|r，m)可用公式10表示：p(l|r,m)=p(l)·Σip(r)·p(mi)---(10)]]>選取出先驗(yàn)概率最大的位置作為機(jī)器人當(dāng)前位置，并依此判斷機(jī)器人是否實(shí)現(xiàn)閉環(huán)。S4：地圖優(yōu)化，根據(jù)閉環(huán)檢測的結(jié)果迭代調(diào)整特征點(diǎn)和攝像頭位姿，具體過程如下：機(jī)器人在全局下的坐標(biāo)ci用公式11表示為：ci=[tiT,θi]T---(11)]]>其中，i＝1，2，…，n表示第i個機(jī)器人位姿在世界坐標(biāo)系的平移的轉(zhuǎn)置，θi，i＝1，2，…，n表示第i個機(jī)器人位姿在世界坐標(biāo)系的方向，n表示圖中所有機(jī)器人位姿的個數(shù)。則對于任意兩個點(diǎn)ci和cj，兩點(diǎn)相對距離h(ci，cj)用公式12表示為：h(ci,cj)=RiT(tj-ti)θj-θi---(12)]]>其中，表示方向的θi旋轉(zhuǎn)矩陣的轉(zhuǎn)置，zij表示點(diǎn)ci和cj的測量距離，則距離誤差表示為eij≡zij-h(ci，cj)，距離誤差的約束函數(shù)用公式13表示為：F(c,e)=Σi,jeijTΛeij---(13)]]>最小化公式13的約束函數(shù)用于查找坐標(biāo)c優(yōu)化的估計(jì)值，采用LM方法進(jìn)行求解，具體過程如下：(1)引入函數(shù)：(H+λ·Hdiag)Δc＝JTΛe(14)其中，H＝JTΛe，Λ＝diag(Λij)，e表示坐標(biāo)c的誤差，J表示求導(dǎo)，H表示矩陣，Hdiag表示矩陣H對角線上的函數(shù)值構(gòu)造的一個對角矩陣，λ表示一個較小的正系數(shù)，Δc表示增量，Λij表示測量距離協(xié)方差的平方。(2)初始化λ和H，分別賦予初值。(3)用梯度下降法求解公式14。(4)根據(jù)求解結(jié)果更新Δc，如果誤差e減小，則將λ的值縮小一半，并更新坐標(biāo)為c＝c+Δc；如果誤差e增大，則將λ的值增大一倍，仍然保留當(dāng)前姿態(tài)c。該發(fā)明方案的有益效果在于，用單目視覺信息來輔助進(jìn)行激光SLAM室內(nèi)定位，利用單目視覺方法來檢測閉環(huán)并給出一個閉環(huán)檢測信號幫助解決激光閉環(huán)檢測成功率過低的問題，提出使用對極幾何約束過濾錯誤匹配的視覺局部特征，提高了視覺局部特征的匹配準(zhǔn)確率，保證了視覺匹配的準(zhǔn)確性；同時，本方案中的視覺信息僅用于建立視覺特征而無需建立視覺地圖，省去了建立視覺地圖、更新視覺地圖耗時的步驟，利用了激光SLAM快速定位的同時，引入準(zhǔn)確的視覺閉環(huán)。上述為本發(fā)明較佳的實(shí)施方式，但本發(fā)明的實(shí)施方式并不受上述內(nèi)容的限制，其他的任何未背離本發(fā)明的精神實(shí)質(zhì)與原理下所作的改變、修飾、替代、組合、簡化，均應(yīng)為等效的置換方式，都包含在本發(fā)明的保護(hù)范圍之內(nèi)。當(dāng)前第1頁1 2 3