劉艷霞,方建軍,楊清梅
(北京聯(lián)合大學自動化學院電氣與控制工程系,北京 100101)
加速度計是果蔬采摘機器人控制必不可少的傳感器。機器人借助加速度計掌握姿態(tài)信息(橫滾、俯仰),控制姿態(tài),保持平衡,并做出相應的動作。果蔬采摘機器人采用霍尼韋爾生產(chǎn)的QA—160T型石英撓性加速度計,它具有機械性能穩(wěn)定、內(nèi)耗小、遲滯小、漂移小、價格低等優(yōu)勢,成為一種十分有發(fā)展前景的撓性加速度計。文獻[1]對石英加速度計的動態(tài)性能進行了測試,結(jié)果表明:加速度計的輸出曲線中包括常值偏差、周期振蕩誤差和白噪聲。文獻[2]利用16位置翻轉(zhuǎn)法對加速度計誤差進行標定和補償。真實加速度采用不同位置時重力加速度在三軸上的投影,利用這16組投影數(shù)據(jù)和加速度計的16組輸出值,通過最小二乘法求解誤差模型參數(shù),對加速度計誤差進行補償。該方法對不同位置時姿態(tài)角精度要求較高,姿態(tài)誤差會大大影響加速度計誤差參數(shù)計算和補償精度。
本文提出一種基于橢球假設的三軸加速度計誤差標定和補償方法,數(shù)據(jù)采集時只需要傳感器姿態(tài)在三維空間大致均勻分布即可。
三軸加速度計由于制造工藝的限制存在零位誤差、靈敏度誤差和三軸不正交誤差。零位誤差指無加速度輸入時,因制造誤差加速度計也會有一定非零伏的輸出電壓[3],可表示為
其中,g0x,g0y,g0z分別為三軸加速度計在x軸、y軸和z軸的零位偏差。
靈敏度誤差是當被測加速度在x,y和z軸的分量相等時,三軸加速度計靈敏度不一致,輸出和標稱值之間出現(xiàn)偏差,可表示為
其中,Sx,Sy,Sz分別為加速度計x,y,z3 個測量軸的實際靈敏度,Sn為靈敏度的標稱值。
理想情況下,加速度計的3個軸是完全正交的,但即便是經(jīng)過廠商精密加工,其敏感軸通常也會偏離正交坐標系微小的角度[4],輸出產(chǎn)生三軸不正交誤差。假設加速度計z軸和正交坐標系pm的z軸完全重合,其它兩軸與正交坐標系的夾角分別為α,β和γ,如圖1所示。
圖1 三軸加速度計不正交誤差Fig 1 Nonorthogonal error of three-axis accelerometer
不正交誤差可表示為
通常,廠商提供的三軸加速度計不正交誤差角度小于0.5°,為計算方便,可以對式 (3)中的三角函數(shù)進行化簡。綜合考慮以上三軸加速度計的各種制造誤差,真實加速度ge和三軸加速度計測量值gm之間的關系如下
共6個獨立的誤差參數(shù),加上3個零點偏差,式(5)中共9個未知參數(shù)。
理想三軸加速度計在同一地點旋轉(zhuǎn)時,重力加速度幅值不變,三軸輸出軌跡為圓球面,但受誤差影響輸出軌跡會產(chǎn)生畸變。對式(5)取ge模的平方可得到
整理為曲面的二次型形式
因三軸加速度計不正交誤差一般小于0.5°,誤差矩陣C嚴格對角占優(yōu)[5]。由此可知矩陣LTL嚴格對角占優(yōu),為正定實對稱矩陣,式(7)為二次型橢球面方程,寫成二次曲面一般方程有
其中,σ =[a,b,c,d,e,f,p,q,r,s]T為橢球系數(shù)向量。另外,由式(6)可以很容易得到
擬合的原理就是找到一個理想橢球使得測量數(shù)據(jù)點到該橢球面的代數(shù)距離平方和最小
也可表示為矩陣形式
一旦確定了橢球系數(shù),就可以利用解析法得到誤差校準矩陣L和零位偏差g0。
橢球擬合問題就轉(zhuǎn)化為求約束下的條件極值??梢酝ㄟ^奇異值分解,求(DTD,C)的廣義特征值λi和對應的廣義特征向量σi。文獻[7]中證明式(14)的特征值中有且僅有一個為正,該特征值對應的特征向量即為待定的橢球系數(shù)向量σ。根據(jù)橢球系數(shù),利用解析的方法即可求出誤差參數(shù)(其中,‖ge‖取當?shù)刂亓铀俣?,?.8 m/s2)
由這些誤差參數(shù)即可求出誤差矩陣C和校準矩陣L,結(jié)合橢球系數(shù)p,q,r進一步得到橢球球心坐標g0。把L和g0代入式(5),即可實現(xiàn)對三軸加速度計的誤差補償。
為驗證上述橢球假設法標定的誤差補償效果,將三軸加速度計固定在三自由度轉(zhuǎn)臺上,為獲得最佳的橢球擬合效果,標定位置的姿態(tài)應盡量均勻分散在圓球表面的各個方向[8]。根據(jù)文獻[8]中的等夾角均勻分布公式,當不同俯仰角時,使轉(zhuǎn)臺沿順時針方向旋轉(zhuǎn)1周,在不同航向角位置采集三軸加速度數(shù)據(jù)(姿態(tài)角不需要太精確,大致均勻分布即可)。
采集的數(shù)據(jù)組成第3節(jié)中提到的設計矩陣D,利用最小二乘法可實現(xiàn)橢球擬合;然后根據(jù)擬合后的橢球系數(shù)解算出誤差參數(shù),對加速度計誤差進行補償,補償結(jié)果如圖2所示。
圖2 三軸加速度計44組數(shù)據(jù)誤差補償Fig 2 Error compensation results of 44 groups of datas of three-axis accelerometer
圖2中加速度計最大最小絕對誤差由校準前的0.2241 m/s2減小為0.0027 m/s2,均方根誤差由校準前的0.0950 m/s2下降到6.2547 ×10-4m/s2。
圖3和圖4分別為另外2次24組數(shù)據(jù)和80組數(shù)據(jù)的誤差補償實驗。
圖3中加速度計最大最小絕對誤差由校準前的0.4608 m/s2減小為 0.002 7 m/s2,均方根誤差由校準前的0.1540 m/s2下降到6.6795 ×10-4m/s2。
圖4 三軸加速度計80組數(shù)據(jù)誤差補償Fig 4 Error compensation results of 80 groups of datas of three-axis accelerometer
圖4中加速度計最大最小絕對誤差由校準前的0.4071 m/s2減小為 0.004 0 m/s2,均方根誤差由校準前的0.1153 m/s2下降到8.6051 ×10-4m/s2。
利用橢球假設法,把加速度計誤差標定問題轉(zhuǎn)化為求橢球約束下的條件極值,實現(xiàn)三軸加速度計誤差補償。該方法對傳感器標定時姿態(tài)角精度要求不高,大致均勻分布即可,操作簡單易行。實驗結(jié)果表明:橢球假設法補償后,加速度計最大最小絕對誤差可以減小100倍左右,精度達到10-3m/s2,補償效果顯著。3次誤差補償實驗結(jié)果也證明了實驗具有重復性。這種補償方法還可推廣應用于陀螺加速度計等慣性器件。
[1]孫 偉,徐愛功,孫 楓.慣導系統(tǒng)中石英加速度計的動態(tài)測試方法研究[J].傳感器與微系統(tǒng),2012,31(10):61-67.
[2]Fang J C H,Sun H W,Cao J J,et al.A novel calibration method of magnetic compass based on ellipsoid fitting[J].IEEE Transactions on Instrumentation and Measurement,2011,60(6):2053-2061.
[3]吳天準,董景新,劉云峰.電容式微機械加速度計閉環(huán)系統(tǒng)的零偏[J].清華大學學報:自然科學版,2005,45(2):201-204.
[4]楊常松,徐曉蘇.捷聯(lián)慣導系統(tǒng)速度計標度因數(shù)和安裝誤差的試驗標定[J].測控技術,2005,24(12):57-59.
[5]李 智,李 翔.基于橢球假設的三軸電子羅盤羅差補償研究[J].儀器儀表學報,2011,32(10):2210-2215.
[6]Nikos G,Michael G S .Head detection and tracking by 2D and 3D ellipsoid fitting[C]//Proceedings of Computer Graphics International Conference,2000:221-226.
[7]Halir R,F(xiàn)lusser J.Numerically stable direct least squares fitting of ellipses[C]//Proc of the 6th International Conference in Central Europe on Computer Graghics and Visualization,1998:125-132.
[8]劉艷霞,李希勝,馮毅博,等.三軸磁羅盤標定位置分布的研究[J].儀器儀表學報,2013,34(3):684-690.