基于改進UKF算法的移動機器人定位方法研究

摘 要：針對移動機器人在多傳感器融合定位過程中因噪聲統計特性未知或不準確引起的定位精度不高的問題，提出了一種基于Sage-Husa濾波改進的無損卡爾曼濾波（UKF）移動機器人定位算法。首先建立了移動機器人定位相關模型；然后根據噪聲統計特性時變特點利用Sage-Husa中的噪聲估計器，對狀態噪聲和量測噪聲進行自適應地估計，減小擾動噪聲給定位解算帶來的誤差；接著在狀態更新時引入收斂因子，加快算法收斂速度；最后將UKF算法和改進的UKF算法應用到實驗室移動機器人中進行仿真實驗。實驗結果表明，所提出的算法對狀態擾動具有較強的抵制能力，對機器人定位的準確性與穩定性的提升具有顯著效果。

關鍵詞：定位；無損卡爾曼濾波；多傳感器融合；Sage-Husa濾波

中圖分類號：TP242 文獻標志碼：A 文章編號：1001-3695（2022）11-016-3303-06

doi： 10.19734/j.issn.1001-3695.2022.04.0173

Research on mobile robot localization based on improved UKF algorithm

Yang Jia1，2， Peng Ruizhao1， Ji Zeyu1， Wang Jiahao1

（1.College of Electrical amp; Electronic Engineering， Chongqing University of Technology， Chongqing 400054， China; 2. Chongqing Energy Internet Engineering Technology Research Center， Chongqing 400054， China）

Abstract：This paper proposed an improved unscented Kalman filter （UKF） mobile robot localization algorithm based on Sage-Husa filter to solve the problem of low localization accuracy caused by unknown or inaccurate noise statistical characteristics in multi-sensor fusion localization process. Firstly， it established the mobile robot localization model. Then， according to the time-varying characteristics of noise statistics， this paper adaptively estimated the state noise and measurement noise to reduce the error of position calculation caused by perturbation noise. The paper also introduced the convergence factor in the state update to accelerate the convergence speed of the algorithm. Finally， it applied the UKF and the improved UKF algorithm to the laboratory mobile robot for simulation experiments. The experimental results show that the proposed algorithm has strong resistance to state disturbance， and has remarkable effect on improving the accuracy and stability of robot positioning.

Key words：localization; unscented Kalman filter; multi-sensor fusion; Sage-Husa filter

基金項目：國家自然科學基金資助項目（52177129）；重慶市教委科學技術研究重點項目（KJZD-K201901102）；重慶市技術創新與應用發展專項面上項目（cstc2020jscx-msxmX0210）；重慶理工大學研究創新項目（gzlcx20223071）

作者簡介：楊佳（1973-），女，重慶人，副教授，碩導，博士，主要研究方向為移動機器人的定位進化算法與優化、控制理論與應用；彭瑞召（1997-），男（通信作者），重慶人，碩士研究生，主要研究方向為移動機器人的定位（1585951762@qq.com）；季澤宇（1997-），男，重慶人，碩士研究生，主要研究方向為移動機器人的定位；王佳豪（1998-），男，重慶人，碩士研究生，主要研究方向為移動機器人的路徑規劃.

0 引言

《智能制造2025》和《新一代人工智能發展規劃》明確將自主智能機器人作為重點發展領域，移動機器人就是自主智能機器人的一種，其應用領域眾多，如倉儲物流、自動駕駛、服務機器人等［1～3］。而移動機器人的定位是完成諸多工作的關鍵部分，也是研究的熱點問題。

移動機器人的定位是指機器人通過感知自身和周圍環境信息，經過一定的數據處理得到自身位姿的過程［4］，即通過對不同傳感器的信息進行處理得到移動機器人的位置和姿態。使用單一的傳感器信息往往不能取得一個精確的機器人位姿，如：常利用里程計或慣性測量單元的慣性導航定位法，會隨著時間的推移產生累積誤差［5］；多利用相機［6］或者激光雷達［7］的地圖匹配定位法，會因為相機易受光線影響或激光雷達數據含有粗差估計出不精確位置信息。所以，為了提高機器人的定位精度，學者們多考慮結合慣性導航定位和地圖匹配定位的相關信息，即采用不受光線影響、測量精度高的激光雷達、里程計和慣性測量單元進行多傳感器信息融合估計出更加精確的機器人的位姿。

