基于多傳感器信息融合的自主跟隨定位及避障方法*

王正家,夏正喬,孫楚杰,王 幸,李 明

(湖北工業大學現代制造質量工程重點實驗室,武漢 430068)

自主跟隨機器人被廣泛的應用于工業、農業及日常生活等多個領域,在自由空間中對移動目標的精準定位是保證其跟隨性能的關鍵,也是長期研究的重點。目前,主要的定位技術有視覺導航定位、GPS導航定位和聲波定位等[1-3]。其中視覺導航定位技術通過識別特征標識實現定位,不僅算法復雜,而且易受環境影響從而丟失目標[4-5];GPS導航定位受場地影響較大,在室內或者城市建筑之間,GPS信號容易丟失,不適用于自由空間[6];聲波定位技術主要采用超聲波定位,對場地要求較低,且不會被光照等環境因素影響,因此它適合用于自主跟隨機器人的定位。文獻[7-9]在移動目標上用單一超聲波信號源發射超聲波信號,在自主跟隨機器人上接收超聲波信號,從而確定移動目標位置,雖然能實現定位,但是由于超聲波傳感器發射角度小,自主跟隨機器人很容易進入超聲波信號盲區,導致跟隨失敗,并且該方法不能檢測移動目標實時移動姿態;當自主跟隨機器人遇到障礙時,只檢測障礙物距離,不檢測障礙物方位,自主跟隨機器人避障時可利用信息較少。

因此,針對上述問題,本文提出了一種基于多傳感器信息融合的自主跟隨定位及避障方法,該方法通過在移動目標上增加多信號源,使自主跟隨機器人與移動目標之間建立冗余信道,彌補單一信號源發射角度小的問題;利用三角定位法測量信號源坐標,并通過最小二乘逼近計算移動目標中心位置;對信號源進行編號,根據測量出的信號源位置、移動目標中心位置和信號源編號,計算移動目標實時移動姿態;在自主跟隨機器人上設置測距模塊,當自主跟隨機器人丟失信號源信號時,能夠根據測距模塊的信息保持跟隨。實驗結果驗證了該方法的有效性。

1 移動目標的基本定位方法

三角定位法是一種常見的測量物體的距離和方位的方法[10],其原理圖如圖1所示。已知A、B的坐標分別為(-a,0)、(a,0)以及被測物M相對于A、B的距離la、lb,計算出M當前的坐標(x,y)及M到A、B中點O的距離d和方向偏角θ。

圖1 三角定位法

由幾何關系可知:

(1)

可得出M點坐標:

(2)

(3)

M的定位特征數據d、方向偏角θ分別為:

(4)

θ=arctan(y/x)

(5)

在工程應用中,通常在被測物M上設置信號源,在測量物的A,B處設置信號接收節點,信號接受節點A,B接收到信號源M發射的信號后,便可計算出距離la、lb,從而計算出M當前的坐標(x,y)及M到A、B中點O的距離d和方向偏角θ。

2 多信號源定位及避障模型

本論文設計的多信號源定位模型原理圖如圖2所示。將N個信號源Mi(i=1,2,3,…,N)布置在移動目標上,這些信號源在以移動目標中心點M為圓心、R為半徑的圓上等角度均勻分布;在跟隨機器人上布置信號接收節點A、B和測距模塊C、D、E,其中測距模塊C、D分別與信號接收節點A、B鄰近,測距模塊E布置在A、B中點O處。

圖2 多傳感器信息融合定位模型

3 多信號源定位及避障算法

為了具體的計算出移動目標中心坐標M(x,y)、距離d、方向偏角θ和實時移動姿態,并且在遇障時自主跟隨機器人能正確的調整運動速度和方向,本文根據上述多信號源定位模型設計了多信號源定位算法。該方法分為計算移動目標中心坐標及特征數據、計算實時移動姿態和遇障分析。

3.1 計算移動目標中心坐標及特征數據

通過從信號源M1開始,到信號源MN發送信號完成截止,以順時針的方向依次進行信號源切換,獲得信號源Mi(i=1,2,…,N)的坐標和特征數據di和θi。對于任意兩個信號源Mj和Mk可以求出對應的移動中心位置Mojk及其特征數據(距離dojk和方向偏角θojk),其原理圖如圖3所示。

圖3 中心位置Mojk計算

圖中Mj(xj,yj)、Mk(xk,yk)分別表示信號源Mj、Mk的坐標,其中j>k。設Mojk的坐標為(xojk,yojk),由幾何關系可知:

(6)

