[發(fā)明專利]一種基于深度相機(jī)和慣性融合的移動(dòng)機(jī)器人定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202211119147.3
• 申請(qǐng)日: 2022-09-13
• 公開（公告）號(hào): CN115371665B
• 公開（公告）日: 2023-06-23
• 發(fā)明（設(shè)計(jì)）人: 王常虹;趙新洋;王振桓;竇赫暄;劉博
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00;G01C21/16;G01C21/20
• 代理公司: 哈爾濱市松花江聯(lián)合專利商標(biāo)代理有限公司 23213
• 代理人: 岳昕

1.一種基于深度相機(jī)和慣性融合的移動(dòng)機(jī)器人定位方法，其特征在于，所述方法具體包括以下步驟：

步驟1、讀取當(dāng)前來(lái)自深度相機(jī)的單目RGB圖像，深度圖像和IMU測(cè)量序列，再對(duì)單目RGB圖像，深度圖像和IMU測(cè)量序列進(jìn)行時(shí)間同步；

所述步驟1的具體過(guò)程為：

以單目RGB圖像的時(shí)間為基準(zhǔn)，獲得時(shí)間戳位于前一幀單目RGB圖像到當(dāng)前幀單目RGB圖像時(shí)間內(nèi)的IMU測(cè)量序列集合，在匹配深度圖像時(shí)，若深度圖像與當(dāng)前幀單目RGB圖像的時(shí)間相差在t₀以內(nèi)，則將當(dāng)前幀單目RGB圖像、獲得的IMU測(cè)量序列集合以及深度圖像打包成數(shù)據(jù)集合，并以深度慣性模式進(jìn)行處理，即繼續(xù)執(zhí)行步驟2；否則，若深度圖像與當(dāng)前幀單目RGB圖像的時(shí)間相差不小于t₀，則去除當(dāng)前的深度圖像，并以單目慣性模式進(jìn)行處理；

步驟2、對(duì)前一幀單目RGB圖像和當(dāng)前幀單目RGB圖像進(jìn)行下采樣，獲得下采樣后的圖像；再追蹤前一幀下采樣后圖像的特征點(diǎn)在當(dāng)前幀下采樣后圖像上的位置，獲得各個(gè)特征點(diǎn)對(duì)；

步驟3、建立深度相機(jī)深度測(cè)量誤差模型；

所述深度相機(jī)深度測(cè)量誤差模型為：

σ_z(z)＝a₁+a₂z+a₃z²

其中，z為當(dāng)前特征點(diǎn)在深度圖像上的深度測(cè)量值，σ_z(z)為當(dāng)前特征點(diǎn)在深度圖像上的深度測(cè)量值為z時(shí)的標(biāo)準(zhǔn)差，a₁,a₂,a₃為待標(biāo)定的系數(shù)；

所述系數(shù)a₁,a₂,a₃的標(biāo)定方式為：

將深度相機(jī)置于滑軌上，當(dāng)深度相機(jī)與白墻的距離為L(zhǎng)₁時(shí)采集多張深度圖像，再分別從采集的每張深度圖像中截取出感興趣區(qū)域，所述感興趣區(qū)域在每張深度圖像中的位置均相同；

對(duì)于感興趣區(qū)域內(nèi)某個(gè)位置的像素點(diǎn)，計(jì)算出該位置的像素點(diǎn)在各個(gè)感興趣區(qū)域中的深度測(cè)量值的方差，再對(duì)感興趣區(qū)域內(nèi)全部位置上的像素點(diǎn)所對(duì)應(yīng)的方差求平均，將得到的平均值作為標(biāo)準(zhǔn)差σ；再計(jì)算出位于感興趣區(qū)域內(nèi)最中心的像素點(diǎn)在各個(gè)感興趣區(qū)域中的深度測(cè)量值的均值μ；

深度相機(jī)在滑軌上滑動(dòng)，當(dāng)深度相機(jī)與白墻的距離每增加ΔL時(shí)均采集到對(duì)應(yīng)距離下的多張深度圖像，直至深度相機(jī)與白墻的距離達(dá)到L₂時(shí)停止，同理，分別計(jì)算出每個(gè)距離下的標(biāo)準(zhǔn)差和均值；

利用獲得的各個(gè)距離下的標(biāo)準(zhǔn)差和均值對(duì)σ＝a₁+a₂μ+a₃μ²進(jìn)行擬合，得到系數(shù)a₁,a₂,a₃的值；

