本發(fā)明涉及計算機視覺技術和圖像處理技術，具體地說是一種基于單目視覺的IMU漂移補償方法。

背景技術：

慣性測量單元(Inertial Measurement Unit，IMU)是集成有三軸加速計、三軸陀螺儀、三軸磁力計的MEMS慣性元件，用于測量物體三軸角速度、三軸加速度和三軸地磁分量。IMU具有體積小、重量輕、可隨身攜帶、穿戴方便等特點，廣泛用于載體方向估計和人體關節(jié)的動作捕捉。

基于IMU的方向估計具有運算量小、動態(tài)特性好的優(yōu)勢，適用于短時間內的方向估計。但由于MEMS傳感器本身由于漂移而造成誤差累積，使得IMU難以實現(xiàn)長期的方向估計。以往基于優(yōu)化的IMU方向估計方法和基于統(tǒng)計學的IMU方向估計方法雖然能夠抑制噪聲對方向估計的影響，但存在如下問題：①在基于加速計的方向估計中，兩類方法都是基于一種假設：IMU采集到的三軸加速度僅包含有重力加速度分量，默認的載體自身的加速度分量為零，即載體自身只能做平緩、慢速的動作；②在基于室內環(huán)境的磁力計的方向估計中，IMU輸出的三軸地磁分量容易受到周邊鐵磁材料的影響而造成誤差；③在基于陀螺儀的方向估計中，陀螺儀輸出的三軸角速度值存在非定常的隨機誤差、偏移誤差和尺度偏差，即便經(jīng)過補償，仍然無法解決MEMS器件自身的長期誤差累積問題。上述缺陷是制約IMU用于長時間方向估計的瓶頸，因此亟需一種魯棒、準確的方向估計方法解決IMU誤差累積的問題，實現(xiàn)長時間的方向估計。

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

針對目前IMU方向估計存在誤差累積、無法適用于長時間方向估計的問題，本發(fā)明提出一種基于單目視覺的IMU漂移補償方法，在相鄰兩幀圖像間的特征點對集合中，采用外級線約束方法得到基于視覺的方向估計值，并將其引入到IMU的校準補償中，解決IMU長時間誤差累積的問題。

本發(fā)明為實現(xiàn)上述目的所采用的技術方案是：一種基于單目視覺的IMU漂移補償方法，包括以下步驟：

步驟1：基于IMU的方向估計；

步驟2：單目攝像頭的方向估計；

步驟3：視覺輔助IMU的誤差補償。

所述步驟1包括以下步驟：

步驟1.1：計算陀螺儀的方向估計

其中表示t時刻由IMU陀螺儀估計出的四元數(shù)，由(q₀,q₁,q₂,q₃)表示，q₁、q₂、q₃分量分別對應旋轉軸在X、Y、Z方向的投影，q₀分量對應IMU圍繞旋轉軸旋轉的角度；表示陀螺儀估計出的四元數(shù)的變化率，Δt表示采樣間隔；

步驟1.2：以最小化估計值誤差為目標，定義目標函數(shù)：

從而得到加速計與磁力計的方向估計其中，和分別表示加速計和磁力計估計值的誤差；

步驟1.3：利用增益自適應互補濾波算法，估計求解IMU坐標系下的方向信息

其中，k_t表示自適應的增益系數(shù)，表示最終的方向估計信息。

所述步驟2包括以下步驟：

步驟2.1：求取像素點偏差平方和(SSD)：針對單目攝像頭采集到的相鄰兩幀圖像，求取像素點偏差平方和SSD；

步驟2.2：如果SSD大于某一指定的閾值τ，則判斷IMU處于運動狀態(tài)；

步驟2.2.1：采用特征點檢測與描述方法SURF方法生成64維特征點描述子；

步驟2.2.2：利用特征點對的描述子具有歐氏距離最小原則實現(xiàn)相鄰幀圖像間特征點的匹配，得到匹配點對集合：{{x₁,x′₁},{x₂,x′₂},…{x_n,x′_n}}；

