李 陽，吳明暉，陳遠浩

（上海工程技術(shù)大學 機械與汽車工程學院，上海 201620）

0 引言

近年來，隨著無人駕駛、人工智能等技術(shù)的發(fā)展，移動機器人越來越受到人們的關(guān)注。移動機器人技術(shù)由諸多技術(shù)構(gòu)成，其中定位技術(shù)最重要、最根本。準確可靠的定位與測姿是移動機器人完成任務的先決條件［1］。

通過學者的不斷探索與研究，目前室內(nèi)移動機器人使用最為普遍的定位算法為自適應蒙特卡洛定位算法（Adaptive Monte Carlo Localization，AMCL），該算法在局部定位方面表現(xiàn)良好，位姿跟蹤準確［2］，但隨著應用發(fā)現(xiàn)，傳統(tǒng)AMCL 算法在全局定位能力上有所不足。機器人每次開始定位工作時，必須給定機器人的初始位姿后，才能進行位姿跟蹤。否則，機器人就必須對環(huán)境進行大量的探索觀測，才能完成位姿估計收斂。人為控制機器人探索環(huán)境會增加工人負擔；讓機器人自主進行環(huán)境探索效率低下并具有危險性。不僅如此，由于缺乏全局定位能力，在機器人發(fā)生打滑、碰撞、人為搬運等機器人綁架問題時，機器人也無法進行位姿糾正，從而造成定位失效。這些問題與不足，都嚴重限制著移動機器人在實際使用中的智能性與可靠性，而通過全局定位系統(tǒng)進行融合輔助是一種可行的解決方案［3］。

目前，全局定位系統(tǒng)主要有WIFI 定位、超寬頻定位（Ultra-Wideband，UWB）、超聲波定位、藍牙定位和射頻識別定位等［4-5］諸多方法。如：Song Xu 等人［6］用WIFI 先進行粗定位，利用WIFI 的指紋識別信息對傳統(tǒng)AMCL 算法的粒子進行初始化，一定程度上提高了傳統(tǒng)AMCL 算法的粒子收斂速度。Yan Wang 等人［7］將UWB 與AMCL 算法相結(jié)合，提出了UFMCL 算法，使其能有效地進行全局定位和機器人綁架復原?？紤]到超聲波的低成本、定位距離遠、定位精準等特點，本文決定采用超聲波作為全局空間定位系統(tǒng)，對AMCL 算法進行融合與輔助。

1 超聲波與AMCL 融合定位模型

1.1 超聲波雙信標定位方法

超聲波是指頻率大于20 kHz 的聲波，其本質(zhì)是一種機械波，依靠介質(zhì)進行傳播［8］。超聲波具有穿透性強、方向性好、能量集中以及反射性好等特點，廣泛應用于測距和定位領域。如圖1 所示，超聲波定位主要依靠三角定位原理［9］，通過固定信標和移動信標之間的距離，計算被測物的空間坐標。

圖1 三角定位原理Fig.1 Principle of triangulation

根據(jù)歐式距離，被測目標坐標計算公式為：

然而，傳統(tǒng)的超聲波定位系統(tǒng)只能反映機器人的空間位置（x0，y0），但無法得出機器人的偏航角yaw，在缺失機器人偏航角的情況下，機器人仍然無法確定所有姿態(tài)信息。因此，提出雙信標定位方法。通過安裝在移動機器人上的兩個信標A1 和A2 的中垂線與世界坐標系的正方向夾角，來解算機器人的偏航角大小，如圖2 所示。

圖2 雙信標定位法Fig.2 Double beacon positioning method

則機器人的空間坐標和偏航角為：

1.2 基于EKF 的超聲定位與IMU 融合模型

由于超聲波的多孔徑效應，當超聲波信標的發(fā)射端和接收端之間被障礙物干擾時，定位誤差會顯著增大，而慣性測量單元（Inertial measurement unit，IMU）卻具有獨特的檢測優(yōu)勢［10］。IMU 不受外界干擾，檢測頻率很高，同時加速度和角速度檢測精準，這些優(yōu)勢能很好的彌補超聲波易受動態(tài)障礙物干擾這一不足。因此，本文采用超聲波定位與IMU 融合的方案，來保證機器人運動狀態(tài)下的定位。