近年來，國內外對移動機器人的多傳感器融合定位算法進行了大量的研究，主要采用卡爾曼濾波作為多傳感器的融合算法，但卡爾曼濾波的優越性有兩個前提條件：a）系統狀態模型和量測噪聲模型都是線性的；b）系統噪聲和測量噪聲統計特性是精確已知的。而在移動機器人運動過程中，系統模型一般是非線性的，并且對于系統噪聲和測量噪聲統計特性先驗已知的條件難以滿足，所以利用本質上是線性的卡爾曼濾波方法往往不能得到理想的濾波結果。為此，研究者們又針對卡爾曼濾波方法欠缺的非線性近似能力進一步研究，并將其引入到了移動機器人的定位研究中，研究的熱點有擴展卡爾曼濾波（extended Kalman filter，EKF）［8］、無損卡爾曼濾波（unscented Kalman filter，UKF）［9］等。EKF的核心思想是利用泰勒公式只將系統狀態模型展開到二階，即通過忽略了其余的高階項對非線性移動機器人運動學系統進行線性化，然后將得到的近似線性化系統模型帶入卡爾曼濾波算法中進行計算，但當移動機器人運動學系統的非線性特性較強時，忽略的高階項造成的誤差會造成EKF的濾波精度遞降甚至無法收斂，此外，EKF在濾波的準備階段，解算非線性系統模型的雅可比矩陣（一階）和海塞矩陣（二階）是其必不可少的步驟，該步驟計算繁復且易于出錯。而UKF算法不利用泰勒級數進行線性化近似且無須計算雅可比矩陣和海塞矩陣，而是借用一組sigma點通過無跡變換（unscented transformation，UT）對系統狀態的后驗概率密度函數進行近似［10］。同EKF算法作對比，UKF 算法的非線性近似精度超過二階，當使用該算法近似化處理非線性系統狀態變量的后驗均值和協方差時，在可實現性、收斂性以及濾波精度等方面都要優于EKF算法［11］。然而，UKF依舊在傳統的卡爾曼濾波構架中，當移動機器人的系統狀態和量測模型噪聲出現不確定性時，該算法難以自適應處理該問題帶來的定位誤差，UKF的濾波精度會下降甚至無法收斂，使得移動機器人的定位精度不高甚至定位失敗。

針對UKF算法存在的缺陷，文獻［12］通過引入Sage-Husa濾波器到UKF算法中，對量測噪聲矩陣R提高了機器人導航定位精度，但忽略了對過程噪聲Q的實時估計；文獻［13］對過程噪聲和量測噪聲進行了估計解算，但其實時性較差；文獻［14］提出了一種新的盲自適應波束的CKF（cubature Kalman filter，CKF），自適應處理系統噪聲和過程噪聲并引入收斂因子，加快了算法的收斂速度，但相比標準UKF，該算法魯棒性較差。本文結合文獻［13，14］的思路，通過將Sage-Husa噪聲估計器引入UKF算法，在移動機器人位姿估計時對噪聲統計特性變化的協方差陣進行自適應的估計，同時在濾波更新時引入收斂因子，在確保定位精度不變的條件下使算法的收斂速度加快，最終使移動機器人的定位精度和穩定性都得到增強。

1 機器人定位模型分析

1.1 坐標系

將參考坐標系設定為全局坐標系XgOgZg，并以機器人中心（xg，yg）為原點，把橫軸作為機器人運動方向，建立機器人坐標系xroryr。機器人的位姿可以通過（x，y，z，ψ，θ，φ）來描述，其中（x，y，z）表示在三維直角坐標系中的坐標，（ψ，θ，φ）分別為移動機器人繞X軸、Y軸、Z軸做旋轉運動轉過的角度。本次研究的移動機器人忽略內部和輪子的關節和自由度，且假定它是在理想水平面上運動的，這樣機器人的運動狀態就可描述為（x，y，0，0，0，φ），簡稱位姿［15］；后續為了敘述方便用（x，y，φ）表示。坐標系如圖1［16］所示。

1.2 里程計模型