步驟2.2.3：結合重采樣一致性分析算法RANSAC移除特征點對中存在的誤匹配點；

步驟2.2.4：利用計算機視覺中的外級線約束方程x′Ex＝0，得到本質矩陣E；

步驟2.2.5：利用奇異值分解，求得旋轉矩陣R_3×3，此處得到的旋轉矩陣R_3×3表示單目攝像頭的方向估計值；

步驟2.3：如果SSD不大于某一指定的閾值τ，則判斷IMU處于靜止狀態(tài)，判定當前時刻單目攝像頭的方向估計值與上一時刻單目攝像頭的方向估計一致。

所述像素點偏差平方和SSD通過下式得到：

其中，P(i,j,t)和P(i,j,t+1)分別表示像素點P(i,j)在t時刻和t+1時刻的圖像像素值，M表示以像素為單位的圖片長度，N表示以像素為單位的圖片寬度。

所述步驟3具體為：

如果SSD大于閾值τ，則判斷載體處于運動狀態(tài)，IMU漂移補償?shù)木唧w實現(xiàn)為：

步驟3.1.1：將基于視覺估計出的旋轉矩陣R_3×3轉化成對應的歐拉角

其中，R_{ij,1≤i,j≤3}表示旋轉矩陣R_3×3中的第i行，第j列的元素；

步驟3.1.2：利用歐拉角的線性插值來補償IMU輸出四元數(shù)，歐拉角對應的四元數(shù)為：

利用歐拉角解算出的四元數(shù)來實現(xiàn)IMU的漂移補償；

如果SSD不大于閾值τ，則判斷載體處于靜止狀態(tài)，IMU漂移補償?shù)木唧w實現(xiàn)為：

步驟3.2.1：將IMU中陀螺儀輸出的t時刻的角速度ω(t)＝(ω_x,t,ω_y,t,ω_z,t)矢量設置為：

ω_x,t＝0,ω_y,t＝0,ω_z,t＝0

步驟3.2.2：設置t時刻加速度矢量α(t)＝(α_x,t,α_y,t,α_z,t)和磁力強度矢量m(t)＝(m_x,t,m_y,t,m_z,t)為：

α_x,t＝α_x,t-1,α_y,t＝α_y,t-1,α_z,t＝α_z,t-1

m_x,t＝m_x,t-1,m_y,t＝m_y,t-1,m_z,t＝m_z,t-1

本發(fā)明具有以下優(yōu)點及有益效果：采用外級線約束方法得出基于視覺的方向估計值，并將其引入到IMU的校準補償中，解決了IMU長時間誤差累計的問題，具有準確、穩(wěn)定、零漂移的優(yōu)點。

附圖說明

圖1為本發(fā)明方法的整體流程圖；

圖2為IMU方向估計流程圖；

圖3為單目視覺方向估計流程圖；

圖4為IMU誤差補償?shù)南到y(tǒng)框圖。

具體實施方式

下面結合附圖及實施例對本發(fā)明做進一步的詳細說明。

本發(fā)明方法包括三部分(如圖1所示)：

(1)基于IMU的方向估計；

(2)單目攝像頭的方向估計；

(3)視覺輔助IMU的誤差補償。

具體實現(xiàn)過程如下所述。

步驟1：基于IMU的方向估計(如圖2所示)。

步驟1.1：計算陀螺儀的方向估計

其中表示t時刻由IMU陀螺儀估計出的四元數(shù)，它由(q₀,q₁,q₂,q₃)表示。其中，四元數(shù)中的q₁、q₂、q₃分量分別對應旋轉軸在X、Y、Z方向的投影，q₀分量對應IMU圍繞旋轉軸旋轉的角度；表示陀螺儀估計出的四元數(shù)的變化率，Δt表示采樣間隔。

步驟1.2：加速計與磁力計的方向估計

以最小化估計值誤差為目標，定義目標函數(shù)：

其中，和分別表示加速計和磁力計估計值的誤差。

步驟1.3：利用增益自適應互補濾波算法，估計求解IMU坐標系下的方向信息

其中k_t表示自適應的增益系數(shù)，表示最終的方向估計信息。

