基于單UWB融合里程計的多機器人相對定位方法

摘 要：針對衛星信號受阻，無預設基礎設施（定位基站、地標等）環境下多機器人間的相對定位問題，提出了一種基于單個超寬帶（ultra-wideband，UWB）融合里程計的多機器人相對定位方法。該方法利用滑動窗口截取歷史時刻的多組機器人間測距信息與里程計預測的機器人位姿，構建非線性最小二乘問題，實現機器人間的相對位姿估計；利用擴展卡爾曼濾波算法估計里程計協方差，并將其以加權的方式運用于非線性優化，抑制滑動窗口內里程計累積誤差對定位結果的影響；最后，利用圖優化算法融合里程計與非線性優化獲得的相對位姿作進一步優化，抑制UWB測量誤差影響，以獲得穩定的相對定位結果。實驗結果表明，在6 m×12 m的真實測試環境中，所提方法能夠獲得0.32 m的相對位置精度和4.16°的相對角度精度，相比于現有多機器人相對定位方案，該方法具有高精度、低成本、部署簡單以及定位穩定的優點。

關鍵詞：相對定位；非線性優化；滑動窗口；擴展卡爾曼濾波；圖優化

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

文章編號：1001－3695（2023）03－032－0839－06

doi：10.19734/j.issn.1001-3695.2022.07.0381

Relative localization method for multiple robots based on

single UWB fusion odometry

Deng Zhongyuana，b，Liu Rana，b，Cao Zhiqianga，Xiao Yufenga，b

（a.School of Information Engineering，b.Robot Technology Used for Special Environment Key Laboratory of Sichuan Province，Southwest University of Science amp; Technology，Mianyang Sichuan 621000，China）

Abstract：Aiming at the problem of relative positioning among multiple robots in GPS-denied，no pre-set infrastructure （positioning base stations，landmarks，etc.） environment，this paper proposed a multi robot relative positioning method based on a single ultra-wideband （UWB） fusion odometry.To obtain relative pose estimates between robots，the method constructed a nonlinear least squares problem by utilizing sliding window to intercept range information between robots and poses predicted by odometry over a period of time.In addition，the method estimated the odometry covariance by utilizing extended Kalman filter and weighted it in a nonlinear optimization to suppress the cumulative odometry error in a sliding window.Finally，it used the general graph optimization（g2o） algorithm to further optimize the relative position and attitude obtained by integrating odometry and nonlinear optimization，which effectively suppressed the influence of UWB measurement error，so as to obtain stable relative positioning results.The experimental results show that the proposed method can achieve an average relative position accuracy of 0.32 m and relative angle accuracy of 4.16° in an indoor environment with the size of 6 m×12 m.Compared with the existing multiple robot relative positioning schemes，the proposed method has the advantages of high accuracy，low cost，simple deployment and strong robustness.

Key words：relative localization；nonlinear optimization；sliding window；extended Kalman filter；general graph optimization

0 引言

多機器人相對定位是實現多機器人協同作業的重要前提［1］。目前，在室外環境中北斗或全球定位系統（global positioning system，GPS）常被用做機器人定位［2］；在衛星信號受阻的室內環境中，預先安裝的傳感器網絡同樣能夠被用做機器人定位［3，4］。但在某些場景中這些外部信息并不存在，且預先設置定位基站需要很大的布置成本［5］，因此在無外部信息的環境下，利用機器人自身攜帶的傳感器實現機器人間的相對定位是必要的。

激光雷達和視覺傳感器常被用于機器人相對定位［6，7］，但激光和視覺信號受環境中障礙物的影響，在有障礙物遮擋或特征稀疏的長走廊等環境下定位精度較差。無線信號對障礙物具有一定的穿透能力，因此無線技術（如ZigBee、Wi-Fi、UWB等）被廣泛應用于室內復雜環境下的定位。ZigBee和Wi-Fi在定位應用中常采用信號強度進行定位［8，9］，由于通信帶寬有限與信號衰減等問題，其通常只能獲得米級的定位精度，不適用于高精度定位應用場景。UWB傳感器具有精度高、功耗低、寬頻帶以及多徑分辨能力強等優點，被廣泛應用于機器人的精確定位［10］，但是UWB通常僅提供距離測量值，單個測距值無法實現機器人間的相對位姿估計。