移動機器人的運動是根據車輪的轉動來控制的，通常在車輪的轉軸安裝光電編碼器，通過測量單位時間內的脈沖數計算出車輪的位移；同時利用慣性測量單元（inertial measurement unit，IMU）的陀螺儀進行機器人偏移角度φk進行測量。設單位時間為Δk，編碼器的參數為τ線/轉，車輪的半徑為r，在單位時間Δk內編碼器產生的脈沖數為σ，則車輪轉動的距離為ΔD=2πrσ/τ，在全局坐標系下，機器人經過時間Δk，從位置狀態Xk=（xk－1，yk－1，φk－1）T移動到位置狀態Xk=（xk，yk，φk）T的過程，如圖2所示。

經過時間Δk，通過里程計測量得到機器人左輪和右輪移動距離分別表示為ΔDL、ΔDR，那么機器人移動距離ΔDk、轉過的角度Δθk和機器人運動半徑R分別為

其中：σL、σR分別為機器人左右輪編碼器產生的脈沖數；l為輪距。

系統運動模型輸入量為uk=（ΔDk，Δθk）T，下一時刻的位姿Xk分為兩種情況：a）直線運動模型，即轉過的角度|Δθk|=0；b）運動軌跡為曲線，把曲線細分為極短時間的運動，即真實運動曲線采用圓弧模型去逼近，這樣的逼近效果優于直線模型［15］。

因為里程計的測量過程會受到噪聲的影響，所以里程計輸入量uk=（ΔDk，Δθk）T不準確。隨著時間的延長，累積的誤差會使機器人的軌跡會產生漂移，其定位的精度會迅速惡化。故增加二維的激光雷達的信息與之信息融合，糾正移動機器人的位姿。

1.3 基于地圖已知的激光雷達定位模型

二維激光雷達具有良好的感知平面環境的性能。從激光數據集中提取特征就可以描述機器人所處的周圍環境，角點特征識別提取相對簡單可靠，通過使用文獻［17，18］的方法對角點進行提取，并構建出高精度角點特征地圖，該地圖上的角點位置參數可以精確表示真實物理角點所在的位置，且地圖上的角點特征的坐標精確已知。因此，當機器人在移動過程中檢測到角點，便會根據先驗角點特征地圖得到當前檢測的角點位置坐標，然后根據二維激光雷達測量的機器人到角點距離和角度信息，解算出機器人的位置。進一步校正機器人里程計模型帶來的誤差，提高機器人的定位精度。

考慮角點特征地圖精確已知的情況，角點可以用來描述環境，這里相當于路標，用二維點表示［17］。定義第i號路標為［ai bi］，i=1，2，…，N，其中N表示總路標個數。機器人與路標關系如圖3所示。

圖3中，cik、εik分別表示機器人與路標的距離和角度信息，（xk，yk，φk）為機器人k時刻的位置狀態，由激光雷達直接測量得到，兩參數與機器人的位置關系滿足

由式（3）（4）可以進一步解算出機器人的位置，因此可以通過角點的位置來糾正移動機器人的位姿。

2 建立機器人模型

2.1 狀態方程

設移動機器人的狀態方程為

其中：Xk－1、Xk分別表示移動機器人在k－1時刻和k時刻的位姿；Wk為過程噪聲；φk，k－1為移動機器人從k－1時刻到k時刻的狀態轉移矩陣。

ay（k）分別表示機器人在k時刻，在機器人坐標系下的X軸方向和Y軸方向上的位置、速度和加速度。

機器人的狀態轉移矩陣φk，k－1為

其中：T為采樣周期。

2.2 觀測方程

根據傳感器里程計和雷達所觀測的信息建立移動機器人的觀測方程，由式（1）可得

其中：ω為機器人的行駛角速度。聯立方程消去ω得機器人左輪和右輪的脈沖數：

又由式（3）移動機器人位置（xk，yk）到觀測點位置（ai，bi）的距離可以表示為

將移動機器人到觀測點的距離cik和安裝在左右輪的編碼器的輸出的脈沖σL、σR作為觀測量，觀測方程可以表示為

其中：Zk為觀測向量；h（·）為量測函數；Vk為量測噪聲；設過程噪聲Wk具有協方差陣Qk，量測噪聲Vk具有協方差陣Rk，其統計特性滿足Wk和Vk不相關且各自的均值為零，即

