基于光電鼠標與慣性導航數據融合的定位系統*

房?；?高向東 葉國良 郭建文 楊宇輝

(①廣東工業大學機電工程學院，廣東 廣州 510006; ②東莞理工學院機械工程學院，廣東 東莞 523000)

超聲檢測具有穿透能力強、靈敏度高、設備輕便及對人體無害的特點，是應用最廣泛且發展最快的無損檢測技術之一，能完成對金屬、非金屬和復合材料等的宏觀缺陷、幾何特性及力學變化等方面的檢測。在超聲無損檢測人工操作超聲探頭過程中面臨著無法自動定位和記錄檢測點位置而導致錯檢漏檢的問題，目前的定位方法有利用傳動裝置與編碼器進行定位精度控制，操作數控工作臺或者機械手獲取實時的掃查位置，結合超聲檢測信號和成像技術，得到和實際的缺陷分布有一一對應關系的圖像，定位精度較高，但這也存在檢測系統成本提高和靈活性降低的問題[1-2]。文獻[3]針對手持超聲成像，提出一種在光學定位條件下基于立體標定模板的超聲探頭標定方法，能同時采集超聲圖像和探頭在攝像機中的圖像，求解兩者之間的空間變換關系，在標定圖像數量較少時也有較高的精度。參考移動機器人的室內定位方法，慣性導航定位是更加靈活、簡便的自主定位方法，它利用運動傳感器不斷從外界獲取物體的位置、姿態信息，通過數據融合算法，更新實時位姿。文獻[4]提出利用加速度傳感器和陀螺儀數據推算超聲檢測過程的運動軌跡，并通過姿態轉換和卡爾曼濾波修正運動信息。但在推算位移時要解決加速度傳感器零點漂移的積累誤差，而且在實際的慣性導航應用中系統噪聲模型往往是時變的，卡爾曼濾波數據融合存在局限性。文獻[5]在系統噪聲統計特性為時變時，提出在拓展卡爾曼濾波基礎上加入自適應過程，能抑制濾波發散現象，算法穩定性和實時性更好。針對積累誤差問題，文獻[6]提出光電鼠標里程計獲取位移信息，而不是通過不斷獲取加速度數據計算位移。光電鼠標能夠直接記錄運動過程，價格低廉且能在航跡推算中消除大部分誤差，精度很高。

針對上述眾多問題，本文提出利用光電鼠標位移傳感器與慣性測量單元(internal measurement unit，IMU)組合導航的定位方法，經過自適應卡爾曼濾波算法融合，得到定位系統的姿態角最優估計，結合光電鼠標輸出的位置坐標解算出定位點的世界坐標。設備簡易輕便，有望能夠快速實現超聲無損檢測過程中超聲探頭的準確定位。

1 定位方法

本文研究的基于光電鼠標和慣性導航數據融合的定位系統是以超聲探頭為目標載體，在傳統手動操作超聲探頭進行無損檢測的基礎上，利用光電鼠標輸出的二維坐標值和慣性測量單元的航推位置信息，達到實時位置標定的目的。光電鼠標獲取移動過程中的鼠標坐標系下的坐標，IMU獲取實時的運動姿態信息，包括加速度、角速度、磁分量等。由于陀螺儀角速度數據積分過程中存在漂移現象，時間越長就會產生越大的定位誤差，利用地磁計數據補償加速度數據解算出姿態角，經過自適應卡爾曼濾波算法，能夠得到更為準確的更新后的姿態角，提高定位精度與可靠度。最后根據光電鼠標、IMU與超聲探頭之間的運動學幾何關系，利用光電鼠標的輸出坐標和姿態角變化解算出世界坐標系下超聲探頭的位姿，正確標定超聲探頭檢測實時位置。如圖1所示為系統整體架構。

1.1 地磁計和加速度計姿態更新算法

當運動物體處在靜止或勻速運動狀態時，物體只受當前狀態下的重力加速度影響。導航坐標系(n系)和載體坐標系(b系)下的加速度關系為：

式中：ab為加速度計輸出[ax,ay,az]T，an為重力矢量[0,0,g]T，方向余弦矩陣

可以算出運動物體的俯仰角和橫滾角：

(1)

運動物體在n系下的磁感應強度為

(2)

式中：φm為相對于磁北的航向角，Δφ為真北和磁北的磁偏角。

當運動過程中不能保證始終與水平面平行時，需要旋轉roll和pitch角，把地磁計的數據轉換到水平面，即

(3)

超聲無損檢測過程一般會保持在水平面，只有航向角變化，那么就只需要根據式(2)地磁計水平方向上磁分量求得航向角。

1.2 陀螺儀角度數據融合算法

加速度計和地磁計可以測得3個姿態角的關系，但容易受到外部環境的干擾，其動態響應較慢，長時間內測量值比較準確。陀螺儀可以積分得到3個姿態角的關系，且不容易受到外部環境的干擾，動態響應較快，短時間內測量值比較準確，但長時間積分會造成累積誤差[7]。所以要根據加速度計、陀螺儀和地磁計數據進行陀螺儀角度數據融合，得到目標載體的姿態角最優估計。

