一種改良Kalman濾波參數(shù)辨識(shí)粗對(duì)準(zhǔn)方法

徐 祥，徐曉蘇，張 濤，李 瑤，周 峰

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

一種改良Kalman濾波參數(shù)辨識(shí)粗對(duì)準(zhǔn)方法

徐 祥1,2，徐曉蘇1,2，張 濤1,2，李 瑤1,2，周 峰1,2

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

針對(duì)傳統(tǒng)基于g信息的粗對(duì)準(zhǔn)的捷聯(lián)慣導(dǎo)系統(tǒng)中，受傳感器噪聲的影響，存在效視運(yùn)動(dòng)無(wú)法提取和雙向量共線的缺點(diǎn)，提出了一種基于改良 Kalman濾波的參數(shù)辨識(shí)粗對(duì)準(zhǔn)方法。該方法通過(guò)構(gòu)建視在重力在初始載體系中的映射模型，利用改良Kalman濾波進(jìn)行模型參數(shù)辨識(shí)，然后通過(guò)識(shí)別參數(shù)重新構(gòu)建視在重力在初始載體系中的映射，解決了由于傳感器噪聲導(dǎo)致有效視運(yùn)動(dòng)無(wú)法正常提取的缺點(diǎn)。利用識(shí)別參數(shù)具有隨估計(jì)次數(shù)增多得到優(yōu)化的特點(diǎn)，構(gòu)造初始時(shí)刻和最終時(shí)刻向量，避免雙向量共線問題。利用改良Kalman濾波算法的自適應(yīng)特點(diǎn)，優(yōu)化參數(shù)識(shí)別精度與速度。轉(zhuǎn)臺(tái)實(shí)驗(yàn)表明，采用改良Kalman濾波方法航向?qū)?zhǔn)精度為-0.0414°，標(biāo)準(zhǔn)差為0.041°，而傳統(tǒng)RLS方法得到的航向精度為-0.0738°，標(biāo)準(zhǔn)差為0.128°。由此可知，本文提出的方法性能更優(yōu)。

捷聯(lián)慣導(dǎo)系統(tǒng)；粗對(duì)準(zhǔn)；改良Kalman濾波；參數(shù)辨識(shí)

捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)分為粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)兩部分[1]，其中精對(duì)準(zhǔn)需要在粗對(duì)準(zhǔn)的基礎(chǔ)上完成。傳統(tǒng)的粗對(duì)準(zhǔn)方法是利用加速度計(jì)對(duì)重力加速度的測(cè)量以及陀螺儀對(duì)地球自轉(zhuǎn)測(cè)量，通過(guò)解析方法計(jì)算載體初始失準(zhǔn)角[2]。

由于實(shí)際載體在初始對(duì)準(zhǔn)過(guò)程中可能存在搖擺等運(yùn)動(dòng)，使得傳統(tǒng)解析方法實(shí)際應(yīng)用受到限制。為此一種基于g信息的捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)方法得到應(yīng)用[3]，該方法利用加速度計(jì)量測(cè)在初始載體系中的映射以及理論重力在初始慣性系中的映射，通過(guò)解析算法計(jì)算初始載體系與初始慣性系之間的方向余弦矩陣，并根據(jù)實(shí)時(shí)運(yùn)動(dòng)信息，計(jì)算運(yùn)動(dòng)情況下載體初始未對(duì)準(zhǔn)角，使得粗對(duì)準(zhǔn)方法得到進(jìn)一步應(yīng)用。

然而基于g信息的捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)方法存在視在重力運(yùn)動(dòng)干擾大以及雙向量共線問題，使得其對(duì)準(zhǔn)精度受到限制。在此基礎(chǔ)上，采用濾波算法提高對(duì)準(zhǔn)精度的方法得到發(fā)展[4-6]，但是在設(shè)計(jì)濾波算法時(shí)選擇合適的濾波器是提高對(duì)準(zhǔn)精度的關(guān)鍵，而在實(shí)際應(yīng)用中，系統(tǒng)噪聲具有多樣性的特點(diǎn)，使得設(shè)計(jì)合適的濾波器存在一定的困難[7]。文獻(xiàn)[8]提出了一種采用遞推最小二乘（RLS）參數(shù)識(shí)別對(duì)視在重力進(jìn)行重構(gòu)的方法，提高了視在重力有效信息的提取，但是需要量測(cè)噪聲的統(tǒng)計(jì)信息，而在實(shí)際應(yīng)用中無(wú)法精確得到，對(duì)準(zhǔn)結(jié)果易出現(xiàn)野值現(xiàn)象。本文在此基礎(chǔ)上，采用改良Kalman濾波算法，利用算法具有的自適應(yīng)特性，使得算法收斂速度和收斂精度得到提高。