目前，UWB定位常采用多UWB方案和UWB與多傳感器融合方案。其中，多UWB定位方案具有較好的定位精度和較強的實時性，但由于在部署時需要考慮各UWB之間的間距，不適用于體積較小的機器人。例如，Nguyen等人［11］通過搭載兩個UWB節點的地面移動平臺對一個搭載四個UWB節點的飛行器進行相對定位；Cao等人［12］通過每個機器人上搭載的具有一定間隔的多個UWB節點實現機器人間的相對定位。UWB與多傳感器融合方案在小型機器人和無人機上應用廣泛。在單機器人定位應用中，Cao等人［13］提出了一種基于單UWB錨點的定位方案，通過單UWB錨點和九軸慣性傳感器（inertial measurement unit，IMU）融合實現對單機器人的定位跟蹤，該方法需要預先知道機器人相對于UWB錨的起始位姿；劉宇等人［14］提出一種UWB融合PDR（pedestrian dead recko-ning）的定位方案，將航跡推算結果與三個固定的UWB基站所估計的機器人位姿進行融合，在百米定位測試中獲得了0.33 m的定位精度。在多機器人應用中，Zhou等人［15］將單UWB、里程計和深度相機進行多傳感器融合，實現了無人機蜂群在復雜環境下的定位；Qi等人［16］通過融合UWB、IMU與GPS信息實現了多無人機間的相對定位。在當前研究中，UWB與多傳感器融合方案通常采用固定UWB錨點，且需要一定的先驗信息（如機器人的起始位姿以及通過激光或視覺傳感器提供的先驗地圖信息），在獲得精確定位的同時增加了定位的成本和定位部署的局限性。

里程計或IMU傳感器在長時間運行中存在累積誤差，嚴重影響定位精度。為消除或減少累積誤差影響，He 等人［17］通過將里程計與視覺信息融合以修正里程計累積誤差實現準確的位置估計，然而引入過多的傳感器會增加系統的計算量和布置成本。Hidalgo-Carrió等人［18］提出一種利用高斯過程建模里程計誤差的方法，通過對運動模型和地面真實數據的殘差進行局部訓練獲得里程計殘差模型，提高機器人定位與建圖的精度，該方法需要預先對里程計和地面真實數據進行訓練，不適用于未知環境下的實時估計。雖然里程計在長時間運行中存在誤差，但在短時間內可以精確測量［19］。UWB在復雜室內環境下易受多徑效應和電磁干擾，導致測距結果不穩定，而通過融合里程計可以有效提高定位精度和穩定性［20］。

本文提出一種采用單UWB融合里程計的多機器人相對定位方法，采用價格低廉的UWB傳感器與機器人自帶的輪式里程計，在無須固定基站和機器人起始位姿等先驗信息的情況下，利用滑動窗口內固定時間間隔的機器人間測距信息與里程計預測的機器人位姿構建非線性最小二乘問題，實現機器人間的相對位姿估計。同時，為抑制滑動窗口內里程計累積誤差對定位結果的影響，利用擴展卡爾曼濾波算法估計里程計協方差，并將其以加權的方式運用于非線性優化中，抑制累積誤差對定位結果的影響。最后，利用圖優化算法融合里程計與非線性優化獲得的相對位姿，以校正UWB測距誤差對定位結果的影響，獲得穩定的相對定位結果。本文的主要貢獻包括：a）提出一種融合UWB與里程計的多機器人相對定位方法，利用機器人搭載的單個UWB與里程計實現多機器人相對定位，降低了系統布置成本，提高了系統的適用性；b）利用圖優化算法融合里程計與非線性優化的定位結果，降低了UWB測距誤差對定位結果的影響，進一步提高了相對定位精度和穩定性。

