李瑞, 王偉, 唐龍鑫, 李潔

(1.重慶郵電大學自動化學院, 重慶 400065; 2.重慶市醫(yī)用電子與信息技術工程研究中心, 重慶 400065;3.重慶郵電大學生物信息學院, 重慶 400065)

高精度姿態(tài)信息對無人機、人體運動姿態(tài)分析和導航定位后期分析起著決定性的作用。然而,在姿態(tài)和位移的估算中,存在著估算精度低、易受磁場和大加速度干擾問題。

目前,常用的3種基礎姿態(tài)解算算法有梯度下降算法、卡爾曼濾波算法、互補濾波算法[1-3]。中外學者為提高姿態(tài)解算精度和抗干擾能力,基于上述3種基礎算法提出很多改進方法。高怡等[4]使用梯度下降法來解算姿態(tài)信息,通過引入自適應因子和常量控制因子來增強算法在復雜環(huán)境下的自適應能力,從提高解算精度。但是從結果來看,精度提升有限,而且并未考慮磁場和加速度的干擾問題。盧艷軍等[5]通過兩次擴展卡爾曼的方法來改進常規(guī)的卡爾曼濾波算法,實驗證明在靜態(tài)和動態(tài)下的姿態(tài)精度都得到改善,具有一定抗磁干擾性質。但是在靜態(tài)測試中,還是不能完全隔絕磁場干擾,偏航角存在較大誤差。Li等[6]在互補濾波算法中引入兩個獨立的參數(shù)和來分別抵抗加速度干擾和磁場干擾,但單純的引入并不能有效的解決復雜環(huán)境對姿態(tài)的影響。

鑒于此,提出一種基于決策樹的自適應互補濾波姿態(tài)解算方法,通過綜合加速度干擾和磁場干擾的程度來自主調節(jié)合適的系統(tǒng)誤差增益參數(shù),以避免外部干擾對姿態(tài)解算精度的影響。

1 姿態(tài)系統(tǒng)概述

根據(jù)慣性導航原理,一般定義兩個基本坐標系,分別為導航坐標系n和載體坐標系b,如圖1所示。其中,導航坐標系Xn-Yn-Zn軸指向東-北-天方向,載體坐標系Xb-Yb-Zb軸構成右手直角坐標系。慣性傳感器的姿態(tài)就是載體坐標系b相對于導航坐標系n的三維方向關系[7]。姿態(tài)表達方式有歐拉角、四元數(shù)和旋轉矩陣3種。因四元數(shù)有著獨特的優(yōu)勢,主流的姿態(tài)算法主要基于四元數(shù)法進行姿態(tài)估算,只有在表示最終估算結果時,將四元數(shù)轉換為易于理解的歐拉角。

Xn-Yn-Zn為導航坐標系;Xb-Yb-Zb為載體坐標系;ο為坐標原點;φ為俯仰角;θ為橫滾角;ψ為航向角圖1 導航坐標系和載體坐標系Fig.1 Navigation coordinate system and carrier coordinate system

2 基于決策樹的互補濾波算法設計

2.1 算法框架

算法系統(tǒng)框架如圖2所示。首先,將零偏補償?shù)慕撬俣葦?shù)據(jù)和原始加速度數(shù)據(jù)構成靜態(tài)檢測單元,判斷姿態(tài)是否發(fā)生變化,如果判斷為靜態(tài),姿態(tài)信息就用上一刻的姿態(tài)更新;判斷為動態(tài),就利用磁場數(shù)據(jù)和加速度數(shù)據(jù)來構成決策樹,決策樹能夠根據(jù)干擾程度自適應的調節(jié)誤差增益進而達到抵抗外界干擾。將加速度誤差和磁場誤差相加,乘上互補濾波的比例調節(jié)系數(shù),再與角速度相加,就可以得到修改后精準的角速度數(shù)據(jù)。然后四元數(shù)的變化率就可以通過上一刻的姿態(tài)信息和角速度數(shù)據(jù)得到,最后通過積分更新四元數(shù)表示的姿態(tài)并進行四元數(shù)歸一化,得到新的姿態(tài)信息。