在機器人上增加了一個IMU，x軸方向與機器人本體坐標系以及超聲波定位系統(tǒng)坐標系的x軸一致?；贗MU 的輸出量，對機器人的運動狀態(tài)進行分析：

式中：（xt，yt）為機器人當前時刻位置，（vx（t），vy（t））為機器人當前運動速度，θt為機器人當前偏航角，Δt為機器人控制周期，εx，εy，εvx，εvy，εθ為空間位置、速度和偏航角的隨機誤差，at為機器人x軸方向加速度。

從公式（4）可以看出，狀態(tài)方程為非線性模型，而EKF 濾波算法非常適合非線性高斯模型，且融合精度高，相對其他濾波器（如UKF、PF 等）計算量也較少。因此，本文采用EKF 進行狀態(tài)融合。為方便融合算法分析，將公式（4）用向量形式表達為

式 中：xt ＝（xt，yt，vx（t），vy（t），θt），ut ＝（at，θt），ε ＝（εx，εy，εvx，εvy，εθ），且ε～N（0，R）。

以上是根據(jù)IMU 傳感器輸出的信息完成的移動機器人推測模型，下面根據(jù)超聲波定位系統(tǒng)建立觀測模型。

根據(jù)超聲波定位原理可以獲得4 個直接觀測量，分別為兩個移動信標在平面內(nèi)的坐標（xA1，yA1）和（xA2，yA2），并根據(jù)雙信標定位方法，得到5 個觀測量：

因此，建立的觀測方程為

式中：觀測誤差δ符合高斯分布N（0，Q），Q為標準方差。

獲得推測模型和觀測模型之后，則可進行EKF融合。其實現(xiàn)步驟如下：

Step 1狀態(tài)預測

Step 2協(xié)方差預測

其中：

Step 3更新卡爾曼濾波增益

其中：

Step 4更新狀態(tài)

Step 5更新協(xié)方差

如此操作，便可以不斷獲得機器人的空間位姿狀態(tài)以及協(xié)方差。

在matlab 中設計三角定位仿真，隨機選取不共線的3 個點（5，41）、（35，10）、（53，30），作為空間中的固定信標。移動信標運動軌跡如圖3 所示，將IMU 運動輸出狀態(tài)與超聲定位結(jié)果進行融合解算。運動過程中，超聲波測距引入較大誤差，模仿障礙物干擾。超聲波定位結(jié)果和融合定位結(jié)果如圖4 所示，誤差對比結(jié)果如圖5 所示。

圖3 機器人超聲定位軌跡圖Fig.3 The trajectory of ultrasonic localization

圖4 仿真定位結(jié)果軌跡對比圖Fig.4 Comparison diagram of simulation location result track

圖5 仿真定位結(jié)果誤差對比圖Fig.5 Comparison diagram of error of simulation positioning results

從仿真定位結(jié)果軌跡對比圖和誤差對比圖中可見，雙信標定位方法雖然能解算出偏航角，獲得機器人完整位姿，但易受障礙物干擾影響，而通過與IMU信息進行融合，可有效降低系統(tǒng)定位的誤差，減小動態(tài)障礙對超聲波系統(tǒng)的定位干擾。

1.3 超聲波系統(tǒng)與AMCL 融合定位模型

1.3.1 傳統(tǒng)AMCL 定位算法

AMCL 定位算法本質(zhì)是基于大數(shù)定律的，該算法的定位方法可以概括為3 個階段：

（1）粒子分布初始化階段：定位開始時，機器人必須處于指定起點，所有初始粒子以指定起點和設定方差參數(shù)進行隨機分布，這樣可將全局定位問題轉(zhuǎn)化成局部定位問題。

（2）粒子更新階段：采樣粒子集｛x（i），i ＝1，2，...，N｝是通過輪式里程計測量的數(shù)據(jù)分布P（xt（i）｜m，x（i）1：t -1，z1：t-1，u1：t-1）而生成的。式中，m為先驗柵格地圖信息；z1：t-1為開始到t -1時刻的觀測信息；u1：t-1為開始到t -1 時刻的運動控制變量。

