基于超寬帶陣列與里程計(jì)的多機(jī)器人相對(duì)定位

劉 冉 曹志強(qiáng) 鄧天睿 鄧忠元 肖宇峰

(西南科技大學(xué)信息工程學(xué)院 綿陽(yáng) 621000)

1 引 言

近年來(lái)，隨著多機(jī)器人應(yīng)用需求的增加，例如，分布式編隊(duì)、多機(jī)器人建圖等。這些應(yīng)用對(duì)相對(duì)定位有較高的需求，因此，面向多智能體間的高精度定位已成為國(guó)內(nèi)外研究的熱點(diǎn)[1]。目前，在室外環(huán)境可以通過(guò)全球定位系統(tǒng)(Global Positioning System, GPS)或北斗衛(wèi)星導(dǎo)航系統(tǒng)為機(jī)器人提供較高的定位精度。然而由于建筑物對(duì)GPS信號(hào)的阻擋，室內(nèi)環(huán)境無(wú)法較好地接收衛(wèi)星信號(hào)，給機(jī)器人室內(nèi)定位帶來(lái)了巨大挑戰(zhàn)[2]。針對(duì)視距下的室內(nèi)環(huán)境，國(guó)內(nèi)外學(xué)者提出了很多成熟的相對(duì)定位技術(shù)和方法[3,4]。但針對(duì)非視距等室內(nèi)場(chǎng)景，實(shí)現(xiàn)機(jī)器人間相對(duì)定位仍存在很大的挑戰(zhàn)。

目前，多機(jī)器人定位方法主要分為全局定位和相對(duì)定位。運(yùn)動(dòng)捕捉系統(tǒng)[5]、超寬帶(Ultra Wideband,UWB)定位系統(tǒng)[6]、WiFi定位系統(tǒng)[7]等為常用的全局定位系統(tǒng)。上述定位系統(tǒng)對(duì)基礎(chǔ)設(shè)備的安裝精度有著極高的要求，否則將會(huì)影響定位的準(zhǔn)確性。全局定位系統(tǒng)需要事先構(gòu)建環(huán)境地圖或標(biāo)簽的分布情況，且每個(gè)移動(dòng)機(jī)器人需額外配置通信設(shè)備，以獲得其他機(jī)器人在環(huán)境中的位置，極大增加了硬件開銷。另外，在緊急情況下，環(huán)境中的基礎(chǔ)設(shè)施結(jié)構(gòu)可能已被破壞，事先安裝的攝像頭或標(biāo)簽均可能無(wú)法正常工作，導(dǎo)致全局定位系統(tǒng)失效，不能為多機(jī)器人在未知環(huán)境中提供定位服務(wù)[8]。

相對(duì)定位系統(tǒng)相較全局定位系統(tǒng)而言，在實(shí)現(xiàn)多機(jī)器人定位上更具靈活性和抗干擾性。在相對(duì)定位系統(tǒng)中，無(wú)需提前安裝基礎(chǔ)設(shè)備，機(jī)器人可通過(guò)自身傳感器獲得測(cè)量范圍內(nèi)的其他機(jī)器人的相對(duì)位置。Scaramuzza等人[9]提出了一種基于同時(shí)定位與地圖構(gòu)建(Simultaneous Localization And Mapping, SLAM)技術(shù)的相對(duì)定位方法，在建圖期間實(shí)現(xiàn)目標(biāo)自主定位，通過(guò)通信網(wǎng)絡(luò)分享各自估計(jì)位置。Saska等人[10]提出一種基于視覺在線標(biāo)定的方法實(shí)現(xiàn)相對(duì)定位，但該方法易受光照影響，且視角小、計(jì)算復(fù)雜度高，在非視距環(huán)境下表現(xiàn)欠佳。Wang等人[11]設(shè)計(jì)了一款具有旋轉(zhuǎn)激光發(fā)射功能和接收功能的硬件裝置，并提出事件觸發(fā) (event-trigged)算法實(shí)現(xiàn)相對(duì)位姿估計(jì)，該方法對(duì)硬件設(shè)施要求高，且不適用于非視距環(huán)境。與激光和視覺傳感器相比，無(wú)線傳感器在非視距的情況下有更好的表現(xiàn)[12,13]。UWB技術(shù)因具有成本低、體積小、功耗低和出色的穿透力，在行業(yè)中得到了廣泛的應(yīng)用。Ghanem等人[14]利用每個(gè)車輛配備的兩個(gè)UWB傳感器實(shí)現(xiàn)兩個(gè)車輛間的相對(duì)定位，但存在定位奇異性問(wèn)題。另外，吳玉秀等人[15]提出了一種基于聲音的分布式多機(jī)器人定位方法，通過(guò)麥克風(fēng)陣列獲得附近機(jī)器人在自身坐標(biāo)系下的相對(duì)位置，但該方法僅在小范圍內(nèi)且機(jī)器人靜止?fàn)顟B(tài)下進(jìn)行了驗(yàn)證。