步驟4、將當(dāng)前幀單目RGB圖像作為滑動(dòng)窗口的最后一幀，將滑窗內(nèi)與當(dāng)前幀單目RGB圖像c_k具有最大視差的關(guān)鍵幀記為c₀，計(jì)算關(guān)鍵幀c₀與當(dāng)前幀c_k之間的平移和旋轉(zhuǎn)初值其中，為關(guān)鍵幀c₀與當(dāng)前幀之間的旋轉(zhuǎn)初值，為關(guān)鍵幀c₀與當(dāng)前幀之間的平移初值；

根據(jù)得到的平移和旋轉(zhuǎn)初值得到關(guān)鍵幀c₀與當(dāng)前幀c_k間的位姿估計(jì)結(jié)果

所述根據(jù)得到的平移和旋轉(zhuǎn)初值得到關(guān)鍵幀c₀與當(dāng)前幀c_k間的位姿估計(jì)結(jié)果其具體過(guò)程為：

采用LM算法對(duì)平移和旋轉(zhuǎn)初值進(jìn)行迭代更新，分別獲得各次迭代更新后的平移和旋轉(zhuǎn)值；

遍歷當(dāng)前幀單目RGB圖像上的特征點(diǎn)，從當(dāng)前幀單目RGB圖像對(duì)應(yīng)的深度圖像獲得當(dāng)前幀單目RGB圖像上每個(gè)特征點(diǎn)的深度測(cè)量值；

并建立如下的誤差方程：

其中，代表從各次迭代更新后的平移和旋轉(zhuǎn)值中選擇出使誤差方程取值最小的平移和旋轉(zhuǎn)值S_3D表示由深度測(cè)量值在3m以內(nèi)的特征點(diǎn)對(duì)組成的集合，S_2D表示由全部特征點(diǎn)對(duì)組成的集合，r_re表示重投影誤差方程，r_ep表示對(duì)極約束誤差方程，表示S_3D中第l₁對(duì)特征點(diǎn)在關(guān)鍵幀c₀中的深度測(cè)量值，表示S_3D中第l₁對(duì)特征點(diǎn)在當(dāng)前幀中的深度測(cè)量值，表示S_3D中第l₁對(duì)特征點(diǎn)對(duì)應(yīng)的歸一化特征點(diǎn)對(duì)，表示S_2D中第l₂對(duì)特征點(diǎn)對(duì)應(yīng)的歸一化特征點(diǎn)對(duì)，||·||代表范數(shù)，Ω_re代表相機(jī)測(cè)量的重投影誤差的協(xié)方差矩陣，Ω_ep代表相機(jī)測(cè)量的對(duì)極約束誤差的協(xié)方差矩陣；

步驟5、以關(guān)鍵幀c₀作為參考幀，基于關(guān)鍵幀c₀與當(dāng)前幀間的位姿估計(jì)結(jié)果進(jìn)行純視覺(jué)位姿估計(jì)，得到滑窗內(nèi)所有的3D路標(biāo)點(diǎn)l₁,...l_m以及滑窗內(nèi)其它各個(gè)關(guān)鍵幀相對(duì)于參考幀的視覺(jué)位姿估計(jì)結(jié)果

所述步驟5的具體過(guò)程為：

步驟51、根據(jù)關(guān)鍵幀c₀與當(dāng)前幀間的位姿估計(jì)結(jié)果以及關(guān)鍵幀c₀與當(dāng)前幀間的特征點(diǎn)對(duì)的坐標(biāo)值，計(jì)算出關(guān)鍵幀c₀與當(dāng)前幀間具有共視關(guān)系的3D路標(biāo)點(diǎn)在關(guān)鍵幀c₀坐標(biāo)系下的坐標(biāo)值；

步驟52、根據(jù)步驟51中計(jì)算出的坐標(biāo)值以及關(guān)鍵幀c₀與當(dāng)前幀間具有共視關(guān)系的3D路標(biāo)點(diǎn)在滑窗內(nèi)其它關(guān)鍵幀的相機(jī)觀測(cè)值，得到滑窗內(nèi)其它關(guān)鍵幀相對(duì)于參考幀的位姿估計(jì)初值；

步驟53、基于步驟52中的位姿估計(jì)初值，利用PnP優(yōu)化算法得到滑窗內(nèi)所有3D路標(biāo)點(diǎn)l₁,...l_m和其它各關(guān)鍵幀相對(duì)于參考幀的視覺(jué)位姿估計(jì)結(jié)果

