[發(fā)明專利]基于稀疏直接法的視覺(jué)慣性里程計(jì)位姿估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010595397.9
• 申請(qǐng)日: 2020-06-23
• 公開(kāi)（公告）號(hào): CN111780754B
• 公開(kāi)（公告）日: 2022-05-20
• 發(fā)明（設(shè)計(jì)）人: 高唱;曾慶喜;陳則王;呂查德;闞宇超
• 申請(qǐng)（專利權(quán)）人: 南京航空航天大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C22/00;G01C25/00
• 代理公司: 南京經(jīng)緯專利商標(biāo)代理有限公司 32200
• 代理人: 施昊

1.基于稀疏直接法的視覺(jué)慣性里程計(jì)位姿估計(jì)方法，其特征在于，具體包括如下步驟：

步驟一：在載體上安裝俯視單目相機(jī)VO和慣性傳感器IMU；對(duì)VO和IMU采集到的數(shù)據(jù)進(jìn)行時(shí)間同步；

步驟二：對(duì)VO和IMU進(jìn)行聯(lián)合初始化，并同步VO和IMU的軌跡；

步驟三：對(duì)第k+1時(shí)刻VO采集到的圖像建立N層圖像金字塔，由上往下依次對(duì)圖像金字塔的每一層進(jìn)行光度補(bǔ)償，從而更新該第k+1時(shí)刻圖像；

步驟四：建立滑動(dòng)窗口，將更新后的第k+1時(shí)刻圖像放入窗口，根據(jù)更新后的第k+1時(shí)刻圖像，計(jì)算在第k+1時(shí)刻時(shí)VO的位姿，若該位姿與上一個(gè)關(guān)鍵幀對(duì)應(yīng)的VO位姿之間的差值大于等于預(yù)設(shè)的閾值，則計(jì)算窗口中第y個(gè)關(guān)鍵幀中每個(gè)像素點(diǎn)的梯度值，并由大到小排列，選擇前300個(gè)像素點(diǎn)作為第y個(gè)關(guān)鍵幀的路標(biāo)點(diǎn)，y＝1,2,…,M,M為滑動(dòng)窗口中關(guān)鍵幀的總個(gè)數(shù)，根據(jù)每一個(gè)關(guān)鍵幀中所有路標(biāo)點(diǎn)的梯度值計(jì)算每一個(gè)關(guān)鍵幀對(duì)滑動(dòng)窗口的貢獻(xiàn)值，將貢獻(xiàn)值最小的關(guān)鍵幀邊緣化，同時(shí)將更新后的第k+1時(shí)刻圖像作為一個(gè)關(guān)鍵幀；基于更新后的第k+1時(shí)刻圖像與上一個(gè)關(guān)鍵幀之間的VO視覺(jué)殘差、更新后的第k+1時(shí)刻圖像與上一個(gè)關(guān)鍵幀之間的IMU慣性殘差以及上一次優(yōu)化計(jì)算得到的先驗(yàn)殘差，建立基于窗口的非線性最小二乘函數(shù)，并求解該非線性最小二乘函數(shù)，得到第k+1時(shí)刻里程計(jì)位姿的最優(yōu)估計(jì)值，將被邊緣化的圖像幀對(duì)應(yīng)的測(cè)量信息轉(zhuǎn)換為先驗(yàn)信息約束加入到滑動(dòng)窗口，得到用于下一次優(yōu)化計(jì)算的先驗(yàn)殘差，并繼續(xù)將下一個(gè)時(shí)刻的圖像放入滑動(dòng)窗口中進(jìn)行計(jì)算；若第k+1時(shí)刻的VO位姿與上一個(gè)關(guān)鍵幀對(duì)應(yīng)的VO位姿之間的差值小于預(yù)設(shè)的閾值，則刪除第k+1時(shí)刻圖像，并繼續(xù)將下一個(gè)時(shí)刻的圖像放入滑動(dòng)窗口中進(jìn)行計(jì)算；

所述步驟三具體為：

步驟3.1：對(duì)第k+1時(shí)刻VO采集到的原始圖像建立一個(gè)N層圖像金字塔，該圖像金字塔的分辨率由下往上逐漸降低，最底層為分辨率最高的原始圖像；

步驟3.2：在該圖像金字塔的頂層圖像的中心選擇一塊大小為J×L的矩形區(qū)域作為模板，計(jì)算該模板的平均灰度值，將該平均灰度值作為第k+1時(shí)刻圖像金字塔的灰度值，得到第k+1時(shí)刻圖像金字塔與第k時(shí)刻圖像金字塔的灰度值的差值，將該差值作為第k+1時(shí)刻圖像金字塔中每一層圖像的光度補(bǔ)償值，從而對(duì)圖像金字塔中每一層圖像進(jìn)行更新。

2.根據(jù)權(quán)利要求1所述的基于稀疏直接法的視覺(jué)慣性里程計(jì)位姿估計(jì)方法，其特征在于，所述步驟二中同步VO和IMU的軌跡包括將IMU坐標(biāo)系轉(zhuǎn)換到VO坐標(biāo)系下，具體為：

步驟2.1:計(jì)算相鄰時(shí)刻之間IMU測(cè)得旋轉(zhuǎn)積分相鄰時(shí)刻之間VO測(cè)量到的旋轉(zhuǎn)并建立如下幾何約束條件：

其中，表示四元數(shù)乘法，q_bc為VO和IMU之間的旋轉(zhuǎn)外參；

步驟2.2：根據(jù)q_bc，計(jì)算得到在VO坐標(biāo)系下從k時(shí)刻到k+1時(shí)刻IMU的旋轉(zhuǎn)為：

3.根據(jù)權(quán)利要求1所述的基于稀疏直接法的視覺(jué)慣性里程計(jì)位姿估計(jì)方法，其特征在于，所述步驟四中，計(jì)算第k+1時(shí)刻VO的位姿具體為：將更新后的第k+1時(shí)刻圖像中每個(gè)像素點(diǎn)的梯度值由大到小排列，選擇前300個(gè)點(diǎn)作為該圖像的路標(biāo)點(diǎn)，根據(jù)這些路標(biāo)點(diǎn)的最小化光度誤差計(jì)算第k+1時(shí)刻VO位姿的李代數(shù)，并通過(guò)稀疏直接法求解該李代數(shù)，得到第k+1時(shí)刻VO的位姿。

4.根據(jù)權(quán)利要求1所述的基于稀疏直接法的視覺(jué)慣性里程計(jì)位姿估計(jì)方法，其特征在于，所述步驟四中，VO視覺(jué)殘差為第k+1時(shí)刻圖像中所有路標(biāo)點(diǎn)灰度值之和與上一個(gè)關(guān)鍵幀中所有路標(biāo)點(diǎn)灰度值之和的差值。

5.根據(jù)權(quán)利要求1所述的基于稀疏直接法的視覺(jué)慣性里程計(jì)位姿估計(jì)方法，其特征在于，所述步驟四中IMU慣性殘差為，對(duì)第k+1時(shí)刻圖像與上一個(gè)關(guān)鍵幀之間IMU測(cè)得的數(shù)據(jù)進(jìn)行預(yù)積分，從而得到IMU誤差傳遞函數(shù)，利用IMU的誤差傳遞函數(shù)處理預(yù)積分量中的噪聲，從而得到IMU慣性殘差。