基于交互式多模型算法的溫棚內機器人定位研究

中圖分類號： TP249 文獻標志碼：A 文章編號：1000-5137（2025）02-0250-09

Abstract：Toaddressthe issues of positioning inaccuracies caused bycumulative erors inodometer（ODOM）and inertial measurementunit（IMU），aswellasnon-line-of-sighterrors inultra-wideband（UWB），aninteractive multi-modelbasedon improvedextendedKalman filter（IMM-IEKF）algorithmwasproposed.Bymeansof thisalgorithmmulti-sensor fusion localizationwasachievedforrobotswithingreenhouse.Firstly，asliding window wasintroduced toprocess measurement informationandadjustthecovariance matrix，whileenhancing thetraditional extendedKalman filter（EKF）.Secondly，the interactive multi-model（（IMM）algorithmwasincorporated，andconstantvelocity（CV）andconstantturn（CT）motionmodels weredesigned.Finally，theimprovedextendedKalmanfilter（IEKF）algorithmwasusedasthefilterof theIMMframework， enablingthefusionlocalizationofUWB，IMUandodometrydatathroughtheIMM-IEKFalgorithm.Experimentalresults demonstrated thatthe proposed IMM-IEKFalgorithmachieved superior positioning performance.Comparedto single UWB， traditional EKF，and IEKF，the positioning accuracy was improved by 49.4% ， 28.8% ，and 18.2% ，respectively，highlighting significant application value.

Key Words：greenhouse；multi-sensor fusionlocalization；extendedKalman filter（EKF）algorithm；interactive multi-model （IMM）algorithm

0引言