1 問題描述

將環境中多機器人間的相對定位假定為機器人i與j之間的相對定位，其中i，j∈［1：N］且i≠j，N為機器人數量。如圖1所示，機器人在無外部基站和先驗條件（地圖信息、機器人起始位姿和UWB坐標）的環境中沿不同軌跡移動，每個機器人搭載一個用于機器人間距離測量的UWB節點和一個用于機器人自身運動狀態測量的里程計，每個機器人上UWB節點的參數配置一致，另外移動機器人底盤均為兩輪差分式驅動。本文方法中，每個移動機器人將測量數據（UWB距離和里程計信息）上傳至服務器，以便數據集中處理。

2.1 基于非線性優化的相對位姿估計

UWB只能獲得兩個機器人之間的距離信息，單組測距信息無法實現機器人間的相對定位，要實現定位需要至少三組機器人間測距信息和機器人位姿。因此，本文通過融合滑動窗口內固定時間間隔的機器人間測距信息與里程計預測的機器人位姿構建非線性最小二乘問題，實現機器人間的相對位姿估計。具體實現方法如下：

將t時刻之前的歷史信息（t時刻前的機器人間測距信息和機器人位姿）通過里程計測量的相對位姿與待估計參數ti，j建立關系。為降低計算量提高運算效率，使用滑動窗口等間隔的截取t-w到t時間段內的（w/τ）組機器人間測距信息和機器人位姿。設Δmt′，ti=（Δxt′，ti，Δyt′，ti，Δθt′，ti）為機器人i里程計測量的機器人i從t′到t時刻的相對位姿，Δmt′，tj為機器人j里程計測量的機器人j從t′到t時刻的相對位姿，則t′時刻機器人i和j的位姿為

相應的姿態圖構建完成后，通過圖優化算法（general graph optimization，g2o）［22］獲得每個機器人精確的定位軌跡，再通過相對變換獲得準確的機器人間相對位姿xti，j。

3 實驗及結果分析

3.1 實驗設置

本實驗對多個TurtleBot2機器人在室內環境中的相對定位進行了測試，實驗平臺如圖3所示。每個機器人需攜帶一個UWB節點（nooploop linktrack），UWB節點的最大測量距離為100 m，數據采樣頻率被設置為50 Hz。采用TurtleBot2底盤作為機器人底盤，其攜帶一個兩輪差分式里程計，輸出頻率為20 Hz，但由于地面不規整，輪子將出現不同程度的打滑及空轉，影響里程計精度。為驗證本文算法的精度，每臺機器人搭載一臺Hokuyo激光雷達用于建圖（gmapping）［23］以及實現自適應蒙特卡羅定位（adaptive Monte Carlo localization，AMCL）［21］，將AMCL所得姿態作為機器人在實驗環境中的真實軌跡，將其與估計值作比較。另外，每個機器人搭載一臺筆記本電腦用于運行機器人操作系統（robot operation system，ROS），并記錄各個模塊采集的數據。

實驗測試中UWB定位部署方案如圖4所示。每個機器人搭載一個UWB節點，通過時差法（TOF）進行雙向測距，獲取各機器人間距離測量信息。與傳統的到達時間差（TDOA）方式不同，本文所提定位方案無須設置多個固定的UWB基站，且無須知道各UWB基站的位置，降低了部署成本。此外，UWB測距信息通過串口上傳至電腦終端，各機器人間通過無線局域網交換里程計信息，最后在電腦終端上通過結合UWB與里程計信息解算出當前機器人相對于其他機器人的相對位姿。