采集粒子集之后，需對每個粒子進行權(quán)重評估和歸一化，如公式（16）：

根據(jù)權(quán)重評估和歸一化的結(jié)果，對粒子集進行重采樣，使之符合后驗分布，且利用KLD 采樣方法，來解決粒子退化問題。在KLD 采樣過程中，后驗概率分布被視為離散分段的單位分布，表示目標分布的子空間。通過引入KL（Kullback-Leibler）散度，也叫相對熵。KLD 采樣方法用一個公式描述估計的概率分布與真實的概率分布之間的近似度，并將兩者誤差限制在一定范圍內(nèi)。公式（17）為推導出的KLD 所需要樣本的最小值Nkld：

其中：Nb為樣本所占子空間的個數(shù)；ε為目標分布誤差的最大值；1-δ為誤差小于ε的概率。

當生成的粒子數(shù)大于Nkld時，采樣過程停止。

（3）位置確定階段：重復步驟（1）、（2），不斷更新機器人采樣粒子集，最后以粒子位置的最大權(quán)重作為機器人的估計位置。即系統(tǒng)在t時刻的狀態(tài)由公式（18）得出：

算法最終的建議分布越接近目標分布，粒子濾波器就越好。

1.3.2 改進超聲定位與AMCL 融合定位算法

由于傳統(tǒng)AMCL 定位算法只在局部定位時較為準確，而在全局定位以及機器人綁架問題上仍存在缺陷。因此，提出將改進的超聲波定位系統(tǒng)與AMCL 定位算法進行結(jié)合。

首先，由超聲波定位系統(tǒng)進行三點定位和雙信標偏航角解算，從而獲得移動機器人包括空間位置與偏航角等所有姿態(tài)。然后，對移動機器人進行工作狀態(tài)判斷：如果移動機器人處于靜止狀態(tài)，則將求解出的位姿用來對AMCL 算法的定位粒子進行初始化。通過超聲波定位系統(tǒng)提供的全局定位信息作為參考，可省略傳統(tǒng)AMCL 算法在全局定位過程中大量的運動探索，來完成粒子收斂的過程，且使得全局定位過程更加安全、高效。如果移動機器人處于動態(tài)，將直接輸出超聲波系統(tǒng)解算出的機器人位姿，并與車載IMU 傳感器進行融合，從而保證超聲波定位系統(tǒng)的輸出結(jié)果不受動態(tài)障礙物影響。最終在運動過程中，超聲波與IMU 的初步融合結(jié)果，進一步與AMCL 的定位結(jié)果通過EKF 濾波器進行融合，來糾正AMCL 可能發(fā)生的機器人綁架問題，保障系統(tǒng)定位的可靠性。

2 實驗驗證

2.1 機器人平臺及實驗環(huán)境搭建

機器人平臺如圖6 所示，上面分別安裝超聲波雷達、輪式里程計和激光雷達，這些傳感器共同完成對環(huán)境空間的感知，最終確定機器人的空間位置和姿態(tài)。超聲波雷達裝在車頂，可保證信號的接收，激光雷達高度約為0.2 m，可識別定位空間中絕大多數(shù)障礙物。機器人搭載工控機，內(nèi)存為8 GB，處理器為Intel 的i7-8650U，1.9 GHz。系統(tǒng)為Ubuntu14.04，各類實驗結(jié)果輸出主要使用了ROS 系統(tǒng)下的Rviz 可視化工具。

圖6 實驗平臺組成Fig.6 Composition of the experimental platform

實驗采用Marvelmind robtics 公司的HW4.9 型超聲波信標，該信標有效探測距離可達30 m，定位精度為±5 cm，調(diào)制器可對半徑150 m 以內(nèi)所有信標進行時間同步；溫度感應器，可針對當前室溫進行溫度補償；工作頻率可調(diào)。信標安裝布局以及機器人運動軌跡如圖7 所示。固定信標、移動信標和激光所掃描障礙輪廓如圖7（a）所示。定位系統(tǒng)以20 Hz的頻率不斷更新輸出定位坐標，機器人在實驗室環(huán)境下沿著A、B、C、D 4 點所圍成的矩形路徑進行運動，如圖7（b）所示。

