亚洲免费av电影一区二区三区,日韩爱爱视频,51精品视频一区二区三区,91视频爱爱,日韩欧美在线播放视频,中文字幕少妇AV,亚洲电影中文字幕,久久久久亚洲av成人网址,久久综合视频网站,国产在线不卡免费播放

        ?

        AHRS姿態(tài)估計的迭代擴展卡爾曼濾波方法

        2022-04-28 04:37:06史宜巧
        機械設(shè)計與制造 2022年4期
        關(guān)鍵詞:測量方法

        史宜巧,趙 輝

        (1.江蘇電子信息職業(yè)學(xué)院智能制造學(xué)院,江蘇 淮安 223003;2.河北工程大學(xué)信息與電氣工程學(xué)院,河北 邯鄲 056038)

        1 引言

        基于微機電系統(tǒng)(micro?electro mechanical system,MEMS)的慣性測量單元(inertial measurement unit,IMU)和磁羅盤組成的航姿參考系統(tǒng)[1(]Attitude and Heading Reference System,AHRS)通過實時測量到的角速率,加速度,以及磁航向,將短期精度較高(角速率)的姿態(tài)計算和長期精度較高(加速度,磁場)的姿態(tài)計算結(jié)果進行融合,從而實現(xiàn)對載體姿態(tài)的估計,在消費級導(dǎo)航,服務(wù)型運動感知等低成本要求的領(lǐng)域已經(jīng)有了廣泛的應(yīng)用[2]。目前常用的AHRS實時姿態(tài)估計方法可大致分為互補濾波[3],QUEST型方法[4?5]以及濾波器估計方法[6?8]。其中濾波器方法可以在姿態(tài)估計的同時對系統(tǒng)誤差參數(shù)進行實時估計,且更易于融合各類元件信息以取得更好的整體性能。因此得到了廣泛的研究。這里研究即為濾波方法。

        在姿態(tài)估計問題中應(yīng)用濾波估計方法需要解決兩個問題,一是觀測模型通常是非線性的;二是姿態(tài)四元數(shù)在估計中不能夠自然歸一化。對此,通常的做法是將過程與觀測模型線性化,以四元數(shù)等姿態(tài)表征參數(shù)表示的估計誤差和傳感器偏差估計誤差作為狀態(tài)量,將加速度計和磁傳感器等外部信息作為觀測量。將問題轉(zhuǎn)化為線性濾波對狀態(tài)量進行求解。例如,乘性擴展卡爾曼濾波[9?10(]Multiplicative Extended Kalman Filter,MEKF)將誤差四元數(shù)作為狀態(tài)量,利用一階近似避免了觀測信息引入的非線性,且隱式地包含了歸一化條件。由于截斷誤差的影響,基于誤差估計的方法估計精度不高,尤其在姿態(tài)變化劇烈條件下可能產(chǎn)生較大的誤差。

        針對上述問題,這里將姿態(tài)變量與傳感器偏差參數(shù)直接作為待求解狀態(tài),提出一種基于非線性濾波的四元數(shù)姿態(tài)估計算法。根據(jù)四元數(shù)姿態(tài)表示原理和傳感器測量輸出模型建立了AHRS 系統(tǒng)四元數(shù)姿態(tài)估計的直接形式非線性狀態(tài)空間模型,采用構(gòu)造偽觀測量方法解決四元數(shù)歸一化問題,在此基礎(chǔ)上推導(dǎo)出了觀測函數(shù)的雅克比矩陣,采用迭代擴展卡爾曼濾波(iter‐ated EKF,IEKF)方法進行濾波求解,實現(xiàn)了對姿態(tài)的實時估計。此外,利用閾值法處理載體機動時產(chǎn)生的運動加速度。通過實際傳感器測量與ABB 機器人同步姿態(tài)參考數(shù)據(jù)集對這里算法進行了驗證,并與基于非線性濾波、互補濾波等的姿態(tài)估計方法以及商用姿態(tài)測量單元的結(jié)果進行了對比。結(jié)果表明,相比現(xiàn)有常用方法,這里算法在姿態(tài)估計準(zhǔn)確度與精度方面具有較好的性能。

        2 四元數(shù)姿態(tài)估計的非線性問題模型

        設(shè)載體自身坐標(biāo)系(B系)為右-前-上坐標(biāo)系,參考系(N系)為東-北-天局部導(dǎo)航坐標(biāo)系(ENU)。姿態(tài)四元數(shù)定義為:

        式中:ψ=|ψ|,且ψ=ψμ;

        μ—N系中的轉(zhuǎn)軸矢量;

        ψ—從系通過轉(zhuǎn)軸轉(zhuǎn)到系所經(jīng)過的角度。

        因此可以得到從B系變換到N系的姿態(tài)變換矩陣為:

        在ENU坐標(biāo)系下可得上式姿態(tài)變換矩陣對應(yīng)的歐拉角為:

        式中:Rij—矩陣(q)相應(yīng)位置的元素。

        根據(jù)式(2)給出的變換關(guān)系,下面推導(dǎo)四元數(shù)姿態(tài)估計的非線性狀態(tài)空間模型。

        2.1 狀態(tài)方程

        考慮到待求解的量和影響姿態(tài)估計精度的因素,將四元數(shù),陀螺儀偏差βω,加速度計偏差βa以及磁傳感器偏差βm作為狀態(tài)量,即:

        根據(jù)各個狀態(tài)量隨著時間的動態(tài)變化過程,下面來導(dǎo)出狀態(tài)方程,四元數(shù)隨時間變化的動態(tài)方程為:

        式中:=ω?βω;ω—陀螺儀讀數(shù);

        βω—陀螺儀偏差;

        υω~N(0,Σω)—陀螺儀測量噪聲,矩陣:

        各個傳感器偏差視為一階Gauss?Markov過程,即:

        式中:系數(shù)矩陣Αi,i=ω,a,m—對角矩陣,對角元素分別為三軸分量對應(yīng)的Gauss?Markov過程系數(shù);ηi~N(0,)為相應(yīng)的過程噪聲。

        對式(5)和式(7)進行離散化,可得離散狀態(tài)方程為:

        上式中狀態(tài)轉(zhuǎn)移矩陣A與過程噪聲vk分別為:

        其中,

        假設(shè)過程噪聲均值為0,則過程噪聲協(xié)方差矩陣為:

        對角上的協(xié)方差矩陣分別為:

        2.2 觀測模型

        以靜止條件下加速度計和磁傳感器的輸出ak,mk為觀測值。各個傳感器的輸出模型可寫為參考系中的自然矢量到體坐標(biāo)系的變換加上測量誤差。

        g—當(dāng)?shù)刂亓铀俣龋?/p>

        根據(jù)式(13)可進一步構(gòu)建觀測模型為:

        3 基于迭代擴展卡爾曼濾波的姿態(tài)估計算法

        在上一節(jié)建立狀態(tài)-空間模型的基礎(chǔ)上,現(xiàn)在可以直接利用IEKF算法對姿態(tài)四元數(shù)及各個傳感器的偏差進行估計,IEKF算法在目標(biāo)跟蹤領(lǐng)域已經(jīng)比較成熟,其原理和推導(dǎo)過程可見文獻[11?12]。在此僅給出算法關(guān)鍵步驟。

        (1)時間更新

        (2)觀測更新

        當(dāng)?shù)猎O(shè)定次數(shù)M或狀態(tài)值穩(wěn)定時,令

        在觀測更新過程中,由于載體本身機動,會產(chǎn)生較大幅值的運動加速度,這一方面會降低測得加速度中包含的姿態(tài)信息量,使得姿態(tài)估計誤差增大,另一方面會與式(13)給出的觀測模型是不相符的,可能導(dǎo)致濾波器發(fā)散。這里采用對觀測值進行閾值判別的方法來改善,即采用如下判別準(zhǔn)則來確定是否進行下一次迭代:

        式中:g—重力加速度;

        ai,i=x,y,z—加速度計的測量值。

        若滿足上式,則認為當(dāng)前的加速度觀測值可以用于觀測更新,否則不進行觀測更新。

        對于觀測方程中的磁傳感器測量模型,當(dāng)?shù)氐卮艆⒖际噶恳话阈枰ㄟ^全球地磁模型計算獲得,為避免對于外部數(shù)據(jù)的依賴,這里中采用構(gòu)造磁參考量的方法,即在式(13)中用近似值bN替代bN。

        當(dāng)前估計得到的磁傳感器零偏誤差。

        根據(jù)上述過程可以得到基于IEKF 的姿態(tài)估計算法步驟如下:

        (2)根據(jù)上一時刻狀態(tài)估計值和基于式(11)和式(12)計算出的過程噪聲協(xié)方差矩陣,利用(15)計算當(dāng)前狀態(tài)預(yù)測值和相應(yīng)的協(xié)方差矩陣Pk|k?1。

        (3)若當(dāng)前加速度計的觀測值滿足式(17),則利用前述的IEKF觀測更新步驟計算,Pk|(k計算過程中利用式(18)構(gòu)造磁參考量);若不滿足,否則回到步驟(2)。

        (4)對當(dāng)前狀態(tài)估計值中的四元數(shù)部分進行歸一化?;氐讲襟E(2)或算法結(jié)束。

        4 結(jié)果與分析

        本小節(jié)基于一個開放的姿態(tài)測量數(shù)據(jù)集[13?15]來驗證這里方法的有效性,同時給出常用的互補濾波姿態(tài)估計方法對該數(shù)據(jù)集的處理結(jié)果和誤差情況作為性能對比。該數(shù)據(jù)集中包括幾種常用低成本慣性測量單元原始數(shù)據(jù)、商用級姿態(tài)傳感器姿態(tài)輸出結(jié)果以及ABB公司工業(yè)機器人給出的姿態(tài)四元數(shù)。各個傳感器輸出數(shù)據(jù)均是以ABB機器人為平臺,在與之固聯(lián)的條件下同步測量得到。因此,工業(yè)機器人輸出的姿態(tài)四元數(shù)可以作為真實姿態(tài)參考。此外,采用同樣與ABB機器人固聯(lián)同步測量的兩型商用高精度姿態(tài)單元Xsens MTi?30和PNI SENTRAL M&M給出的姿態(tài)估計結(jié)果作為對比。

        這里方法和其他幾種姿態(tài)估計算法的輸入均采用數(shù)據(jù)集中包含的Inversense 公司MPU9150 型IMU 測量數(shù)據(jù),除集成有MEMS加速度計和陀螺儀外,該型IMU還包括了一個磁羅盤(三軸磁強計)。測量數(shù)據(jù)包括IMU與機器人固聯(lián)轉(zhuǎn)動時MPU9150傳感器測得的三軸加速度、三軸角速度與三軸磁場數(shù)據(jù)。IMU與ABB機器人固聯(lián)轉(zhuǎn)動的軌跡與姿態(tài),如圖1所示。機器人轉(zhuǎn)動速度為0.3m/s。

        圖1 IMU與ABB機器人固聯(lián)轉(zhuǎn)動軌跡與姿態(tài)Fig.1 Trajectory and Orientation of ABB Robot Borne IMUs

        MPU9150測得的三軸加速度,三軸角速度,三軸磁場等原始數(shù)據(jù),如圖2所示。圖中顯然可見轉(zhuǎn)動過程中的數(shù)次機動在傳感器測量結(jié)果導(dǎo)致了不同程度的瞬態(tài)變化。基于該組數(shù)據(jù),采用這里方法與Madgwick互補濾波算法進行對姿態(tài)四元數(shù)進行估計求解。

        圖2 MPU9150原始數(shù)據(jù)(轉(zhuǎn)動速度0.3m/s)Fig.2 Raw Data from MPU9150(Spinning Sspeed 0.3m/s)

        為了直觀地比較姿態(tài)估計準(zhǔn)確性,利用式將各個算法估計及機器人給出的參考四元數(shù)轉(zhuǎn)換為歐拉角。這里算法與Madgwick互補濾波算法得到的歐拉角,MTi?30 給出的歐拉角以及PNI SENTRAL M&M給出的歐拉角與機器人運動過程中的參考姿態(tài)的對比情況,如圖3~圖6所示。(圖中:roll—橫滾角;pitch—俯仰角;yaw—偏航角,下文同)

        圖3 這里算法姿態(tài)估計vs.參考姿態(tài)Fig.3 Proposed vs.Referenced

        圖6 PNI SENTRAL M&M傳感器姿態(tài)估計vs.參考姿態(tài)Fig.6 Orientation Estimates:PNI SENTRAL M&M vs.Referenced

        從圖中可見,相比各個算法及姿態(tài)測量單元給出的結(jié)果,根據(jù)這里算法計算出的歐拉角與機器人得到的歐拉角整體上最為接近,且在機動情況下的姿態(tài)跟蹤響應(yīng)要優(yōu)于其他方法給出的結(jié)果,同時可見兩型姿態(tài)傳感器在機動時均出現(xiàn)了較大的姿態(tài)誤差。

        圖4 Madgwick算法姿態(tài)估計vs.參考姿態(tài)Fig.4 Orientation Estimates:Madgwick vs.Referenced

        圖5 XSENS MTi?30傳感器姿態(tài)估計vs.參考姿態(tài)Fig.5 Orientation Estimates:MTi?30 vs.Referenced

        根據(jù)上述兩種算法估計結(jié)果和兩型姿態(tài)傳感器輸入結(jié)果,通過下式進行計算時間平均的均方根誤差(RMSE)。

        式中:T—測量時間;

        γkk時刻的(任一)歐拉角的參考值和估計值。

        進一步地,依據(jù)式(19)給出了各個方法所得歐拉角的(時間)均方根誤差,如圖7所示。圖中顯然可見這里算法的誤差最小,這表明這里算法的姿態(tài)估計結(jié)果與ABB機器人給出參考姿態(tài)在時間均方意義上最為接近;圍繞均方根誤差值的標(biāo)準(zhǔn)差,如圖8所示。可見這里方法估計誤差在整體上的波動最小。

        圖7 各個方法所得歐拉角的均方根誤差Fig.7 Root Mean Square Error of Euler Angle by Each Method

        圖8 各個方法所得歐拉角誤差圍繞均方根誤差的標(biāo)準(zhǔn)差Fig.8 Std.of Euler Angle Centered by RMSE for Each Method

        5 總結(jié)

        這里針對低成本AHRS系統(tǒng)姿態(tài)估計準(zhǔn)確性不足問題提出一種基于非線性濾波求解的姿態(tài)估計方法。通過構(gòu)建基于直接形式姿態(tài)估計的非線性狀態(tài)空間模型采用迭代擴展卡爾曼濾波方法實現(xiàn)了對姿態(tài)四元數(shù)與傳感器偏差的實時估計。通過實測數(shù)據(jù)對這里算法進行了驗證。結(jié)果表明,這里算法在姿態(tài)估計準(zhǔn)確性與估計精度上優(yōu)于現(xiàn)有常用姿態(tài)估計方法。下一步需要針對如何從算法優(yōu)化和模型改進上進一步提高基于直接形式非線性姿態(tài)估計方法的計算效率等方面展開研究。

        猜你喜歡
        測量方法
        把握四個“三” 測量變簡單
        學(xué)習(xí)方法
        滑動摩擦力的測量和計算
        滑動摩擦力的測量與計算
        測量的樂趣
        可能是方法不對
        用對方法才能瘦
        Coco薇(2016年2期)2016-03-22 02:42:52
        測量
        四大方法 教你不再“坐以待病”!
        Coco薇(2015年1期)2015-08-13 02:47:34
        賺錢方法
        色琪琪一区二区三区亚洲区| 久久综合国产乱子伦精品免费| 国产欧美精品区一区二区三区| 巨乳av夹蜜桃站台蜜桃机成人| 精品国产一区二区av麻豆不卡| 久久国产精品一区二区三区| 亚洲国产成人精品无码一区二区 | 五月婷婷六月激情| 日本久久一区二区三区高清| 国产成人精品日本亚洲i8| 国产综合在线观看| 北条麻妃毛片在线视频| 日产乱码一区二区国产内射| 久草视频这里只有精品| 国产午夜伦鲁鲁| 国产精品jizz观看| 美腿丝袜美腿国产在线| 国产亚洲精品色婷婷97久久久| 极品少妇一区二区三区四区| 欧美黄色免费看| 亚洲国产成人精品一区刚刚| 国产精品女直播一区二区| 中文字字幕在线精品乱码| 亚洲成a∨人片在线观看无码| 亚洲福利视频一区二区三区| 成人无码一区二区三区| 久久久久国产一区二区三区| 日韩无码尤物视频| 在线观看国产视频午夜| 国产在线 | 中文| 啪啪无码人妻丰满熟妇| 亚洲av成人无网码天堂| 小说区激情另类春色| 人妻av一区二区三区精品| 日韩极品视频在线观看免费| 精品女同一区二区三区| 欧美一区二区三区激情| 精品福利一区| 在线av野外国语对白| 精品久久中文字幕一区| 中文字幕日韩三级片|