基于四元數(shù)法的航向修正算法分析

戚憲東,郭英,孫玉曦,姬現(xiàn)磊

(山東科技大學(xué) 測(cè)繪科學(xué)與 工程學(xué)院,山東 青島 266590)

0 引 言

捷聯(lián)慣導(dǎo)將陀螺儀和加速度計(jì)直接安裝在載體上,省掉了機(jī)電式導(dǎo)航平臺(tái),利用計(jì)算機(jī)軟件建立一個(gè)“數(shù)學(xué)平臺(tái)”來代替機(jī)電平臺(tái)實(shí)體。由于其結(jié)構(gòu)簡單且抗干擾能力強(qiáng),目前已成為導(dǎo)航定位、無人機(jī)、智能交通等領(lǐng)域的研究熱點(diǎn)之一[1]。

姿態(tài)解算是捷聯(lián)式慣性導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù),通過姿態(tài)矩陣可以得到載體的姿態(tài)和導(dǎo)航參數(shù)計(jì)算需要的數(shù)據(jù),是捷聯(lián)式慣導(dǎo)算法中的重要工作[1]。載體的姿態(tài)和航向體現(xiàn)了載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的方位關(guān)系。用于描述載體坐標(biāo)系與導(dǎo)航坐標(biāo)系轉(zhuǎn)換關(guān)系的方法主要有歐拉角法、方向余弦法和四元數(shù)法等[2]。其中,歐拉角法在求解姿態(tài)時(shí)存在奇點(diǎn),不能用于全姿態(tài)的解算[3];方向余弦法可用于全姿態(tài)的解算但計(jì)算量大,不能滿足實(shí)時(shí)性要求[4];四元數(shù)法算法簡單,易于操作,只需要求解四個(gè)參數(shù),計(jì)算量小,可以在實(shí)現(xiàn)過程中修正漂移,應(yīng)用比較廣泛[5]。而四元數(shù)微分方程的解算主要用到龍格-庫塔法[6],文獻(xiàn)[7]介紹了一階龍格-庫塔法,其存在解算精度和效率較低的問題;文獻(xiàn)[8]詳細(xì)敘述了四階龍格-庫塔法,在解算時(shí)收斂性較好,穩(wěn)定性較高。因此,尋求一種收斂性好和穩(wěn)定性高的高階計(jì)算方法,能夠高效、快速的解算四元數(shù)微分方程,保證四元數(shù)解算航向的精度。

在姿態(tài)解算過程中,除了優(yōu)化解算方法外,還存在陀螺儀長時(shí)間工作的漂移誤差。消除由陀螺儀的漂移誤差導(dǎo)致的航向誤差一直是阻礙室內(nèi)定位精度提升的難點(diǎn)[9],為了減少航向誤差,研究人員提出了幾種航向修正的方法:Borenstein等人[10]提出了基于腳踏式慣性測(cè)試單元(IMU)的啟發(fā)式偏移減少算法(HDR)和啟發(fā)式漂移消除算法(HDE)。樓喜中等人[11]在HDE的基礎(chǔ)上,將航向角作為直接修改的對(duì)象,消除了部分陀螺儀積分誤差。

本文利用行人在室內(nèi)直線運(yùn)動(dòng)情況下的航向角偏差作為誤差反饋回系統(tǒng),來修正陀螺儀的角速度輸出,從而抑制陀螺儀的漂移,進(jìn)而修正四元數(shù)法解算的航向,并將其與參考航向?qū)Ρ纫则?yàn)證四元數(shù)法的正確性和有效性,并分析不同采樣頻率下的二階龍格-庫塔法與四階龍格-庫塔法解算航向精度,得到提高采樣頻率能減少航向解算誤差,提高定位精度,對(duì)捷聯(lián)式慣性導(dǎo)航技術(shù)有一定的應(yīng)用參考價(jià)值。

1 系統(tǒng)模型

1.1 姿態(tài)矩陣的描述