為分析本文定位算法的性能，實驗在兩個不同的場景下完成。實驗場景1如圖5所示，環境中存在障礙物（紙箱）和用于遮擋UWB信號的隔板（白色隔板），兩個機器人從不同位置出發，進行兩次實驗，第一次實驗以0.2 m/s的速度在一個9 m×18 m的環境中移動，兩個機器人的移動軌跡不同，機器人1沿著9 m×18 m的L型路徑移動，機器人2沿著7 m×14 m的長方形軌跡移動；第二次實驗機器人移動速度更改為1 m/s，移動軌跡與第一次相同。實驗場景2如圖6所示，環境中存在障礙物，三個機器人從不同位置出發進行一次實驗，以0.2 m/s的速度在一個6 m×12 m的環境中移動，每個機器人的移動軌跡都不同，機器人2和3沿著5 m×5 m的兩個不同的正方形路徑移動，機器人1則沿著6 m×12 m的T型路徑移動。

3.2 實驗分析

以定位結果的相對位置精度和相對角度精度對本文方法的有效性進行評估，相對位姿精度定義為整個定位過程中所有相對定位結果與AMCL的相對定位結果做均方根誤差計算。

3.2.1 機器人移動速度對定位精度影響

為驗證機器人移動速度對本文算法定位精度的影響，在實驗場景1中，分別進了機器人速度為0.2 m/s和1 m/s的相對定位實驗，滑動窗口大小和間隔設置為w=100 s和τ=5 s，其他參數保持一致。不同速度下機器人間相對定位分析結果如表1所示。機器人速度為0.2 m/s和1 m/s時，非線性優化與里程計相對定位誤差對比曲線如圖7所示。

將機器人移動速度設置為1 m/s所獲得的相對定位精度與機器人移動速度設置為0.2 m/s所獲得的相對定位精度相比，本文提出的非線性優化算法的相對位置精度和相對角度精度分別降低了約23.1%和14.4%。結果表明，機器人移動速度對本文所提非線性優化方法具有一定影響但影響不大；由圖7可知，在整個定位過程中本文定位算法的定位精度相對穩定。由于滑動窗口的設置，本文定位算法精度主要受限于里程計和UWB的測量精度，高機動環境中，在保證滑動窗口內里程計與UWB自身測量精度穩定的情況下，本文相對定位算法能夠獲得穩定的相對定位結果，所以本文相對定位算法受機器人移動速度影響較小。在下文的實驗分析中，為保證實驗數據采集的穩定性，實驗機器人移動速度均設置為0.2 m/s。

3.2.2 多機器人相對定位性能分析

為驗證本文所提的多機器人相對定位算法性能，在實驗場景2中，將三個機器人移動速度均設置為0.2 m/s，滑動窗口大小w與滑動窗口內時間間隔τ設置為三組不同的配置：w=50 s，τ=2.5 s；w=100 s，τ=5 s；w=200 s，τ=10 s，其余參數保持不變。在三個不同滑動窗口配置下，分別進行了基于里程計的相對定位實驗、基于非線性優化的相對定位實驗、基于加權非線性優化的相對定位實驗以及基于加權非線性優化結合圖優化的相對定位實驗。

多機器人相對定位性能分析結果如表2所示。當滑動窗口設置為w=50 s，τ=2.5 s時，非線性優化的相對位置精度相比于里程計提高了約27.3%，相對角度精度提高了約43.0%；當滑動窗口設置為w=200 s，τ=10 s時，非線性優化的相對位置精度相比于里程計提高了約66.2%，相對角度精度提高了約72.3%。實驗結果表明，基于非線性優化的相對位姿估計方法能夠實現多機器人間的相對定位并獲得了較好的定位精度。當滑動窗口設置為w=50 s，τ=2.5 s時，由于截取的多組待優化UWB數據差異較小，導致優化效果降低，產生了較大的定位誤差。在實際應用中應合理調節滑動窗口大小和間隔，以獲得更好的定位效果。

