本發(fā)明涉及一種圖像特征追蹤方法和裝置，具體涉及一種imu輔助的視覺特征魯棒跟蹤方法及裝置。

背景技術：

在移動機器人相關技術研究中，導航技術屬于其核心技術，也是實現(xiàn)智能化和自主移動的關鍵技術，其中基于視覺傳感器的機器人導航技術是目前國內(nèi)外應用比較普遍導航技術，而圖像特征跟蹤的準確度直接決定著視覺導航的精確度。隨著應用場景越來越復雜，導航精度要求越來越高，僅僅依靠視覺傳感器已難以滿足處理速度以及系統(tǒng)魯棒性的要求：在少紋理區(qū)域場景，高速運動或抖動引起圖像模糊場景因無法進行有效的特征跟蹤而使視覺導航表現(xiàn)不佳。隨著低成本微機電慣性測量模塊的出現(xiàn)，使解決上述問題成為了可能。

技術實現(xiàn)要素：

本發(fā)明的目的是針對現(xiàn)有技術中的不足，提供一種imu輔助的視覺特征魯棒跟蹤方法及裝置，其與純視覺特征跟蹤技術相比魯棒性更高，特征跟蹤速度更快，相同幀特征跟蹤數(shù)量提高20％到30％；系統(tǒng)構(gòu)造簡潔，在成本增加較低的情況下，極大的提高了系統(tǒng)的抗干擾能力，適用于少紋理區(qū)域以及高速運動，抖動等純視覺特征跟蹤無法使用的場景。

為實現(xiàn)上述目的，本發(fā)明公開了如下技術方案：

一種imu輔助的視覺特征魯棒跟蹤方法，包括如下步驟：

s1圖像特征提取及相機標定：

s101制作已知尺寸的標定板，高和寬大小分別為11*9個2cm正方格；

s102不同姿態(tài)下靜止拍攝至少13張照片；

s103提取圖像的goodfeature/corner特征；

s104同時標定出相機各個姿態(tài)下的相機內(nèi)參和相機外參；

s2imu數(shù)據(jù)建模及濾波；

s201對imu數(shù)據(jù)進行預處理：處理掉極大值和極小值，中間大幅度躍變野值；靜止狀態(tài)獲取零飄值，所有有效數(shù)據(jù)減去零飄；

s202對預處理后的imu數(shù)據(jù)進行濾波：利用一階ar模型對數(shù)據(jù)進行建模，使用卡爾曼濾波器進行數(shù)據(jù)處理；

s3多傳感器坐標系姿態(tài)標定；

s301標定板豎直放置，使用imu_cam裝置中的相機，在至少十個不同位姿下靜止拍下至少十張照片；

s302在拍攝每一張照片的靜止位置，記錄下至少十組imu_cam裝置imu的加速度數(shù)據(jù)，并進行相應的數(shù)據(jù)濾波；

s303利用豎直分量在相機坐標系和在imu坐標系中的數(shù)值關系，計算出imu坐標和相機坐標系之間的姿態(tài)關系；

s4imu輔助視覺特征跟蹤建模：

s401使用imu_cam裝置中的相機拍攝自然場景，獲取相機圖片，提取出該幀圖片中的klt特征；

s402記錄下連續(xù)兩幀之間imu的數(shù)據(jù)，并進行數(shù)據(jù)濾波，計算出連續(xù)兩幀圖像間imu姿態(tài)改變；

s403根據(jù)imu的姿態(tài)改變，已知相機和imu之間固定的姿態(tài)關系，求得相機的姿態(tài)改變；利用相機和圖像之間的投影關系，換算出圖像klt特征變化后的新坐標，以此新坐標作為下一幀圖像該klt特征的起始點進行搜索，最終獲得klt的新坐標。

進一步的，所述步驟s1中，相機使用ueyes工業(yè)相機，分辨率為640×480，幀率為25幀每秒；特征使用goodfeature,每一幀都要把所有特征提取到，并將特征的圖片坐標精確到亞像素。

進一步的，所述步驟s2中，imu使用mti系列產(chǎn)品，數(shù)據(jù)輸出頻率在60-120幀每秒，幀率是相機幀率的2-4倍；數(shù)據(jù)包括加速度，平移速度，旋轉(zhuǎn)角速度；原始數(shù)據(jù)處理包括最大最小值屏蔽，中間大野值丟棄；靜態(tài)零飄獲取，零飄消除；數(shù)據(jù)的一階ar建模以及卡爾曼濾波。