3 標準 UKF及改進UKF算法

標準UKF算法的步驟如下：

a）初始化。

定義初始時刻狀態變量估計值X^0等于X0的均值，其均值和方差分別表示為X^0、P0。

b）計算sigma點。

對于一個均值為、方差為P的n維狀態向量x，利用以下公式計算得到2n+1個sigma點χ以及各點對應的權值w。

4 仿真實驗

為了驗證本文提出的基于Sage-Husa濾波改進的無損卡爾曼濾波（UKF）移動機器人定位算法的有效性，本文采用實驗室搭建的移動機器人作為實驗平臺，其中里程計（型號：美國MMP公司的DC Gearmotors系列）安裝在兩驅動輪上，IMU（型號：CHR-UM6）位于車體中，激光雷達型號為思嵐A1增強版，本實驗平臺的傳感器以及底盤參數已經過校驗，避免了因為驅動控制誤差影響算法質量的驗證，如圖5所示。運行環境為體育館室內平坦的路面，利用ROS（robot operating system）對傳感器的數據進行獲取，然后把數據代入MATLAB進行改進的UKF和標準的UKF在過程噪聲與觀測噪聲統計特性兩者均未知時的仿真，相關參數設置如表1所示。

4.1 狀態噪聲和量測噪聲統計特性的控制精度對比

在MATLAB軟件中進行編程仿真，檢驗改進的UKF和標準的UKF對系統狀態噪聲和量測噪聲統計特性的控制精度。仿真過程中移動機器人的速度設置為東北向45° 0.5 m/s恒速運行，圖6、7分別為標準UKF和改進的UKF位移噪聲控制精度，對比圖6、7的位移噪聲，可見隨著濾波的進行，標準的UKF算法使x軸的位移噪聲維持在-0.55～0.55 cm，y軸的位移噪聲-0.60～0.60 cm；而改進的UKF算法使x軸的位移噪聲基本維持在-0.30～0.30 cm，y軸位移噪聲在-0.35～0.35 cm，x軸和y軸噪聲位移控制精度分別提高了約45.4%、41.7% 。

圖8、9分別為標準UKF和改進的UKF速度噪聲統計特性控制精度對比，可知速度位移噪聲因為受統計特性的先驗信息不準確的影響，濾波的初始階段，標準的UKF使x軸和y軸的速度位移噪聲分別維持在-0.2～0.2 cm/s和-0.20～0.15 cm/s；改進的UKF使x軸和y軸的速度位移噪聲分別維持在-0.12～0.12 cm/s、-0.12～0.12 cm/s，x軸和y軸噪聲速度控制精度提高了約40%，隨著濾波的進行（約60步長后），速度噪聲濾波完全收斂。圖10為改進的收斂UKF估計速度噪聲曲線。

圖10、11分別為標準UKF和改進的UKF觀測距離噪聲統計對比。由圖可見，標準UKF和改進的UKF的觀測距離噪聲分別為-0.30～0.30 cm、-0.20～0.2 cm，觀測距離噪聲的控制精度提高了33.3%。

4.2 收斂因子的驗證

為了增強系統的實時性，利用收斂因子μ加快本文算法收斂速度，引入收斂因子后，以速度噪聲為例，如圖12所示。對比圖12和9可以看出，引入收斂因子后，濾波速度加快，x軸和y軸的速度噪聲分別75和55步長時收斂，而未引入收斂因子之前x軸和y軸的速度噪聲分別在105、90步長時收斂，在x軸和y軸收斂速度分別提高了28.5%和38.8%。從控制精度方面分析，引入的收斂因子不僅在一定程度上加快了算法的收斂速度，而且對濾波精度的影響可以忽略不計，因此驗證了收斂因子的有效性。

4.3 位置估計實驗

為了進一步確認本文提出的改進UKF算法的位置估計性能，將本文算法與標準UKF濾波算法、AFUKF算法［22］、carto-grapher算法（表2中簡稱CARTO）進行位置估計實驗、實驗預設路徑為直徑為1 000 cm的圓形，選取預設軌跡的4個位置：A點（0，0）、B點（500，500）、C點（0，1000）、D點（-500，500），控制機器人跟蹤預設的路徑運行50次，各點的位置估計的數據如表2所示，表中單位均為cm。位置評判標準如下：

