本發(fā)明涉及計(jì)算機(jī)視覺技術(shù)以及數(shù)據(jù)處理和信息融合技術(shù)，具體是一種基于慣性引導(dǎo)采樣的視覺IMU方向估計(jì)方法。

背景技術(shù)：

人體動(dòng)作捕捉系統(tǒng)獲得的高分辨率和大容量的數(shù)據(jù)可以被用來研究疾病的起因，并有助于疾病的預(yù)防和治療，廣泛應(yīng)用于生物醫(yī)療領(lǐng)域，例如，臨床的步態(tài)分析、門診病人的身體復(fù)原、腿部關(guān)節(jié)復(fù)原、老年人活動(dòng)監(jiān)測(cè)和評(píng)估以及視覺障礙人群的恢復(fù)。隨著MEMS傳感器的成本越來越低、體積越來越小，出現(xiàn)了加速計(jì)、陀螺計(jì)和磁力計(jì)集成的慣性測(cè)量單元(Inertial Measurement Unit，IMU)。現(xiàn)有的人體動(dòng)作捕捉系統(tǒng)就是通過在人體各個(gè)部位固定數(shù)量不等的IMU來實(shí)現(xiàn)的?？紤]到傳感器模塊的配置以及人體的限制，人體的姿勢(shì)數(shù)據(jù)可以通過信號(hào)融合來獲得。IMU優(yōu)勢(shì)在于不依賴于某個(gè)特定的基礎(chǔ)條件，也不受視線的限制；然而，1)方向估測(cè)容易受本地磁場(chǎng)的影響，而地球磁場(chǎng)卻容易受到周圍電子產(chǎn)品或者是鐵制材料的影響；2)位置和方向測(cè)量是根據(jù)集成的雙慣性傳感器獲得的，該方法在長(zhǎng)時(shí)間的對(duì)人體進(jìn)行跟蹤的時(shí)候會(huì)產(chǎn)生低頻漂移以及錯(cuò)誤積累的問題，無法對(duì)人體動(dòng)作進(jìn)行準(zhǔn)確的定量評(píng)估。上述兩個(gè)缺點(diǎn)使得IMU不利于其在醫(yī)療領(lǐng)域的推廣應(yīng)用。

為了克服IMU的局限性，有學(xué)者提出了利用可穿戴的攝像機(jī)對(duì)IMU的漂移進(jìn)行補(bǔ)償?shù)囊曈XIMU方向估計(jì)方法，通過采集攝像機(jī)對(duì)周圍環(huán)境的觀測(cè)圖像序列，得到基于視覺的方向估計(jì)值，再通過卡爾曼濾波的方法與IMU進(jìn)行融合，從而實(shí)現(xiàn)更為準(zhǔn)確的方向估計(jì)。然而這一補(bǔ)償方法在圖像處理過程中存在相鄰幀特征點(diǎn)匹配耗時(shí)和誤匹配的問題，大大制約了運(yùn)算速度和計(jì)算精度。

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

針對(duì)現(xiàn)有IMU方向估計(jì)方法存在精度低和長(zhǎng)時(shí)間誤差累積的問題，以及基于視覺的方向估計(jì)方法存在匹配耗時(shí)和誤匹配的問題，本發(fā)明提出了一種基于 慣性引導(dǎo)采樣的視覺IMU方向估計(jì)方法，充分利用IMU的方向估計(jì)信息，引導(dǎo)視覺方向估計(jì)中匹配點(diǎn)對(duì)的采樣過程和誤匹配點(diǎn)的移除過程，保證了視覺IMU方向估計(jì)的精度，提高了視覺IMU方向估計(jì)的運(yùn)算速度。

本發(fā)明為實(shí)現(xiàn)上述目的所采用的技術(shù)方案是：一種基于慣性引導(dǎo)采樣的視覺IMU方向估計(jì)方法，包括以下步驟：

步驟1：基于增益自適應(yīng)互補(bǔ)濾波器的IMU方向估計(jì)；

步驟2：基于尺度旋轉(zhuǎn)不變的特征檢測(cè)方法，檢測(cè)與匹配特征點(diǎn)檢測(cè)與匹配；

步驟3：基于視覺IMU融合的方向估計(jì)。

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

步驟1.1：計(jì)算陀螺儀的方向估計(jì)；

步驟1.2：計(jì)算加速計(jì)與磁力計(jì)的方向估計(jì)；

步驟1.3：基于互補(bǔ)濾波算法的方向估計(jì)；

步驟1.4：IMU姿態(tài)估計(jì)。

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