進一步的，所述步驟3中，標定板跟相機的標定板相同尺寸，同時保持豎直放置于一個固定位置；在每一個靜態(tài)位置，使用該裝置中的相機拍攝一張照片，同時使用該裝置中的imu獲取此姿態(tài)下的十組加速度數(shù)據(jù)并進行imu數(shù)據(jù)濾波；利用豎直分量在相機坐標系和在imu坐標系中的數(shù)值關系，計算出imu坐標系和相機坐標系之間的姿態(tài)關系。

本發(fā)明還公開一種imu輔助的視覺特征魯棒跟蹤裝置，包括：

imu慣性數(shù)據(jù)采集模塊，由一系列慣性數(shù)據(jù)測量單元組成，能夠?qū)崟r感應imu的旋轉(zhuǎn)角度信息，加速度信息以及平移速度信息；

cam圖像處理模塊，由一個工業(yè)攝像頭組成，以25幀每秒的實時速率進行圖像捕捉，在獲取的圖像上進行klt特征提??；

數(shù)據(jù)通信融合模塊，各個傳感器和主控模塊的通信，其借款包括網(wǎng)口、usb和串口，由imu獲取的慣性數(shù)據(jù)和相機獲取的圖像數(shù)據(jù)進行數(shù)據(jù)處理后進行時間戳對齊，包括時間戳插值和平滑；

cam/imu姿態(tài)標定模塊，使用固定尺寸的相機標定板，豎直固定放置，獲取到重力加速度在相機坐標系和imu坐標系的分量后，計算出cam和imu之間的姿態(tài)關系；

klt特征追蹤模塊，從當前獲取的圖像中提取klt特征；計算當前幀到下一幀圖像之間imu的旋轉(zhuǎn)累積和平移累積，把這種旋轉(zhuǎn)平移關系轉(zhuǎn)換到相機坐標系下，再映射到圖像坐標系中獲取klt特征的預測坐標，把此預測坐標作為klt特征的初始搜索值進行全局搜索，獲取到該klt特征的精確像素坐標。

本發(fā)明公開的一種imu輔助的視覺特征魯棒跟蹤方法及裝置，具有以下有益效果：

1.本發(fā)明對比傳統(tǒng)的純視覺特征跟蹤，具有特征搜索速度更快，精度更高，魯棒性更強的特點：通過融合imu的慣性信息，可以對圖像特征的初始定位進行預判，從而減少搜索區(qū)域，減少搜索時間，提高特征定位精度以及幀間特征追蹤的穩(wěn)定性，實驗表明每幀跟蹤數(shù)量普遍提高20％以上。

2.本發(fā)明提出一種有效的處理imu數(shù)據(jù)的方法，既imu數(shù)據(jù)濾波方法，每6幀處理一次，去除最大最小值，濾除野值，消去零飄，之后對數(shù)據(jù)進行一階ar建模，最后進行卡爾曼濾波；靜態(tài)實驗結(jié)果顯示，此濾波后的慣性數(shù)據(jù)比原數(shù)據(jù)更接近真實數(shù)據(jù)，零飄消除同時方差縮小10倍。

3.本發(fā)明裝置構(gòu)造簡單，標定工具僅使用相機標定板，制作簡單；使用重力在兩個坐標系的投影的姿態(tài)標定方法簡潔明了，能滿足實際上需要。本裝置模塊與主機間的通訊使用成熟的usb，232串口，網(wǎng)口等標準工業(yè)接口，便于移植和拓展。

附圖說明

圖1為本發(fā)明的特征跟蹤系統(tǒng)整體功能模塊圖；

圖2為本發(fā)明裝置中imu的功能模塊示意圖；

圖3為本發(fā)明中imu姿態(tài)推演和顯示框圖；

圖4為本發(fā)明中klt特征跟蹤的算法執(zhí)行流程圖；

圖5為本發(fā)明中imu慣性數(shù)據(jù)輔助的klt特征跟蹤原理演示圖；

圖6為本發(fā)明中imu數(shù)據(jù)處理結(jié)果演示圖；

圖7為本發(fā)明特征追蹤與純視覺特征追蹤橫擺運動效果對比圖；

圖8為本發(fā)明特征追蹤與純視覺特征追蹤俯仰運動效果對比圖；

圖9為本發(fā)明特征追蹤與純視覺特征追蹤旋轉(zhuǎn)運動效果對比圖。

具體實施方式

下面將對本發(fā)明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發(fā)明一部分實施例，而不是全部的實施例。基于本發(fā)明中的實施例，本領域普通技術人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本發(fā)明保護的范圍。

本發(fā)明的核心是提供一種imu輔助的視覺特征魯棒跟蹤方法及裝置，其與純視覺特征跟蹤技術相比魯棒性更高，特征跟蹤速度更快，相同幀特征跟蹤數(shù)量提高20％到30％；系統(tǒng)構(gòu)造簡潔，在成本增加較低的情況下，極大的提高了系統(tǒng)的抗干擾能力，適用于少紋理區(qū)域以及高速運動，抖動等純視覺特征跟蹤無法使用的場景。