增益自適應互補濾波算法[Tian Y,Wei H,Tan J.An adaptive-gain complementary filter for real-time human motion tracking with marg sensors in free-living environments[J].Neural Systems and Rehabilitation Engineering,IEEE Transactions on,2013,21(2):254-264]。以上介紹的基于IMU增益自適應互補濾波算法的方向估計中，由于存在陀螺儀的偏差和隨機誤差、地磁分量受到周圍鐵磁材料的干擾、加速度計采集的加速度信息除了有可能包含有重力以外的載體加速度分量之外還存在偏差和隨機誤差，以上三個缺陷是制約IMU長時間方向估計的主要因素。為了抑制IMU方向估計的誤差，需要引入其他類型傳感器輔助實現(xiàn)IMU的漂移補償。

步驟2：單目攝像頭的方向，估計利用單目視覺的方向估計信息實現(xiàn)IMU的漂移補償(如圖3所示)。

步驟2.1：求取像素點偏差平方和(SSD)。

針對單目攝像頭采集到的相鄰兩幀圖像，求取像素點偏差平方和SSD：

其中，P(i,j,t)和P(i,j,t+1)分別表示像素點P(i,j)在t時刻和t+1時刻的圖像像素值，M表示以像素為單位的圖片長度，N表示以像素為單位的圖片寬度。

步驟2.2：如果SSD大于某一指定的閾值τ，則判斷IMU處于運動狀態(tài)。

步驟2.2.1：采用魯棒性好的特征點檢測與描述方法SURF方法(Speeded Up Robust Features,SURF)[Bay H,Tuytelaars T,Van Gool L.Surf:Speeded up robust features[M].Computer vision–ECCV 2006.Springer Berlin Heidelberg,2006:404-417]，生成64維特征點描述子；

步驟2.2.2：利用特征點對的描述子具有歐氏距離最小原則實現(xiàn)相鄰幀圖像間特征點的匹配，得到匹配點對集合：{{x₁,x′₁},{x₂,x′₂},…{x_n,x′_n}}；

步驟2.2.3：結合重采樣一致性分析算法RANSAC移除特征點對中存在的誤匹配點；

步驟2.2.4：利用計算機視覺中的外級線約束方程x′Ex＝0，得到本質矩陣E；

步驟2.2.5：利用奇異值分解(Singular Value Decomposition,SVD)分解，求得旋轉矩陣R_3×3，此處得到的旋轉矩陣R_3×3表示單目攝像頭的方向估計值。

步驟3：視覺輔助IMU的誤差補償，利用單目視覺的方向估計信息實現(xiàn)IMU的漂移補償(如圖4所示)。

步驟3.1：如果SSD大于閾值τ，則判斷載體處于運動狀態(tài)。

步驟3.1.1：將基于視覺估計出的旋轉矩陣R_3×3轉化成對應的歐拉角轉化過程如下：

其中，R_{ij,1≤i,j≤3}表示旋轉矩陣R_3×3中的第i行，第j列的元素。

步驟3.1.2：利用歐拉角的線性插值來補償IMU輸出四元數(shù)。

歐拉角對應的四元數(shù)為：

利用歐拉角解算出的四元數(shù)來實現(xiàn)IMU的漂移補償。

步驟3.2：如果SSD小于閾值τ，則判斷載體處于靜止狀態(tài)。

步驟3.2.1：將IMU中陀螺儀輸出的t時刻的角速度ω(t)＝(ω_x,t,ω_y,t,ω_z,t)矢量設置為：

ω_x,t＝0,ω_y,t＝0,ω_z,t＝0

步驟3.2.2：設置t時刻加速度矢量α(t)＝(α_x,t,α_y,t,α_z,t)和磁力強度矢量m(t)＝(m_x,t,m_y,t,m_z,t)為：

α_x,t＝α_x,t-1,α_y,t＝α_y,t-1,α_z,t＝α_z,t-1

m_x,t＝m_x,t-1,m_y,t＝m_y,t-1,m_z,t＝m_z,t-1 。