權(quán)值動(dòng)態(tài)分配的加權(quán)平均法在姿態(tài)解算中的應(yīng)用

江 杰，高 超，虞麗娜

（內(nèi)蒙古科技大學(xué)信息工程學(xué)院，包頭 014010）

捷聯(lián)慣導(dǎo)系統(tǒng)是一種不依賴外部信息，也不向外輻射能量的自主導(dǎo)航系統(tǒng)，通過(guò)測(cè)量其載體或運(yùn)動(dòng)系統(tǒng)在慣性參考系中的加速度，并對(duì)其時(shí)間進(jìn)行積分，變換至導(dǎo)航坐標(biāo)系中，進(jìn)而得到導(dǎo)航坐標(biāo)系中的速度、偏角、位置信息等。因其對(duì)外界依賴性小、隱蔽性好的特點(diǎn)已廣泛應(yīng)用于交通運(yùn)輸、航空、航天、航海等領(lǐng)域。姿態(tài)解算算法是捷聯(lián)慣導(dǎo)系統(tǒng)的核心技術(shù)之一。目前，姿態(tài)解算方法主要有歐拉角法、方向余弦法和四元數(shù)法，而其中每個(gè)算法都各有其優(yōu)缺點(diǎn)，都存在一定的誤差，解算精度也有所不同[1-5]。

隨著多傳感器數(shù)據(jù)融合技術(shù)的不斷發(fā)展和進(jìn)步，該技術(shù)在生產(chǎn)、生活、工業(yè)、軍事、航空航天等諸多領(lǐng)域都有了非常廣泛地應(yīng)用。其中，在實(shí)際應(yīng)用中最常見(jiàn)的主要包括以下幾種數(shù)據(jù)融合的方法[6-11]：

（1）依據(jù)被融合對(duì)象的統(tǒng)計(jì)學(xué)相關(guān)特性和相關(guān)的概率模型進(jìn)行數(shù)據(jù)估計(jì)的數(shù)據(jù)融合方法，包括貝葉斯估計(jì)、統(tǒng)計(jì)決策理論等；

（2）利用常用規(guī)則來(lái)進(jìn)行推理的數(shù)據(jù)融合方法，包括模糊規(guī)則推理、產(chǎn)生式規(guī)則推理等理論方法；

（3）對(duì)被融合項(xiàng)直接進(jìn)行融合的方法，包括加權(quán)平均法等。

針對(duì)單一姿態(tài)解算算法解算之后存在一定誤差的問(wèn)題，采用了在捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)解算過(guò)程中運(yùn)用加權(quán)平均法的解決方案。因?yàn)樵趹?yīng)用加權(quán)平均法的過(guò)程中，每個(gè)被融合項(xiàng)的最優(yōu)權(quán)值系數(shù)的大小取決于每個(gè)姿態(tài)解算算法對(duì)姿態(tài)進(jìn)行解算而得出的角度解算誤差方差[12]，角度解算誤差的方差取決于各個(gè)算法的角度解算誤差，解算誤差又取決于角度的真實(shí)值，而在實(shí)際應(yīng)用中角度的真實(shí)值是不能準(zhǔn)確獲得的，因此角度解算誤差方差這一數(shù)值同樣是不能準(zhǔn)確獲得的。對(duì)于這個(gè)問(wèn)題，本文提出了一個(gè)具有實(shí)際意義的解決方案，即由一種動(dòng)態(tài)分配權(quán)值的方法[12-13]來(lái)對(duì)被融合對(duì)象的權(quán)值進(jìn)行估算，然后將歐拉角法、方向余弦法和四元數(shù)法解得的角度進(jìn)行加權(quán)平均融合，以提高單一算法的解算精度，并將此方法應(yīng)用到姿態(tài)解算模塊中驗(yàn)證其解算精度。

1 姿態(tài)解算算法

一般而言，在進(jìn)行姿態(tài)解算時(shí)都要經(jīng)過(guò)以下步驟：

（1）獲得被解算載體的角速度或角增量（這里指的是對(duì)于選定的基準(zhǔn)坐標(biāo)系而言）。該值一般可以通過(guò)慣性器件直接獲得，也可以通過(guò)動(dòng)力學(xué)積分而來(lái)；