一種imu輔助的視覺特征魯棒跟蹤方法，包括如下步驟：

s1圖像特征提取及相機標定：

s101制作已知尺寸的標定板，高和寬大小分別為11*9個2cm正方格；

s102不同姿態(tài)下靜止拍攝至少13張照片；

s103提取圖像的goodfeature/corner特征；

s104同時標定出相機各個姿態(tài)下的相機內(nèi)參和相機外參；

s2imu數(shù)據(jù)建模及濾波；

s201對imu數(shù)據(jù)進行預處理：處理掉極大值和極小值，中間大幅度躍變野值；靜止狀態(tài)獲取零飄值，所有有效數(shù)據(jù)減去零飄；

s202對預處理后的imu數(shù)據(jù)進行濾波：利用一階ar模型對數(shù)據(jù)進行建模，使用卡爾曼濾波器進行數(shù)據(jù)處理；

s3多傳感器坐標系姿態(tài)標定；

s301標定板豎直放置，使用imu_cam裝置中的相機，在至少十個不同位姿下靜止拍下至少十張照片；

s302在拍攝每一張照片的靜止位置，記錄下至少十組imu_cam裝置imu的加速度數(shù)據(jù)，并進行相應的數(shù)據(jù)濾波；

s303利用豎直分量在相機坐標系和在imu坐標系中的數(shù)值關系，計算出imu坐標和相機坐標系之間的姿態(tài)關系；

s4imu輔助視覺特征跟蹤建模：

s401使用imu_cam裝置中的相機拍攝自然場景，獲取相機圖片，提取出該幀圖片中的klt特征；

s402記錄下連續(xù)兩幀之間imu的數(shù)據(jù)，并進行數(shù)據(jù)濾波，計算出連續(xù)兩幀圖像間imu姿態(tài)改變；

s403根據(jù)imu的姿態(tài)改變，已知相機和imu之間固定的姿態(tài)關系，求得相機的姿態(tài)改變；利用相機和圖像之間的投影關系，換算出圖像klt特征變化后的新坐標，以此新坐標作為下一幀圖像該klt特征的起始點進行搜索，最終獲得klt的新坐標。

在本發(fā)明的一種實施例中，所述步驟s1中，相機使用ueyes工業(yè)相機，分辨率為640×480，幀率為25幀每秒；特征使用goodfeature,每一幀都要把所有特征提取到，并將特征的圖片坐標精確到亞像素。

在本發(fā)明的一種實施例中，所述步驟s2中，imu使用mti系列產(chǎn)品，數(shù)據(jù)輸出頻率在60-120幀每秒，幀率是相機幀率的2-4倍；imu數(shù)據(jù)包括加速度，平移速度，旋轉(zhuǎn)角速度；imu原始數(shù)據(jù)處理包括最大最小值屏蔽，中間大野值丟棄；靜態(tài)零飄獲取，零飄消除；數(shù)據(jù)的一階ar建模以及卡爾曼濾波。

在本發(fā)明的一種實施例中，所述步驟3中，標定板跟相機的標定板相同尺寸，同時保持豎直放置于一個固定位置；在每一個靜態(tài)位置，使用該裝置中的相機拍攝一張照片，同時使用該裝置中的imu獲取此姿態(tài)下的十組加速度數(shù)據(jù)并進行imu數(shù)據(jù)濾波；利用豎直分量在相機坐標系和在imu坐標系中的數(shù)值關系，計算出imu坐標系和相機坐標系之間的姿態(tài)關系。

見圖1，本發(fā)明還公開一種imu輔助的視覺特征魯棒跟蹤裝置，包括：

imu慣性數(shù)據(jù)采集模塊，由一系列慣性數(shù)據(jù)測量單元組成，能夠?qū)崟r感應imu的旋轉(zhuǎn)角度信息，加速度信息以及平移速度信息；

cam圖像處理模塊，由一個工業(yè)攝像頭組成，以25幀每秒的實時速率進行圖像捕捉，在獲取的圖像上進行klt特征提??；

數(shù)據(jù)通信融合模塊，各個傳感器和主控模塊的通信，其借款包括網(wǎng)口、usb和串口，由imu獲取的慣性數(shù)據(jù)和相機獲取的圖像數(shù)據(jù)進行數(shù)據(jù)處理后進行時間戳對齊，包括時間戳插值和平滑；

cam/imu姿態(tài)標定模塊，使用固定尺寸的相機標定板，豎直固定放置，獲取到重力加速度在相機坐標系和imu坐標系的分量后，計算出cam和imu之間的姿態(tài)關系；