步驟1.1.1：求解t+Δt時(shí)刻描述IMU方向的四元數(shù)的變化率

其中，四元數(shù)中的q₁、q₂、q₃分量分別對(duì)應(yīng)旋轉(zhuǎn)軸在X、Y、Z方向的投影，q₀分量對(duì)應(yīng)IMU圍繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度；符號(hào)表示四元數(shù)乘積，表示t時(shí)刻IMU陀螺儀的四元數(shù)估計(jì)值，ω_t+Δt表示t+Δt時(shí)刻陀螺儀的輸出值。

步驟1.1.2：求解t+Δt時(shí)刻四元數(shù)的估計(jì)值

步驟1.1.3：歸一化處理四元數(shù)估計(jì)值，得出t+Δt時(shí)刻由陀螺儀估計(jì)出的四元數(shù)值

所述步驟1.2通過高斯牛頓的迭代優(yōu)化算法計(jì)算加速計(jì)與磁力計(jì)的方向估 計(jì)值首先確定初始時(shí)刻的加速度值z(mì)_α和磁場(chǎng)強(qiáng)度值z(mì)_m，再根據(jù)t時(shí)刻加速計(jì)和磁力計(jì)的輸出值α(t)和m(t)，以最小化估計(jì)值誤差為目標(biāo)，建立目標(biāo)函數(shù)：

其中，和分別表示由加速計(jì)和磁力計(jì)估計(jì)值的誤差，計(jì)算過程為：

其中，表示的共軛四元數(shù)。

所述步驟1.3聯(lián)合t+Δt時(shí)刻陀螺儀的方向估計(jì)值和以及加速計(jì)和磁力計(jì)的方向估計(jì)值采用增益自適應(yīng)互補(bǔ)濾波算法求解IMU坐標(biāo)系下IMU的方向信息

其中，k_t表示自適應(yīng)的增益系數(shù)，表示t時(shí)刻加速計(jì)、磁力計(jì)、陀螺儀融合后的方向估計(jì)信息，表示t時(shí)刻加速計(jì)與磁力計(jì)的方向估計(jì)值。

所述步驟1.4采用運(yùn)動(dòng)力學(xué)中的扭轉(zhuǎn)表示和指數(shù)映射算法，初步估計(jì)IMU的姿態(tài)信息，具體包括以下步驟：

步驟1.4.1：根據(jù)IMU估計(jì)出的四元數(shù)得到與四元數(shù)相對(duì)應(yīng)的旋轉(zhuǎn)軸ω＝(cosω_x，cosω_y，cosω_z)和繞軸旋轉(zhuǎn)的角度θ，求解過程是：θ＝2cos^-1q₀

步驟1.4.2：根據(jù)扭轉(zhuǎn)與指數(shù)映射理論，將(ω，θ)其映射為坐標(biāo)系間的旋轉(zhuǎn)和平移。已知初始時(shí)刻的位置g_global，n(0)與坐標(biāo)系間的轉(zhuǎn)換關(guān)系(R，T)后，計(jì)算n時(shí)刻IMU的位置g_global，n(θ)，其映射關(guān)系如下所示：

其中，表示與ω相對(duì)應(yīng)的扭轉(zhuǎn)參數(shù)，v表示繞軸旋轉(zhuǎn)的角度。

其中，q_g表示初始時(shí)刻坐標(biāo)系原點(diǎn)與載體之間的向量。

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

步驟2.1：基于SURF算法的特征點(diǎn)檢測(cè)；

步驟2.2：相鄰幀間圖像特征點(diǎn)的匹配。

所述步驟2.2具體為：

通過計(jì)算特征點(diǎn)描述子之間的歐氏距離實(shí)現(xiàn)相鄰幀間特征點(diǎn)的匹配，表示P與P₁的特征點(diǎn)描述子之間的歐式距離，表示P與P₂的特征點(diǎn)描述子之間的距離，如果與的比值小于預(yù)定義的閾值δ_D，即則接受這一對(duì)匹配點(diǎn)；其中，對(duì)于圖像中的某個(gè)特征點(diǎn)P，其特征點(diǎn)描述子記為D_P＝[x₁，x₂，…x₆₄]^T；待匹配圖像中與P點(diǎn)對(duì)應(yīng)的第一近鄰點(diǎn)和第二近鄰點(diǎn)分別記為P₁和P₂；所述第一近鄰點(diǎn)和第二近鄰點(diǎn)分別指待匹配圖像的特征點(diǎn)中，與P點(diǎn)描述子歐氏距離最近的描述子的特征點(diǎn)和次近的描述子的特征點(diǎn)；P₁和P₂的特征點(diǎn)描述子分別記為P點(diǎn)與P₁和P₂的特征點(diǎn)描述子之間的歐式距離和可表示為分別表示為：