由于里程計累積誤差對定位結果的影響只存在于滑動窗口內部，所以里程計累積誤差影響將隨滑動窗口變大而增加。對滑動窗口設置為w=200 s，τ=10 s和w=100 s，τ=5 s實驗結果進行比較，相對定位結果如表2所示。當滑動窗口設置為w=200 s，τ=10 s時非線性優化算法與加權非線性優化算法的誤差曲線對比如圖8所示，當滑動窗口設置為w=200 s時，非線性優化的相對位置和角度精度相對于滑動窗口設置為w=100 s分別降低了約12.2%和23.8%，而本文提出的加權非線性優化算法則將相對位置和角度精度分別提高了約6.8%和3.4%。結果表明，在滑動窗口設置為w=200 s時，隨著滑動窗口的增大，本文方法對定位精度有顯著提升，表明本文的里程計協方差估計與加權方法能夠有效抑制滑動窗口內部里程計累積誤差影響。

由于UWB在室內復雜環境下受到多徑效應、電磁干擾等因素影響，導致加權非線性優化結果仍存在較大的波動，所以采用圖優化方法對加權非線性優化結果進行二次優化。對滑動窗口設置為w=100 s，τ=5 s的定位結果進行分析，如表2所示。相比于加權非線性優化方法，圖優化方法的相對位置精度提高了約31.9%，相對角度精度提高了約14.4%，相對位置精度達到0.32 m，相對角度精度達到4.16°。圖9為滑動窗口設置為w=100 s， τ=5 s時不同算法的估計軌跡：（a）不同機器人里程計軌跡；（b）假設機器人1位姿已知，加權非線性優化得到的機器人2和3的軌跡；（c）以機器人1里程計為參考，圖優化得到的不同機器人軌跡。由圖9（b）可知，由于UWB在測量過程中存在的誤差，導致定位軌跡存在大量的離散點，軌跡波動較大，而通過圖優化融合里程計后，定位軌跡被校正如圖9（c）所示。結果表明，本文提出的基于圖優化的相對位姿估計校正了UWB測量誤差對定位結果的影響，提高了定位精度和穩定性。

在多機器人相對定位實驗中，所提出的相關算法運行在一款內存為16 GB搭載AMD R5-4600H 3.6 GHz CPU的筆記本電腦上。相關算法的時間消耗如表3所示。由表3可知，三個機器人之間完成一次基于加權非線性優化的相對定位所需時間為23 ms，當加入圖優化后時間消耗為188 ms，滿足一定的實時性需求。

本文所提出的多機器人相對定位方法與現有定位方案定位精度對比如表4所示。由表4可知，本文方法的定位精度與文獻［12］所獲得的定位精度接近，但在文獻［12］的定位方案中，每個機器人需搭載四個UWB。與同樣采用單UWB的單UWB錨點定位方案［13］相比，本文的定位方法位置精度提升了約33.3%，且本文方法允許搭載UWB的機器人自由移動，無須知道機器人的起始位姿。相比于UWB融合IMU的兩設備定位方案［24］，本文方法位置精度提升了約41.8% ，并進行了多機器人的實驗驗證。此外，本文所采用的UWB定位模塊成本低于700元，定位成本遠低于現有采用UWB基站、激光雷達或視覺的多機器人相對定位方案。

4 結束語

本文提出一種融合UWB與里程計的多機器人定位方案，在無基站和GPS信號情況下，解決多機器人的相對定位問題。與現有方案相比，本文方法使用單個UWB融合機器人自帶的輪式里程計，在無須固定基站和機器人起始位姿等先驗信息情況下實現多機器人間的相對定位，并獲得了小于0.5 m的相對定位精度，降低了多機器人相對定位成本和部署難度。通過融合多個時刻的UWB測量信息，消除了里程計累積誤差；并利用圖優化算法融合里程計與非線性優化結果，矯正了UWB測量誤差對定位結果的影響。本文方法不僅適用于多機器人相對定位，對于能夠獲得設備間的距離信息和設備自身位姿變換的嵌入式設備等均適用。在未來的工作中，將針對算法的實時性進行優化，并將該方法應用于多機器人編隊控制中。

