基于自適應濾波的頭部姿態跟蹤方法研究(通訊)

                劉世良^1，2，  仇振安³，  丁全心²，  張勁鋒²

    （1．光電控制技術重點實驗室，河南洛陽471000；2．中國航空工業集團公司洛陽電光設備研究所，河南洛陽471000；3．陸航駐洛陽地區軍事代表機構，河南洛陽471000）

摘要：針對視覺式頭部定位系統存在著噪聲統計特性掌握不充分的問題，設計了一種新的基于自適應濾波的頭部姿態跟蹤方法，其綜合了視覺測量算法和自適應濾波算法。在獲取n對2D/3D（圖像點／空間點）匹配對的基礎上，先利用正交迭代算法( 01)解算頭部姿態，然后應用自適應濾波器估計精度更高的頭部姿態，最后針對帶有時變噪聲且噪聲統計特性掌握不充分的測量值，應用頭部姿態跟蹤方法進行了仿真測試，結果顯示該方法的測量精度有很大提高，并且驗證了所設計的頭部姿態跟蹤方法是合理且有效的。

關鍵詞：頭部定位系統；姿態跟蹤；正交迭代算法；自適應濾波

中圖分類號：V243.6   文章編號：1671  - 637X( 2016) 04 - 0033 - 04

0引言

    頭部姿態跟蹤就是跟蹤測量頭部相對于世界坐標系的3個姿態角度。頭部姿態跟蹤是虛擬現實和增強現實的核心技術之一，目前廣泛應用的頭部姿態跟蹤方式有電磁式、光電式和計算機視覺式。與電磁式、光電式相比，視覺式頭部定位技術在解決頭部姿態跟蹤定位時，干擾小、測量誤差小、運算速度快、穩定可靠，因而得到了廣泛的應用?；�于視覺式的頭部姿態定位技術根據傳感器（攝像機）安裝位置分為外置傳感器視覺式和盔載傳感器視覺式兩大類。兩者基本原理相似，區別在于特征點的選取及其相應的圖像處理部分不同?；�于視覺式的頭部姿態定位技術是根據空間中n個控制點的世界坐標與其在單幅圖像中的坐標之間的對應關系，通過求解PnP問題完成頭部姿態的跟蹤、定位。求解PnP問題的方法可分為線性算法和非線性迭代算法兩大類。經典的線性算法有直接線性變換法(DLT)和EPnP算法，雖然兩者計算速度很快，但是易受噪聲干擾且精度不高；非線性迭代算法通過對某種目標函數進行最優化迭代，可以獲取相對精度很高的數值解。經典的非線性迭代算法有Leven-berg-Marquardt提出的最大似然估計(LM)算法和正交迭代( OI)算法。OI算法精度略次于LM算法，但是速度卻遠快于LM算法。通過PNP問題的求解可以得到各個時刻頭部相對于飛機坐標系的頭部姿態角。

    圖像處理環節引入的圖像噪聲最終會帶人PNP算法解算的頭部姿態角誤差中，而且頭部姿態角誤差會隨著測量角度和測量環境的變化而變化。目前對這些綜合誤差因素造成的頭部姿態角測量值的噪聲統計特性掌握還不充分。而且目前視覺式頭部定位技術只能求解當前時刻的頭部姿態角，不能預測和跟蹤下一時刻的頭部姿態，對于拍攝死角引起的頭部姿態測量丟失問題無能為力。針對視覺測量中存在的非線性和離散化等特點，通過建立合適的運動模型，應用擴展卡爾曼濾波( EKF)對頭部運動的姿態進行估計，而對于測量噪聲和系統噪聲統計特性掌握不充分和時變性，EKF就不再適用了，濾波效果明顯下降。本文在EKF的基礎上引入自適應濾波進行最優化估計，獲取了更高精度的頭部姿態角。

1方法描述

    本文跳過相機標定、特征識別和特征匹配等前期圖像處理工作，直接利用前期圖像處理的結果，即在獲取n對2D/3D（圖像點／空間點）坐標點匹配對——目標特征點在二維圖像坐標系和三維世界坐標系中的坐標點對的基礎上進行算法研究。圖1所示為2 D/3D坐標點匹配對在相機坐標系的示意圖。

為攝像機內參數；R為攝像機相對于世界坐標系的旋轉矩陣；T為平移向量。如圖2所示，在獲取2D/3D點對后，把匹配點對代入OI算法中解算出3個頭部姿態角：方位角、俯仰角和橫滾角，最后由自適應濾波算法估計出最優頭部姿態。

2  頭部姿態求解算法

    1998年LU提出的全局收斂的OI算法以目標空間共線性誤差為目標函數，利用最小二乘原理進行迭代優化，可以得到高精度的數值解且具有抗噪性強和實時性

則有

3基于自適應濾波的頭部姿態估計

    頭部運動一般是非線性的，其運動軌跡很難用具體的運動函數來表達。由于頭部運動具有慣性和連續性，所以頭部當前運動狀態和過去運動狀態是密切相關的。假設頭部運動可以模擬為勻速轉動的運動模型來逼近實際的頭部機動運動過程，并且引入系統狀態噪聲W_k來補償勻速運動模型與頭部實際機動運動中帶有的無規則加速度的誤差，它為零均值的高斯噪聲，且其方差陣為con（W_k）=Q _k。假設V為觀測噪聲，其反映了圖像處理誤差和正交迭代算法誤差等綜合誤差因素造成的觀測值的偏差。設它為零均值的高斯噪聲，且其方差陣為con(V_K)=R _k。系統狀態方程和測量方程為