通常假定捷聯(lián)系統(tǒng)“數(shù)學(xué)平臺(tái)”模擬地理坐標(biāo)系,即導(dǎo)航坐標(biāo)系;而確定載體的姿態(tài)矩陣即為研究載體坐標(biāo)系(b系)和導(dǎo)航坐標(biāo)系(n系)的空間轉(zhuǎn)動(dòng)關(guān)系,一般用載體坐標(biāo)系相對(duì)導(dǎo)航坐標(biāo)系的三次轉(zhuǎn)動(dòng)角確定[1]。載體的姿態(tài)由橫滾角γ,俯仰角θ,航向角φ來描述。

1.2 四元數(shù)微分方程

隨著捷聯(lián)式慣性導(dǎo)航技術(shù)的發(fā)展,為了更簡便地描述剛體的角運(yùn)動(dòng),采用了四元數(shù)這個(gè)數(shù)學(xué)工具,用它來彌補(bǔ)通常描述剛體角運(yùn)動(dòng)的3個(gè)歐拉角參數(shù)在設(shè)計(jì)控制系統(tǒng)時(shí)的不足[3]。

規(guī)定q為一個(gè)四元數(shù)向量,即:

(1)

則q滿足微分方程:

(2)

四元數(shù)姿態(tài)矩陣的微分方程只要解四個(gè)一階微分方程式即可,計(jì)算量小,容易實(shí)現(xiàn),是導(dǎo)航解算中常用的方法。

2 基于四元數(shù)的航向解算

常用的姿態(tài)描述方法有:歐拉角法、方向余弦法和四元數(shù)法。由于相對(duì)比其他兩個(gè)方法,四元數(shù)方法具有計(jì)算簡單、易于操作、且能避免出現(xiàn)歐拉角所產(chǎn)生的“奇點(diǎn)”問題的優(yōu)點(diǎn),故本文選擇四元數(shù)法來解算姿態(tài)信息。

2.1 四元數(shù)法姿態(tài)矩陣計(jì)算的步驟如下:

第1步:初始化四元數(shù)。初始姿態(tài)信息設(shè)置:航向角為90°、俯仰角和橫滾角都設(shè)置為0°.初始化四元數(shù)(q0,q1,q2,q3)為

(3)

式中:φ0、θ0、γ0分別為初始的航向角、俯仰角和橫滾角。

第2步:四元數(shù)微分方程的實(shí)時(shí)解算。下面簡述二階龍格-庫塔法和四階龍格-庫塔法解算四元數(shù)微分方程的基本原理:

方法1:利用二階龍格-庫塔法實(shí)時(shí)更新四元數(shù)。

解算公式[6]:

(4)

(5)

根據(jù)上述公式即可獲得用二階龍格-庫塔法實(shí)時(shí)更新的四元數(shù)。

方法2:利用四階龍格-庫塔法實(shí)時(shí)更新四元數(shù)。

解算公式[8]:

(6)

式中:T為步長;q(t)、q(t+T)分別為t、t+T時(shí)刻的四元數(shù)。根據(jù)上述公式即可獲得用四階龍格-庫塔法實(shí)時(shí)更新的四元數(shù)。

第3步:四元數(shù)歸一化

(7)

第4步:由四元數(shù)計(jì)算旋轉(zhuǎn)矩陣并計(jì)算航向角φ

(8)

(9)

通過以上步驟的解算,可以利用陀螺儀輸出的角速度數(shù)據(jù)得到行人航向,但由于陀螺儀的輸出會(huì)隨著時(shí)間的增長而產(chǎn)生漂移,從而影響航向解算精度。

2.2 航向修正的改進(jìn)HDE算法

基于室內(nèi)環(huán)境下,行人多為直線行走的情況,其航向角變化較小,利用航向角偏差作為誤差反饋回系統(tǒng),來修正陀螺儀的角速度輸出,從而抑制陀螺儀的漂移,提高定位精度,具體算法步驟如下。