其中：E表示平均歐氏距離誤差；Ns表示實驗總次數，取50；xk、yk表示機器人位置的橫、縱坐標；x*k、y*k表示機器人預設路徑的真實橫、縱坐標。

由此可見，與標準UKF濾波算法、AFUKF算法、cartographer算法相比，本文提出改進UKF算法誤差率穩定在7%以內，具有更高的準確性和穩定性。

5 結束語

本文主要對移動機器人多傳感器融合定位過程中噪聲作出處理，針對噪聲統計未知或不確定的情況，采用基于Sage-Husa濾波改進UKF算法對移動機器人定位過程中狀態噪聲和量測噪聲的協方差進行實時的估計更新，提高了移動機器人的定位精度，此外，通過引入收斂因子保障了本文改進的算法的收斂速度；從仿真實驗結果來看，本文方法與標準UKF相比，更好地將過程噪聲和量測噪聲控制到較小的范圍，對于擾動噪聲有較強的抑制能力，將收斂因子進行合理化的取值在一定程度上加快了算法收斂速度，讓移動機器人定位的準確性和穩定性均更優。下一步研究方向為圖優化類的移動機器人定位算法。

參考文獻：

［1］He Wei，Li Zhijun，Philip C L，et al. Survey of human-centered intelligent robots： issues and challenges ［J］. IEEE/CAA Journal of Automatica Sinica，2017，4（4）： 602-609.

［2］Baraglia J，Cakmak M，Nagai Y，et al. Efficient human-robot collaboration： when should a robot take initiative？ ［J］. International Journal of Robotics Research，2017，36（5-7）： 563-579.

［3］Ajoudani A，Zanchettin A M，Ivaldi S，et al. Progress and prospects of the human-robot collaboration ［J］. Auton Robots，2018，42（5）： 957-975.

［4］吳鵬，李東京，贠超. 一種慣性傳感器與編碼器相結合的AGV航跡推算系統 ［J］. 機電工程，2018，35（3）： 310-316. （Wu Peng，Li Dongjing，Yun Chao. A AGV trajectory calculation system combining inertial sensors and encoders ［J］. Electromechanical Engineering，2018，35（3）： 310-316.）

［5］Su Yun，Wang Ting，Shao Shiliang，et al. GR-LOAM： LiDAR-based sensor fusion SLAM for ground robots on complex terrain ［J］ . Robotics and Autonomous Systems，2021，140（5）： 103759.

［6］許智賓，李宏偉，張斌，等. 雙目視覺與慣導融合的移動機器人定位方法 ［J］. 測繪學報，2021，50（11）： 1512-1521. （Xu Zhibin，Li Hongwei，Zhang Bin，et al. Binocular vision and inertial navigation fusion of mobile robot localization method ［J］. Plotting Journal，2021，50（11）： 1512-1521.）

［7］賈曉輝，徐文楓，劉今越，等. 基于慣性測量單元輔助的激光里程計求解方法 ［J］. 儀器儀表學報，2021，42（1）： 39-48. （Jia Xiaohui，Xu Wenfeng，Liu Jinyue，et al. Laser odometer solution method based on inertial measurement unit ［J］. Journal of Instruments，2021，42（1）： 39-48.）

［8］謝良波，劉西西，王勇，等. 基于RFID載波相位的室內EKF定位算法 ［J］. 通信學報，2022，43（3）： 124-134. （Xie Liangbo，Liu Xixi，Wang Yong，et al. Indoor EKF positioning algorithm based on RFID carrier phase ［J］. Journal of Communication，2022，43（3）： 124-134.）

［9］趙玏洋，閆利. 移動機器人SLAM位姿估計的改進四元數無跡卡爾曼濾波 ［J］. 測繪學報，2022，51（2）： 212-223. （Zhao Leyang，Yan Li. Improved quaternion unscented Kalman filter for SLAM pose estimation of mobile robots ［J］. Plotting Journal，2022，51（2）： 212-223.）