在實際的慣性導航應用中，系統噪聲的噪聲方差Q和觀測噪聲誤差R往往是未知的或者是變動的，隨著時間推移，將會使估計誤差超出理想濾波范圍，甚至產生發散現象[8]。所以可以使用自適應卡爾曼濾波算法，在卡爾曼濾波的基礎上，加入自適應過程，利用量測數據自動更新時變噪聲參數Q和R，提高濾波過程的精確度，得到最優估計。

Sage-Husa卡爾曼濾波算法是Sage和Husa提出的一種自適應濾波算法，可以在線計算系統噪聲和量測噪聲，減少由模型誤差帶來的影響[9]。在此基礎上引入遺忘因子，逐步發展出簡化Sage-Husa算法以及改進Sage-Husa卡爾曼濾波算法。研究者通過噪聲模型的統計特性分析發現當系統噪聲和觀測噪聲都未知的情況下，并不能同時把它們估計出來，必須要有一個已知項。通常認為組合導航定位模型下，系統噪聲具有穩定性而只考慮觀測測噪聲的更新[10]。與傳統卡爾曼濾波相比，Sage-Husa自適應卡爾曼濾波具有更好的實時性、適應性和魯棒性，能夠提高濾波精度，并且得到了廣泛應用。

首先建立系統狀態方程和量測方程：

(4)

式中：Xk為k時刻的狀態向量：A為狀態轉移矩陣；B為控制矩陣；H為量測矩陣；U為輸入控制向量，在該模型中為陀螺儀輸出的角速度；Zk為k時刻的量測向量，在該模型中為加速度計與地磁計測量的航向角；wk和vk分別為系統過程噪聲和量測噪聲。

具體的算法流程如下：

①k時刻狀態一步預測方程：

②一步預測協方差方程：

③濾波增益方程：

⑥k時刻協方差：Pk=(I-KkHk)Pk|k-1

⑧權重系數：dk=(1-b)/(1-bk+1) 0

其中，b為遺忘因子，取值范圍[0.95,0.99]。遺忘因子通過改變加權系數，更多地接受新數據對濾波的影響，并使濾波過程保持在理想誤差范圍內。

1.3 光電鼠標與IMU定位運動學分析

光電鼠標是基于光學導航技術，通過光學反射獲取連續的表面圖像，利用數學關系確定運動的方向和幅度來測量位置的變化[11]。影響光電鼠標定位精度的要素有兩個：分辨率和刷新率。

(1)當一個光電鼠標的分辨率固定時，其分辨率越高，移動相同物理距離時獲得更多的坐標數量，位置信息更精確。

(2)刷新率是指單位時間內采集和處理圖像的數量，刷新率越高則CMOS成像信息也越連貫，通過信號處理器處理得到的移動距離和方向也越準確，能處理到更為微小的移動。

通過每一幀圖像的像素分析，計算出位移增量，但這個增量是基于鼠標坐標系的絕對位移，而非基于某一個確定的世界坐標系的絕對位移。本文的定位方法是要得到世界坐標系下超聲探頭的絕對位置坐標，所以需要對鼠標的輸出進行坐標變換。

如圖2為傳感器坐標關系圖[12]。只考慮二維平面直角坐標系，對光電鼠標、IMU和超聲探頭的檢測過程進行建模分析。定義OwXwYw為世界坐標系，OtXtYt為超聲探頭坐標系，OsXsYs為鼠標坐標系。其中Ot和Os分別為超聲探頭和鼠標傳感器的幾何中心，二者之間的安裝距離為L;超聲探頭坐標系與世界坐標系的夾角為θ，可以認為是每次旋轉運動時的旋轉角;鼠標坐標系與超聲探頭坐標系的夾角φ為安裝偏差角，L與Xt方向的夾角為ψ。

在世界坐標系下，光電鼠標和超聲探頭的相對位置滿足

(5)

(6)

(7)

即

上式是在鼠標和超聲探頭存在安裝距離和安裝角情況下的定位公式。假設超聲探頭坐標系與鼠標坐標系的原點重合，即L=0且ψ=0，但安裝角φ存在，根據光電鼠標和IMU的輸出，在世界坐標系下，超聲探頭的定位公式為

(8)

根據上式，θi通常會隨旋轉運動而改變，而安裝角偏差φ為固定值。為了求出此安裝偏差角，可以在鼠標移動過程中保持旋轉角θi=0恒定，通過實驗能夠擬合出此偏差角：

(9)

2 實驗與結果

2.1 光電鼠標與慣性測量單元的軟硬件開發與實驗裝置設計