1 粗對(duì)準(zhǔn)坐標(biāo)系定義

① 導(dǎo)航系n：選取“東-北-天”為導(dǎo)航系；

② 地球系e：原點(diǎn)位于地心，ze軸從地心沿地球北極朝上，xe軸位于赤道平面，從地心指向載體所在子午線，ye軸與ze軸、xe軸構(gòu)成右手坐標(biāo)系；

③ 初始慣性系i0：選取粗對(duì)準(zhǔn)起始t0時(shí)刻地球系作為初始慣性系，并且不隨地球運(yùn)動(dòng)而改變；

④ 初始載體系b0：粗對(duì)準(zhǔn)起始t0時(shí)刻載體坐標(biāo)系，并且相對(duì)慣性系不變；

⑤ 載體系b：定義“右-前-上”為載體坐標(biāo)系。

2 基于g信息的粗對(duì)準(zhǔn)原理

根據(jù)記錄兩個(gè)時(shí)間點(diǎn)的gb0和gi0，可以得到：

基于g信息的粗對(duì)準(zhǔn)直接利用加速度計(jì)量測(cè)信息進(jìn)行初始方向余弦求解，由于加速度計(jì)含有噪聲，使得有效加速度信息受到隨機(jī)噪聲影響，從而影響方向余弦的求解精度。由于解析計(jì)算需要兩個(gè)時(shí)刻的向量，為避免雙向量共線問題，需要較長(zhǎng)的時(shí)間間隔。

3 基于改良Kalman參數(shù)辨識(shí)算法

為解決基于g信息的粗對(duì)準(zhǔn)存在的問題，一種參數(shù)辨識(shí)的方法得到應(yīng)用[10-11]。它先利用式(4)構(gòu)造參數(shù)識(shí)別模型，利用 RLS算法計(jì)算參數(shù)，然后重構(gòu)向量gb0（t），最后完成初始方向余弦矩陣的求解，實(shí)現(xiàn)初始對(duì)準(zhǔn)。

3.1 參數(shù)辨識(shí)模型

根據(jù)方向余弦矩陣的分解方法，有如下關(guān)系：

對(duì)式(8)進(jìn)一步簡(jiǎn)化，并構(gòu)造待識(shí)別的參數(shù)模型：

式中，矩陣A為待識(shí)別的參數(shù)矩陣。

3.2 改良Kalman濾波

依據(jù)上面分析，采用改良 Kalman濾波器進(jìn)行參數(shù)識(shí)別[12]，由于視在重力三個(gè)軸上的分量互不相關(guān)，在此以z軸為例，構(gòu)建改良Kalman濾波模型：

式中：ηz表示量測(cè)隨機(jī)噪聲，

根據(jù)上述模型，構(gòu)造改良Kalman濾波如下：

式中：i=x,y,z；Λi（k）表示自適應(yīng)量測(cè)噪聲；ei（k）表示殘差。

3.3 向量重構(gòu)

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

為驗(yàn)證本文提出的算法，本文實(shí)驗(yàn)采用仿真實(shí)驗(yàn)和轉(zhuǎn)臺(tái)實(shí)驗(yàn)兩種方法分別驗(yàn)證，從收斂速度和對(duì)準(zhǔn)精度兩個(gè)方面進(jìn)行分析，對(duì)比傳統(tǒng)基于g信息的粗對(duì)準(zhǔn)方法、RLS參數(shù)辨識(shí)粗對(duì)準(zhǔn)及本文提出的基于改良Kalman濾波參數(shù)辨識(shí)粗對(duì)準(zhǔn)，并從收斂速度和收斂精度兩個(gè)方面進(jìn)行比較和分析。

4.1 仿真實(shí)驗(yàn)

為使仿真實(shí)驗(yàn)更接近實(shí)際，模擬艦船系泊狀態(tài)下初始對(duì)準(zhǔn)，設(shè)定搖擺參數(shù)選擇如表1~2所示。

表1 搖擺參數(shù)設(shè)定Tab.1 Setting of swinging parameters