（2）選定某種姿態(tài)解算的方法獲得姿態(tài)的四元數(shù)或者歐拉角等的相關(guān)信息，主要通過(guò)對(duì)步驟（1）中獲得的角速度或角增量求積分而得到；

（3）通過(guò)相關(guān)計(jì)算求出姿態(tài)矩陣，得出姿態(tài)角。

其基本流程圖如圖1所示。

圖1 姿態(tài)解算流程圖Fig.1 Flow chart of attitude solution

1.1 歐拉角法

用歐拉角法求解姿態(tài)矩陣和姿態(tài)角，即是將運(yùn)動(dòng)學(xué)方程直接積分，需要求解3個(gè)參數(shù)，當(dāng)俯仰角為 90°時(shí)，方程具有奇異性，滾動(dòng)角和偏航角無(wú)法定值，而且積分的誤差會(huì)不斷累積，這就是應(yīng)用歐拉運(yùn)動(dòng)學(xué)方程求解姿態(tài)矩陣方法的主要缺陷[1,3,5]。

1.2 方向余弦法

方向余弦法雖然無(wú)奇點(diǎn)，可用于全姿態(tài)解算，但是需要求解九個(gè)參數(shù)，計(jì)算量大，耗時(shí)較長(zhǎng)，對(duì)內(nèi)存要求較高[1,3,5]。

1.3 四元數(shù)法

四元數(shù)法需要求解4個(gè)參數(shù)，其計(jì)算無(wú)奇點(diǎn),計(jì)算量小，工程中比較常用。但對(duì)于實(shí)時(shí)算法而言，應(yīng)用四元數(shù)法卻存在計(jì)算誤差會(huì)隨著計(jì)算次數(shù)的增多不斷增大的問(wèn)題，最終會(huì)使結(jié)果偏離真實(shí)值過(guò)大，甚至根本不能求解出結(jié)果[1,3,5]。

2 算法的加權(quán)平均融合

本文所用的最優(yōu)權(quán)值分配原則與每種算法解算的姿態(tài)角度的均方差σi(i=1,2,3)有關(guān)。因此，要獲得最優(yōu)權(quán)值分配，首先必須知道解算的姿態(tài)角度的測(cè)量方差。而實(shí)際情況是，一般并不能獲取到具體的σ值，對(duì)于此問(wèn)題，本文首先實(shí)時(shí)地估算出每種算法解算出的姿態(tài)解算方差的值，依據(jù)最優(yōu)權(quán)值的分配原則來(lái)分配每個(gè)解算結(jié)果的權(quán)值，最終將各個(gè)解算結(jié)果進(jìn)行加權(quán)平均，以實(shí)現(xiàn)各解算結(jié)果的融合?；谡鎸?shí)角度值并非已知的前提，假設(shè)第i種算法在tk(k=1，2，…，N)時(shí)刻解算得到的姿態(tài)角度為Yi(i=1，2，3)，那么就能夠參考tk時(shí)刻各種算法解算角度的平均值來(lái)確定當(dāng)前真實(shí)的角度值，而各種算法解算的姿態(tài)角度與平均角度值之間存在的差值就可以當(dāng)作是其與真實(shí)角度值之間的差值，那么就能夠得出單一算法解算的角度在tk時(shí)刻的角度偏差的方差，就能夠依據(jù)最優(yōu)權(quán)值分配的原則獲得tk時(shí)刻的姿態(tài)角度的權(quán)值，進(jìn)而得到加權(quán)平均融合之后的姿態(tài)角度，將其作為姿態(tài)解算模塊的測(cè)量結(jié)果應(yīng)用于定位導(dǎo)航中。

tk時(shí)刻各種算法解算角度的平均值為：

式中，Yi(k)為第 種算法在k時(shí)刻解算后的測(cè)量值。tk時(shí)刻各種算法解算的角度與平均值的偏差：

第i種算法在前面k次解算誤差的平均值為：

在tk時(shí)刻第i種算法的解算誤差的標(biāo)準(zhǔn)差為：

將式（4）解得的標(biāo)準(zhǔn)差看作是各角度解算均方差的近似，然后根據(jù)最優(yōu)權(quán)值分配原則，就可以得tk時(shí)刻的權(quán)值：

接著可以得到tk時(shí)刻加權(quán)融合后的姿態(tài)角度為：

3 試驗(yàn)分析