常見的機器人定位技術，如全球衛星導航系統（GNSS）、里程計（ODOM）等在復雜的溫室環境中，無法獲取較高精度的定位信息[1-3].目前，超寬帶（UWB）技術在室內定位中展現出高精度、低功耗和高穩定性的優勢，但在遮擋情況下會產生非視距（NLOS）誤差，影響定位精度[4l.為克服這一挑戰，引入了基于慣性測量單元（IMU）的慣性導航系統（INS），該系統能實時測量載體的運動狀態并推算位置，但存在誤差累積問題[5].而未被遮擋的UWB定位信息又可以為IMU提供校準，體現較好的互補性，能有效彌補單一定位方式的不足[6.因此，基于UWB和IMU進行室內融合定位的方法得到了廣泛關注.

然而在實際應用中，UWB和IMU的融合定位仍存在噪聲干擾、精度不足等問題，當前研究聚焦于優化測量數據和改進融合算法，以提高定位精度和穩定性.ZENG等提出通過信道沖激響應檢測結合支持向量機（SVM）訓練獲取準確測量值，通過擴展卡爾曼濾波（EKF）算法融合IMU和UWB，實現定位.GUO等8]濾除NLOS信息，校正IMU的累計漂移，提出改進UWB和IMU松耦合的EKF融合定位算法.NAHEEM等[9通過步長和航向信息對UWB定期校準，并在慣性導航誤差框架下，通過可變噪聲方差卡爾曼濾波算法融合UWB和IMU.此外，機器人運動狀態具有復雜性，單一運動模型難以準確匹配其運動狀態，從而影響定位精度.交互式多模型（IMM）算法針對目標可能存在的多個模型，通過模型濾波估計、概率更新和加權計算，實現對運動目標狀態的實時定位跟蹤，已應用于車輛及無人機等領域[0-13].YUNITA等[14]提出交互式多模型-卡爾曼濾波（IMM-KF）算法，對車輛進行定位，仿真結果表明，勻速-勻轉速（CV-CT）濾波模型誤差最小，但該模型在非線性系統中定位效果較差.陳曉楠等[15]提出基于EKF算法的交互式多模型跟蹤（IMM-EKF）算法，對IMM濾波器中的狀態預測協方差矩陣進行重構，實現毫米波雷達對行人軌跡的跟蹤.楊秀建等[16]提出了一種復雜環境的UWB/LiDAR/IMU組合定位方法，先設計時變因子進行UWB/LiDAR重定位，再結合IMU通過IMM-UKF進行位置結算，該方法定位精度較高，但成本較高，計算量較大.

基于以上研究背景，本文在UWB/IMU融合定位的基礎上，提出一種基于改進擴展卡爾曼濾波的交互式多模型（IMM-IEKF）算法來實現機器人的高精度定位，在UWB/IMU的基礎上增加ODOM，獲取輪速信息以推算機器人運動狀態，并引入滑動窗口處理觀測數據調整協方差矩陣，提出改進的EKF（IEKF）算法;根據機器人的運動特性，設計CV和CT兩種運動模型，結合IMM算法估計機器人的最優狀態，實現機器人在溫室復雜環境下的高精度定位.

1 IEKF算法

EKF算法是處理傳感器融合的經典算法之一，該算法通過局部線性化近似處理，逼近系統的真實狀態，能充分利用不同傳感器信息，適合于實時性要求較高的應用場景.針對EKF算法中存在的數據波動問題，本文提出改進方法，通過引入滑動窗口機制，處理觀測數據，增強數據間聯系，減小波動，并動態調整協方差矩陣，提升濾波效果和系統的實時性及穩健性.圖1為基于滑動窗口的IEKF算法流程.先設定窗口大小為 N ，再判斷數據緩存是否超出窗口，然后進行有效值篩選，最后計算加權平均值并調整協方差矩陣，進行狀態更新.

有效測量的篩選過程如下：

式中： μ 為觀測值的均值； σ 為觀測值的標準差； x 為觀測值； y 為有效觀測值集合.通過式（3）進行有效值的篩選.

加權平均的計算和協方差矩陣的調整流程如下：

式中： M 為有效觀測值的數量； ω_j 為有效觀測值的權重； 為有效觀測值的加權平均； R_adj 為調整后的協方差矩陣； R 為初始化協方差矩陣.

在計算加權平均時，考慮根據數據實時性，對新舊數據賦予不同權重，使新數據權重更高;在協方差矩陣的動態調整中，若有效觀測值的數量減少，則表明當前數據集的不確定性和波動性相應增大，對此，增大參數 R 的數值以反映這種增高的不確定性.最終，將處理后的協方差矩陣 R_adj 和加權平均 帶入進行狀態更新，并進行下一次的數據檢測.

2 基于IMM的多傳感器融合定位

2.1 運動模型

由于本文中機器人的運動過程以勻速直線和轉彎為主，采用CV模型和CT模型作為機器人的運動模型，并將其狀態向量分別記為 X_cv 和 X_cT ，狀態轉移矩陣分別記為 F_cv 和 F_CT ，

式中 ：x，y，θ，ν，ω ， d 分別表示機器人的橫坐標、縱坐標、航向角、線速度、角速度及采樣周期，

2.2 IMM-IEKF算法

以IEKF作為IMM算法中的狀態濾波器，構建IMM-IEKF算法框架，如圖2所示.

2.2.1 輸入交互

首先確定當前時刻模型的概率，以及下一時刻的轉移概率，如圖3所示，運動模型間的概率轉換符合馬爾科夫模型概率轉移關系.其中， M_ij ， i=1，2，j=1 ，2，表示車輛由運動模型 i 轉換到運動模型 j 的概率.

在k-1時刻，設模型i的模型概率為 μ_i（k-1） ，則各模型的預測概率歸一化常數

模型 i 到模型 j 的混合概率為：

根據輸入的狀態估計 x_i1（k-1） 和 x_i2（k-1） ，計算模型 j 的混合狀態估計為：

x_ij（k-1）=Σ_i=1^Nx_i（k-1）μ_ij（k-1）.

2.2.2 狀態濾波

以IEKF算法作為IMM算法的濾波器，根據上述混合狀態估計 x_ij（k-1） 和觀測輸入 z（k-1） 進行濾波，計算狀態向量預測值與觀測值的向量殘差 y_j（k） 、信息觀測預測協方差矩陣 S_j（k） ，以及各模型的狀態估計 x_j（k）

y_j（k）=z（k-1）-H（k）x_ij（k-1），

S_j（k）=H（k）P（k-1）H（k）^T+R（k），

x_j（k）=x_j（k-1）+K_j（k）y_j（k），

式中： H（k） 為 k 時刻觀測矩陣； P（k-1） 為 k-1 時刻的狀態估計誤差協方差矩陣； K_j（k） 為 k 時刻第 j 個模 型的卡爾曼增益矩陣； R（k） 為 k 時刻觀測噪聲協方差矩陣.

2.2.3 模型概率更新

完成狀態濾波后，通過計算模型的近似函數 A_j ，更新運動模型概率 μ_j（k）

式中：c為歸一化常數，

2.2.4 估計融合

利用2.2.3節更新的模型概率對2.2.2節中車輛運動模型的估計 x_j（k） 進行狀態估計組合，最終得到機器人的定位結果：

x（k）=Σ_j=1²x_j（k）μ_j（k）.

3 仿真與實驗驗證

3.1 仿真分析

在仿真實驗中，設定仿真目標原點為 （0，0） ，先后進行勻速、勻轉向、勻速及勻轉向運動，勻速線速度為 0.2m?s^-1 ，勻轉向角速度為 0.2rad?s^-1 ，采樣周期 T=0.02s ，初始狀態為 [0000.20]^T ，模型轉移矩陣為 μ=[0.8 0.2].圖4為仿真實驗中真實軌跡和三種定位算法的軌跡對比圖.從圖4的局部放大圖可以看出，本文所提出的IMM-IEKF算法定位結果最接近真實軌跡，且IEKF算法的定位效果優于傳統EKF算法.

圖5為3種定位算法在采樣點位置的均方根誤差（RMSE）對比圖.從圖5中可以看出，整體上本算法在采樣點位置的RMSE值均小于其他算法，且IEKF算法在采樣點位置的均方根誤差均小于EKF算法.

表1給出了定位RMSE值和定位精度，可以看出，本算法的定位精度可達 0.0392m ，相較于單一UWB、傳統EKF和IEKF算法，分別提高了 57.2%，27.3% 和 7.8% ，其中IEKF算法的定位效果要優于傳統的EKF算法，精度提升了 21.2% ，進一步說明了IEKF算法的有效性.

3.2 實驗驗證

開展實際測試，用于驗證算法的性能.圖6為本文實驗測試平臺，在AutoLabor機器人上搭載AutoLaboros和ROS-noetic系統環境的工控機、AH10OB慣導和UWB定位標簽，UWB定位基站及標簽采用搭載DW3210定位芯片的廣州聯網科技的D-DWM-PG3.9產品，定位精度誤差為 ±0.10m

本實驗在配備高精度Vicon光學動作捕捉系統的實驗室中進行，通過紅外高速攝像機捕捉安裝在機器人上的發光標記點，作為機器人運動軌跡的真實值.測試平臺與實驗場景如圖7所示.通過Vicon光學動作捕捉系統對UWB基站進行位置標定，以UWB坐標系為基準坐標系，定位標簽布置于機器人的幾何中心，定位基站布置在大小為 2.5m×4.0m 的矩形區域中.

圖8和圖9為實驗中機器人定位軌跡和采樣點的單點定位誤差對比圖.可以看出，相比于EKF和IEKF算法，本算法最終定位軌跡與真實軌跡貼合度最高，在采樣點處的定位誤差最小.

表2給出了實驗中機器人定位RMSE值和定位精度，可以看出，本算法的定位精度可達 0.0468m 相較于單一UWB、傳統EKF和IEKF算法，定位精度分別提高了 49.4% 28.8% 和 18.2% ，其中，IEKF算法的定位效果優于傳統的EKF算法，定位精度提升了 12.9%

4結語

本文針對溫室機器人由于ODOM和IMU的累計誤差，以及UWB非視距誤差等影響，導致定位不準的問題，提出IMM-IEKF算法，將滑動窗口引人EKF的狀態更新過程，處理測量信息并調整協方差矩陣，將IEKF算法作為IMM算法的濾波器，結合CV模型和CT模型，實現對機器人的狀態估計.仿真和實際實驗結果表明，本算法能顯著提升機器人定位精度.未來的研究將聚焦于對算法的進一步優化，以應對真實系統中更為復雜的動態環境和多變的操作條件，并致力于將定位系統穩定地融人智能農業生態系統中，實現數據共享.

參考文獻：

[1］程雪聰，劉福才，黃茹楠.基于卡爾曼濾波和粒子濾波融合的UWB室內定位算法[J].計量學報，2022，43（10）： 1335-1340. CHENGX C，LIUFC，HUANG RN. UWB indor positioning algorithmbasedon Kalmanfilter and particle filterfusion[J]. ActaMetrologicaSinica，2022，43（10）：1335-1340.

[2]KARNfK J，STREIT J. Summary of available indor location techniques [J]. IFAC-PapersOnLine，2016，49（25）： 311-317.

[3]LIS K，WANG X，CAOYD，etal.Self-supervised deep visual odometry with online adaptation[C]//2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition.Seattle： IEEE，2020：6338-6347.

[4］楊招兵.基于視覺和超寬帶融合的室內跟蹤定位系統[D].上海：上海交通大學，2020. YANG ZB.An indoor tracking and localization system with vision and ultra-wideband（UWB）[D].Shanghai： Shanghai Jiao Tong University，2020.

[5]IASECHKO M，SHELUKHINO，MARANOV A，et al.Evaluation ofthe use of inertial navigation systems to improve theaccuracyof object navigation[J].International Jourmal of Computer Scienceamp; Network Security，2O21，21（3）： 71-75.

[6] 馬曉爽，劉錫祥，張同偉，等.基于因子圖的AUV多傳感器組合導航算法[J].中國慣性技術學報，2019，27（4）： 454-459. MAX S，LIUX X，ZHANG TW，et al.AUV multi-sensor integrated navigationalgorithm based on factor graph [J]. Journal of Chinese Inertial Technology，2019，27（4）：454-459.

[7]ZENG Z Q，LIUS，WANG L.NLOS detection and mitigation forUWB/IMU fusion systembasedon EKF and CIR[C]/ 2018 IEEE 18th International Conference on Communication Technology. Chongqing： IEEE，2018：376-381.

[8]GUOSL，ZHANGYT，GUIX Z，et al.Animproved PDR/UWB integrated systemfor indor navigationappications[J]. IEEESensors Journal，2020，20（14） ：8046-8061.

[9] NAHEEMK，KIMMS.Alow-cost fot-placed UWBand IMU fusion-based indoorpedestriantracking systemforIoT applications[J].Sensors，2022，22（21）：8160.

[10]CHOIJ，PARK J，HUHK.Robustobject tracking against sensorfilures withcentralized IMMfilter[J].IEEE Access， 2024，12：85203-85218.

[11] 王法安，殷國棟，莊偉超，等.基于多基站IMMKF-DOA輔助車輛協同定位算法研究［J].機械工程學報，2023， 59（4）：125-134. WANG FA，YIN G D，ZHUANG W C，et al.IMMKF-DOA auxiliary vehicle cooperative localization algorithm based on multi-base station[J]. Journal of Mechanical Engineering，2023，59（4）：125-134.

[12] 紀冰輝，王若楠，周陬．一種基于無人機蜂群的目標跟蹤算法[J].桂林電子科技大學學報，2023，43（3）： 231-238. JI B H，WANG R N，ZHOU Z.A target tracking algorithm based on UAV swarm [J].Journal of Guilin University of Electronic Technology，2023，43（3）：231-238.

[13]ZENGY，LU WB，YUB，et al. Improved IMM algorithm basedonsupprt vectorregresson forUAV tracking［J]. Journal of Systems Engineering and Electronics，2022，33（4）：867-876.

[14]YUNITA M，SURYANA J，IZZUDDIN A.Eror performance analysis of IMM-Kalman filter for maneuvering target trackingapplication[C//20206th InternationalConferenceonWirelessandTelematics.Yogyakarta：IEEE，2020：1-6.

[15] 陳曉楠，張子闊，索繼東，等.基于擴展卡爾曼濾波的交互式多模型跟蹤算法研究[J].現代電子技術，2024， 47（13）：71-76. CHEN XN，ZHANG Z K，SUO JD，et al. Research on interacting multiple model tracking algorithmbased on extended Kalman filter[J].Modern Electronics Technique，2024，47（13）：71-76.

[16]楊秀建，敖鵬，沈世全，等.面向復雜環境的UWB/LiDAR/IMU組合定位方法[J].中國慣性技術學報，2024， 32（7）：654-662. YANG X J，AO P，SHEN S Q，et al. UWB/LiDAR/IMU combined localization method for complex environment [J]. Journal of Chinese Inertial Technology，2024，32（7）：654-662.

（責任編輯：包震宇，顧浩然）