仿真實(shí)驗(yàn)設(shè)定傳感器參數(shù)如表2所示。

表2 傳感器誤差設(shè)定Tab.2 Setting of sensor errors

圖1 三種算法計(jì)算 gb0Fig.1 Gravitational apparent motion in b0frame

三種算法計(jì)算得到的g0b如圖1 所示，從圖中可以看出，傳統(tǒng)基于g信息的粗對(duì)準(zhǔn)，由于受到加速度計(jì)的隨機(jī)噪聲的影響，呈現(xiàn)出較大的噪聲特性，這種噪聲將會(huì)直接影響最終的對(duì)準(zhǔn)結(jié)果，使得對(duì)準(zhǔn)精度存在較大的波動(dòng)，容易引起向量共線問題。而采用參數(shù)辨識(shí)法估計(jì)gb0，能夠有效地減小隨機(jī)噪聲對(duì)視在重力計(jì)算的影響。由圖1可知，采用RLS和改良Kalman濾波方法得到視在重力已經(jīng)較好地消除了隨機(jī)噪聲的影響，并且由于改良Kalman濾波的自適應(yīng)特點(diǎn)，使得其計(jì)算的視在重力具有收斂速度快，噪聲特性小的優(yōu)點(diǎn)，這種特性直接影響了對(duì)準(zhǔn)結(jié)果的特性。由于參數(shù)辨識(shí)無(wú)法消除加速度計(jì)零偏，所以無(wú)論是RLS方法還是改良Kalman濾波都在計(jì)算gb0與真實(shí)gb0之間存在一個(gè)常值誤差，根據(jù)基于g信息的粗對(duì)準(zhǔn)誤差特性可知，這個(gè)常值誤差即為最終的對(duì)準(zhǔn)極限精度。

圖2 是三種對(duì)準(zhǔn)結(jié)果誤差曲線圖。表3 給出了RLS方法與改良Kalman濾波方法得到的統(tǒng)計(jì)分析數(shù)據(jù)，由于傳統(tǒng)基于g信息的粗對(duì)準(zhǔn)，噪聲特性較明顯，在此并未給出此方法的統(tǒng)計(jì)特性分析。仿真數(shù)據(jù)表明，采用改良Kalman濾波與RLS方法在水平軸上的收斂速度幾乎相當(dāng)，但是在方位軸上，采用改良Kalman濾波在相同的時(shí)間段具有較小的標(biāo)準(zhǔn)差，表明其收斂速度更快。同時(shí)改良Kalman濾波方法也提高了對(duì)準(zhǔn)結(jié)果的精度。

圖2 三種粗對(duì)準(zhǔn)誤差Fig.2 Coarse alignment errors

表3 兩種方法粗對(duì)準(zhǔn)誤差結(jié)果Tab.3 Experiment results for coarse alignment errors for two methods (°)

4.2 轉(zhuǎn)臺(tái)實(shí)驗(yàn)

采用仿真實(shí)驗(yàn)得到的結(jié)果是在理想情況下得到的，而改良Kalman濾波的優(yōu)勢(shì)在于不需要量測(cè)量的統(tǒng)計(jì)信息，這更加符合實(shí)際情況。為驗(yàn)證本文提出的算法，設(shè)計(jì)轉(zhuǎn)臺(tái)實(shí)驗(yàn)，搖擺參數(shù)如表1 所示，對(duì)準(zhǔn)時(shí)間為600 s，IMU數(shù)據(jù)輸出率為200 Hz，對(duì)準(zhǔn)過(guò)程采用計(jì)算機(jī)實(shí)時(shí)采集傳感器數(shù)據(jù)計(jì)算。

轉(zhuǎn)臺(tái)實(shí)驗(yàn)傳感器安裝方式如圖3所示，實(shí)際對(duì)準(zhǔn)結(jié)果如圖4、圖5和表4所示。

圖4是轉(zhuǎn)臺(tái)實(shí)驗(yàn)g0b曲線圖，轉(zhuǎn)臺(tái)實(shí)驗(yàn)結(jié)果如圖5所示，從對(duì)準(zhǔn)誤差曲線可以發(fā)現(xiàn)，水平方向能夠快速收斂到極限精度。由于粗對(duì)準(zhǔn)極限精度由加速度計(jì)零偏決定，所以縱搖和橫搖對(duì)準(zhǔn)精度存在一定差異。在方位對(duì)準(zhǔn)上可以看到，采用改良 Kalman濾波方法具有較快的收斂速度和較高的對(duì)準(zhǔn)精度。

