收稿日期：2023-08-05

基金項目：2021年福建省教育廳中青年教師教育科研項目（JAT210856）；2021年校級課題（ZZY2021B047）；2022年校級課題（ZZY2022Z23）

作者簡介：陳秀端（1980-），女，福建漳州人，講師，碩士，研究方向為物理學、電子與通信.E-mail：670041055@qq.com.

*通信作者：朱笑花（1980-），女，福建漳平人，副教授，碩士，研究方向為智能算法.E-mail：zhangjiwei03@126.com.

文章編號：2095-6991（2024）04-0038-06

摘要：為了融合各類定位傳感器的優(yōu)勢，滿足復雜條件下導航系統(tǒng)的使用需求，利用誤差狀態(tài)的卡爾曼濾波器融合慣性傳感器和全局傳感器等設(shè)備的觀測數(shù)據(jù)，并通過EVO軌跡評估工具分析了該算法性能.實驗結(jié)果表明，融合算法在x軸與z軸上與理想采樣值基本吻合，在x軸上與理想采樣值的最大誤差為4.8 m，在z軸上最大誤差為2.6 m，絕對軌跡誤差均方根值為3.02 m，每秒的相對位姿誤差為0.12 m，從而證明了卡爾曼濾波融合的方案可以滿足定位精度的需求.研究搭建了一個基于多傳感融合的定位算法，使得該融合方案在復雜環(huán)境下的定位精度得到了提升，并且面對各種干擾環(huán)境下具有一定的穩(wěn)定性.

關(guān)鍵詞：IMU；SINS；ESKF；多傳感融合；定位導航