所述步驟3包含以下步驟：

步驟3.1：期望步驟得到本次迭代的姿態(tài)信息；

步驟3.2：最大化步驟確定本次迭代中的內(nèi)點(diǎn)集合；

反復(fù)迭代步驟3.1和步驟3.2，得到最終的參數(shù)估計(jì)集合θ^*＝{R^*，T^*，O_n^*}，其中，R^*即為可穿戴視覺IMU的方向估計(jì)，θ^*表示載體姿態(tài)與匹配點(diǎn)對(duì)集合，T^*表示最終解算出的載體的平移分量，O_n^*表示最終做出的匹配點(diǎn)對(duì)集合。

所述步驟3.1包含以下步驟：

步驟3.1.1：利用步驟2中得到的內(nèi)點(diǎn)集合，通過外級(jí)線約束方程和最小二乘法估計(jì)本質(zhì)矩陣E；

步驟3.1.2：對(duì)本質(zhì)矩陣E做奇異值分解，估算出載體的姿態(tài)信息：旋轉(zhuǎn)分量R和平移分量T；

步驟3.1.3：建立后驗(yàn)貝葉斯估計(jì)模型：

其中，(η₁，η₂，…η_n)表示相鄰幀圖像間的匹配的特征點(diǎn)對(duì)，θ表示視覺IMU姿態(tài)估計(jì)值和匹配點(diǎn)對(duì)的集合，定義為：

θ＝{R，T，O_n}

其中，O_n＝(o₁，o₂，…o_n)表示匹配點(diǎn)對(duì)的集合，元素o_i服從伯努利分布，用于判斷第i個(gè)匹配點(diǎn)對(duì)是否正確；o_i＝1表示樣本點(diǎn)為內(nèi)點(diǎn)；o_i＝0表示樣本點(diǎn)為外點(diǎn)。

所述步驟3.2具體為：判斷迭代樣本點(diǎn)：

P{o_i|{R，T}}＝max{P(o_i＝1|{R，T})，P(o_i＝0|{R，T})}

其中，P(o_i＝1|{R，T})和P(o_i＝0|{R，T})分別表示為：

其中，表示Sampson誤差，S和σ為預(yù)先設(shè)定的窗口大小和方差，取決于具體應(yīng)用；y_n表示第n幅圖像中的第i個(gè)特征點(diǎn)的坐標(biāo)，f(x_n)表示第n幅圖像中的第i個(gè)特征點(diǎn)所對(duì)應(yīng)的外級(jí)線，o_i取值為1或0由P(o_i＝1|{R，T})和P(o_i＝0|{R，T})的大小決定：如果P(o_i＝1|{R，T})≥P(o_i＝0|{R，T})，則o_i取值為1；反之，o_i取值為0。

本發(fā)明具有以下優(yōu)點(diǎn)：

1.本發(fā)明方法采用增益自適應(yīng)互補(bǔ)濾波器，能夠在初始迭代過程中剔除明顯的誤匹配點(diǎn)對(duì)，提高了方向估計(jì)的準(zhǔn)確性；

2.本發(fā)明將IMU的姿態(tài)估計(jì)信息作為初值引入到視覺方向估計(jì)中，通過迭代移除誤匹配點(diǎn)對(duì)，加快了方向估計(jì)的進(jìn)程，有效避免了采用隨機(jī)初始值帶來的運(yùn)算量大的問題。

附圖說明

圖1為發(fā)明方法原理圖。

具體實(shí)施方式

下面結(jié)合附圖及實(shí)例對(duì)本發(fā)明做進(jìn)一步的詳細(xì)說明。

本發(fā)明主要分為三部分，圖1所示為本發(fā)明方法原理圖，具體實(shí)現(xiàn)過程如下所述。

步驟1：基于增益自適應(yīng)互補(bǔ)濾波器的IMU方向估計(jì)。

IMU包含三個(gè)主要的傳感器：三軸陀螺儀、三軸加速計(jì)和三軸磁力計(jì)。IMU的姿態(tài)估計(jì)包括三類傳感器的方向估計(jì)，并對(duì)其估計(jì)值進(jìn)行融合。

步驟1.1：計(jì)算陀螺儀的方向估計(jì)。

步驟1.1.1：求解t+Δt時(shí)刻描述IMU方向的四元數(shù)的變化率(也稱導(dǎo)數(shù))公式如下：