圖3 轉(zhuǎn)臺(tái)實(shí)驗(yàn)實(shí)物圖Fig.3 Turntable and IMU

圖4 轉(zhuǎn)臺(tái)實(shí)驗(yàn)計(jì)算 gb0Fig.4 Gravitational apparent motion of turntable test

圖5 轉(zhuǎn)臺(tái)實(shí)驗(yàn)粗對(duì)準(zhǔn)誤差Fig.5 Alignment errors of turntable test

表4 轉(zhuǎn)臺(tái)實(shí)驗(yàn)兩種方法粗對(duì)準(zhǔn)誤差結(jié)果Tab.4 Turntable experiment results for coarse alignment errors for two methods (°)

5 結(jié) 論

針對(duì)傳統(tǒng)基于g信息的粗對(duì)準(zhǔn)存在的問題，本文設(shè)計(jì)了基于改良 Kalman濾波參數(shù)識(shí)別粗對(duì)準(zhǔn)方法，并從仿真實(shí)驗(yàn)和轉(zhuǎn)臺(tái)實(shí)驗(yàn)兩方面驗(yàn)證了改良 Kalman濾波方法對(duì)準(zhǔn)的可行性及有效性，通過(guò)將改良Kalman濾波與RLS方法進(jìn)行對(duì)比。實(shí)驗(yàn)表明，在水平對(duì)準(zhǔn)上，兩種方法收斂速度與收斂精度相當(dāng)。但是由于改良Kalman濾波具有自適應(yīng)特點(diǎn)，并能夠有效減小量測(cè)信息統(tǒng)計(jì)誤差，使得改良 Kalman濾波能夠有更快的收斂速度及更高的對(duì)準(zhǔn)精度。

（Reference）：

[1] He Hong-yang, Xe Jiang-ning, Li Jing-shu, et al. Improved fast backtracking alignment approach for strapdown inertial navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 179-183.

[2] 王新龍. 捷聯(lián)式慣導(dǎo)系統(tǒng)動(dòng), 靜基座初始對(duì)準(zhǔn)[M]. 西北工業(yè)大學(xué)出版社, 2013. Wang Xin-long. Initial alignment for strapdown inertial navigation system on moving and static base[M]. Northwestern Polytechnical University Press Ltd, 2013.

[3] Qin Fang-jun, Li An, Xu Jiang-ning. Improved fast alignment method of strapdown INS using bidirectional processes and denoising[J]. Journal of Chinese Inertial Technology, 2014. 22(4): 453-458.

[4] 吳楓, 秦永元, 周琪. 間接解析自對(duì)準(zhǔn)算法誤差分析[J]. 系統(tǒng)工程與電子技術(shù), 2013, 35(3): 586-590. Wu Feng, Qin Yong-yuan, Zhou Qi. Error analysis of indirect analytic alignment algorithm[J]. Systems Engineering and Electronics, 2013, 35(3): 586-590.

[5] Malakar B, Roy B K. A novel application of adaptive filtering for initial alignment of strapdown inertial navigation system[C]//2014 International Conference on Circuits, Systems, Communication and Information Technology Applications. IEEE, 2014: 189-194.

[6] 嚴(yán)恭敏, 白亮, 翁浚, 等. 基于頻域分離算子的 SINS抗晃動(dòng)干擾初始對(duì)準(zhǔn)算法[J]. 宇航學(xué)報(bào), 2011, 32(7): 1486-1490. Yan Gong-min, Bai Liang, Weng Jun, et al. SINS antirocking disturbance initial alignment based on frequency domain isolation operator[J]. Journal of Astronautics, 2011, 32(7): 1486-1490.

[7] Li Q, Ben Y, Sun F. A novel algorithm for marine strapdown gyrocompass based on digital filter[J]. Measurement, 2013, 46(1): 563-571.

[8] Liu X, Zhao Y, Liu X, et al. An improved self-alignment method for strapdown inertial navigation system based on gravitational apparent motion and dual-vector[J]. Review of Scientific Instruments, 2014, 85(12): 125108.

[9] 嚴(yán)恭敏, 秦永元, 衛(wèi)育新, 等. 一種適用于 SINS 動(dòng)基座初始對(duì)準(zhǔn)的新算法[J]. 系統(tǒng)工程與電子技術(shù), 2009, 31(3): 634-637. Yan Go ng-min, Qin Yong-yuan, Wei Yu-xin, et al. New initial alignment algorithm for SINS on moving base[J]. Systems Engineering and Electronics, 2009, 31(3): 634-637.