w為單位為rad的處理后角速度;a為加速度;m為磁力計矩陣;wbias為零偏矩陣;w′為零偏處理后角速度;ka為加速度增益;km為磁場增益;ew為磁場誤差矩陣;ev為加速度誤差矩陣;Kp為比例調節(jié)系數(shù);為四元數(shù)變化率;為更新后姿態(tài);為更新后姿態(tài);為歸一化后姿態(tài)圖2 系統(tǒng)算法流程Fig.2 System algorithm flow

2.2 靜態(tài)檢測單元

慣性傳感器處于靜止時,偏航角最終會收斂到地磁場方向。如果沒有分辨靜態(tài)和動態(tài)狀態(tài),處于靜止的傳感器在受磁場干擾的環(huán)境下,不可避免地會造成偏航角誤差。所以當傳感器處于靜止時,最好的辦法就是保持當前的姿態(tài)不變,這樣就可以抵抗外界磁場帶來的干擾。

2.2.1 陀螺儀零偏估計

陀螺儀零偏是影響角速度精度的重要因素之一,它會隨溫度、時間而改變,無法在出場標定時校正,需在使用過程中修正[8]。

考慮到陀螺儀的零偏是一個緩變的信號,可將零偏看作低頻的噪聲并通過低通濾波的方式進行估算。為了避免濾掉有用的信息,僅在慣性傳感器發(fā)生微小抖動時進行。可以認為在一定的時間范圍內,微小抖動時間越長,零偏值越大。

改進低通濾波法表示為

(1)

式(1)中:wbias為零偏估計值;fc為截止頻率;t為時間;w為角速度原始數(shù)據(jù);t為時間;函數(shù)fb(w,wmin)的功能是統(tǒng)計角速度w小于wmin的時間,其中,wmin為設定的靜態(tài)判斷閾值;tbmin和tbmax分別為最小、最大靜態(tài)時間。

實際應用中,物體不可能絕對靜止,一些細微抖動會讓陀螺儀不可避免的產生偏移,而按照上述方法,可有效去除細小抖動。如圖3所示。

圖3 三軸陀螺儀零偏補償Fig.3 Three-axis gyroscope zero compensation

2.2.2 靜態(tài)檢測

加速度數(shù)據(jù)和陀螺儀數(shù)據(jù)均可用于靜態(tài)檢測,但最好用兩種數(shù)據(jù)融合做判斷,這樣判斷會更加精準[9]。通過給角速度和加速度變化值設置閾值的方法來共同判斷傳感器當前的狀態(tài),判定的準則分為加速度判定和角速度判定。

基于陀螺儀的靜態(tài)準則有

(2)

式(2)中:thgyro為設定的角速度閾值;gx、gy和gz為三軸角速度測量值。

當陀螺儀任一軸的絕對值超過設定的閾值時,則處于動態(tài);反之如果連續(xù)檢測100次,一直滿足靜態(tài)準則,則處于角速度靜止狀態(tài)。

基于加速度計的靜態(tài)準則有

(3)

式(3)中:axt、ayt、azt分別為t時刻X軸加速度測量值;axt-1、ayt-1、azt-1為t-1時刻X軸的加速度測量值;thacc為加速度閾值。

當加速度計任一軸的此刻輸出值減去上一刻輸出值的差值的絕對值超過閾值時,處于動態(tài);反之如果連續(xù)檢測100次,一直滿足靜態(tài)準則,則處于加速度靜止狀態(tài)。

當同時滿足角速度檢測靜止和加速度檢測靜止時,系統(tǒng)處于靜態(tài),反之處于動態(tài),如圖4所示。

圖4 靜態(tài)檢測圖Fig.4 Static detection map

2.3 互補濾波

姿態(tài)解算從理論上講只用陀螺儀就可以實現(xiàn),但由于陀螺儀在積分過程中會產生累計誤差。如果沒有外力干涉,解算可信度將會越來越低,故需要加速度計對重力進行比對和補償,用來修正陀螺儀的垂直誤差。對于豎直軸上的旋轉,加速度計是無能為力的,此時就需要磁力計測量水平面的地磁方向來修正陀螺儀的水平誤差。而互補濾波法就是根據(jù)傳感器的特性不同,取長補短,融合數(shù)據(jù)得到更加準確的姿態(tài)。