其中，滑窗的長(zhǎng)度為10，即滑窗內(nèi)有包括當(dāng)前幀在內(nèi)的10個(gè)關(guān)鍵幀，代表滑窗內(nèi)的各關(guān)鍵幀相對(duì)于參考幀的視覺(jué)位姿估計(jì)結(jié)果；

步驟6、利用數(shù)據(jù)集合中的IMU測(cè)量序列得到滑窗內(nèi)各關(guān)鍵幀之間的相對(duì)位姿變換；

所述步驟6的具體過(guò)程為：

其中，和為第i個(gè)IMU時(shí)刻IMU測(cè)量得到的IMU坐標(biāo)系下加速度和角速度，b_a和b_g為第i個(gè)IMU時(shí)刻的加速度計(jì)和陀螺儀的偏置，b_k表示第k幀IMU坐標(biāo)系，和分別為第i個(gè)IMU時(shí)刻相對(duì)于b_k的位移預(yù)積分，速度預(yù)積分和旋轉(zhuǎn)預(yù)積分，為當(dāng)前時(shí)刻相對(duì)于b_k的旋轉(zhuǎn)矩陣，代表旋轉(zhuǎn)運(yùn)算符，δt為當(dāng)前時(shí)刻與上一時(shí)刻的時(shí)間間隔，是從IMU陀螺儀得到的當(dāng)前第i時(shí)刻到第i+1時(shí)刻的姿態(tài)變化估計(jì)值，和分別為第i+1個(gè)IMU時(shí)刻相對(duì)于b_k的位移預(yù)積分，速度預(yù)積分和旋轉(zhuǎn)預(yù)積分；

在理想情況下，視覺(jué)運(yùn)動(dòng)估計(jì)所得的旋轉(zhuǎn)等于IMU預(yù)積分得到的旋轉(zhuǎn)值，則有目標(biāo)函數(shù)：

其中，為由視覺(jué)運(yùn)動(dòng)估計(jì)得到的b_k+1相對(duì)于第c₀幀IMU坐標(biāo)系的旋轉(zhuǎn)，為由視覺(jué)運(yùn)動(dòng)估計(jì)得到的b_k相對(duì)于第c₀幀IMU坐標(biāo)系的旋轉(zhuǎn)，b_k+1表示第k+1幀IMU坐標(biāo)系，為理想情況下IMU旋轉(zhuǎn)預(yù)積分得到的b_k+1相對(duì)于b_k的旋轉(zhuǎn)，B代表滑窗中的所有幀的序號(hào)，||·||代表范數(shù)；

已知實(shí)際的IMU旋轉(zhuǎn)預(yù)積分的值理想的IMU旋轉(zhuǎn)預(yù)積分由下式求得：

其中，為旋轉(zhuǎn)預(yù)積分相對(duì)于陀螺儀偏置的雅克比矩陣，δb_g為陀螺儀偏置的變化量；

經(jīng)過(guò)化簡(jiǎn)和Cholesky分解得：

其中，上角標(biāo)T代表矩陣的轉(zhuǎn)置，上角標(biāo)-1代表矩陣的逆；

求解出陀螺儀的偏置的變化值δb_g后，利用新的偏置b_g＝b_g+δb_g對(duì)IMU預(yù)積分值進(jìn)行重新計(jì)算；

步驟7、將步驟5和步驟6的結(jié)果對(duì)齊，求得參考幀c₀到世界坐標(biāo)系的旋轉(zhuǎn)矩陣以及各幀的速度值

再利用旋轉(zhuǎn)矩陣得到滑窗中各幀相對(duì)于世界坐標(biāo)系的平移和旋轉(zhuǎn)，根據(jù)各幀的速度值得到各幀相對(duì)于世界坐標(biāo)系的速度；

步驟8、基于步驟3中建立的深度相機(jī)深度測(cè)量誤差模型，對(duì)滑窗中所有的路標(biāo)點(diǎn)進(jìn)行聯(lián)合深度估計(jì)，根據(jù)聯(lián)合深度估計(jì)結(jié)果對(duì)路標(biāo)點(diǎn)進(jìn)行分類，根據(jù)分類結(jié)果對(duì)路標(biāo)點(diǎn)進(jìn)行篩選；

所述步驟8的具體過(guò)程為：