中圖分類號：TP391""" 文獻標志碼：A

Research on Location and Navigation Algorithm Basedon Multi Sensor Fusion and Kalman Filter

CHEN Xiu-duan1， ZHU Xiao-hua2*

（1. College of Intelligent Manufacturing， Zhangzhou Vocational and Technical College， Zhangzhou 363000， Fujian， China；

2. School of Physics and Information Engineering， Minnan Normal University， Zhangzhou 363000， Fujian， China）

Abstract：In order to integrate the advantages of various positioning sensors and meet the needs of navigation systems under complex conditions， Kalman filter with error state is used to fuse the observation data of inertial sensor and global sensor， and the performance of the algorithm is analyzed by EVO trajectory evaluation tool. The results show that the fusion algorithm is basically consistent with the ideal sampling value on the X-axis and Z-axis. The maximum error between the fusion algorithm and the ideal sampling value on the X-axis is 4.8 m， the maximum error on the Z-axis is 2.6 m， the absolute trajectory error is 3.02 m， and the relative pose error per second is 0.12 m. It is proved that the Kalman filter fusion scheme can meet the demand of positioning accuracy. A positioning algorithm based on multi-sensor fusion is researched and built， which improves the positioning accuracy in complex environments and has certain stability in the face of various interference environments.

Key words：IMU; SINS; ESKF; multi-sensor fusion; positioning navigation

0" 引言

作為一種應用性很強的高新技術(shù)，定位導航技術(shù)在無人駕駛、智能機器人、車載定位、手機導航等領(lǐng)域具有關(guān)鍵作用［1］.定位導航技術(shù)可以在傳感器的幫助下感知并采集周圍環(huán)境信息，通過信號轉(zhuǎn)換和位置姿態(tài)估算幫助搭載對象在未知和復雜環(huán)境下完成指定運動［2-4］.在給定的環(huán)境下，有多種多樣的傳感器可以識別當前位置.從位置信息的提供者來看，傳感器可以分為兩大類：一類是慣性導航系統(tǒng)，如捷聯(lián)慣性導航系統(tǒng)（Strap-down Inertial Navigation System， SINS）和解析式慣性導航系統(tǒng)等；另一類是全球定位系統(tǒng)，如GPS定位、北斗衛(wèi)星導航等［5］.基于單一傳感器信息進行位置姿態(tài)估計往往難以滿足定位導航要求，例如通過里程計對模型進行航跡推算就存在較大誤差［6］，因此利用多傳感器采集模型運動信息和運行環(huán)境信息進行位姿估計是目前最有效的技術(shù)手段［7］.慣性測量單元（Inertial Measurement Unit， IMU）技術(shù)可以在得知對象位置和姿態(tài)后對下一時刻的位置和姿態(tài)進行估算［8］.本文在以上研究的基礎(chǔ)上，融合卡爾曼濾波（Kalman Filtering， KF）對多傳感定位導航系統(tǒng)展開研究，從而提升系統(tǒng)的精準度.

1" 結(jié)合KF的多傳感融合定位導航研究

1.1" 慣性測量單元位置與姿態(tài)解算的仿真

傳感器作為系統(tǒng)測量部件，可以將采集到的信號進行轉(zhuǎn)換并通過函數(shù)或方程的形式描述系統(tǒng)的運動［9］.在描述系統(tǒng)運動的過程中，通常會定義兩個坐標系，分別為參考坐標系和機體坐標系，其中機體坐標系常用慣性測量單元（IMU）來表示，IMU能夠在很短的時間里得到3個軸向的加速度與角速度，它擁有采樣高頻、抗干擾能力強等優(yōu)點，能夠很好地彌補圖像傳感器的缺陷.假設(shè)用T矩陣描述IMU相對于參考坐標系的轉(zhuǎn)換關(guān)系，則IMU坐標系下的Pc在參考坐標系下的位置pw表達式為：

pw=Rwcpc+twc，（1）

其中，Rwc表示旋轉(zhuǎn)矩陣，twc表示平移矩陣.IMU利用3個單軸加速度計和3個單軸陀螺儀，分別測量載體的加速度信號和載體相對于參考坐標系的角速度信號，從而計算出載體下一時刻的姿態(tài)位置.當載體相對慣性空間作加速運動時，加速度中的質(zhì)量塊會在慣性的影響下向反方向發(fā)生位移，此時的加速度測量值是通過對質(zhì)量塊的受力測量得到的.最終得到的加速度測量值am表達式為：

am=fma-g，（2）

其中，f表示質(zhì)量塊所受的力，m表示質(zhì)量塊的質(zhì)量，a表示加速度，g表示重力.在三維空間中，為了滿足測量單元的受力分析，會將3個方向上的測量單元各自正交放置.在質(zhì)量塊質(zhì)量和主動軸運算速度已知的情況下，載體的角速度ω表達式為：

Fc=-2m（ω×v），（3）

其中，F(xiàn)c表示當前時刻的科氏力，v表示主動軸往復運動的速度.通過加速度計和陀螺儀便可以組成捷聯(lián)慣性導航系統(tǒng)（SINS），對傳感器采集到的數(shù)據(jù)進行解算并得到模型姿態(tài)位置信息，捷聯(lián)慣導解算流程如圖1所示.

在圖1中，SINS通過比力變換、速度積分和位置更新等步驟最終實現(xiàn)對模型姿態(tài)位置的定位與更新，其中姿態(tài)更新是整個慣導解算過程的核心，直接影響速度與位置的更新以及慣導解算的求解精度［10］.整個SINS的誤差可以劃分為兩種，即確定誤差和隨機誤差.其中，確定誤差包括儀器誤差、讀數(shù)誤差、尺度因子誤差和零偏等等.其中零偏和尺度因子會隨著元器件工作溫度的變化發(fā)生漂移，從而產(chǎn)生一定的誤差.IMU的隨機誤差主要是由游走誤差和白噪聲引起的，在加速度計中，IMU的隨機誤差可表示為：

ab=qbw（aw+gw）+ba+na，（4）

其中，ab表示加速度計測量值，b表示隨機游走，n表示白噪聲，下角標w表示參考坐標系下的值，qbw表示四元數(shù)的坐標系轉(zhuǎn)換.在式（4）中隨機游走為布朗運動形式的離散模型，而白噪聲一般指服從高斯分布的外部噪聲，因此相當于進行了高斯噪聲的疊加，最終導致了誤差的隨機性.同理可得IMU的隨機誤差在陀螺儀中的表達式為：

ωb=ωb+b+n，（5）

其中，ωb表示陀螺儀測量值，ωb表示載體坐標系下的角速度.為了進一步測量IMU的隨機游走誤差和零偏不穩(wěn)定性，國際上常采用艾倫方差法計算相關(guān)隨機誤差系數(shù)，艾倫標準差雙對數(shù)曲線如圖2所示.

在圖2中斜率為0所對應的地方即為零偏不穩(wěn)定系數(shù)，斜率為±0.5的地方分別為角速度和加速度隨機游走，中心波動的位置表示白噪聲和正弦項.時間τ=1與對應的斜率曲線交點即為相關(guān)隨機誤差系數(shù)的值.首先將IMU靜置幾小時，設(shè)置N個數(shù)據(jù)點并將每個數(shù)據(jù)點分成n個子集，最終得到的艾倫方差AVAR（τ）的表達式為：

AVAR（τ）=12（N/n-1）∑N/n-1i=1（ωi+1-ωi）2，（6）

其中，時間τ與方差的平方根組成的艾倫方差曲線如圖2所示.

1.2" 結(jié)合KF的多傳感融合定位導航

對于描述系統(tǒng)的狀態(tài)變化，在連續(xù)系統(tǒng)中可以通過對系統(tǒng)狀態(tài)量求導來表達，而在離散系統(tǒng)中，有些狀態(tài)量無法直觀表達，需要對狀態(tài)進行觀測，從而輔助獲得相關(guān)狀態(tài)量.在狀態(tài)估計中常用濾波器進行數(shù)據(jù)融合、狀態(tài)估計和映射關(guān)系等.因此研究采用KF對歷史數(shù)據(jù)進行融合，設(shè)初始狀態(tài)為x，運動系統(tǒng)的輸入為u，觀測量為z，運動輸入噪聲為w，觀測噪聲為n，則濾波迭代流程如圖3所示.

根據(jù)圖3可知，系統(tǒng)的當前狀態(tài)量只和前一時刻的狀態(tài)量相關(guān)，二者滿足f的映射關(guān)系，和歷史其他時刻的狀態(tài)量無關(guān)，符合馬爾科夫假設(shè).系統(tǒng)當前時刻的狀態(tài)量與觀測量滿足h的映射關(guān)系.f與h的映射關(guān)系可表示為：

xk=f（xk-1，uk，wk），

zk=h（xk，nk）.（7）

KF對于線性系統(tǒng)狀態(tài)方程有非常好的觀測效果，卡爾曼增益矩陣k是該算法的核心.當預報數(shù)據(jù)和觀測數(shù)據(jù)均有誤差時，就必須通過協(xié)方差矩陣來權(quán)衡預報和觀測數(shù)據(jù)，才能獲得最佳估計值［11］.如果式（7）為線性系統(tǒng)且噪聲服從的高斯分布，則過程噪聲wk和觀測噪聲nk的高斯分布為：

wk～N（0，R），nk～N（0，Q），（8）

其中，R和Q分別表示過程噪聲wk和觀測噪聲nk的高斯分布范圍，其狀態(tài)空間表達式為：

xk=F（xk-1，uk）+Bk-1wk，

zk=G（xk）+Cknk.（9）

其中，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，G為觀測矩陣，B為運動噪聲的驅(qū)動矩陣，C為觀測噪聲的驅(qū)動矩陣.卡爾曼濾波由狀態(tài)預測與狀態(tài)更新兩部分組成，假設(shè)上一時刻狀態(tài)量的后驗方差為P∧k－1，則卡爾曼濾波在k時刻的狀態(tài)量預測值xk可表示為：

xk=F（x∧k－1，uk），（10）

其中，x∧k－1表示上一時刻狀態(tài)量.根據(jù)高斯分布的線性變換，預測的后驗方差P-k可表示為：

P-k=Fk－1P∧k－1FTK－1+Bk－1QkBTk－1.（11）

接下來對卡爾曼增益矩陣K進行更新，更新后的卡爾曼增益矩陣Kk可表示為：

Kk=P-kGTk（GkP-kGTk+CkRkCTk）－1.（12）

根據(jù)k時刻的卡爾曼增益，得到的方差估計P∧k可表示為：

P∧k=（I－KkGk）P-k.（13）

最終根據(jù)卡爾曼增益的權(quán)重信息和當前的觀測信息得到此刻的最大似然估計可表示為：

x∧k=xk+Kk（zk－G（x∧k））.（14）

對于系統(tǒng)的狀態(tài)可分為名義狀態(tài)x、誤差狀態(tài)δx和真值狀態(tài)3種.真值狀態(tài)受到系統(tǒng)擾動和噪聲干擾是無法確定的；名義狀態(tài)則是系統(tǒng)的理想狀態(tài)，是一個大信號；誤差狀態(tài)則是具備線性化特質(zhì)的小信號，該方法可用于高斯濾波器的線性化.將名義狀態(tài)轉(zhuǎn)化為大信號，用卡爾曼濾波器將其融合后，會使狀態(tài)方程變得更加復雜，而將誤差狀態(tài)轉(zhuǎn)化為小信號，不僅避免了矩陣復雜化的問題，而且大大降低了KF的修正頻率.本文選擇將KF與誤差狀態(tài)進行融合.融合后的KF的誤差值總是接近于0，使得KF一直在原點附近工作，在 KF展開狀態(tài)方程的過程中，通過逼近原點的方法，使線性化過程更加合理、有效.

2" 結(jié)合KF的多傳感融合定位導航性能測試

本文所有的實驗和算法的驗證都使用Kitti數(shù)據(jù)的2011_09_30_30_drive_0027數(shù)據(jù)集，此數(shù)據(jù)由4組攝像頭，Velodyne激光雷達， GPS， IMU同時獲取.并將以上傳感器固定于汽車上，在城市和鄉(xiāng)鎮(zhèn)采集數(shù)據(jù)，利用差分RT3003-GPS等高精度定位設(shè)備獲取位姿真值.算法通過C++和OpenCV實現(xiàn)，CPU為Intel Corei78750 H，顯卡為NVIDIA GeForce GTX 1050Ti，電腦操作系統(tǒng)為Windows 10 64 bit.圖形界面使用 Pangolin 實現(xiàn)，在與軌跡的真實值作對比時，使用 EVO（Evaluation Of Odometry and SLAM）工具進行數(shù)據(jù)對齊.

通過誤差評價來表現(xiàn)算法的有效性.其中，絕對軌跡誤差直接對比真值與估計值的整體誤差，是直觀反映全局精度的一個評價體系；相對位姿誤差可以評價相鄰時刻的局部誤差值，它忽略了整體的累計漂移，僅僅比較當前時刻與下一時刻之間的誤差，反映了航位推算定位時，兩幀之間的推算精度.

為了測試IMU慣導解算的精確度，首先自定義一組已知運動方程的曲線并仿真出一組無噪音干擾的理想IMU采樣數(shù)值；接著，將陀螺儀及加速度計的樣本白噪聲以及隨機游走白噪聲加入到數(shù)據(jù)集中；最后，利用歐拉積分以及中值積分進行IMU的運動解算，得到的IMU解算結(jié)果如圖4所示.從圖4（a）中可以看出，通過歐拉積分和中值積分對IMU進行解算，中值積分得到的解算仿真與理想采樣值擬合程度更高.從圖4（b）中可以看出，IMU的解算仿真在前6 s與理想采樣值基本吻合，而第6 s之后逐漸產(chǎn)生了誤差并隨著時間的推移誤差逐漸變大，在x軸和y軸上，中值積分的運動解算與理想采樣值更加吻合，而在z軸上歐拉積分與中值積分都發(fā)生了較大程度的漂移，由此可見IMU只適用于短時間內(nèi)的位置姿勢測算.

接下來將KF引入到IMU中并測量其在三維空間內(nèi)的測算誤差，最終得到的測算誤差結(jié)果如圖5所示.從圖5可知，結(jié)合KF的IMU在x軸與z軸上與理想采樣值基本吻合，進一步計算可知在x軸上，本文模型與理想采樣值的誤差在5 m以內(nèi)，最大誤差為4.8 m.在z軸上本文模型的誤差幅度在3 m以內(nèi)，最大誤差為2.6 m.而結(jié)合KF的IMU在y軸的表現(xiàn)上則稍差一些，進一步計算可知在y軸上，研究模型與理想采樣值的誤差幅度在10 m以內(nèi)，最大誤差為10.0 m.這是因為在y軸方向上，缺少足夠的運動激勵，使得被測模型在y軸方向的位置與姿勢估算誤差更大，但是從總體上來看，被測模型在y軸方向的誤差量級與x軸和y軸一致.綜上所述研究模型在三維空間內(nèi)的誤差均小于10 m，符合實驗要求，IMU在3個軸上的誤差產(chǎn)生了較大程度的縮小，從而證明了融合KF算法對于IMU定位導航精準度起到了非常顯著的作用.

為了進一步探究本文模型在定位導航方面的精準度性能，利用KF算法融合IMU和GPS的觀測數(shù)據(jù)，在完成融合后計算其與真實路徑之間的軌跡誤差值，最終得到的絕對軌跡誤差值和相對軌跡誤差值如圖6所示.

結(jié)合KF的IMU絕對軌跡誤差曲線如圖6（a）所示.從圖6（a）中可以看出，觀測模型中位數(shù)的取值為2.8 m，均值取值為3.6 m，均方根值為3.0 m，標準差的取值范圍在［2.5，3.8］之間.本文模型的絕對軌跡誤差曲線共計52 s，在標準差范圍內(nèi)，最大誤差為6.7 m.結(jié)合KF的IMU相對軌跡誤差曲線如圖6（b）所示.從圖6（b）中可以看出，觀測模型中位數(shù)的取值為0.08 m，均值取值為0.11 m，均方根值為0.13 m，標準差的取值范圍在［0.07，0.14］之間.研究模型的相對軌跡誤差曲線處在標準差范圍內(nèi)的共計86 s，最大誤差為0.59 m.誤差值的主要來源是GPS采樣頻率較低，因此利用誤差狀態(tài)下的KF對IMU的誤差值進行修正可以較大程度上提升研究模型整體的定位精度.綜上所述，通過KF算法融合了兩種定位傳感器的觀測數(shù)據(jù)后，定位精度得到了較大的提高，百米均方根誤差精度大概為3.02 m，而相對位姿誤差精度大概為0.12 m/s，說明本文提出的融合方案可以滿足定位精度的需求.

3" 結(jié)語

為了進一步提高多傳感融合定位導航算法的準確度，以IMU為研究對象結(jié)合KF進行融合與改進，并搭建多傳感融合的定位導航估算模型.實驗結(jié)果顯示，在對IMU解算的性能測試中，IMU的解算仿真在前6 s與理想采樣值基本吻合，而第6 s之后逐漸產(chǎn)生了誤差并隨著時間的推移誤差逐漸變大；在20 s時，歐拉積分在x軸上的誤差達到了32 m而在y軸上的誤差達到了50 m.而結(jié)合KF的IMU在x軸與z軸上與理想采樣值基本吻合，研究模型在x軸上與理想采樣值的誤差幅度在5 m以內(nèi)，最大誤差為4.8 m.在z軸上研究模型的誤差幅度在3 m以內(nèi)，最大誤差為2.6 m.估算誤差得到了大幅減少，從而證明了融合KF的IMU模型的準確度得到了很大程度的提升.利用KF算法融合IMU和GPS，構(gòu)建IMU的預測方程和融合后的觀測方程并計算誤差軌跡，最終得到的絕對軌跡誤差值和相對軌跡誤差值大部分時刻處于模型標準差區(qū)間內(nèi)，基本滿足實驗要求.由于本文測試是基于采樣后的數(shù)據(jù)進行融合，在實際情況下各傳感器之間的數(shù)據(jù)處理和傳輸存在滯后性和不同步性，因此在硬件應用上還存在一定的限制.

參考文獻：

［1］ XING H，LIU Y，GUO S，et al.A multi-sensor fusion self-localization system of a miniature underwater robot in structured and GPS-denied environments［J］.IEEE Sensors Journal，2021，21（23）：27136-27146.

［2］ LIU W，LIU Y，BUCKNALL R.Filtering based multi-sensor data fusion algorithm for a reliable unmanned surface vehicle navigation［J］.Journal of Marine Engineering amp; Technology，2023，22（2）：67-83.

［3］ WANG K，CAO C，MA S，et al.An optimization-based multi-sensor fusion approach towards global drift-free motion estimation［J］.IEEE Sensors Journal，2021，21（10）：12228-12235.

［4］ 趙航，紀新春，陸一，等.面向車輛導航的多用戶云端地磁匹配定位技術(shù)研究［J］.傳感技術(shù)學報，2022，35（6）：844-849.

［5］ 葉俊華.基于智能終端的多傳感器融合行人導航定位算法研究［J］.測繪學報，2023，52（1）：170-170.

［6］ 劉宇，賀竹川，路永樂，等.基于粒子濾波的IMU/UWB組域內(nèi)自主導航定位研究［J］.傳感器與微系統(tǒng)，2022，41（12）：47-50.

［7］ 王甘楠，田昕，魏國亮，等.基于RNN的多傳感器融合室內(nèi)定位方法［J］.計算機應用研究，2021，38（12）：3725-3729.

［8］ 董伯麟，柴旭.基于IMU/視覺融合的導航定位算法研究［J］.壓電與聲光，2020，42（5）：724-728.

［9］ CHEN G W，F(xiàn)AN Z Y，WEI Z S，et al.An attitude calculation algorithm based on WNN-EKF［J］.Journal of Measurement Sciense and Instrumentation，2022，13（2）：138-146.

［10］ 趙文龍，高建燁，何濤.融合多傳感器的自主AGV定位研究［J］.現(xiàn)代制造工程，2021（10）：85-90.

［11］ 王金柱，李駿馳，董亮，等.復雜環(huán)境下基于BP-EKF的UWB-IMU定位方法［J］.自動化技術(shù)與應用，2021，40（4）：19-23.

［責任編輯：李" 嵐］