基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法
【專利摘要】本發(fā)明揭示了一種基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，所述移動(dòng)機(jī)器人包括激光雷達(dá)和視覺(jué)傳感器，本發(fā)明根據(jù)激光雷達(dá)采集的數(shù)據(jù)和視覺(jué)傳感器采集的數(shù)據(jù)對(duì)各粒子的預(yù)測(cè)位置的權(quán)值進(jìn)行更新，然后對(duì)權(quán)值高的粒子進(jìn)行重采樣，得到移動(dòng)機(jī)器人在t時(shí)刻的真實(shí)位置分布。與現(xiàn)有技術(shù)相比，本發(fā)明技術(shù)方案綜合了激光雷達(dá)的精度高特性和視覺(jué)的信息完整性，可適用范圍更廣，并彌補(bǔ)了視覺(jué)定位不穩(wěn)定的缺點(diǎn)；此外，在本發(fā)明的一個(gè)實(shí)施例中還對(duì)傳統(tǒng)的粒子濾波采樣模型進(jìn)行了改進(jìn)，使粒子的多樣性得到了保證。
【專利說(shuō)明】
基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及移動(dòng)機(jī)器人的定位方法，特別是基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合 定位方法。
【背景技術(shù)】
[0002] 隨著移動(dòng)機(jī)器人的不斷發(fā)展，同時(shí)定位與建圖（SLAM)已經(jīng)成為了移動(dòng)機(jī)器人發(fā)展 的一個(gè)重大命題。如何解決此問(wèn)題，有多種方法，從模型上分，分為概率定位模型(如粒子濾 波)和絕對(duì)定位模型(如GPS)等;從地圖來(lái)分有柵格地圖，拓?fù)涞貓D等;從傳感器角度分有超 聲定位，激光雷達(dá)定位，視覺(jué)定位等。
[0003] 概率模型內(nèi)存和CPU的限制，絕對(duì)定位模型需要一些設(shè)備如：人工信標(biāo)，或者一些 特殊環(huán)境，如室內(nèi)就不適合GPS;柵格地圖會(huì)存在著柵格誤差，拓?fù)涞貓D維護(hù)成本比較大，摧 毀后難以恢復(fù);超聲存在著精度問(wèn)題；目前正常的激光雷達(dá)都是二維激光雷達(dá)，維度低，對(duì) 環(huán)境的識(shí)別度不高;視覺(jué)定位主要有基于特征的定位，基于三維圖匹配的定位，基于三維點(diǎn) 簇的定位等，視覺(jué)定位算法目前不是很成熟。
[0004] 但是，對(duì)于移動(dòng)機(jī)器人來(lái)說(shuō)，定位不僅要有一定的精度，并且對(duì)于快速性也有很高 的要求。
[0005] 基于上述問(wèn)題，發(fā)明人分別基于激光雷達(dá)定位和視覺(jué)定位的現(xiàn)有成果，綜合考慮 激光雷達(dá)的快速準(zhǔn)確優(yōu)點(diǎn)和視覺(jué)傳感器信息豐富的優(yōu)點(diǎn)，通過(guò)粒子濾波算法，實(shí)現(xiàn)移動(dòng)機(jī) 器人的快速、精確的定位。

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