由式(6)算出的Mojk的坐標(xojk,yojk)有兩個,并不能確定移動中心位置。本文使用向量叉積[11]的方法來確定移動中心位置。用MojkMj表示由Mojk到Mj的向量,用MojkMk表示由Mojk到Mk的向量,則MojkMk×MojkMj的行列式為:

(7)

由于信號源是順時針編號排布的,則:

(8)

即:

(xk-xojk)(yj-yojk)-(xj-xojk)(yk-yojk)<0

(9)

(10)

(11)

3.2 計算實時移動姿態

圖4 移動姿態計算

(12)

3.3 遇障分析

當信號源由M1到MN切換發送信號完成后,信號接收節點A、B都沒有接收到任意信號源的信號或者其中一個信號接受節點沒有接收到任意信號源的信號,自主跟隨機器人將會啟動測距模塊C、D、E,測距模塊會進行測距,根據測距結果,自主跟隨機器人重新調整運動速度和方向。在自主跟隨機器人與移動目標之間的障礙有如圖5所示5種方式。

圖5 遇障方式

當障礙物狀態如圖5(a)所示時,測距模塊C將測出自主跟隨機器人到障礙物的距離,D、E不能測到障礙物,此時自主跟隨機器人可以判斷出左側有障礙物,并根據障礙物的距離,控制向右轉向的角度和速度,圖5(e)狀態與此相似;當障礙物狀態如圖5(b)所示時,測距模塊C、E將測出自主跟隨機器人到障礙物的距離,D不能測到障礙物,此時自主跟隨機器人可以判斷出左側和中間有障礙物,并根據障礙物的距離,控制向右轉向的角度和速度,圖5(d)狀態與此相似;當障礙物狀態如圖5(c)所示時,測距模塊C、D、E將測出自主跟隨機器人到障礙物的距離,此時自主跟隨機器人只能通過提示告知移動目標已丟失跟蹤目標。

4 多信號源定位及避障實驗

根據某工程的應用需求,需要在500 mm～2500 mm范圍內實現自主跟隨,跟隨精度要求為±30 mm。本次實驗在移動目標上的以100 mm為半徑的圓上均勻安裝12個超聲波傳感器(JSN-SR04T分體式超聲波傳感器,其最遠射程可達4 m,精度為5 mm,發射角度60°),并將超聲波傳感器順時針從1開始編號,其中1號超聲波傳感器正對前進方向。在自主跟隨跟隨機器人上的兩個信號接收節點距離為400 mm,超聲波測距模塊(HC-SR04一體式超聲波傳感器,其最遠射程可達4 m,精度為5 mm)以200 mm間隔排布,整個裝置的示意圖如圖2所示。本文使用無線模塊(NRF24L01)作為通信模塊和同步超聲波信號的發射與接收。

為了測試本文方法的定位精度和定位效果,本文設計了一個自主跟隨定位實驗。將自主跟隨機器人固定不動,使移動目標在距離兩信號接受節點所連成的直線50 0 mm、1 000 mm、1 500 mm、2 000 mm、2 500 mm的平行直線上移動,起點和終點在垂直方向上距離O點1 000 mm,如圖6所示。

圖6 定位實驗示意圖

從起點開始,每隔100 mm記錄一個數據,將測量結果用MATLAB繪制成圖,其軌跡結果如圖7所示。所記錄的數據x坐標誤差、y坐標誤差如圖8所示。移動姿態方向偏差如圖9所示。

圖7 測量軌跡

圖8 x、y坐標誤差曲線

圖9 移動姿態方向偏差

由圖7可知,自主跟隨機器人測量移動目標的軌跡與實際軌跡偏差很小,由圖8、圖9可知,x軸誤差和y軸誤差在±30 mm以內,方向偏角在±15°以內,能夠滿足跟隨的精度要求。

為了驗證本方法的避障能力,使自主跟隨機器人隨移動目標前進,并在其跟隨過程中添加如圖5所示障礙,實驗結果顯示,障礙在圖5(a)、5(b)、5(d)、5(e)狀態下,自主跟隨機器人能避開障礙物,及時跟隨移動目標前進,在圖5(c)狀態下,自主跟隨機器人丟失跟隨目標。

5 結論

本文提出了一種基于多傳感器信息融合的自主跟隨定位及避障方法,該方法通過增加多信號源,使自主跟隨機器人與移動目標之間建立冗余信道,彌補了單一信號源發射角度小的問題;通過多傳感器信息融合,能夠準確的計算出移動目標的位置和實時移動姿態;合理的使用測距模塊,能夠有效的避開多種情況下的障礙。本文建立了多信號源模型和算法,對該算法進行了實驗,實驗結果表明,該算法能夠準確的對移動目標進行定位和移動姿態測量,并且能夠避開障礙,保持跟隨,具有一定的工程應用價值。