航跡推算在機(jī)器人定位中起著至關(guān)重要的作用，其根據(jù)里程計(jì)或慣性測(cè)量單元，在短時(shí)間內(nèi)能夠提供精準(zhǔn)的姿態(tài)估計(jì)[16—18]。但隨著長(zhǎng)時(shí)間運(yùn)行，會(huì)出現(xiàn)不可避免的累計(jì)誤差，影響姿態(tài)估計(jì)的精度。國(guó)內(nèi)外研究人員致力于將航跡推算與其他傳感器源融合，提高定位精度。針對(duì)UWB與里程計(jì)數(shù)據(jù)融合，San Martín等人[19]和Magnago等人[20]分別提出擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)和無(wú)跡卡爾曼濾波(Unscented Kalman Filter,UKF)等不同的數(shù)據(jù)融合方案。此外，針對(duì)相對(duì)定位，Guo等人[21]提出了一種基于UWB的協(xié)作相對(duì)定位方法，該方法基于UWB測(cè)量距離值和IMU位移測(cè)量值來(lái)估計(jì)多個(gè)無(wú)人機(jī)的位置，但該文中僅討論了與靜態(tài)無(wú)人機(jī)間的相對(duì)定位問(wèn)題。Liu等人[22]提出了一種融合慣性測(cè)量單元(Inertial Measurement Unit, IMU)和UWB的相對(duì)定位方法，但未討論相對(duì)角度的估計(jì)問(wèn)題。Hardy等人[23]融合了IMU、磁強(qiáng)計(jì)、點(diǎn)對(duì)點(diǎn)測(cè)距和俯視攝像頭實(shí)現(xiàn)無(wú)人機(jī)組高精度姿態(tài)估算。該方法計(jì)算復(fù)雜度較高，因此不適用于復(fù)雜環(huán)境中多機(jī)器人的應(yīng)用。

