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