其中，四元數(shù)中的q₁、q₂、q₃分量分別對(duì)應(yīng)旋轉(zhuǎn)軸在X、Y、Z方向的投影，q₀分量對(duì)應(yīng)IMU圍繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度；符號(hào)表示四元數(shù)乘積，表示t時(shí)刻IMU陀螺儀的四元數(shù)估計(jì)值，ω_t+Δt表示t+Δt時(shí)刻陀螺儀的輸出值。

步驟1.1.2：求解t+Δt時(shí)刻四元數(shù)的估計(jì)值

步驟1.1.3：歸一化處理四元數(shù)估計(jì)值，得出t+Δt時(shí)刻由陀螺儀估計(jì)出的四元數(shù)值

步驟1.2：計(jì)算加速計(jì)與磁力計(jì)的方向估計(jì)。

通過高斯牛頓的迭代優(yōu)化算法[Boyd S，Vandenberghe L.Convex optimization[M].Cambridge universitypress，2004]計(jì)算加速計(jì)與磁力計(jì)的方向估計(jì)值首先確定初始時(shí)刻的加速度值z(mì)_α和磁場(chǎng)強(qiáng)度值z(mì)_m，再根據(jù)t時(shí)刻加速計(jì)和磁力計(jì)的輸出值α(t)和m(t)，以最小化估計(jì)值誤差為目標(biāo)，建立目標(biāo)函數(shù)：

其中，和分別表示由加速計(jì)和磁力計(jì)估計(jì)值的誤差，計(jì)算過程為：

其中，表示的共軛四元數(shù)。

步驟1.3：基于互補(bǔ)濾波算法的方向估計(jì)。

聯(lián)合t+Δt時(shí)刻陀螺儀的方向估計(jì)值和以及加速計(jì)和磁力計(jì)的方向估計(jì)值采用增益自適應(yīng)互補(bǔ)濾波算法[TianY，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坐標(biāo)系下IMU的方向信息

其中k_t表示自適應(yīng)的增益系數(shù)，表示t時(shí)刻加速計(jì)、磁力計(jì)、陀螺儀融合后的方向估計(jì)信息，表示t時(shí)刻加速計(jì)與磁力計(jì)的方向估計(jì)值。

步驟1.4：IMU姿態(tài)估計(jì)。

采用運(yùn)動(dòng)力學(xué)中的扭轉(zhuǎn)表示和指數(shù)映射算法，初步估計(jì)IMU的姿態(tài)信息，具體過程包括：

步驟1.4.1：根據(jù)IMU估計(jì)出的四元數(shù)得到與四元數(shù)相對(duì)應(yīng)的旋轉(zhuǎn)軸ω＝(cosω_x，cosω_y，cosω_z)和繞軸旋轉(zhuǎn)的角度θ，求解過程是：θ＝2cos^-1q₀

步驟1.4.2：根據(jù)扭轉(zhuǎn)與指數(shù)映射理論，將(ω，θ)其映射為坐標(biāo)系間的旋轉(zhuǎn)和平移。已知初始時(shí)刻的位置g_{global，n}(0)與坐標(biāo)系間的轉(zhuǎn)換關(guān)系(R，T)后，計(jì)算n時(shí)刻IMU的位置g_global，n(θ)，其映射關(guān)系如下所示：

其中表示與ω相對(duì)應(yīng)的扭轉(zhuǎn)參數(shù)，v表示繞軸旋轉(zhuǎn)的角度，q_g表示初始時(shí)刻坐標(biāo)系原點(diǎn)與載體之間的向量。

步驟2：基于尺度旋轉(zhuǎn)不變的特征檢測(cè)方法，檢測(cè)與匹配特征點(diǎn)檢測(cè)與匹配。

步驟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]的特征點(diǎn)檢測(cè)；

步驟2.2：相鄰幀間圖像特征點(diǎn)的匹配。

通過計(jì)算特征點(diǎn)描述子之間的歐氏距離實(shí)現(xiàn)相鄰幀間特征點(diǎn)的匹配。對(duì)于圖像中的某個(gè)特征點(diǎn)P，其特征點(diǎn)描述子記為D_P＝[x₁，x₂，…x₆₄]^T。待匹配圖像中與P點(diǎn)對(duì)應(yīng)的第一近鄰點(diǎn)和第二近鄰點(diǎn)分別記為P₁和P₂。所述第一近鄰點(diǎn)和第二近鄰點(diǎn)分別指待匹配圖像的特征點(diǎn)中，與P點(diǎn)描述子歐氏距離最近的描述子的特征點(diǎn)和次近的描述子的特征點(diǎn)。P₁和P₂的特征點(diǎn)描述子分別記為P點(diǎn)與P₁和P₂的特征點(diǎn)描述子之間的歐式距離和可表示為分別表示為：