本文針對(duì)室內(nèi)場(chǎng)景的多機(jī)器人相對(duì)定位問(wèn)題，提出了多UWB節(jié)點(diǎn)與里程計(jì)融合的相對(duì)定位方法。該方法需在每個(gè)機(jī)器人上安裝多個(gè)UWB節(jié)點(diǎn)，如圖1所示。UWB節(jié)點(diǎn)安裝在移動(dòng)機(jī)器人上的特定位置，通過(guò)最小化多組UWB節(jié)點(diǎn)的距離測(cè)量值來(lái)實(shí)現(xiàn)相對(duì)姿態(tài)估計(jì) (相對(duì)位置和航向)。由于UWB距離測(cè)量值存在誤差，因此相對(duì)姿態(tài)估計(jì)值不可避免地產(chǎn)生不規(guī)則波動(dòng)。雖然里程計(jì)長(zhǎng)時(shí)間運(yùn)行存在累計(jì)誤差，但短期內(nèi)精度較高，且軌跡平滑。因此，利用相鄰時(shí)刻里程計(jì)測(cè)量值對(duì)非線性優(yōu)化所得相對(duì)姿態(tài)進(jìn)行約束，提高定位精度。另外，為保證系統(tǒng)實(shí)時(shí)性利用滑動(dòng)窗口的思想僅優(yōu)化特定時(shí)間段的測(cè)量數(shù)據(jù)。圖優(yōu)化過(guò)程中不能確定UWB位姿估計(jì)誤差的大小，對(duì)優(yōu)化結(jié)果造成一定的影響，因此，通過(guò)粒子濾波融合里程計(jì)和滑動(dòng)窗口優(yōu)化后的相對(duì)姿態(tài)，實(shí)現(xiàn)準(zhǔn)確的姿態(tài)估計(jì)。本文的創(chuàng)新之處在于：(1) 提出一種基于多UWB節(jié)點(diǎn)距離測(cè)量值，通過(guò)非線性優(yōu)化實(shí)現(xiàn)移動(dòng)機(jī)器人之間相對(duì)姿態(tài)估計(jì)的方法。(2) 設(shè)計(jì)滑動(dòng)窗口，并基于圖優(yōu)化算法對(duì)滑動(dòng)窗口內(nèi)的數(shù)據(jù)進(jìn)行優(yōu)化，進(jìn)一步提升位姿估計(jì)精度，并保證算法實(shí)時(shí)性。(3) 利用粒子濾波融合里程計(jì)和滑動(dòng)窗口優(yōu)化后的相對(duì)位姿，獲得更準(zhǔn)確的相對(duì)姿態(tài)估計(jì)。(4) 本文在真實(shí)環(huán)境中驗(yàn)證算法，實(shí)驗(yàn)結(jié)果表明，在12 m×6 m的環(huán)境中3個(gè)移動(dòng)機(jī)器人能到達(dá)0.312 m的定位誤差以及4.903°的角度誤差，且滿足系統(tǒng)的實(shí)時(shí)性。

2 系統(tǒng)概述

如圖1所示，移動(dòng)機(jī)器人在未知的、無(wú)基礎(chǔ)設(shè)施的場(chǎng)景中實(shí)現(xiàn)相對(duì)定位。每個(gè)機(jī)器人攜帶里程計(jì)和4個(gè)UWB節(jié)點(diǎn)，UWB節(jié)點(diǎn)構(gòu)成一個(gè)正方形矩形陣列。每個(gè)機(jī)器人上的UWB節(jié)點(diǎn)的收發(fā)頻率與UWB陣列的距離配置相同。另外，移動(dòng)機(jī)器人底盤均為兩輪差分驅(qū)動(dòng)。本文所提方法中，每個(gè)移動(dòng)機(jī)器人攜帶處理單元，將測(cè)量數(shù)據(jù) (UWB距離和里程計(jì)信息) 上傳至服務(wù)器，以便數(shù)據(jù)集中處理。

3 算法設(shè)計(jì)

本文所提基于多UWB節(jié)點(diǎn)的機(jī)器人相對(duì)定位算法由3部分組成，分別為：基于UWB的機(jī)器人間相對(duì)位姿估計(jì)、融合里程計(jì)約束的機(jī)器人間相對(duì)位姿估計(jì)和基于粒子濾波的機(jī)器人間相對(duì)位姿估計(jì)。此3部分緊密相關(guān)、層層遞進(jìn)：(1) 基于UWB的機(jī)器人間相對(duì)位姿估計(jì)，僅利用UWB距離測(cè)量值通過(guò)非線性優(yōu)化算法實(shí)現(xiàn)相對(duì)姿態(tài)估計(jì)。(2) 融合里程計(jì)約束的機(jī)器人間相對(duì)位姿估計(jì)，通過(guò)圖優(yōu)化算法對(duì)滑動(dòng)窗口進(jìn)行優(yōu)化，利用里程計(jì)相鄰時(shí)刻的相對(duì)位移對(duì)相對(duì)姿態(tài)估計(jì)進(jìn)行約束，獲得更為準(zhǔn)確的估計(jì)。(3) 基于粒子濾波的機(jī)器人間相對(duì)位姿估計(jì)，通過(guò)粒子濾波器融合里程計(jì)和滑動(dòng)窗口優(yōu)化后的相對(duì)位姿，進(jìn)一步提升定位精度。整體算法框架如圖2所示。

3.1 基于UWB的機(jī)器人間相對(duì)位姿估計(jì)