參考文獻：

［1］劉劍鋒，孫力帆，普杰信，等.基于剛性約束的雙移動機器人協同定位［J］.電子學報，2020，48（9）：1777-1785.（Liu Jianfeng，Sun Lifan，Pu Jiexin，et al. Cooperative localization in a team of two mobile robots based on rigid constraints［J］.Acta Electronica Sinica，2020，48（9）：1777-1785.）

［2］樊簡，趙碩，蔡文碩，等.采用網格搜索的非合作衛星時差累積協同定位方法［J］.國外電子測量技術，2022，41（3）：56－62.（Fan Jian，Zhao Shuo，Cai Wenshuo，et al. Grid search and difference of times integrated geometric localization of ground targets via multiple noncooperative satellites［J］.Foreign Electronic Measurement Technology，2022，41（3）：56－62.）

［3］Zafari F，Gkelias A，Leung K K.A survey of indoor localization systems and technologies［J］.IEEE Communications Surveys amp; Tutorials，2019，21（3）：2568－2599.

［4］薛塬，蘇偉，王洪超，等.支持異步TDOA測量的確定性定位網絡［J］.電子學報，2019，47（10）：2017－2024.（Xue Yuan，Su Wei，Wang Hongchao，et al. A deterministic localization network for TDOA measurement［J］.Acta Electronica Sinica，2019，47（10）：2017－2024.）

［5］賀軍義，吳夢翔，宋成，等.基于UWB的密集行人三維協同定位算法［J］.計算機應用研究，2022，39（3）：790-796.（He Junyi，Wu Mengxiang，Song Cheng，et al. Three-dimensional cooperative location algorithm of dense pedestrians based on UWB［J］.Application Research of Computers，2022，39（3）：790-796.）

［6］焦傳佳，江明，徐勁松，等.基于激光信息的移動機器人定位研究［J］.電子測量與儀器學報，2021，35（9）：1－9.（Jiao Chuanjia，Jiang Ming，Xu Jinsong，et al. Research on positioning of mobile robot based on laser information［J］.Journal of Electronic Measurement and Instrumentation，2021，35（9）：1－9.）

［7］Hyeon J，Kim J，Doh N.Pose correction for highly accurate visual localization in large-scale indoor spaces［C］//Proc of IEEE/CVF International Conference on Computer Vision.Piscataway，NJ：IEEE Press，2021：15974-15983.

［8］田增山，未平，李澤，等.基于Wi-Fi的室內實時角度定位算法［J］.電子學報，2021，49（2）：408－416.（Tian Zengshan，Wei Ping，Li Ze，et al. Indoor real-time localization algorithm based on angle of arrival of Wi-Fi signal［J］.Acta Electronica Sinica，2021，49（2）：408－416.）

［9］王飛，童敏明，白琪，等.基于RSSI的ZigBee動態加權質心四點定位算法［J］.計算機應用研究，2018，35（9）：2663－2665.（Wang Fei，Tong Minming，Bai Qi，et al.ZigBee dynamic weighted centroid four points location algorithm based on RSSI［J］.Application Research of Computers，2018，35（9）：2663－2665.）

［10］Shule W，Almansa C M，Queralta J P，et al. UWB-based localization for multi-UAV systems and collaborative heterogeneous multi-robot systems：a survey［J］.Procedia Computer Science，2020，175：357－364.

［11］Nguyen T M，Zaini A H，Wang Chen，et al.Robust target-relative localization with ultra-wideband ranging and communication［C］//Proc of IEEE International Conference on Robotics and Automation.Pisca-taway，NJ：IEEE Press，2018：2312－2319.

［12］Cao Zhiqiang，Liu Ran，Yuen C，et al.Relative localization of mobile robots with multiple ultra-wideband ranging measurements［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2021：5857－5863.