為了檢驗(yàn)這種動(dòng)態(tài)分配權(quán)值的加權(quán)平均融合方法應(yīng)用于捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)解算中所得姿態(tài)角度的精度，本文將此方法應(yīng)用于慣性元件MPU6050對(duì)姿態(tài)角的實(shí)際測(cè)量中進(jìn)行試驗(yàn)驗(yàn)證，其中陀螺儀的量程設(shè)定為±1000°/s，主要依據(jù)三軸陀螺儀所測(cè)量的角速度值進(jìn)行姿態(tài)解算。

本文以偏航角為例，設(shè)定其在-90°～+90°逐漸變化，隨機(jī)選取其中幾段時(shí)間內(nèi)歐拉角法、方向余弦法和四元數(shù)法的解算誤差曲線與融合之后解算誤差曲線的對(duì)比，驗(yàn)證將這種動(dòng)態(tài)權(quán)值分配的加權(quán)平均融合方法應(yīng)用于姿態(tài)解算模塊中進(jìn)行姿態(tài)解算的可行性和有效性。

圖2、3、4、5分別給出了隨機(jī)選取試驗(yàn)過(guò)程中的一段時(shí)間內(nèi)，單一算法與融合之后的解算誤差曲線。

圖6為各個(gè)算法誤差曲線的匯總，其中黑色的曲線即是各種算法融合之后的誤差曲線，可以清楚地看出將3種算法融合之后所得角度的誤差要明顯低于其中任何一種算法所得角度的誤差，并且誤差曲線也更加趨于穩(wěn)定，這充分證明了這種權(quán)值動(dòng)態(tài)分配的加權(quán)平均融合算法在姿態(tài)解算中有效地提高了解算精度，降低了解算誤差。另外，對(duì)各解算結(jié)果進(jìn)行數(shù)據(jù)分析，如表1所示。

由表中的數(shù)據(jù)分析結(jié)果表明，在同等條件下，加權(quán)平均得到的姿態(tài)角誤差平均值低于任一單一算法，其標(biāo)準(zhǔn)差同樣也低于任意單一解算算法的標(biāo)準(zhǔn)差。這也驗(yàn)證了，通過(guò)動(dòng)態(tài)分配權(quán)值的加權(quán)平均法對(duì)姿態(tài)解算方法的解算結(jié)果進(jìn)行融合是可行的。

圖2 偏航角為47°時(shí)方向余弦法解算誤差曲線Fig.2 Solution error curve of direction cosine algorithm with yaw angle being 47°

圖3 偏航角為47°時(shí)歐拉角法解算誤差曲線Fig.3 Solution error curve of euler angle algorithm with yaw angle being 47°

圖4 偏航角為47°時(shí)四元數(shù)法解算誤差曲線Fig.4 Solution error curve of quaternion algorithm with yaw angle being 47°

圖5 偏航角為47°時(shí)融合后的解算誤差曲線Fig.5 Solution error curve after fusing with yaw angle being 47°

圖6 偏航角為47°時(shí)誤差曲線的對(duì)比圖Fig.6 Comparison chart of solution error curve with yaw angle being 47°

表1 不同算法下偏航角度的誤差統(tǒng)計(jì) （°）

4 結(jié)論

本文給出了歐拉角法、方向余弦法和四元數(shù)法3種姿態(tài)解算方法，分別陳述了它們各自的優(yōu)勢(shì)和不足，并在捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)解算中加入了加權(quán)平均的數(shù)據(jù)融合方法。對(duì)于實(shí)際中不能夠得到真實(shí)的角度解算誤差方差這一問(wèn)題，提出了一種對(duì)角度解算誤差進(jìn)行實(shí)時(shí)估計(jì)并動(dòng)態(tài)分配權(quán)值的方法，將歐拉角法、方向余弦法和四元數(shù)法解算所得角度進(jìn)行加權(quán)平均融合。對(duì)單一算法的解算結(jié)果及融合之后的結(jié)果進(jìn)行了試驗(yàn)分析，結(jié)果表明，融合后的解算精度比單一算法有了明顯提高，可以將該種融合方法應(yīng)用于姿態(tài)解算模塊中進(jìn)行姿態(tài)解算。

