姚文凱 邢礫文 武警工程大學(xué)

1 引言

最近幾年來由于微慣性測(cè)量單元技術(shù)以及計(jì)算機(jī)的技術(shù)的不斷進(jìn)步與發(fā)展，陀螺儀、加速度計(jì)和磁傳感器被廣泛應(yīng)用于定位與導(dǎo)航之中。微型慣性測(cè)量單元全稱Miniature Inertial Measurement Unit，它是一種應(yīng)用十分廣泛的的MEMS，它的陀螺儀、加速度計(jì)和磁傳感器能夠應(yīng)用在確認(rèn)運(yùn)動(dòng)載體的拘役位置、運(yùn)動(dòng)速度以及載體的姿態(tài)信息。微慣性器件的主要特點(diǎn)有低成本、體積小、功耗低、全固態(tài)、可靠性高、精度低等。微型慣性測(cè)量單元的陀螺儀、加速度計(jì)和磁傳感器都有著各自的局限，如果它們進(jìn)行單獨(dú)的工作，會(huì)因?yàn)樗鼈冏陨淼母髯匀毕菀鸨容^大的偏差，從而難以在室內(nèi)精確的對(duì)人的姿態(tài)進(jìn)行測(cè)量。為了提高對(duì)采集到的姿態(tài)信息的精度，本文將會(huì)將RBF與卡爾曼濾波融合起來構(gòu)建一個(gè)新的濾波模型對(duì)人的姿態(tài)信息進(jìn)行解算，這種算法能夠有效的消除卡爾曼濾波模型本身引起的誤差，從而使的構(gòu)建的模型以及濾波過程中使用的參數(shù)對(duì)最優(yōu)估計(jì)值的影響減少以及提高模型對(duì)動(dòng)態(tài)環(huán)境的外來干擾的抗干擾能力。

2 姿態(tài)解算的常規(guī)方法

目前比較常見的傳統(tǒng)姿態(tài)更新算法有歐拉角法、方向余弦法、四元數(shù)法幾種。

2.1 歐拉角法

在利用歐拉角法實(shí)現(xiàn)姿態(tài)角計(jì)算的時(shí)候，首先需要利用旋轉(zhuǎn)關(guān)系來建立姿態(tài)角的微分方程，然后通過離散方法對(duì)所建立的微分方程進(jìn)行離散化，最后根據(jù)遞推公式來實(shí)現(xiàn)對(duì)姿態(tài)角的實(shí)時(shí)計(jì)算。但是歐拉角法在俯仰角的大小接近90度的時(shí)候會(huì)出現(xiàn)計(jì)算“奇點(diǎn)”，這個(gè)時(shí)候微分公式就會(huì)出現(xiàn)退化現(xiàn)象。因此，用歐拉角法計(jì)算人體的姿態(tài)角存在著比較大的局限性，只適用于水平姿態(tài)角變化不大的情況。

2.2 方向余弦法

用方向余弦法對(duì)人體的姿態(tài)進(jìn)行計(jì)算的時(shí)候可以利用微分方程來直接對(duì)姿態(tài)矩陣中的九個(gè)元素進(jìn)行計(jì)算，并且計(jì)算的時(shí)候不會(huì)出現(xiàn)利用歐拉角法時(shí)候存在的退化現(xiàn)象，能夠?qū)崿F(xiàn)對(duì)全姿態(tài)運(yùn)載體的計(jì)算。但是這種方法要求同時(shí)對(duì)線性微分方程組中的多個(gè)線性方程進(jìn)行計(jì)算，存在著比較大的計(jì)算量，難以實(shí)現(xiàn)實(shí)時(shí)計(jì)算。

2.3 四元數(shù)法

利用四元數(shù)可以實(shí)現(xiàn)人的坐標(biāo)系與參坐標(biāo)系之間的關(guān)系。四元數(shù)可以認(rèn)為是向量與市屬之間的組合，為了表示方便，這里將采用q來對(duì)四元數(shù)進(jìn)行表示，具體表示方式如公式2-1所示。

四元數(shù)法算法簡單，但在計(jì)算精度方面存在著一定的缺陷，為了提高對(duì)人體姿態(tài)信息的計(jì)算精度，本文將在四元數(shù)法的基礎(chǔ)上，引入RBF-卡爾曼融合濾波模型來解算姿態(tài)信息。

3 RBF卡爾曼融合濾波模型算法

3.1 RBF神經(jīng)網(wǎng)絡(luò)模型

RBF神經(jīng)網(wǎng)絡(luò)中有輸入層、隱含層和輸出層三個(gè)層次，每個(gè)層次都有各自的功能。隱含層的神經(jīng)元激活函數(shù)由徑向基函數(shù)構(gòu)成。隱含層組成的數(shù)組運(yùn)算單元中都有一個(gè)中心向量c，c和輸入?yún)?shù)向量的維數(shù)是相同的，在計(jì)算非線性激活函數(shù)的時(shí)候需要利用到二者之間的歐式距離，具體定義為。因此，隱含層的輸出為非線性激活函數(shù)如公式（3-1）所示。