klt特征追蹤模塊，從當前獲取的圖像中提取klt特征；計算當前幀到下一幀圖像之間imu的旋轉(zhuǎn)累積和平移累積，把這種旋轉(zhuǎn)平移關系轉(zhuǎn)換到相機坐標系下，再映射到圖像坐標系中獲取klt特征的預測坐標，把此預測坐標作為klt特征的初始搜索值進行全局搜索，獲取到該klt特征的精確像素坐標。

硬件支持模塊，為上述硬件提供相應支持。

如圖2，本發(fā)明中的imu慣性數(shù)據(jù)采集模塊具體包括3d陀螺、3d加速度/磁強測量模塊、a/d模數(shù)轉(zhuǎn)換模塊、dsp數(shù)據(jù)融合模塊，存儲模塊和通訊接口模塊。與外部主機連接。

如圖3，本發(fā)明中的imu慣性數(shù)據(jù)的姿態(tài)處理主要包括如下步驟：

第一步：確定一個初始姿態(tài)矩陣，之后把姿態(tài)的矩陣表達轉(zhuǎn)換為四元數(shù)表達；

第二步：實時監(jiān)測imu的角速度數(shù)據(jù)，累積角度并轉(zhuǎn)換為四元數(shù)進行推演；

第三步：已知上一個時刻姿態(tài)的四元數(shù)表達，經(jīng)過累加角度的四元數(shù)轉(zhuǎn)換得到當前時刻的姿態(tài)四元數(shù)，并轉(zhuǎn)化為姿態(tài)矩陣輸出當前姿態(tài)。

如圖5，本發(fā)明imu慣性數(shù)據(jù)輔助的klt特征跟蹤過程如下：同一物體在相鄰兩幅圖的投影，特征點pt由于相機的大尺度運動而落在了收斂區(qū)域外從而導致klt跟蹤失?。辉谙鄼C內(nèi)參和imu_cam相對位姿已標定的情況下，可以利用陀螺儀角速度數(shù)據(jù)計算出此時的單應性矩陣，再求得仿射變換模型參數(shù)的近似預測值，從而獲得klt跟蹤模型新的搜索起始點通常情況下默認的搜索起始點是上一幀最后迭代的結(jié)果pt。藍色部分是搜索收斂域，相機位姿大尺度變化時，可能導致搜索起始點pt不在收斂域內(nèi)，使用陀螺儀數(shù)據(jù)使搜索起始點修正為該點在收斂區(qū)域ct+1內(nèi)，從而得到更好的跟蹤效果。

如圖4，imu輔助的視覺特征魯棒跟蹤方法流程主要包括如下步驟：

第一步：配置imu_cam裝置中相機參數(shù)和imu的參數(shù)，主要包括相機幀率和分辨率，imu的數(shù)據(jù)輸出頻率和數(shù)據(jù)格式；使用標定板完成相機內(nèi)參標定；初始化一個初始位姿，使用裝置中的相機拍攝自然場景，獲取相機圖片，提取出該幀圖片中的klt特征。

第二步：在相機兩幀之間實時獲取imu數(shù)據(jù)，并進行數(shù)據(jù)處理，主要進行大野值濾除，零飄消除，以及基于一階ar模型的卡爾曼濾波，處理結(jié)果參閱附圖6，其中z-original為原始數(shù)據(jù)，z-zeroshift為野值和零飄濾除后的結(jié)果，z-kalman為ar建模及卡爾曼濾波后的結(jié)果。濾波后的imu數(shù)據(jù)根據(jù)附圖3所示原理邏輯計算出連續(xù)兩幀圖像間imu的姿態(tài)改變。

第三步：依附圖5所示imu慣性數(shù)據(jù)輔助的klt特征跟蹤原理：根據(jù)imu的姿態(tài)改變，已知相機和imu之間固定的姿態(tài)關系，求得相機的姿態(tài)改變；利用相機和圖像之間的投影關系，換算出圖像klt特征變化后的新坐標，以此新坐標作為下一幀圖像該klt特征的起始點進行搜索，最終精確獲得klt的新坐標。

由附圖7，附圖8，附圖9，本發(fā)明與純視覺跟蹤結(jié)果對比可知，跟蹤的魯棒性大幅提高。

以上所述僅是本發(fā)明的優(yōu)選實施方式，而非對其限制；應當指出，盡管參照上述各實施例對本發(fā)明進行了詳細說明，本領域的普通技術人員應當理解，其依然可以對上述各實施例所記載的技術方案進行修改，或?qū)ζ渲胁糠只蛘呷考夹g特征進行等同替換；而這些修改和替換，并不使相應的技術方案的本質(zhì)脫離本發(fā)明各實施例技術方案的范圍。