已知在導航坐標系中,重力加速度G可表示為

G=[0,0,1]

(4)

加速度預測值as的計算公式為

(5)

式(5)中:vx、vy、vz分別為三軸加速度預測值;q0、q1、q2、q3為組成四元數(shù)的四位數(shù),其中,q0是標量,q1、q2、q3是矢量。

(6)

因為加速度計的測量值ab=[ax,ay,az]與預測值as存在誤差,故通過向量叉乘可得加速度誤差ev,可表示為

(7)

(8)

同理,磁力計在導航坐標系的參考方向為[bx, 0,bz],地球上不同地方的磁場方向和大小均不一致,故bx和bz為未知量。但是可以使用磁力計測量值來模擬當?shù)氐拇艌?步驟如下。

首先,設地磁場的值為

h=[hx,hy,hz]T

(9)

(10)

(11)

(12)

式(12)中:hx、hy、hz分別為通過磁力計計算的地磁場在導航坐標系下的三軸分量;bx為水平面上的地磁;bz為垂直面上的地磁。

則磁場的預測值ms可表示為

(13)

根據(jù)磁力計的測量值與預測的誤差,同樣通過向量叉乘得到磁場誤差ew,可表示為

(14)

陀螺儀的原始測量值w的單位為(°)/s,因此需要將測量值轉化為單位為rad的值,可表示為

(15)

然后將加速度誤差和磁場誤差來加入原始角速度中,得到修正后的角速度w″為

w″=w′+Kp(kaev+kmew)

(16)

式(16)中:Kp為比例調節(jié)系數(shù);ka和km分別為加速度增益和磁場增益。

(17)

四元數(shù)乘法的具體表達式為

(18)

然后通過積分更新姿態(tài)信息,并且歸一化有

(19)

最后,四元數(shù)表示的姿態(tài)轉化為歐拉角[10],歐拉角包含繞x軸的俯仰角Pitch、繞y軸的橫滾角Roll和繞z軸的偏航角Yaw,其轉換公式為

(20)

2.4 決策樹

決策樹是一種屬于監(jiān)督學習的分類方法。在本決策樹中主要分為兩大部分,分別為加速度干擾檢測部分和磁場干擾檢測部分。

根據(jù)加速度干擾特性,將加速度干擾分為3種類型:弱加速度干擾、弱加速度干擾超時、強加速度干擾,主要采用加速度幅值‖a‖和加速度干擾計時器atime來判定。

(21)

由于磁力計的敏感性,磁場數(shù)據(jù)非常容易受到外界干擾,將磁場干擾分為4個類型:弱磁干擾、弱磁干擾超時、強磁干擾和強磁干擾超時。在過往的抗磁研究中,普遍檢測磁場的參數(shù)會選用磁場強度和磁傾角[11],但在不同的地方,地磁場的差異會導致磁場強度的標準發(fā)生變化,故導致磁場強度和磁傾角發(fā)生偏差,系統(tǒng)就會錯誤的判斷為產生了磁場干擾,影響算法的精準性。本文算法將陀螺儀引入,選用磁力計計算的偏航角和陀螺儀計算的偏航角之差dpsi和磁場干擾計時器mtime作為判定原則來判斷是否發(fā)生磁場干擾,增加了判定干擾的準確性,其表達式為

(22)

將4個參數(shù)用來共同構成決策樹,如圖5所示,首先,分別判斷dpsi和‖a‖是否屬于設定的(thm1,thm2)和(tha1,tha2)閾值區(qū)間內,其中,thm1為弱磁左界限,thm2為弱磁右界限,tha1為弱加速度左界限,tha2為弱加速度右界限。再判斷其時間是否超過時間閾值thtime。最后得到加速度干擾的3種狀態(tài)和磁力干擾的4種狀態(tài),將之兩兩組合,一共可以得到12種組合。將12種狀態(tài)分配出不同的km和ka兩種誤差增益應對不同的干擾。