3.1  EKF

    針對頭部運動的非線性和OI算法獲取的觀測值離散化的特點，應用方法比較成熟的EKF來估計頭部位姿，其濾波方程為

3.2  自適應濾波算法

    目前對于系統噪聲W_k和姿態觀測噪聲V_K的統計特性掌握不充分，而且隨著環境變化，觀測噪聲是時變噪聲。使用傳統的EKF進行頭部姿態估計和預測，因為不能合適選取QK和RK，將導致估計精度的下降。對于這種情況，引入自適應濾波對系統狀態噪聲和測量噪聲進行統計更新，從而進一步提高頭部位姿的估計和預測精度。在K時刻，利用測量值對預測值進行修正后，前K個頭部姿態狀態信息可以對系統狀態噪聲和測量噪聲統計信息進行更新。引入狀態估計差值

著K值的增大，新信息利用率會越來越小。為了使自適應濾波更有效地利用新信息，需要固定新信息的利用效率，更改式(17)為式(18)可使新信息的利用率不隨著時間的積累而衰弱，同時過去很久的信息會隨時間的更新而衰弱。

    自適應濾波利用估計殘差來對統計不充分且時變的系統狀態噪聲和測量噪聲統計參數Q_K和R_K進行實時修正更新，以提高頭部姿態的估計精度。自適應濾波只需在EKF的基礎上加入噪聲估計器：式(16)、式(18)、式(19)和式(20)即可。

4仿真及分析

系統的仿真流程如圖3所示。

利用Matlab2012a對上述算法進行仿真驗證。在給出攝像機內參數K、目標特征點在世界坐標系中的坐標和運動模型的前提下，先根據運動模型推算出頭部姿態角并轉化成頭部姿態參數R和T，然后由式(1)計算出目標特征點在圖像坐標系中理論的2D坐標，最后在2D坐標中添加相應的干擾噪聲進行仿真實驗。

4.1  PnP算法仿真比較

 由于系統在攝像機標定、圖像畸變校正、特征中心坐標提取等圖像處理環節會引入隨機誤差，根據PnP問題的特點，這些隨機誤差最終會累積在圖像特征點的2D坐標中，因此在2D圖像特征點坐標中添加均值為0、均方差以步長0.1像素，在0.1～2像素的范圍內增長的隨機高斯圖像噪聲；隨機給定旋轉矩陣和平移向量，每個步長進行2000次隨機實驗。以3個頭部姿態角均方差為例對DLT算法、EPnP算法和OI算法進行仿真測試，仿真結果如圖4所示。

    從仿真結果來看，3種算法解算的頭部姿態角誤差均方差都隨著圖像特征點噪聲的增加而線性增大。而且非線性的OI算法性能優于線性的DLT算法和EPnP算法。由此可以看出，非線性的OI算法對于解算頭部姿態角來說，性能更好，可以為后面的自適應頭部姿態估計提供更加精確的測量值。

4.2濾波算法仿真比較

    在4.1節的仿真基礎上，利用OI算法解算得到每一時刻的頭部姿態測量值。由于系統在圖像處理環節和OJ算法解算頭部姿態環節中會引入誤差，目前對這些綜合誤差的統計并不充分，而且隨著環境因素的改變，還會引入時變的測量噪聲，所以在仿真時應在圖像坐標中加入適當的時變噪聲。假沒頭部運動的初始位置為方位角45⁰，俯仰角30⁰，橫滾角5⁰，初始角速度分別為0. 015(⁰)/s，0.01(⁰)/s，0.009(⁰)/s，測量周期設為T= 20 ms，相關時間30 min，分段添加時變噪聲。仿真結果如圖5所示。

  從圖5可以明顯看出，經過2種濾波方法處理后，頭部姿態角的精度都有所提高，3個姿態角的誤差范圍與校正前相比明顯減小。EKF雖然能夠改善頭部姿態角的誤差范圍，但是對于未充分統計的噪聲，其估計效果明顯不如自適應濾波；對于加入時變噪聲的頭部姿態角測量值，EKF的濾波精度隨著測量精度的變差而降低，而自適應濾波卻能很好地適應變化了的噪聲。濾波初期的前50 s內，自適應濾波的濾波精度和EKF相當，隨著對噪聲方差陣Q_K和R_K的統計更新，自適應濾波開始優化，50 s以后能夠完全適應時變的噪聲并進行更精確的濾波。自適應濾波的濾波精度可以達到0.1⁰以內，對于加入時變噪聲的觀測值能夠快速穩定地跟蹤。

5結論

 本文提出的基于自適應濾波的頭部姿態跟蹤方法，首次針對視覺式的頭部姿態定位技術中帶有時變且未充分統計噪聲的測量值進行了自適應濾波估計研究。經仿真驗證，該算法合理有效并且獲得了比EKF更高的精度。本文對提高傳統的計算機視覺頭部姿態定位方法的精度，提供了一種可行、有效的理論方案依據。對于本文提出的自適應濾波算法需要較長時間才能達到穩態的不足，在未來的研究中將根據實際應用需要，逐步對自適應濾波算法進行改進，在保證實時性的前提下縮短達到穩態所需要的時間，并提高跟蹤的精度。