對(duì)于滑窗內(nèi)的任意一個(gè)3D路標(biāo)點(diǎn)，遍歷該3D路標(biāo)點(diǎn)在滑窗中每個(gè)關(guān)鍵幀上的測(cè)量值，測(cè)量值包括該3D路標(biāo)點(diǎn)在相機(jī)歸一化平面上的2D坐標(biāo)和在深度圖像上的深度測(cè)量值；

若該3D路標(biāo)點(diǎn)在關(guān)鍵幀上存在深度測(cè)量值，且深度測(cè)量值小于3m，則根據(jù)步驟3建立的深度相機(jī)深度測(cè)量誤差模型得到深度測(cè)量噪聲方差將該3D路標(biāo)點(diǎn)的2D像素坐標(biāo)測(cè)量噪聲方差記為則該3D路標(biāo)點(diǎn)的三維測(cè)量協(xié)方差為：

其中，I₂為2階單位矩陣，為該3D路標(biāo)點(diǎn)在相機(jī)中2D像素坐標(biāo)(u_i,v_i)和深度測(cè)量值z(mì)_i的雅克比；

其中，f_x,f_y表示相機(jī)x,y方向的焦距，c_x,c_y表示相機(jī)x,y方向的光心；

通過(guò)滑窗內(nèi)幀間的位姿變換將該3D路標(biāo)點(diǎn)轉(zhuǎn)換到c₀幀相機(jī)坐標(biāo)系中，則將協(xié)方差轉(zhuǎn)換到c₀幀相機(jī)坐標(biāo)系下為：

其中，為c₀幀相機(jī)坐標(biāo)系下的協(xié)方差；

利用得到新的深度測(cè)量噪聲方差將該3D路標(biāo)點(diǎn)的深度測(cè)量值小于3m且重投影后2D觀測(cè)誤差小于設(shè)定閾值的N個(gè)觀測(cè)投影到相機(jī)坐標(biāo)系下，每個(gè)觀測(cè)的投影記為d_i′，i＝1,2,…,N，則該3D路標(biāo)點(diǎn)在c₀幀中的聯(lián)合深度估計(jì)值為

若該3D路標(biāo)點(diǎn)在滑窗內(nèi)的跟蹤次數(shù)大于等于4，則表明該3D路標(biāo)點(diǎn)為穩(wěn)健特征點(diǎn)，將該3D路標(biāo)點(diǎn)歸為A類，A類的3D路標(biāo)點(diǎn)無(wú)需參與下一輪的聯(lián)合深度估計(jì)；

若該3D路標(biāo)點(diǎn)在滑窗內(nèi)的跟蹤次數(shù)小于4，則表明該3D路標(biāo)點(diǎn)仍屬新特征點(diǎn)，將該3D路標(biāo)點(diǎn)歸為B類，B類的3D路標(biāo)點(diǎn)在下一循環(huán)仍需進(jìn)行聯(lián)合深度估計(jì)；

對(duì)于沒(méi)有實(shí)現(xiàn)聯(lián)合深度估計(jì)的3D路標(biāo)點(diǎn)，根據(jù)共視幀的2D觀測(cè)值采用多視圖三角化的方法實(shí)現(xiàn)聯(lián)合深度估計(jì)，對(duì)于三角化誤差小于等于設(shè)定的閾值且跟蹤次數(shù)大于等于4的3D路標(biāo)點(diǎn)，將其歸為A類，對(duì)于三角化誤差小于等于設(shè)定閾值且跟蹤次數(shù)小于4的3D路標(biāo)點(diǎn)，則將其歸為B類，對(duì)于三角化誤差大于設(shè)定閾值的3D路標(biāo)點(diǎn)，則直接將其歸為C類，C類特征點(diǎn)為離群點(diǎn)不再參與后續(xù)優(yōu)化，在本輪循環(huán)結(jié)束后被剔除；

步驟9、根據(jù)篩選后的路標(biāo)點(diǎn)建立點(diǎn)特征投影誤差模型；

步驟10、建立平面約束誤差模型；

步驟11、根據(jù)滑窗中各幀相對(duì)于世界坐標(biāo)系的平移和旋轉(zhuǎn)以及各幀相對(duì)于世界坐標(biāo)系的速度，建立IMU預(yù)積分約束模型；

步驟12、根據(jù)步驟9至步驟11的模型建立全局優(yōu)化目標(biāo)函數(shù)，對(duì)全局優(yōu)化目標(biāo)函數(shù)進(jìn)行求解得到滑窗內(nèi)各幀的位姿估計(jì)結(jié)果。