本次實驗主要分為光電鼠標和慣性導航兩部分的數據采集與分析。前者硬件平臺主要包括Arduino UNO、Arduino USB Host Shield和光電鼠標。利用Arduino拓展板讓Arduino UNO設備連接一個USB光電鼠標，通過Arduino實時獲取鼠標移動過程中輸出的像素坐標，并轉換為真實的物理坐標封裝起來，方便后續處理。這里需要安裝Arduino USB Host Shield2.0和ThreadController開發庫。后者硬件平臺為IMU模塊，該模塊的核心為MPU6050傳感器，集成了高精度的陀螺儀、加速度計、地磁計，能夠快速解算出所需的實時加速度、角速度和磁分量數據。在實驗前需在Arduino上測試鼠標輸出坐標的準確性，在IMU自帶調試軟件中設置回傳速率、波特率、輸出物理量等參數。如圖3所示為硬件連接框圖，圖4為實驗裝置圖。

本次實驗光電鼠標的分辨率為1 200 DPI，設置數據回傳速率100 Hz；IMU設置的主要參數有回傳速率100 Hz、設備地址0x50、波特率115 200 bit/s等。預制一個116 mm×96 mm的矩形環，保證當鼠標在環內運動時有準確的世界坐標。在保證Arduino輸出的鼠標數據和IMU輸出的姿態數據準確的前提下，根據串口通信協議，分析設備地址、數據傳輸方式和計算公式，通過MATLAB軟件編寫上述兩個傳感器的數據采集接口，使兩個傳感器能保持通信同步，得到的位置坐標更精確。

2.2 角度數據融合結果與分析

利用上述的實驗裝置，在移動過程中保持與水平面平行，不間斷地改變航向角的方向，通過MATLAB建立的數據采集接口統一采集保存為離線數據，利用 MATLAB軟件編寫的Sage-Husa自適應卡爾曼濾波算法程序對其進行數據融合來獲取準確姿態角。

圖5是傳感器在移動過程中通過角速度積分和使用Sage-Husa自適應卡爾曼濾波算法得到的航向角曲線。圖6是圖5中角速度積分與自適應卡爾曼濾波數據融合的航向角差值曲線。選取的遺忘因子數值為0.95，由圖5和圖6可以看出，陀螺儀積分得到的航向角在35 s左右發生了0.3°的漂移，且在移動過程中存在著累積誤差，特別是在運動方向發生變化時，產生的誤差更明顯。經過自適應卡爾曼濾波算法進行數據融合，既能很好地消除陀螺儀的積分飄移，也能明顯地降低數據的噪聲影響，使估計的姿態角更平滑。

2.3 定位結果與分析

通過上述的實驗測試，在預制的矩形環內移動，對采集的鼠標坐標和角度離線數據進行坐標變換處理，通過MATLAB軟件編程繪制了預定軌跡、坐標轉換前后的運動軌跡對比圖，如圖7所示。圖8是鼠標在矩形環內移動時經過自適應卡爾曼濾波的航向角變化圖。從運動軌跡上看，坐標轉換前鼠標的輸出坐標僅僅是相對于鼠標坐標系的絕對坐標，其軌跡并不能反映實際運動軌跡。結合每個數據點的角度變化和鼠標輸出的坐標增量進行坐標變換，得到解算后的世界坐標系下的運動軌跡與預定軌跡重合度很高，有少許的波動，但能更好地反映檢測點的實際位置。

表1 預定軌跡移動解算坐標與真實坐標結果對比

具體分析，表1是在預定軌跡中移動的四個定位點的解算坐標和真實坐標結果對比。起始點為(0，0)，沿圖8中1-2-3-4順序移動，從解算軌跡和坐標可以看出，在整個過程中x軸方向的位置偏差較小，而y軸方向特別是1-2過程的數據缺失出現了0.132 cm的位置偏差，在3-4過程雖然y軸方向并未出現數據波動，但由于前面檢測過程的位置偏差的累積，最終定位點4還是有0.179 cm的位置偏差。出現數據偏離的原因可能是實驗裝置在實際移動過程中受到振動或沖擊產生了誤差，特別是光電鼠標，在光學反射過程中可能存在丟失幀的情況，導致數據的缺失，從而影響定位效果。

3 結語

本文為解決超聲波無損檢測人工操作超聲探頭時無法自動標定位置而導致錯檢漏檢的問題，提出利用光電鼠標與慣性測量單元組合導航的定位方法。通過自適應卡爾曼濾波算法，經過地磁計的補償，消除了陀螺儀的積分漂移，獲得姿態角的最優估計。根據傳感器運動學幾何關系，利用鼠標輸出數據和姿態角變化解算出超聲探頭在世界坐標下的絕對位置，能實現檢測過程的準確定位。但在定位結果對比中出現數據缺失現象導致細微地偏離預定軌跡，說明在實驗裝置設計制作或者位置估計中存在不足，今后將在這兩個方面繼續深入研究，實現設備簡易輕便、檢測過程快速精確且實時位置標定的目標。