[0006] 本發(fā)明專利申請(qǐng)的目的在于提供一種用于移動(dòng)機(jī)器人的快速、精確的定位方法， 以解決【背景技術(shù)】中的問(wèn)題。
[0007] 為達(dá)到以上目的，本發(fā)明專利申請(qǐng)的技術(shù)方案如下： S1:初始化各粒子位置及各粒子的柵格地圖和視覺(jué)特征地圖，建立各粒子的全局柵格 地圖和全局視覺(jué)特征地圖； S2:初始化移動(dòng)機(jī)器人的位置PtQ及建立移動(dòng)機(jī)器人的激光雷達(dá)全局點(diǎn)圖； S3:根據(jù)里程計(jì)和陀螺儀得到移動(dòng)機(jī)器人在t時(shí)刻的預(yù)測(cè)位置Pt; S4:根據(jù)移動(dòng)機(jī)器人的估計(jì)位置Pt和激光雷達(dá)在t時(shí)刻采集到的數(shù)據(jù)得到修正后的預(yù) 測(cè)位置P't; S5:根據(jù)移動(dòng)機(jī)器人在t-1時(shí)刻和t時(shí)刻的位置變化和各粒子在t-1時(shí)刻的位置，采樣得 至Ijt時(shí)刻各粒子的估計(jì)位置； S6:根據(jù)t時(shí)刻激光雷達(dá)采集的數(shù)據(jù)得到t時(shí)刻各粒子的估計(jì)位置的概率PL; S7:根據(jù)t時(shí)刻視覺(jué)傳感器采集的圖像得到t時(shí)刻各粒子的估計(jì)位置的概率PV; S8:根據(jù)各粒子的估計(jì)位置的概率PL、PV對(duì)粒子權(quán)值進(jìn)行更新； S9:對(duì)權(quán)值高的粒子進(jìn)行重采樣，得到移動(dòng)機(jī)器人在t時(shí)刻的真實(shí)位置分布。
[0008] 與現(xiàn)有技術(shù)相比，本發(fā)明申請(qǐng)具有以下優(yōu)點(diǎn)： 1、 本發(fā)明技術(shù)方案綜合了激光雷達(dá)的精度高特性和視覺(jué)的信息完整性，可適用范圍更 廣(室內(nèi)室外都可），通過(guò)激光雷達(dá)與視覺(jué)傳感器的結(jié)合，彌補(bǔ)了視覺(jué)定位不穩(wěn)定的缺點(diǎn)； 2、 本發(fā)明技術(shù)方案對(duì)傳統(tǒng)的粒子濾波采樣模型進(jìn)行了改進(jìn)，使粒子的多樣性得到了保 證； 3、 本發(fā)明技術(shù)方案因引入自適應(yīng)因子，可根據(jù)環(huán)境的情況實(shí)時(shí)調(diào)度激光雷達(dá)與視覺(jué)傳 感器的權(quán)重。
【附圖說(shuō)明】
[0009] 圖1為本發(fā)明申請(qǐng)的流程圖； 圖2為本發(fā)明實(shí)施例中的移動(dòng)機(jī)器人的示意圖。
【具體實(shí)施方式】
[0010]下面結(jié)合附圖和【具體實(shí)施方式】對(duì)本發(fā)明方案進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。
[0011] 如圖2所示，本實(shí)施例中的移動(dòng)機(jī)器人包括里程計(jì)、陀螺儀、激光雷達(dá)、攝像機(jī)(視 覺(jué)傳感器）、驅(qū)動(dòng)系統(tǒng)和核心控制板(M⑶），具體地，本實(shí)施例中的驅(qū)動(dòng)系統(tǒng)由左、右輪構(gòu)成， 并分別由不同的馬達(dá)驅(qū)動(dòng)，應(yīng)該可以理解的是，移動(dòng)機(jī)器人還可包括其他部分（如吸塵系 統(tǒng)、傳感器系統(tǒng)、報(bào)警系統(tǒng)等），由于與本發(fā)明的技術(shù)方案無(wú)關(guān)，因此不在此說(shuō)明。
[0012] 如圖1所示，本發(fā)明移動(dòng)機(jī)器人的基于粒子濾波算法的定位方法包括以下步驟： S1:初始化各粒子位置及各粒子的柵格地圖和視覺(jué)特征地圖，建立各粒子的全局柵格 地圖和全局視覺(jué)特征地圖。
[0013] 初始化各粒子位置及各粒子的柵格地圖的過(guò)程如下： 在柵格地圖中，將移動(dòng)機(jī)器人的初始位置坐標(biāo)設(shè)定為(〇，〇，〇)，則在坐標(biāo)點(diǎn)(〇，〇，〇)附 近隨機(jī)生成由N個(gè)粒子s\、權(quán)值為Λ的組成的粒子集StHsV w\}，其中k=l，2,. . .N，這N個(gè) 粒子都是移動(dòng)機(jī)器人下一時(shí)刻可能達(dá)到的位置，由于不知道移動(dòng)機(jī)器人的運(yùn)動(dòng)速度和方 向，假定移動(dòng)機(jī)器人下個(gè)時(shí)刻到這些粒子的概率相等，因此各粒子的概率為1/N，在本實(shí)施 例中，粒子N的個(gè)數(shù)為50個(gè)，因此&={Λ，0.02}。由于此時(shí)移動(dòng)機(jī)器人的各粒子只有一個(gè)柵 格地圖，因此可將該柵格地圖作為各粒子的全局柵格地圖進(jìn)行存儲(chǔ)。
[0014] 初始化各粒子的視覺(jué)特征地圖的過(guò)程如下： 將視覺(jué)傳感器(如攝像機(jī))采集的圖像通過(guò)SIFT算法，建立圖像金字塔，并對(duì)每層金字 塔提取特征，然后得到128維描述子的視覺(jué)特征地圖。同樣地，由于此時(shí)各粒子只有一個(gè)視 覺(jué)特征地圖，因此將其作為各粒子的全局視覺(jué)特征地圖進(jìn)行存儲(chǔ)。
[0015] S2:初始化移動(dòng)機(jī)器人的位置PtQ及建立移動(dòng)機(jī)器人的激光雷達(dá)全局點(diǎn)圖。
[0016] 激光雷達(dá)可以直接獲得環(huán)境的水平剖面圖，測(cè)得的環(huán)境點(diǎn)的信息是2D極坐標(biāo)，表 示為，其中，r表示2D激光雷達(dá)掃描獲得的距離值，Ψ為水平掃描角。激光雷達(dá)通過(guò)T0F、三角 測(cè)距等原理將移動(dòng)機(jī)器人在位置Pto的障礙信息轉(zhuǎn)換為特征點(diǎn)坐標(biāo)，由此建立起初始的激光 雷達(dá)全局點(diǎn)圖并存儲(chǔ)。
[0017] S3:根據(jù)里程計(jì)和陀螺儀得到移動(dòng)機(jī)器人在t時(shí)刻的預(yù)測(cè)位置Pt。
[0018] 本步驟是通過(guò)以下子步驟實(shí)現(xiàn)的： S31:通過(guò)航跡推算得到移動(dòng)機(jī)器人的預(yù)測(cè)位置Pt=[x(t)，y(t)，theta(t)]，航跡推算 公式如下：
其中，[x(t)，y(t)]為移動(dòng)機(jī)器人t時(shí)刻的位置，theta(t)為移動(dòng)機(jī)器人t時(shí)刻航向，[X (t-1) y(t-l)]為移動(dòng)機(jī)器人t-Ι時(shí)刻的位置，theta(t-l)為移動(dòng)機(jī)器人t-Ι時(shí)刻航向，sr (卜1)、81(卜1)為移動(dòng)機(jī)器人卜1時(shí)刻到切寸刻右輪、左輪行走的距離，13為左、右輪之間的輪 軸距。
[0019] 為了便于理解，假設(shè)t-Ι時(shí)刻為移動(dòng)機(jī)器人初始化的時(shí)刻，移動(dòng)機(jī)器人在初始化時(shí) 朝著叉軸正方向，因此，1^(1：-1)，7(1：-1)，1:116七3(1：-1)] = [0,0,0]，進(jìn)而得到|^(1:)，7(1:) ,theta(t)]= [(sr(t-l)+sl(t-l))/2,0, (sr(t~l)+sl(t-l))/b]〇
[0020] S32:陀螺儀更新航跡推算所得到的航向theta(t); 1±6七3(1：)=1：116七36(1：)，其中1：116七36(1：)為1：時(shí)刻陀螺儀的讀數(shù)，本實(shí)施例中直接用陀螺 儀讀數(shù)替換了里程計(jì)航跡推導(dǎo)出來(lái)的角度theta(t)，原因是陀螺儀得到的角度非常高。
[0021] S4:根據(jù)移動(dòng)機(jī)器人的估計(jì)位置Pt和激光雷達(dá)在t時(shí)刻采集到的數(shù)據(jù)得到修正后 的預(yù)測(cè)位置P't。
[0022] 步驟S4是通過(guò)以下方式實(shí)現(xiàn)的： S41:將t時(shí)刻采集到的激光雷達(dá)數(shù)據(jù)坐標(biāo)轉(zhuǎn)換到激光雷達(dá)全局點(diǎn)圖； XG(k)=XL(k)*cos(theta(t))_YL(k)*sin(theta(t))+x(t); YG(k)=XL(k)*sin(theta(t))+YL(k)*cos(theta(t))+y(t); 其中[XL(k)，YL(k)]為當(dāng)前移動(dòng)機(jī)器人位置激光雷達(dá)采到的某一個(gè)(第k個(gè))數(shù)據(jù)；[XG (k)，YL(k)]為將第k個(gè)數(shù)據(jù)轉(zhuǎn)換到全局的全局坐標(biāo)；[奴0,7(0，也的&(0]為丨時(shí)刻移動(dòng)機(jī) 器人的位置和航向，即Pt。
[0023] S42:對(duì)轉(zhuǎn)換后的各激光雷達(dá)數(shù)據(jù)坐標(biāo)尋找滿足閾值條件的匹配點(diǎn)，并保存所有的 匹配對(duì)； 每個(gè)轉(zhuǎn)換后的激光雷達(dá)數(shù)據(jù)坐標(biāo)只有一個(gè)匹配點(diǎn)。比如一個(gè)點(diǎn)在l〇cm范圍（閾值一般 10cm或者5cm)內(nèi)有多個(gè)匹配點(diǎn)存在，那就選取離它最近的那個(gè)匹配點(diǎn)，如果存在兩個(gè)或兩 個(gè)以上的最近匹配點(diǎn)，則隨機(jī)選擇一個(gè)。
[0024] S43:通過(guò)最小二乘法求得使匹配對(duì)模型函數(shù)最小的移動(dòng)機(jī)器人位置； 即通過(guò)里程計(jì)和陀螺儀結(jié)合運(yùn)動(dòng)模型計(jì)算出來(lái)的初始預(yù)測(cè)Pt，再融合激光定位算法， 通過(guò)ICP匹配相連倆幀激光數(shù)據(jù)，通過(guò)最小二乘法得到修正后的移動(dòng)機(jī)器人預(yù)測(cè)位置，具體 如下：
其中X為所有匹配對(duì)的局部坐標(biāo)，即激光雷達(dá)采集到的數(shù)據(jù)，坐標(biāo)轉(zhuǎn)換之前的坐標(biāo)所組 成的矩陣;X形式為
；Y為所有匹配對(duì)的全局坐標(biāo)，即采集到的數(shù)據(jù)匹配 上的全局地圖上的點(diǎn)組成的矩陣，Υ的形式為：
，ΧΤ，ΥΤ為Χ，γ的轉(zhuǎn)置;Α為轉(zhuǎn)換關(guān)系，A 的形式為
，其中[x(t)，y(t)，theta(t)]為t時(shí)刻移動(dòng)機(jī)器人位 置； S44:重復(fù)步驟S41至S43直到移動(dòng)機(jī)器人位置收斂，得到修正后的移動(dòng)機(jī)器人位姿P't。
[0025] 此處的收斂條件是通過(guò)迭代計(jì)算得到的移動(dòng)機(jī)器人位置和航向幾乎不再發(fā)生變 化，即在在極小的范圍（可以忽略不計(jì)的范圍）內(nèi)波動(dòng)或者不再變化。
[0026] S5:根據(jù)移動(dòng)機(jī)器人在t-Ι時(shí)刻和t時(shí)刻的位置變化和各粒子在t-Ι時(shí)刻的位置，采 樣得到t時(shí)刻各粒子的估計(jì)位置。
[0027] 如果t時(shí)刻是第一次對(duì)移動(dòng)個(gè)機(jī)器人進(jìn)行定位，在t-Ι時(shí)刻為移動(dòng)機(jī)器人初始化的 時(shí)刻，具體實(shí)現(xiàn)過(guò)程如下： xp(i,t)=xp(i,t-l)+(x(t)-x(t-l))；yp(i,t)=yp(i,t-l)+(y(t)-y(t-l))； 其中[xp(i ,t) yp(i,t)]，[xp(i ,t_l) 7口(;[，1：-1)]分別為某一個(gè)粒子(第;[個(gè)粒子)1:時(shí) 刻和t-1時(shí)刻的位置；[x(t) y(t)]，[x(t_l) y(t-l)]為ICP定位出來(lái)的移動(dòng)機(jī)器人t時(shí)刻和 t_l時(shí)刻的位置。
[0028] S6:根據(jù)t時(shí)刻激光雷達(dá)采集的數(shù)據(jù)得到t時(shí)刻各粒子的估計(jì)位置的概率PL; 步驟S6在本實(shí)施例中根據(jù)scan matching算法得到t時(shí)刻各粒子位置坐標(biāo)的估計(jì)概率 PL，具體實(shí)現(xiàn)過(guò)程如下： S61:采集t時(shí)刻激光雷達(dá)掃描的數(shù)據(jù)信息并通過(guò)某一粒子的位置坐標(biāo)轉(zhuǎn)換到該粒子全 局坐標(biāo)系； XG(k)=XL(k)*cos(thetap(i,t))-YL(k)*sin(theta(i))+xp(i,t); YG(k)=XL(k)*sin(thetap(i，t))+YL(k)*cos(theta(i))+yp(i，t); 其中[XL(k)，YL(k)]為當(dāng)前移動(dòng)機(jī)器人位置激光雷達(dá)采到的某一個(gè)(第k個(gè))數(shù)據(jù)；[XG (k)，YL(k)]為將第k個(gè)數(shù)據(jù)轉(zhuǎn)換到全局坐標(biāo)系（即世界坐標(biāo)系）的全局坐標(biāo)，即在全局坐標(biāo) 系（即世界坐標(biāo)系）形成一個(gè)坐標(biāo)點(diǎn)，在下面簡(jiǎn)稱為點(diǎn)圖；[xp(i，t)，yp(i，t)，thetap(i，t)] 為某一個(gè)粒子(第i個(gè)粒子)t時(shí)刻的位置和航向。
[0029] S62:將轉(zhuǎn)換后得到的點(diǎn)圖轉(zhuǎn)換為該粒子的概率柵格地圖； 該步驟在本實(shí)施例中的具體如下： 首先，將轉(zhuǎn)換后的點(diǎn)圖中各位置的概率設(shè)置為0.5;接著，將0.5用log算子log化得到 1〇區(qū)形式概率?以18,78)=1<^(?(18,78)/(1-?(18,78)))，其中？(1，7)為1<^化之后位置(叉區(qū)， yg)的log概率，P(xg，yg)為真實(shí)概率即轉(zhuǎn)換之前的概率;然后，轉(zhuǎn)換采集到的數(shù)據(jù)到柵格地 圖： xgrid(k)=XG(k)/d;ygrid(k)=YG(k)/d;其中，[XG(k)，YG(k)]步驟S61 得到的某一個(gè)數(shù) 據(jù)的在全局坐標(biāo)系下的全局坐標(biāo);d為柵格的分辨率(一般為5cm) ; [xgrid(k) ygrid(k)]為 轉(zhuǎn)換之后的柵格位置;再按照二維高斯分布得到此柵格位置周圍3*3范圍內(nèi)的概率;最后將 得到的概率用log算子公式轉(zhuǎn)換為log形式概率;從而各柵格位置log形式概率為 PL(xg，yg)=PLb(xg，yg)+PLa(xg，yg);其中PL(xg，yg)，PLb(xg，yg)，PLa(xg，yg)分別為 位置[xg，yg]處的更新之后，更新之前，和要更新進(jìn)去(加入進(jìn)去）的概率。
[0030] S63:通過(guò)條件概率公式和log算子得到該粒子的位置坐標(biāo)的估計(jì)概率pi; 先將各柵格位置的概率從log形式轉(zhuǎn)換到真實(shí)概率，即用上面log化算子的反函數(shù)轉(zhuǎn)換 到真實(shí)概率； P(xg，yg) = l-1/ (l + exp(PL(xg，yg));其中exp為自然指數(shù)函數(shù)；然后根據(jù)
[0031] S64:重復(fù)步驟S61至S63直到完成所有粒子的位置坐標(biāo)的估計(jì)概率。
[0032] S7:根據(jù)t時(shí)刻視覺(jué)傳感器采集的圖像得到t時(shí)刻各粒子的估計(jì)位置的概率PV。
[0033] S71:對(duì)t時(shí)刻采集到的視覺(jué)圖像進(jìn)行高斯濾波得到高斯金字塔； 將攝像機(jī)采集的圖像通過(guò)SIFT算法，建立圖像金字塔，并對(duì)每層金字塔提取特征，然后 得到128維描述子的視覺(jué)特征地圖。
[0034] S72:尋找關(guān)鍵點(diǎn)并刪除錯(cuò)誤的關(guān)鍵點(diǎn)； 關(guān)鍵點(diǎn)就是突變點(diǎn)，或者說(shuō)局部極值點(diǎn)（分界點(diǎn)），錯(cuò)誤的關(guān)鍵點(diǎn)即是不能完全滿足突 變點(diǎn)的條件的點(diǎn)，它滿足一部分，有可能一開(kāi)始會(huì)被誤選中。由于關(guān)鍵點(diǎn)的尋找和錯(cuò)誤點(diǎn)的 刪除是本領(lǐng)域技術(shù)人員的一般常識(shí)，在這里就不詳細(xì)說(shuō)明了。
[0035] S73:求得關(guān)鍵點(diǎn)的主方向并生成SIFT描述子，從而得到特征地圖； S74:由t時(shí)刻的各粒子的位置坐標(biāo)將特征地圖進(jìn)行坐標(biāo)轉(zhuǎn)換，從而得到各粒子的視覺(jué) 特征地圖； S75:通過(guò)匹配各粒子的視覺(jué)特征地圖和全局視覺(jué)特征地圖得到各粒子的估計(jì)位置的 概率PV; S8:根據(jù)各粒子的估計(jì)位置的概率PL、PV對(duì)粒子權(quán)值進(jìn)行更新。
[0036]對(duì)各粒子的權(quán)值更新過(guò)程如下： S81:根據(jù)t時(shí)刻各粒子的柵格地圖匹配率得到t時(shí)刻?hào)鸥竦貓D匹配率的分布； S82:根據(jù)t時(shí)刻各粒子的視覺(jué)特征地圖匹配率得到t時(shí)刻視覺(jué)特征地圖匹配率的分布； S83:通過(guò)聯(lián)合概率抽樣統(tǒng)計(jì)方法得到自適應(yīng)因子omega;
S84:由自適應(yīng)因子omega加權(quán)各粒子的激光雷達(dá)柵格地圖估計(jì)概率PL和視覺(jué)特征地圖 估計(jì)概率PV，得到各粒子的概率P(k)，計(jì)算公式如下： 其中，P(k)為第k個(gè)粒子的權(quán)值，PL(k)，PV(k)分別為第k個(gè)粒子激光雷達(dá)地圖概率和視覺(jué)特 征地圖概率。
[0037] S9:對(duì)權(quán)值高的粒子進(jìn)行重采樣，得到移動(dòng)機(jī)器人在t時(shí)刻的真實(shí)位置分布。
[0038] 通過(guò)設(shè)定閾值，舍棄權(quán)值小于閾值的粒子，復(fù)制權(quán)值高的粒子，使得粒子總數(shù)保持 不變。
[0039]以上是本發(fā)明的較佳實(shí)施例的詳細(xì)說(shuō)明，不認(rèn)定本發(fā)明只局限于這些說(shuō)明。對(duì)于 本發(fā)明所屬技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明構(gòu)思的前提下所作出的等同替 代或明顯變形，且性能或用途相同，都應(yīng)當(dāng)視為本發(fā)明所提交的權(quán)利要求書(shū)確定的保護(hù)范 圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，其特征在于，所述移動(dòng)機(jī)器人 包括里程計(jì)和陀螺儀、激光雷達(dá)和視覺(jué)傳感器，所述方法包括以下步驟： S1:初始化各粒子位置及各粒子的柵格地圖和視覺(jué)特征地圖，建立各粒子的全局柵格 地圖和全局視覺(jué)特征地圖； S2:初始化移動(dòng)機(jī)器人的位置PtQ及建立移動(dòng)機(jī)器人的激光雷達(dá)全局點(diǎn)圖； S3:根據(jù)里程計(jì)和陀螺儀得到移動(dòng)機(jī)器人在t時(shí)刻的預(yù)測(cè)位置Pt; S4:根據(jù)移動(dòng)機(jī)器人的估計(jì)位置Pt和激光雷達(dá)在t時(shí)刻采集到的數(shù)據(jù)得到修正后的預(yù)測(cè) 位置P't; S5:根據(jù)移動(dòng)機(jī)器人在t-1時(shí)刻和t時(shí)刻的位置變化和各粒子在t-1時(shí)刻的位置，采樣得 到t時(shí)刻各粒子的估計(jì)位置； S6:根據(jù)t時(shí)刻激光雷達(dá)采集的數(shù)據(jù)得到t時(shí)刻各粒子的估計(jì)位置的概率PL; S7:根據(jù)t時(shí)刻視覺(jué)傳感器采集的圖像得到t時(shí)刻各粒子的估計(jì)位置的概率PV; S8:根據(jù)各粒子的估計(jì)位置的概率PL、PV對(duì)粒子權(quán)值進(jìn)行更新； S9:對(duì)權(quán)值高的粒子進(jìn)行重采樣，得到移動(dòng)機(jī)器人在t時(shí)刻的真實(shí)位置分布。2. 如權(quán)利要求1所述的基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，其特征在于，所 述移動(dòng)機(jī)器人為包括左右輪的移動(dòng)機(jī)器人，所述步驟S2包括以下子步驟： S31:通過(guò)航跡推算得到移動(dòng)機(jī)器人的位姿Pi[x(t)，y(t)，theta(t)]，航跡推算公式如 下：其中，[x(t)，y(t)]為移動(dòng)機(jī)器人t時(shí)刻的位置，theta(t)為移動(dòng)機(jī)器人t時(shí)刻航向，[X (t-1) y(t-l)]為移動(dòng)機(jī)器人t-Ι時(shí)刻的位置，theta(t-l)為移動(dòng)機(jī)器人t-Ι時(shí)刻航向，sr (t-1 )、sl (t-1)為移動(dòng)機(jī)器人t-1時(shí)刻到t時(shí)刻右輪、左輪行走的距離，b為左、右輪之間的輪 軸距； S32:陀螺儀更新航跡推算所得到的航向theta(t)。3. 如權(quán)利要求1所述的基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，其特征在于，所 述步驟S4包括以下子步驟： S41:將t時(shí)刻采集到的激光雷達(dá)數(shù)據(jù)坐標(biāo)轉(zhuǎn)換到激光雷達(dá)全局點(diǎn)圖； S42:對(duì)轉(zhuǎn)換后的各激光雷達(dá)數(shù)據(jù)坐標(biāo)尋找滿足閾值條件的匹配點(diǎn)，并保存所有的匹配 對(duì)； S43:通過(guò)最小二乘法求得使匹配對(duì)模型函數(shù)最小的移動(dòng)機(jī)器人位置； S44:重復(fù)步驟S41至S43直到移動(dòng)機(jī)器人位置收斂，得到修正后的移動(dòng)機(jī)器人位姿P't。4. 如權(quán)利要求1所述的基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，其特征在于，所 述步驟S6包括： S61:采集t時(shí)刻激光雷達(dá)掃描的數(shù)據(jù)信息并通過(guò)某一粒子的位置坐標(biāo)轉(zhuǎn)換到該粒子全 局坐標(biāo)系； S62:將轉(zhuǎn)換后得到的點(diǎn)圖轉(zhuǎn)換為該粒子的概率柵格地圖； S63:通過(guò)條件概率公式和log算子得到該粒子的位置坐標(biāo)的估計(jì)概率pi; S64:重復(fù)步驟S61至S63直到完成所有粒子的位置坐標(biāo)的估計(jì)概率。5. 如權(quán)利要求1所述的基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，其特征在于，所 述步驟S7包括： S71:對(duì)t時(shí)刻采集到的視覺(jué)圖像進(jìn)行高斯濾波得到高斯金字塔； S72:尋找關(guān)鍵點(diǎn)并刪除錯(cuò)誤的關(guān)鍵點(diǎn)； S73:求得關(guān)鍵點(diǎn)的主方向并生成SIFT描述子，從而得到特征地圖； S74:由t時(shí)刻的各粒子的位置坐標(biāo)將特征地圖進(jìn)行坐標(biāo)轉(zhuǎn)換，從而得到各粒子的視覺(jué) 特征地圖； S75:通過(guò)匹配各粒子的視覺(jué)特征地圖和全局視覺(jué)特征地圖得到各粒子的估計(jì)位置的 概率PV。6. 如權(quán)利要求1所述的基于激光和視覺(jué)的移動(dòng)機(jī)器人的混合定位方法，其特征在于，所 述步驟S8包括： S81:根據(jù)t時(shí)刻各粒子的柵格地圖匹配率得到t時(shí)刻?hào)鸥竦貓D匹配率的分布； S82:根據(jù)t時(shí)刻各粒子的視覺(jué)特征地圖匹配率得到t時(shí)刻視覺(jué)特征地圖匹配率的分布； S83:通過(guò)聯(lián)合概率抽樣統(tǒng)計(jì)方法得到自適應(yīng)因子omega; S84:由自適應(yīng)因子omega加權(quán)各粒子的激光雷達(dá)柵格地圖估計(jì)概率PL和視覺(jué)特征地圖 估計(jì)概率PV，得到各粒子的概率P(k)，計(jì)算公式如下：其中，P(k)為第k個(gè)粒子的權(quán)值，PL(k)，PV(k)分別為第k個(gè)粒子激光雷達(dá)地圖概率和視 覺(jué)特征地圖概率。
【文檔編號(hào)】G01C21/20GK105865449SQ201610198296
【公開(kāi)日】2016年8月17日
【申請(qǐng)日】2016年4月1日
【發(fā)明人】鄧龍, 李崇國(guó), 楊勇, 宮海濤
【申請(qǐng)人】深圳杉川科技有限公司