融合超聲波信息的機(jī)器人室內(nèi)定位方法研究

李 陽，吳明暉，陳遠(yuǎn)浩

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

0 引言

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

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

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

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

1.1 超聲波雙信標(biāo)定位方法

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

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

根據(jù)歐式距離，被測(cè)目標(biāo)坐標(biāo)計(jì)算公式為：

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

圖2 雙信標(biāo)定位法Fig.2 Double beacon positioning method

則機(jī)器人的空間坐標(biāo)和偏航角為：

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

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

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

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

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

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

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

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

因此，建立的觀測(cè)方程為

式中：觀測(cè)誤差δ符合高斯分布N（0，Q），Q為標(biāo)準(zhǔn)方差。

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

Step 1狀態(tài)預(yù)測(cè)

Step 2協(xié)方差預(yù)測(cè)

其中：

Step 3更新卡爾曼濾波增益

其中：

Step 4更新狀態(tài)

Step 5更新協(xié)方差

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.3.2 改進(jìn)超聲定位與AMCL 融合定位算法

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

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

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

2.1 機(jī)器人平臺(tái)及實(shí)驗(yàn)環(huán)境搭建

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

圖6 實(shí)驗(yàn)平臺(tái)組成Fig.6 Composition of the experimental platform

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

圖7 超聲波信標(biāo)分布和機(jī)器人軌跡圖Fig.7 Ultrasonic beacon distribution and robot trajectory diagram

2.2 實(shí)驗(yàn)結(jié)果與分析

2.2.1 實(shí)驗(yàn)一：局部定位實(shí)驗(yàn)

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

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

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

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

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

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

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

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

2.2.2 實(shí)驗(yàn)二：融合算法的全局定位時(shí)間測(cè)試

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

表2 改進(jìn)算法定位時(shí)間比較Tab.2 Comparison of location time of improved algorithm

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

3 結(jié)束語

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