基于光電鼠標與慣性導航數(shù)據(jù)融合的定位系統(tǒng)*

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

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

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

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

1 定位方法

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

1.1 地磁計和加速度計姿態(tài)更新算法

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

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

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

(1)

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

(2)

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

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

(3)

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

1.2 陀螺儀角度數(shù)據(jù)融合算法

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

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

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

首先建立系統(tǒng)狀態(tài)方程和量測方程：

(4)

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

具體的算法流程如下：

①k時刻狀態(tài)一步預測方程：

②一步預測協(xié)方差方程：

③濾波增益方程：

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

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

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

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

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

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

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

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

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

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

(5)

(6)

(7)

即

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

(8)

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

(9)

2 實驗與結果

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

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

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

2.2 角度數(shù)據(jù)融合結果與分析

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

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

2.3 定位結果與分析

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

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

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

3 結語

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