第1步:判別行人運(yùn)動(dòng)狀態(tài)

通常利用當(dāng)前航向與歷史航向的變化幅值來判斷行人的運(yùn)動(dòng)軌跡是否為直線:

(10)

如果Δφi<10°時(shí),則表示航向變化不明顯,判定行人沿著直線行走。

確定行人沿直線行走后,采用下式對(duì)航向進(jìn)行修正:

(11)

第2步:設(shè)置主導(dǎo)航向

行人初始的主方向?yàn)?°,沿著順時(shí)針方向依次為45°、90°、135°、180°、225°、270°、315°,一共8個(gè)主方向,角度間隔δ為45°,當(dāng)前航向與主航向的偏差可用下式計(jì)算:

(12)

式中:INT(x)為取整函數(shù);φi為當(dāng)前航向角。

利用改進(jìn)的HDE航向修正算法,對(duì)行人在室內(nèi)環(huán)境下的直線行走情況進(jìn)行修正,抑制了陀螺儀的漂移,提高了定位精度。

2.3 行人運(yùn)動(dòng)軌跡推算

將經(jīng)HDE算法修正過的航向角信息與加速度計(jì)分析所得的步長信息相結(jié)合,獲得精確的定位信息?；趹T性傳感器的行人室內(nèi)定位的關(guān)鍵技術(shù)是行人航跡推算(PDR)。所謂航跡推算,即由上一位置的坐標(biāo)結(jié)合當(dāng)前一步的航向角信息與步長信息求解當(dāng)前位置的坐標(biāo),公式為[11]

(13)

式中:x,y為位置坐標(biāo); S為步長。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 數(shù)據(jù)采集

實(shí)驗(yàn)采用低成本的魅藍(lán)NOTE3手機(jī)作為行人導(dǎo)航數(shù)據(jù)采集模塊,進(jìn)行測(cè)試實(shí)驗(yàn)。本文利用安卓軟件“導(dǎo)航項(xiàng)目(AndroSensor)”為工具來獲取傳感器數(shù)據(jù),該軟件簡單實(shí)用,可以滿足實(shí)驗(yàn)的需求。為了達(dá)到較高的測(cè)量精度,在采集數(shù)據(jù)之前要對(duì)手機(jī)的內(nèi)置傳感器進(jìn)行校準(zhǔn),加速度計(jì)和陀螺儀要進(jìn)行零偏校準(zhǔn)。

實(shí)驗(yàn)數(shù)據(jù)采集頻率分別設(shè)為100 Hz、200 Hz,實(shí)驗(yàn)人員手持智能手機(jī),平放于身前,以正常步速按設(shè)計(jì)的路線行走。行走路線以主導(dǎo)方向行走為主,實(shí)驗(yàn)場(chǎng)地為山東科技大學(xué)測(cè)繪學(xué)院辦公樓的回型走廊,行走的矩形路線長為62 m、寬為42 m.

3.2 實(shí)驗(yàn)分析

不同采樣頻率下,利用二階龍格-庫塔法和四階龍格庫塔法解算四元數(shù)微分方程,對(duì)解算的行人航向進(jìn)行HDE算法修正后,推算行人航跡如圖1所示。

分析實(shí)驗(yàn)結(jié)果如表1所示。

表1 實(shí)驗(yàn)精度分析

注:TTD為路線閉合差與其總路線的比值

由圖1和表1分析可知:

1) 本算法的解算結(jié)果與參考路徑對(duì)比,驗(yàn)證了四元數(shù)法解算航向的有效性和可行性。

