[發(fā)明專利]一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110245725.7
• 申請(qǐng)日: 2021-03-05
• 公開(kāi)（公告）號(hào): CN113012196B
• 公開(kāi)（公告）日: 2023-03-24
• 發(fā)明（設(shè)計(jì)）人: 李奎霖;魏武;曾錦秀;肖文煜
• 申請(qǐng)（專利權(quán)）人: 華南理工大學(xué)
• 主分類號(hào): G06T7/246
• 分類號(hào): G06T7/246;G06T7/73;G06V10/771;G06F18/25;G01C21/16;G01C21/18;G01C21/34
• 代理公司: 廣州粵高專利商標(biāo)代理有限公司 44102
• 代理人: 何淑珍;江裕強(qiáng)

1.一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于，包括以下步驟：

步驟S1、采集機(jī)器人周邊圖像序列；

步驟S2、對(duì)獲取的圖像幀，使用改進(jìn)后的FAST特征點(diǎn)提取算法，并引入四叉樹(shù)快速篩選特征點(diǎn)，使得FAST特征點(diǎn)分均勻分布；

步驟S3、提取圖像幀的線特征，計(jì)算線特征描述子；

步驟S4、對(duì)前后圖像幀提取的FAST特征點(diǎn)進(jìn)行跟蹤匹配，獲取幀間估計(jì)位姿，再由該估計(jì)位姿，將前后幀中的點(diǎn)、線特征重投影到當(dāng)前幀，并對(duì)線特征進(jìn)行跟蹤匹配；所述對(duì)線特征進(jìn)行跟蹤匹配則通過(guò)最小化重投影誤差實(shí)現(xiàn)：

其中l(wèi)＝[l₁,l₂]^T是投影直線，l₁代表前幀中待匹配的線特征，l₂代表后幀中待匹配的線特征，p^T、q^T是匹配線段的兩個(gè)端點(diǎn)的齊次坐標(biāo)；

步驟S5、確認(rèn)當(dāng)前幀是否為關(guān)鍵幀，若非關(guān)鍵幀則丟棄當(dāng)前幀，并把其視覺(jué)信息與IMU預(yù)積分信息暫存?zhèn)鬟f給下一幀；

步驟S6、將當(dāng)前幀加入滑動(dòng)窗口，結(jié)合滑窗內(nèi)已配對(duì)的點(diǎn)線特征約束與IMU預(yù)積分緊耦合進(jìn)行后端優(yōu)化，輸出幀間運(yùn)動(dòng)位姿。

2.根據(jù)權(quán)利要求1所述的一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于：所述改進(jìn)后的FAST特征點(diǎn)提取算法具體為：根據(jù)圖像大小劃分網(wǎng)格；遍歷每個(gè)網(wǎng)格，在每個(gè)網(wǎng)格上提取FAST特征點(diǎn)，將圖像視為一個(gè)N叉樹(shù)根節(jié)點(diǎn)，即N等分，如果根節(jié)點(diǎn)特征點(diǎn)個(gè)數(shù)大于1，則把該節(jié)點(diǎn)迭代等分生成四個(gè)子節(jié)點(diǎn)，迭代終止條件為當(dāng)某個(gè)子節(jié)點(diǎn)內(nèi)只含一個(gè)特征點(diǎn)或不含特征點(diǎn)；最終選取的特征點(diǎn)即為最優(yōu)。

3.根據(jù)權(quán)利要求2所述的一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于：步驟S3具體是使用LSD線特征提取算法提取線特征，并使用LBD算法計(jì)算線特征描述子。

4.根據(jù)權(quán)利要求3所述的一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于：所述LSD(Line Segment Detector)線特征提取算法具體為：通過(guò)高斯采樣縮小圖像尺寸為原圖的a％，在使得圖像不變模糊的前提下減弱原圖中存在的鋸齒效應(yīng)，對(duì)圖像中每一點(diǎn)的像素梯度進(jìn)行排序，把像素梯度方向的像素點(diǎn)進(jìn)行聯(lián)合，生成直線支持區(qū)域。

5.根據(jù)權(quán)利要求4所述的一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于：所述LBD(Line Band Descriptor)算法具體為：在檢測(cè)出的LSD線特征進(jìn)行像素梯度累加，通過(guò)累計(jì)線特征中各行相關(guān)聯(lián)的梯度信息構(gòu)造描述符，計(jì)算各行描述符矩陣的均值和標(biāo)準(zhǔn)差，基于最近鄰匹配準(zhǔn)則，根據(jù)描述符的距離對(duì)線端進(jìn)行匹配；

LBD描述子具體為：確定LSD線段條帶區(qū)域與梯度方向；計(jì)算條帶每行梯度權(quán)重；統(tǒng)計(jì)每行梯度直方圖，構(gòu)成描述符矩陣；計(jì)算均值和標(biāo)準(zhǔn)差串聯(lián)成LBD描述子。

6.根據(jù)權(quán)利要求5所述的一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于：在步驟S4中，對(duì)前后幀特征點(diǎn)進(jìn)行跟蹤匹配通過(guò)基于最小化光度誤差的直接法進(jìn)行，選取特征點(diǎn)及其周?chē)南袼貕K光度誤差最小作為最終的誤差函數(shù)；采取雙線性插值法來(lái)求取其灰度值，最小化光度誤差函數(shù)為：

E_u,v＝I_v[p`]-I_u[p]

其中p為前圖像幀I_u中的FAST特征點(diǎn)，p`表示p在當(dāng)前圖像幀I_v中的反投影坐標(biāo)。

7.根據(jù)權(quán)利要求6所述的一種基于雙目相機(jī)與慣導(dǎo)傳感器信息融合的定位方法，其特征在于：在步驟S5中，關(guān)鍵幀選取策略為：

已連續(xù)N次當(dāng)前幀沒(méi)有被視為關(guān)鍵幀，當(dāng)前幀能夠跟蹤到不少于h個(gè)FAST點(diǎn)特征和至少m條線特征，或者當(dāng)前幀與參考關(guān)鍵幀中的點(diǎn)特征和線特征共有數(shù)量不超過(guò)參考關(guān)鍵幀的3/4。