圖7 超聲波信標分布和機器人軌跡圖Fig.7 Ultrasonic beacon distribution and robot trajectory diagram

2.2 實驗結(jié)果與分析

2.2.1 實驗一：局部定位實驗

超聲波定位系統(tǒng)單獨定位軌跡及與IMU 融合后的軌跡如圖8 所示。

圖8 超聲波定位系統(tǒng)定位軌跡圖Fig.8 The positioning trajectory diagram of Ultrasonic positioning system

可以看出，超聲波定位效果較好，定位精度在厘米級別，但在箭頭所指的動態(tài)障礙物干擾處，發(fā)生了定位異常，出現(xiàn)定位丟失或者較大的跳變。而將超聲波定位結(jié)果與IMU 進行融合后，IMU 的測姿連貫性對超聲波定位的定位異常產(chǎn)生了很好的約束作用，提高了超聲波定位系統(tǒng)的定位可靠性。

如圖9 所示，實驗中也使用了傳統(tǒng)AMCL 算法進行定位，傳統(tǒng)AMCL 算法可以獲得較好的定位結(jié)果，但僅僅依靠里程計推測航跡，激光雷達進行觀測，當環(huán)境觀測的匹配度降低時，依靠采樣粒子所解算出的位置差會變大，在本次實驗中最大定位誤差達到了25 cm。而本文提出的超聲波與AMCL 融合定位算法的解算結(jié)果與參考軌跡重合度較高，整個定位過程位姿跟蹤準確。通過EKF 濾波對超聲波定位系統(tǒng)的輸出結(jié)果與AMCL 定位結(jié)果進行融合，增強了整個系統(tǒng)的定位魯棒性和準確性。

圖9 AMCL 定位和融合算法定位軌跡圖Fig.9 AMCL localization and fusion algorithm localization trajectory diagram

本文融合算法與單一傳感器定位、傳統(tǒng)AMCL算法定位對比效果如圖10 所示。表1 中給出了里程計、超聲波、AMCL 定位以及本文融合定位算法在X和Y方向上的誤差統(tǒng)計?？梢钥闯?，融合算法的定位誤差小于3 cm，證明了融合算法在局部定位方面的優(yōu)勢。

圖10 融合算法與其他定位方式軌跡對比圖Fig.10 Comparison between fusion algorithm and other positioning methods

表1 各種定位結(jié)果誤差統(tǒng)計／mTab.1 Error statistics of various positioning results／m

2.2.2 實驗二：融合算法的全局定位時間測試

在地圖中任意選擇10 個位置進行定位，分別記錄在單純依靠AMCL 算法的全局定位時間和融合算法下的定位時間。實驗結(jié)果見表2。

表2 改進算法定位時間比較Tab.2 Comparison of location time of improved algorithm

由對比結(jié)果可見，本文提出的融合超聲波傳感器的定位算法可以大大節(jié)約傳統(tǒng)AMCL 定位算法的粒子收斂時間；通過超聲波信息更新全局定位，也避免了機器人在粒子收斂過程的運動探索，增強了移動機器人定位的智能性與安全性。

3 結(jié)束語

本文設計了一種融合超聲波信息的改進自適應蒙特卡洛定位算法，解決了傳統(tǒng)AMCL 算法在全局定位方面效率低、不成功等問題。在本算法中，當進行全局定位時，通過雙信標解算機器人姿態(tài)，并與IMU 進行信息融合，得到了可靠的全局定位信息，依靠此信息可迅速完成AMCL 算法的全局定位粒子初始化，避免了傳統(tǒng)AMCL 算法漫長的粒子收斂過程，大大節(jié)約了機器人全局定位時間。同時，改進的超聲波全局定位信息與AMCL 定位進一步融合，在一定程度上提高了位姿跟蹤的定位精度，改善了定位效果，也保障了最終定位的穩(wěn)定性與可靠性。