其中，bj是一個(gè)為正的標(biāo)量，它表示的是公式（3-1）中高斯函數(shù)的寬度的大小，而m則表示的是整個(gè)RBF神經(jīng)網(wǎng)絡(luò)中隱藏節(jié)點(diǎn)的個(gè)數(shù)。

為了計(jì)算方便，這里將該基函數(shù)的寬度定義為1，于是就可以得到公式（3-2）所示的神經(jīng)網(wǎng)絡(luò)輸出逼近函數(shù)。

針對(duì)上述RBF神經(jīng)網(wǎng)絡(luò)，通過試驗(yàn)所測(cè)量出來的不同已知姿態(tài)條件下微慣性測(cè)量組合輸出電壓作為訓(xùn)練樣本進(jìn)行訓(xùn)練，來實(shí)現(xiàn)神經(jīng)網(wǎng)絡(luò)中不同隱藏節(jié)的數(shù)值修正。

3.2 RBF-卡爾曼融合濾波模型

在本次研究中，首先先用陀螺儀和加速度計(jì)來實(shí)現(xiàn)人在運(yùn)動(dòng)過程中所安裝的MIMU的角速度以及加速度，并且利用磁力計(jì)對(duì)測(cè)量的角速度和加速度所存在的加速度以及角速度進(jìn)行校準(zhǔn)。然后將獲取到的三種姿態(tài)信息過擴(kuò)展卡爾曼濾波器進(jìn)行信息融合得到經(jīng)過RBF神經(jīng)網(wǎng)絡(luò)進(jìn)行校準(zhǔn)前的姿態(tài)信息。然后將三個(gè)傳感器的信息和校準(zhǔn)前的姿態(tài)信息反向輸入到建立好的RBF神經(jīng)網(wǎng)絡(luò)中系統(tǒng)中實(shí)現(xiàn)神經(jīng)網(wǎng)絡(luò)中的訓(xùn)練與預(yù)測(cè)。圖3-1為RBF卡爾曼融合濾波的結(jié)構(gòu)框圖。

圖3-1 RBF神經(jīng)網(wǎng)絡(luò)模型

4 仿真分析

在本次實(shí)驗(yàn)中，為了實(shí)現(xiàn)仿真，本次實(shí)驗(yàn)利用一個(gè)九軸的IMU模塊以及一個(gè)GPS接收模塊在室內(nèi)進(jìn)行實(shí)驗(yàn)，其中其中IMU模塊的陀螺儀精度為0.05°/s，加速度計(jì)精度為0.01g，GPS模塊的定位精度為2.5m，測(cè)速精度為0.1m/s。將二者固定于同一個(gè)平臺(tái)上并進(jìn)行仿真運(yùn)動(dòng)550s，并實(shí)時(shí)的記錄在實(shí)驗(yàn)中獲得的數(shù)據(jù)，采用前400s的數(shù)據(jù)作為訓(xùn)練樣本按照設(shè)計(jì)方案對(duì)網(wǎng)絡(luò)進(jìn)行訓(xùn)練，將后150s的數(shù)據(jù)輸入網(wǎng)絡(luò)中預(yù)測(cè)該時(shí)間內(nèi)的絡(luò)參數(shù)誤差，將誤差反饋給RBF神經(jīng)網(wǎng)絡(luò)模型得到修正后的定位導(dǎo)航參數(shù)，并與真實(shí)軌跡進(jìn)行對(duì)比。為了驗(yàn)證該方法的優(yōu)越性，進(jìn)行了單純的卡爾曼濾波的誤差融合進(jìn)行了對(duì)比分析。

圖4-1 原始數(shù)據(jù)的姿態(tài)角

圖4-2 經(jīng)過卡爾曼處理的姿態(tài)角

圖4-3經(jīng)過RBF-卡爾曼融合處理的姿態(tài)角

圖4-1是沒有經(jīng)過任何處理的姿態(tài)角，圖4-2是經(jīng)過單純的四元數(shù)-卡爾曼濾波進(jìn)行濾波后的姿態(tài)角，而圖3-3是經(jīng)過RBF-四元數(shù)卡爾曼濾波后的姿態(tài)角。從圖4-1、4-2、4-3的對(duì)比可以發(fā)現(xiàn)，經(jīng)過卡爾曼濾波進(jìn)行濾波后的姿態(tài)角比沒有經(jīng)過處理的姿態(tài)角誤差要小，而融合了RBF神經(jīng)網(wǎng)絡(luò)后的卡爾曼濾波的效果又要比過單純的卡爾曼濾波效果要好。因此，該方案是可行的。

5 結(jié)論

通過本次實(shí)驗(yàn)發(fā)現(xiàn)，如果不采取算法對(duì)收集到的數(shù)據(jù)進(jìn)行處理，那么姿態(tài)角容易受到漂移和噪聲的影響，會(huì)產(chǎn)生較大的誤差。但是經(jīng)過四元數(shù)法和卡爾曼融合處理后的姿態(tài)角受到的影響明顯減弱，說明四元數(shù)法和卡爾曼融合在姿態(tài)角解算過程中起到很重要的作用，而在四元數(shù)法和卡爾曼融合處理的基礎(chǔ)上加入RBF神經(jīng)網(wǎng)絡(luò)控制進(jìn)行輔助，能夠進(jìn)一步的使得姿態(tài)角受到的影響明顯減弱。