［13］Cao Yanjun，Yang Chenhao，Li Rui，et al. Accurate position tracking with a single UWB anchor［C］//Proc of IEEE International Confe-rence on Robotics and Automation.Piscataway，NJ：IEEE Press，2020：2344－2350.

［14］劉宇，謝宇，彭慧，等.基于UWB/PDR的航向發散自適應修正算法研究［J］.電子測量技術，2022，45（3）：98-103.（Liu Yu，Xie Yu，Peng Hui，et al. Research on adaptive correction algorithm of heading divergence based on UWB/PDR［J］.Electronic Measurement Technology，2022，45（3）：98-103.）

［15］Zhou Xin，Wen Xiangyong，Wang Zhepei，et al. Swarm of micro flying robots in the wild［J］.Science Robotics，2022，7（66）：eabm5954.

［16］Qi Yang，Zhong Yisheng，Shi Zongying.Cooperative 3D relative localization for UAV swarm by fusing UWB with IMU and GPS［J］.Journal of Physics：Conference Series，2020，1642：012028.

［17］He Yijia，Guo Yue，Ye Aixue， et al. Camera-odometer calibration and fusion using graph based optimization［C］//Proc of IEEE Internatio-nal Conference on Robotics and Biomimetics.Piscataway，NJ：IEEE Press，2017：1624-1629.

［18］Hidalgo-Carrió J，Hennes D，Schwendner J， et al.Gaussian process estimation of odometry errors for localization and mapping［C］//Proc of IEEE International Conference on Robotics and Automation.Pisca-taway，NJ：IEEE Press，2017：5696－5701.

［19］周牧，耿小龍，謝良波，等.室內Wi-Fi/PDR自適應魯棒卡爾曼濾波融合定位方法［J］.電子學報，2019，47（1）：9-15.（Zhou Mu，Geng Xiaolong，Xie Liangbo，et al. Indoor Wi-Fi/PDR fusion localization based on adaptive and robust Kalman filter［J］.Acta Electronica Sinica，2019，47（1）：9-15.）

［20］胡悅，李旭，徐啟敏，等.衛星拒止環境下基于因子圖的智能車可靠定位方法［J］.儀器儀表學報，2021，42（11）：79－86.（Hu Yue，Li Xu，Xu Qimin，et al.Reliable positioning method of intelligent vehicles based on factor graph in GNSS-denied environment［J］.Chinese Journal of Scientific Instrument，2021，42（11）：79－86.）

［21］Thrun S，Burgard W，Fox D.Probabilistic robotics［M］.Cambridge，MA：MIT Press，2005.

［22］Kümmerle R，Grisetti G，Strasdat H，et al.g2o：a general framework for graph optimization［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：3607－3613.

［23］Grisetti G，Stachniss C，Burgard W.Improved techniques for grid mapping with Rao-Blackwellized particle filters［J］.IEEE Trans on Robotics，2007，23（1）：34－46.

［24］Cossette C C，Shalaby M，Saussié D，et al. Relative position estimation between two UWB devices with IMUs［J］.IEEE Robotics and Automation Letters，2021，6（3）：4313－4320.

收稿日期：2022－07－19；修回日期：2022－09－16 基金項目：國家自然科學基金資助項目（12205245，12175187）；國家重點研發計劃資助項目（2019YFB1310805）

作者簡介：鄧忠元（1996－），男，四川德陽人，碩士研究生，主要研究方向為多機器人定位導航；劉冉（1986－），男（通信作者），安徽淮北人，副研究員，碩導，博士，主要研究方向為室內定位、SLAM（ran.liu.86@hotmail.com）；曹志強（1996－），男，四川南充人，碩士，主要研究方向為室內定位、多機器人定位；肖宇峰（1978－），男，湖南常德人，教授，博導，博士，主要研究方向為網絡通信系統、智能機器人系統．