[10] 嚴(yán)恭敏, 翁浚, 趙長(zhǎng)山, 等. 捷聯(lián)慣導(dǎo)系統(tǒng)改進(jìn)參數(shù)辨識(shí)初始對(duì)準(zhǔn)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2010, 18(5): 523-526. Yan Gong-min, Weng Jun, Zhao Chang-shan, et al. Improved parameter identification method for SINS initial alignment[J]. Journal of Chinese Inertial Technology, 2010, 18(5): 523-526.

[11] Liu X, Zhao Y, Liu Z, et al. A novel self-alignment method for SINS based on parameter recognition and dual-velocity vectors[J]. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2015: 0954410014568798.

[12] 秦永元. 捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)的參數(shù)辨識(shí)法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 1990(2): 1-16. Qin Yong-yuan. Parameters ldentifieration Method of the Alignment of a strapdown Inertial Navigation system[J]. Journal of Chinese Inertial Technology, 1990(2): 1-16.

Abstract: Aiming at the problem of poor observability of yaw measurement for foot-mounted attitude heading reference systems (AHRS), an indoor pedestrian navigation method by using low cost foot-mounted AHRS and shoulder-mounted compass is proposed. In this method, the yaw measured from the shoulder-mounted compass is directly used for the calculation of the attitude transformation matrix for the foot-mounted AHRS. And then, when the person is in a stance phase during walk, a Kalman filter (KF) is used to restrict the INS error drift. Experimental results show that the proposed method can effectively reduce the mean error of the position by about 30% compared with that without the shoulder-mounted compass.

Key words: indoor pedestrian navigation; inertial navigation system; Kalman filter; yaw drift; foot-mounted attitude heading reference system

Improved Kalman filter for SINS coarse alignment based on parameter identification

XU Xiang1,2, XU Xiao-su1,2, ZHANG Tao1,2, LI Yao1,2, ZHOU Feng1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Nanjing 210096, China; 2. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)

Traditional g-based coarse alignment in SINS has two problems under the influences of sensor noises. The first one is that the effective gravitational apparent motion information is difficult to extract, and the second is how to avoid the collinear problem between two vectors. To solve these two problems, a new algorithm based on an improved Kalman filter is proposed. This method reconstructs the gravitational vectors based on parameter identification method, so the stochastic noises in accelerometer measurements are reduced. Since the parameters are constants, it can avoid the collinear by the reconstructed vectors. The improved Kalman filtering algorithm is adaptive, and it does not need the statistical information of the accelerometer measurements. Turntable tests show that the yaw error and the standard deviation of the new method are -0.0414° and 0.041° respectively, while the yaw error and the standard deviation of the recursive least-squares method is -0.0738° and 0.041° respectively, showing that the new method has better performance.

strapdown inertial navigation system; coarse alignment; improved Kalman filter; parameter identification

Improved indoor pedestrian navigation method using low-cost foot-mounted AHRS and shoulder-mounted compass

XU Yuan1, CHEN Xi-yuan2,3, WANG Yi-min1, MA Si-yuan1
(1. School of Electrical Engineering, University of Jinan, Jinan 250022, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China; 3. Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology Ministry of Education, Nanjing 210096, China)

U666.1

：A

2016-03-30；

：2016-04-12

國(guó)家自然科學(xué)基金項(xiàng)目（51175082，61473085，51375088）；優(yōu)秀青年教師教學(xué)科研資助計(jì)劃（2242015R30031）；微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室基金（201403）；基于大數(shù)據(jù)架構(gòu)的公安信息化應(yīng)用公安部重點(diǎn)實(shí)驗(yàn)室（浙江警察學(xué)院）開放課題資助項(xiàng)目（2015DSJSYS002）

徐祥（1988—），男，博士生，從事導(dǎo)航定位研究。E-mail: xuxiang@seu.edu.cn

聯(lián) 系 人：徐曉蘇（1961—），男，教授，博士生導(dǎo)師，從事測(cè)控技術(shù)與導(dǎo)航定位領(lǐng)域的研究。E-mail: xxs@seu.edu.cn

1005-6734(2016)03-0320-05

10.13695/j.cnki.12-1222/o3.2016.03.008