3.2 融合里程計(jì)約束的機(jī)器人間相對(duì)姿態(tài)估計(jì)

通常情況下，移動(dòng)機(jī)器人的尺寸較小，因此機(jī)器人上UWB節(jié)點(diǎn)之間距離受到了限制。另外，由于UWB距離測(cè)量值的誤差，對(duì)非線性優(yōu)化結(jié)果造成很大的影響。本文中所有移動(dòng)機(jī)器人均攜帶里程計(jì)，將一段時(shí)間內(nèi)的里程計(jì)數(shù)據(jù)以及非線性優(yōu)化得到的相對(duì)位姿構(gòu)成滑動(dòng)窗口。利用里程計(jì)在短時(shí)間內(nèi)相對(duì)位移準(zhǔn)確的特性，對(duì)滑動(dòng)窗口中的相對(duì)姿態(tài)估計(jì)進(jìn)行約束，提升定位精度。圖優(yōu)化算法[24]中的圖由頂點(diǎn)和邊構(gòu)成，本文中將相鄰時(shí)刻里程計(jì)的相對(duì)位移以及基于非線性優(yōu)化算法所得相對(duì)姿態(tài)估計(jì)值作為圖的邊，移動(dòng)機(jī)器人不同時(shí)刻的位姿作為頂點(diǎn)，利用邊約束頂點(diǎn)，構(gòu)建待優(yōu)化的位姿圖。隨著邊與頂點(diǎn)數(shù)量的增加，優(yōu)化所需時(shí)間也相應(yīng)地變長(zhǎng)，因此本文通過(guò)滑動(dòng)窗口來(lái)減少算法的計(jì)算開銷。圖優(yōu)化算法中的整體約束表述為

3.3 基于粒子濾波的機(jī)器人間相對(duì)姿態(tài)估計(jì)

在更新步驟之后，將執(zhí)行重采樣以獲得新的粒子集，這對(duì)于避免粒子群退化至關(guān)重要。理論上，粒子的權(quán)重越大，在重采樣過(guò)程中被選中的概率越大。

4 實(shí)驗(yàn)與分析

4.1 實(shí)驗(yàn)環(huán)境搭建

本文設(shè)計(jì)數(shù)據(jù)采集平臺(tái)如圖4所示。每個(gè)移動(dòng)機(jī)器人攜帶4個(gè)UWB節(jié)點(diǎn)(nooploop linktrack)，構(gòu)成正方形矩陣陣列，且每個(gè)UWB節(jié)點(diǎn)的最大測(cè)量距離為100 m，數(shù)據(jù)采集頻率被設(shè)置為50 Hz。兩輪差分式移動(dòng)機(jī)器人底盤的里程計(jì)輸出頻率為20 Hz，但由于地面不規(guī)整，輪子將出現(xiàn)不同程度的打滑及空轉(zhuǎn)現(xiàn)象，影響里程計(jì)精度。為驗(yàn)證本文算法的精度，每臺(tái)機(jī)器人搭載一臺(tái)Hokuyo激光雷達(dá)，用于建圖(gmapping)以及實(shí)現(xiàn)自適應(yīng)蒙特卡洛定位(Adaptive Monte Carlo Localization, AMCL)[26]，國(guó)內(nèi)外研究者常將AMCL所得姿態(tài)作為機(jī)器人在室內(nèi)環(huán)境中的真實(shí)軌跡，將其與估計(jì)值做比較，詳情參考文獻(xiàn)[27]。另外，每個(gè)移動(dòng)機(jī)器人搭載一臺(tái)筆記本電腦用于運(yùn)行機(jī)器人操作系統(tǒng)(Robot Operation System, ROS)，并記錄各個(gè)模塊采集的數(shù)據(jù)。