2) 二階龍格-庫塔法與四階龍格-庫塔法解算四元數(shù)微分方程,用四元數(shù)表示歐拉角,并用HDE算法修正航向,推算行人軌跡,其在100 Hz采樣頻率下,軌跡閉合差分別為2.847 m和2.813 m;200 Hz采樣頻率下,軌跡閉合差分別為1.164 m和1.159 m,解算結(jié)果相差不大,主要是由于兩種方法解算的是更新后的四元數(shù),其在轉(zhuǎn)換為歐拉角以及HDE算法修正航向的過程中,原始數(shù)據(jù)進(jìn)行了處理,導(dǎo)致高階方法解算結(jié)果優(yōu)勢(shì)并不是很明顯。

3) 當(dāng)提高采樣頻率至200 Hz后,行人軌跡與參考路線較為吻合,且二階龍格-庫塔法解算行人軌跡閉合差減小了1.683 m,TTD誤差大大減小;四階龍格-庫塔法解算行人軌跡閉合差減小了1.654 m,TTD誤差也大幅減小。驗(yàn)證了在傳感器一定工作范圍內(nèi),提高采樣頻率,可以減小航向解算誤差,提高定位精度。

4 結(jié)束語

本文以低成本智能手機(jī)為實(shí)驗(yàn)對(duì)象,選用四元數(shù)法解算載體航向信息、二階龍格-庫塔法和四階龍格-庫塔法分別進(jìn)行四元數(shù)更新,然后把經(jīng)過四元數(shù)解算的航向信息通過改進(jìn)的HDE算法修正后,與步長信息相結(jié)合,實(shí)現(xiàn)位置解算。實(shí)驗(yàn)分析了不同采樣頻率下的二階龍格-庫塔法與四階龍格-庫塔法解算的航向精度,結(jié)果表明，提高采樣頻率能減少航向解算誤差,提高定位精度。本文工作對(duì)實(shí)際導(dǎo)航定位工作具有一定的指導(dǎo)意義。

[1] 張榮輝,賈宏光,陳濤,等. 基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J]. 光學(xué)精密工程, 2008, 16(10):1963-1970.

[2] 王立波. 捷聯(lián)慣導(dǎo)系統(tǒng)設(shè)計(jì)與數(shù)據(jù)處理技術(shù)研究[D]. 西安:西安電子科技大學(xué), 2011.

[3] 黃國英. 微分方程初值問題的加權(quán)改進(jìn)歐拉數(shù)值解法[J]. 成功:教育版, 2011(8):287-287.

[4] 李連仲,王小虎,蔡述江. 捷聯(lián)慣性導(dǎo)航、制導(dǎo)系統(tǒng)中方向余弦矩陣的遞推算法[J]. 宇航學(xué)報(bào), 2006, 27(3):349-353.

[5] 王彤,馬建倉,秦濤,等. 基于旋轉(zhuǎn)四元數(shù)的姿態(tài)解算算法[J]. 彈箭與制導(dǎo)學(xué)報(bào), 2014,34(3):15-16.

[6] 楊陽. 龍格庫塔法求模糊微分方程的數(shù)值解[D]. 哈爾濱:哈爾濱工業(yè)大學(xué), 2015.

[7] 張麗娟,張翔,關(guān)天冶. 一階常微分方程初值問題的數(shù)值算法[J]. 通化師范學(xué)院學(xué)報(bào), 2017, 38(8):22-24.

[8] 馮建強(qiáng),孫詩一. 四階龍格——庫塔法的原理及其應(yīng)用[J]. 數(shù)學(xué)學(xué)習(xí)與研究, 2017(17):3-5.

[9] 曾慶軍,劉慧婷,張明. 基于HDR的陀螺儀隨機(jī)漂移分析與處理[J]. 江蘇大學(xué)學(xué)報(bào)(自然科學(xué)版), 2016, 37(3): 332-336.

[10] BORENSTEIN J, OJEDA L. Heuristic drift elimination for personnel tracking systems[J]. Journal of Navigation, 2010, 63(4): 591-606.

[11] 樓喜中,周樂宇,葉敏展, 等. 基于角度的HDE算法在室內(nèi)行人航跡定位中的研究[J]. 傳感技術(shù)學(xué)報(bào), 2015,28(4): 598-602.