［10］蔡煊，王長林. 基于改進UKF的BDS/IMU組合列車定位方法 ［J］. 西南交通大學學報，2020，55（2）： 393-400. （Cai Xuan，Wang Changlin. BDS/IMU combined train positioning method based on improved UKF ［J］. Journal of Southwest Jiaotong University，2020，55（2）： 393-400.）

［11］Julier S J，Uhlmann J K. Unscented filtering and nonlinear estimation ［J］. Proceedings of the IEEE，2004，92（3）： 401-422.

［12］馬宏偉，張璞，毛清華，等. 基于捷聯慣導和里程計的井下機器人定位方法研究 ［J］. 工礦自動化，2019，45（4）： 35-42. （Ma Hongwei，Zhang Pu，Mao Qinghua，et al. Research on positioning method of downhole robot based on strapdown inertial navigation and odometer ［J］. Industry and Mine Automation，2019，45（4）： 35-42.）

［13］王君婷，王振杰. 改進的抗差UKF在AUV水下聲學導航中的應用 ［J］. 導航定位學報，2018，6（2）： 103-110. （Wang Junting，Wang Zhenjie. Application of improved anti-difference UKF in AUV underwater acoustic navigation ［J］. Journal of Navigation and Positioning，2018，6（2）： 103-110.）

［14］錢華明，劉可，馬俊達. 基于自適應CKF的恒模盲波束形成算法 ［J］. 系統工程與電子技術，2016，38（6）： 1258-1261. （Qian Huaming，Liu Ke，Ma Junda. Constant modulus blind beamforming algorithm based on adaptive CKF ［J］. System Engineering and Electronic Technology，2016，38（6）： 1258-1261.）

［15］蔣正鋼. 基于激光雷達的巡檢機器人自主定位技術研究與應用 ［D］. 成都： 電子科技大學，2019. （Jiang Zhenggang. Research and application of autonomous positioning technology of inspection robot based on laser radar ［D］. Chengdu： University of Electronic Science and Technology of China，2019.）

［16］趙海鵬，杜玉紅，丁娟，等. 移動機器人中激光雷達測距測角標定方法 ［J］. 紅外與激光工程，2019，48（6）： 371-378. （Zhao Haipeng，Du Yuhong，Ding Juan，et al. Calibration method for laser radar ranging and angle measurement in mobile robots ［J］. Infrared and Laser Engineering，2019，48（6）： 371-378.）

［17］滿增光，葉文華，肖海寧. 從激光掃描數據中提取角點特征的方法 ［J］. 南京航空航天大學學報，2012，44（3）： 379-383. （Man Zengguang，Ye Wenhua，Xiao Haining. Method for corner feature extraction from laser scan data［J］. Journal of Nanjing University of Aeronautics amp; Astronautics，2012，44（3）： 379-383.）

［18］程榮. 基于多胞體集員估計的機器人定位與建圖研究 ［D］. 武漢： 華中科技大學，2020. （Cheng Rong. Robot localization and mapping based on multi-cell collective estimation ［D］. Wuhan： Huazhong University of Science and Technology，2020.）

［19］黃小平，王巖. 卡爾曼濾波原理及應用-MATLAB仿真 ［M］. 北京： 電子工業出版社，2016： 103-115. （Huang Xiaoping，Wang Yan. Kalman filtering principle and application-MATLAB simulation ［M］. Beijing： Electronic Industry Press，2016： 103-115.）

［20］魏偉，秦永元，張曉冬，等. 對Sage-Husa算法的改進 ［J］. 中國慣性技術學報，2012，20（6）： 678-686. （Wei Wei，Qin Yongyuan，Zhang Xiaodong，et al. The improvement of Sage-Husa algorithm ［J］. China Journal of Inertial Technology，2012，20（6）： 678-686.）

［21］Liu Jianfeng，Sun Lifan，Pu Jiexin，et al. Hybrid cooperative localization based on robot-sensor networks［J］. Signal Processing， 2021，188： 108242.

［22］王秉洲，王慧斌，沈潔，等. 基于自適應漸消UKF的FastSLAM算法 ［J］. 計算機科學，2020，47（9）： 213-218. （Wang Bingzhou，Wang Huibin，Shen Jie，et al. FastSLAM algorithm based on adaptive fading UKF ［J］. Computer Science，2020，47（9）： 213-218.）