為驗(yàn)證本文提出的算法的性能，共設(shè)置以下兩組實(shí)驗(yàn)。實(shí)驗(yàn)1-a中，僅使用兩個(gè)移動(dòng)機(jī)器人，機(jī)器人2靜止不動(dòng)，機(jī)器人1以0.2 m/s的速度沿著7×6 m的矩形軌跡運(yùn)動(dòng)，在運(yùn)動(dòng)過(guò)程中，各組模塊實(shí)時(shí)記錄數(shù)據(jù)，如圖5(a)所示；實(shí)驗(yàn)1-b為驗(yàn)證算法在非視距環(huán)境不規(guī)則運(yùn)行路徑下的表現(xiàn)，機(jī)器人2前方被遠(yuǎn)高于機(jī)器人的木板遮擋，機(jī)器人1沿著不規(guī)則路徑運(yùn)動(dòng)，如圖5(b)、圖6所示。實(shí)驗(yàn)2：在該實(shí)驗(yàn)中，3個(gè)相互獨(dú)立的移動(dòng)機(jī)器人，在12 m×6 m的環(huán)境中沿著3條不同路徑以0.2 m/s的速度同時(shí)運(yùn)動(dòng)，如圖5(c)所示，兩個(gè)機(jī)器人沿著不同的矩形軌跡運(yùn)動(dòng)，另一個(gè)機(jī)器人沿著T型軌跡運(yùn)動(dòng)。

4.2 實(shí)驗(yàn)1

在實(shí)驗(yàn)1-a中，機(jī)器人1圍繞機(jī)器人2沿著矩形軌跡運(yùn)動(dòng)數(shù)圈，如圖5(a)所示。該實(shí)驗(yàn)設(shè)置不同的UWB節(jié)點(diǎn)距離配置，檢驗(yàn)不同算法在不同配置下的性能。分別設(shè)置0.3 m, 0.5 m和0.7 m的UWB間距。在每組測(cè)試中，機(jī)器人1和機(jī)器人2的UWB節(jié)點(diǎn)距離配置以及各傳感器輸出頻率始終保持一致。

由于UWB傳感器自身存在測(cè)距誤差及數(shù)據(jù)波動(dòng)，因此不同UWB距離配置對(duì)非線性優(yōu)化的結(jié)果有較大影響。如表1所示，對(duì)比不同UWB配置對(duì)相對(duì)定位性能的影響。其中，UWB節(jié)點(diǎn)距離配置為0.7 m時(shí)，獲得0.37 m的位置誤差且具有0.22 m的標(biāo)準(zhǔn)差，角度誤差為5.05°以及標(biāo)準(zhǔn)差為3.63°；對(duì)比距離配置為0.3 m，位置誤差降低22%，角度誤差降低33%，且具有更小的標(biāo)準(zhǔn)差。表明位置誤差和角度誤差隨著UWB間距的增加逐漸降低，且具有更小的數(shù)據(jù)波動(dòng)。實(shí)際應(yīng)用中，通常根據(jù)移動(dòng)機(jī)器人體積大小選擇不同的UWB節(jié)點(diǎn)距離配置。

不同的滑動(dòng)窗口長(zhǎng)度，對(duì)圖優(yōu)化算法的時(shí)間消耗以及定位精度有一定的影響，ω長(zhǎng)度越大所需時(shí)間消耗越多。不同窗口長(zhǎng)度ω的定位精度如表1所示，由此表可見位置誤差和角度誤差隨長(zhǎng)度ω增加而降低。

對(duì)比UWB節(jié)點(diǎn)距離0.3 m時(shí)，ω=80相較于ω=5的位置誤差和角度誤差分別下降21%和57%。但當(dāng)ω=160時(shí)位置誤差和角度誤差反而增加，可能由于ω過(guò)大，導(dǎo)致算法執(zhí)行時(shí)間太久，影響系統(tǒng)的實(shí)時(shí)性。

為保證估計(jì)精度和系統(tǒng)實(shí)時(shí)性，在后續(xù)算法中選擇滑動(dòng)窗口長(zhǎng)度ω=30。本文提出的粒子濾波算法與文獻(xiàn)[22]所提的基于粒子濾波融合IMU和UWB距離測(cè)量數(shù)據(jù)的相對(duì)定位方法對(duì)比，兩個(gè)粒子濾波中，粒子數(shù)量均為500。實(shí)驗(yàn)結(jié)果表明，本文所提算法具有更優(yōu)的表現(xiàn)，如表1所示。與文獻(xiàn)[22]方法相比，位置精度提升55%，角度精度提升56%，且估計(jì)軌跡更加平滑，誤差分布波動(dòng)更穩(wěn)定，證明算法性能更穩(wěn)定可靠，如圖7所示。