[1]杜海龍,張榮輝,劉平,等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算模塊的實(shí)現(xiàn) [J]. 光學(xué)精密工程, 2008, 16(10):1957-1962.

DU Hailong, ZHANG Ronghui, LIU Ping, et al. Realization of attitude algorithm module in strapdown inertial guidance system[J]. Optics and Precision Engineering, 2008, 16(10):1957-1962.

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

ZHANG Ronghui, JIA Hongguang, CHEN Tao, et al. Attitude solution for strapdown inertial navigation system based on quaternion algorithm[J].Optics and Precision Engineering, 2008, 16(10):1964-1970.

[3]李海濤. 加速度計(jì)/磁強(qiáng)計(jì)捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算方法研究[D].太原: 中北大學(xué), 2007.

LI Haitao. Study on attitude algorithm of accelerometers/magnetometers strapdown inertial navigation system[D]. Taiyuan: North University of China, 2007.

[4]陳廷楠, 張登成. 雙歐法與四元數(shù)法的應(yīng)用比較[J]. 飛行力學(xué) , 1996, 14(4):60-64.

CHEN Tingnan, ZHANG Dengcheng. Application comparision of dual euler method and quaternion method[J]. Flight Dynamics, 1996,14(4):60-64.

[5]楊志菊, 袁剛, 徐新林. 飛行器姿態(tài)解算方法及仿真研究 [J].遼寧工業(yè)大學(xué)學(xué)報(bào)(自然科學(xué)版), 2010, 30(5):307-309.

YANG Zhiju, YUAN Gang, XU Xinlin. Aircraft postures algorithm and their simulation study[J]. Journal of Liaoning University of Technology(Natural Science Edition), 2010,30(5):307-309.

[6]WALTZ L. Multi-sensor data fusion[M]. Massachusetts: Artech House, 1990:3-15.

[7]范曉靜. 基于多傳感器信息融合的機(jī)器人避障和導(dǎo)航控制研究[D]. 沈陽(yáng):沈陽(yáng)理工大學(xué), 2008.

FAN Xiaojing. Study of robot’s obstacle avoidance and navigation control based on multi-sensor information fusion technology[D].Shenyang:Shenyang Ligong University, 2008.

[8]趙敏華, 安毅生, 黃永宣. 多傳感器信息融合技術(shù)在智能駕駛系統(tǒng)中的應(yīng)用 [J]. 自動(dòng)化與儀器儀表 , 2003, 29(1):30-32.

ZHAO Minhua, AN Yisheng, HUANG Yongxuan. Application of multi-sensor information fusion technology in intelligent drive system[J].Automation and Instrumentation, 2003, 29(1):30-32.

[9]THAWORNWONG S, ENKE D. The adaptive selection of financial and economic variables for use with artificial neural networks[J].Neuorcomputing, 2004, 56(1):205-232.

[10]BEAUVAIS M, LAKSHMANAN S. CLARK: a heterogeneous sensor fusion method for finding lanes and obstacles[J]. Image and Vision Computing, 2000,18(5):397-413.

[11]王淑芳, 任蘭珍. 基于多傳感器信息融合技術(shù)的智能駕駛系統(tǒng)模型 [J]. 北京石油化工學(xué)院學(xué)報(bào) , 2006, 14(3):52-55.

WANG Shufang, REN Lanzhen. Intelligent drive system model based on multi-sensor information fusion technology[J]. Journal of Beijing Institute of Petro-chemical Technology, 2006, 14(3):52-55.

[12]凌林本, 李滋剛, 陳超英,等. 多傳感器數(shù)據(jù)融合時(shí)權(quán)的最優(yōu)分配原則 [J]. 中國(guó)慣性技術(shù)學(xué)報(bào) , 2000, 8(2):37-39.

LING Linben, LI Zigang, CHEN Yingchao, et al.The optimal distribution principle of weight when multi-sensor data is fusing[J]. Journal of Chinese Inertial Technology,2000, 8(2):37-39.

[13]郝燕玲, 劉志平. 權(quán)值動(dòng)態(tài)分配的 GFSINS 多角速度加權(quán)平均融合算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2010, 18(1):17-21.

HAO Yanling, LIU Zhiping. Fusion algorithm of angular velocity weighted averages for GFSINS based on dynamically allocating weights[J].Journal of Chinese Inertial Technology, 2010, 18(1):17-21.