dpsi為磁力計和陀螺儀計算的偏航角之差;‖a‖為加速度幅值;atime為加速度干擾時間;mtime為磁干擾時間;(tha1,tha2)為弱加范圍;(thm1,thm2)為弱磁范圍;thtime為時間閾值圖5 決策樹邏輯Fig.5 Decision tree logic

3 實驗系統(tǒng)設計和結果分析

3.1 硬件系統(tǒng)和算法參數(shù)

本系統(tǒng)主要核心控制器為STM32F407,慣性傳感器為維特JY931,它包含了三軸加速度計(量程為±16g,g為重力加速度)、三軸陀螺儀[量程±2 000°/s]和三軸磁力計。整個硬件系統(tǒng)如圖6所示。

MCU為微控制單元;IIC為集成電路總線;JY931為慣性傳感器;PC為個人電腦圖6 硬件系統(tǒng)Fig.6 Hardware system

本系統(tǒng)慣性傳感器采樣率為500 Hz,低通濾波參數(shù)設置截止頻率fc=0.019 894Hz;最小靜態(tài)時間tbmin=1 s;最大靜態(tài)時間tbmax=8 s;靜態(tài)判斷閾值wmin=(5, 5, 3)。靜態(tài)檢測參數(shù)設置:角速度閾值thgyro=2°/s;加速度閾值thacc=0.025 m/s2;比例調節(jié)系數(shù)Kp=1。

3.2 無外部磁體干擾測試

本實驗主要測試本文方法的可行性和優(yōu)越性。實驗平臺如圖7(d)所示,實驗具體步驟如下:將慣性傳感器水平的固定在旋轉機械臂上,機械臂通過電腦(PC)端精準控制其旋轉角度,分別采取20、60、120 s時長的運動數(shù)據(jù),每個實驗重復5次。最后比較九軸梯度下降算法(gradient descent algorithm,GDA)、改進的顯式互補濾波算法(improved explicit complementary filtering,IECF)、無靜態(tài)檢測的決策樹互補濾波算法(decision tree complementary filtering without static,DTWS)、雙線性卡爾曼濾波算法(double linear kalman filter algorithm,DLKF)[12-13]和本文算法估算姿態(tài)歐拉角均方根誤差的平均值,如表1所示。圖7(a)～圖7(c)為慣性傳感器在20 s內水平旋轉0° ～ 30°往返,運動間隔時間為2 s。

表1 不同算法歐拉角均方根誤差的平均值Table 1 Average value of root mean square error of Euler angles for different algorithms

圖7 三軸歐拉角和實驗平臺Fig.7 Three-axis Euler angles and experimental platform

3.3 磁物體干擾測試

本實驗是慣性傳感器60 s內水平旋轉從0°～90°往返,運動間隔時間為2 s。其中,用磁鐵進行隨機的磁場干擾,從圖8(a)中可以看出,實驗中出現(xiàn)多次隨機的外部磁干擾。結果[圖8(b)～圖8(d)]顯示:本文算法、GDA和IECF的偏航角均方根誤差分別為0.96°、3.88°、15.08°,表明本文算法相較于其他兩種算法有著抗干擾優(yōu)勢。

圖8 抗磁實驗歐拉角Fig.8 Antimagnetic experiment Euler angle

4 結論

加速度干擾是普遍存在且難以回避的,因為加速度干擾伴隨著運動而存在,不能完全消除,只能抑制。得出以下結論。

(1)從無帶磁物體干擾測試實驗結果可以看出,提出的基于決策樹的互補濾波算法具有可行性,而且相較于其他算法,抑制加速度干擾能力更強,解算精度更高。

(2) 在外部磁場干擾下,相對于GDA算法,所提算法抗干擾能力提高了75.3%。

解算的精度對于后續(xù)的研究有著關鍵性的作用,此算法將用于后續(xù)手勢軌跡重構研究中,進一步提高軌跡重構的精度。