表1 實(shí)驗(yàn)1：不同估計(jì)方法在不同的UWB節(jié)點(diǎn)配置下，位置誤差以及角度誤差的評(píng)估結(jié)果

為驗(yàn)證算法在非視距、非規(guī)則路徑條件下的性能。設(shè)置實(shí)驗(yàn)1-b，機(jī)器人1與機(jī)器人2之間被木板遮擋，且機(jī)器人1在實(shí)驗(yàn)場(chǎng)地中沿不規(guī)則路徑移動(dòng)，如圖6所示。該實(shí)驗(yàn)中，設(shè)置UWB節(jié)點(diǎn)距離配置為0.5 m，滑動(dòng)窗口長(zhǎng)度為ω=30以及粒子數(shù)量為500。估計(jì)軌跡如圖6所示，里程計(jì)在長(zhǎng)期運(yùn)行過(guò)程中出現(xiàn)明顯的累計(jì)誤差，位置誤差和角度誤差分別為0.48 m和8.61°。通過(guò)本文所提出的算法，軌跡得到明顯的提升，與真實(shí)軌跡相比，獲得0.29 m的位置誤差和4.39°的角度誤差，該結(jié)果證明了該算法在上述條件下的有效性。

4.3 實(shí)驗(yàn)2

為充分驗(yàn)證本文所提方法的實(shí)用性，設(shè)置實(shí)驗(yàn)2，3個(gè)移動(dòng)機(jī)器人被控制沿不同的軌跡同時(shí)運(yùn)動(dòng)，且每個(gè)機(jī)器人起點(diǎn)不同，如圖5(c)所示。綜合實(shí)驗(yàn)1分析結(jié)果，在實(shí)驗(yàn)2中，確保定位精度和系統(tǒng)實(shí)時(shí)性，設(shè)置滑動(dòng)窗口長(zhǎng)度為ω=30，UWB節(jié)點(diǎn)距離為0.5 m。表2給出不同定位算法的精度。如表2所示，3個(gè)移動(dòng)機(jī)器人在僅使用里程計(jì)的情況下，獲得相對(duì)位置誤差和相對(duì)角度誤差分別為1.466 m和22.076°。3個(gè)機(jī)器人里程計(jì)估計(jì)軌跡如圖8(a)所示，顯而易見，隨著時(shí)間的累計(jì)3個(gè)移動(dòng)機(jī)器人的里程計(jì)都發(fā)生了不同程度的漂移，其中機(jī)器人1的里程計(jì)漂移最為嚴(yán)重。與里程計(jì)相比，僅利用UWB數(shù)據(jù)通過(guò)非線性優(yōu)化估計(jì)得到的相對(duì)姿態(tài)，精度得到提高，且不存在累計(jì)誤差。如圖8(b)所示，假設(shè)機(jī)器人1姿態(tài)已知，估計(jì)機(jī)器人2和機(jī)器人3相對(duì)于機(jī)器人1的姿態(tài)，獲得位置誤差0.528 m和角度誤差7.413°。由于UWB距離測(cè)量數(shù)據(jù)的誤差，造成估計(jì)值產(chǎn)生無(wú)規(guī)則的波動(dòng)，且在個(gè)別位置誤差較大。

表2 實(shí)驗(yàn)2：不同估計(jì)方法在w=30及UWB間距0.5 m情況下評(píng)估結(jié)果

利用圖優(yōu)化算法對(duì)滑動(dòng)窗口中的數(shù)據(jù)進(jìn)行優(yōu)化，通過(guò)相鄰時(shí)刻里程計(jì)的相對(duì)位移對(duì)非線性優(yōu)化結(jié)果進(jìn)行約束，進(jìn)一步提升定位精度，被優(yōu)化后的滑動(dòng)窗口組成的運(yùn)動(dòng)軌跡如圖8(c)所示。在假設(shè)機(jī)器人1姿態(tài)已知的情況下，獲得位置誤差0.424 m和角度誤差5.648°。與非線性優(yōu)化相比，被優(yōu)化后的滑動(dòng)窗口在位置誤差和角度誤差上分別下降了19%和23%。