如果與的比值小于預(yù)定義的閾值δ_D，即則接受這一對(duì)匹配點(diǎn)。依據(jù)這一匹配規(guī)則，找到所有相鄰幀間的特征匹配點(diǎn)對(duì)，作為初始的匹配點(diǎn)對(duì)。

由于以上匹配方法僅考慮特征點(diǎn)描述子的相似程度，而圖像間存在周期性的紋理和相似的斑塊，仍然會(huì)造成誤匹配，需要進(jìn)一步把誤匹配點(diǎn)對(duì)移除掉。

步驟3：基于視覺IMU融合的方向估計(jì)。

旋轉(zhuǎn)矩陣R和平移向量T作為初始姿態(tài)信息帶入到相機(jī)本質(zhì)矩陣E＝T_×·R(T_×是關(guān)于平移向量T的負(fù)斜對(duì)稱矩陣)的估計(jì)中。

根據(jù)外級(jí)線約束方程x′Ex＝0，來初步判斷內(nèi)點(diǎn)和外點(diǎn)。所述內(nèi)點(diǎn)，是指兩幅圖像間正確的匹配點(diǎn)對(duì)；所述外點(diǎn)，是指兩幅圖像間錯(cuò)誤的匹配點(diǎn)對(duì)。

視覺IMU融合的方向估計(jì)是利用兩幅圖像間的匹配點(diǎn)對(duì)和IMU的姿態(tài)估計(jì)信息來建立后驗(yàn)貝葉斯估計(jì)模型，通過期望最大化算法來迭代估計(jì)出方向的。具體步驟如下：

步驟3.1：期望步驟得到本次迭代的姿態(tài)信息。

步驟3.1.1：利用步驟2中得到的內(nèi)點(diǎn)集合，通過外級(jí)線約束方程和最小二乘法估計(jì)本質(zhì)矩陣E。

步驟3.1.2：對(duì)本質(zhì)矩陣E做奇異值分解，估算出載體的姿態(tài)信息：旋轉(zhuǎn)分量R和平移分量T。

步驟3.1.3：建立后驗(yàn)貝葉斯估計(jì)模型：

其中(η₁，η₂，…η_n)表示相鄰幀圖像間的匹配的特征點(diǎn)對(duì)，θ表示視覺IMU姿態(tài)估計(jì)值和匹配點(diǎn)對(duì)的集合，定義為：

θ＝{R，T，O_n}

其中O_n＝(o₁，o₂，…o_n)表示匹配點(diǎn)對(duì)的集合，元素o_i服從伯努利分布，用于 判斷第i個(gè)匹配點(diǎn)對(duì)是否正確。o_i＝1表示樣本點(diǎn)為內(nèi)點(diǎn)；o_i＝0表示樣本點(diǎn)為外點(diǎn)。

步驟3.2：最大化步驟確定本次迭代中的內(nèi)點(diǎn)集合。

判斷迭代樣本點(diǎn)：

P{o_i|{R，T}}＝max{P(o_i＝1|{R，T})，P(o_i＝0|{R，T})}

其中P(o_i＝1|{R，T})和P(o_i＝0|{R，T})分別表示為：

其中表示Sampson誤差，S和σ為預(yù)先設(shè)定的窗口大小和方差，取決于具體應(yīng)用。y_n表示第n幅圖像中的第i個(gè)特征點(diǎn)的坐標(biāo)，f(x_n)表示第n幅圖像中的第i個(gè)特征點(diǎn)所對(duì)應(yīng)的外級(jí)線。

o_i取值為1或0由P(o_i＝1|{R，T})和P(o_i＝0|{R，T})的大小決定：如果P(o_i＝1|{R，T})≥P(o_i＝0|{R，T})，則o_i取值為1；反之，o_i取值為0。

反復(fù)迭代步驟3.1和步驟3.2，得到最終的參數(shù)估計(jì)集合θ^*＝{R^*，T^*，O_n^*}，其中θ^*表示載體姿態(tài)與匹配點(diǎn)對(duì)集合，T^*表示最終解算出的載體的平移分量，O_n^*表示最終做出的匹配點(diǎn)對(duì)集合，R^*即為可穿戴視覺IMU的方向估計(jì)。