文獻(xiàn)[22]所提的基于粒子濾波融合IMU和UWB距離測(cè)量值的相對(duì)定位方法的結(jié)果與本文所提粒子濾波算法對(duì)比，其估計(jì)軌跡如圖8(d)和圖8(e)所示。粒子數(shù)量均設(shè)為500。本文的粒子濾波算法，融合滑動(dòng)窗口當(dāng)前時(shí)刻估計(jì)值和里程計(jì)測(cè)量值，獲得位置誤差0.312 m和角度誤差4.903°。相較滑動(dòng)窗口優(yōu)化結(jié)果，精度分別提升了26%和13%。采用文獻(xiàn)[22]的方法，將UWB距離值和里程計(jì)通過(guò)粒子濾波融合，取得位置誤差0.534 m和角度誤差7.234°。本文所提方法在位置精度和角度精度上都得到極大提升，精度分別提升41%和32%。另外，與實(shí)驗(yàn)1相比，由于移動(dòng)機(jī)器人數(shù)量的增加以及實(shí)驗(yàn)空間的擴(kuò)大，UWB的測(cè)距誤差也隨之增加，因此整體表現(xiàn)稍差。

本文涉及的相關(guān)算法，在一款內(nèi)存為12.0 GB搭載Intel i5-6300HQ 2.30 GHz CPU的裝有ROS系統(tǒng)的筆記本電腦上運(yùn)行。相關(guān)算法所消耗的計(jì)算時(shí)間如圖9所示，本文提出的基于粒子濾波融合滑動(dòng)窗口和里程計(jì)的方法需消耗時(shí)間86 ms，其中包括：非線性優(yōu)化、ω=30的滑動(dòng)窗口以及粒子數(shù)為500的粒子濾波器總共花費(fèi)的計(jì)算時(shí)間，如圖9所示。對(duì)比文獻(xiàn)[22]所提粒子濾波算法，時(shí)間消耗大約為其2倍。本文所提方法能夠達(dá)到大約12 Hz的輸出頻率，亦能滿足大多數(shù)的機(jī)器人應(yīng)用。

5 結(jié)束語(yǔ)

本文提出一種基于對(duì)UWB與里程計(jì)融合的移動(dòng)機(jī)器人相對(duì)定位方法。在僅采用UWB傳感器的情況下，通過(guò)非線性優(yōu)化算法能夠?qū)崿F(xiàn)機(jī)器人間的相對(duì)姿態(tài)估計(jì)。利用相鄰時(shí)刻里程計(jì)的相對(duì)位移對(duì)非線性優(yōu)化結(jié)果進(jìn)行約束，提升定位精度。另外，為確保系統(tǒng)實(shí)時(shí)性，利用滑動(dòng)窗口的思想僅對(duì)特定時(shí)間內(nèi)的數(shù)據(jù)進(jìn)行優(yōu)化。由于UWB相對(duì)姿態(tài)約束中的信息矩陣的不確定，對(duì)優(yōu)化結(jié)果造成影響，因此基于粒子濾波融合滑動(dòng)窗口優(yōu)化后的機(jī)器人間的相對(duì)姿態(tài)和里程計(jì)，進(jìn)一步提升相對(duì)姿態(tài)估計(jì)精度。通過(guò)3個(gè)配置 (UWB距離配置和各傳感器收發(fā)頻率) 相同的移動(dòng)機(jī)器人在12 m×6 m的室內(nèi)場(chǎng)地進(jìn)行實(shí)地測(cè)試，實(shí)驗(yàn)結(jié)果表明，本算法在滿足實(shí)時(shí)定位的基礎(chǔ)上，能夠達(dá)到0.312 m的定位誤差以及4.903°的角度誤差。為多機(jī)器人相對(duì)定位提供了一種具有參考意義的方法。在后續(xù)的研究中，探索其他更有效的方式來(lái)提升精度以及減少時(shí)間消耗，并投入到多機(jī)器人協(xié)作與編隊(duì)控制中。