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

(湖北工業(yè)大學(xué)現(xiàn)代制造質(zhì)量工程重點(diǎn)實(shí)驗(yàn)室,武漢 430068)

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

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

1 移動(dòng)目標(biāo)的基本定位方法

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

圖1 三角定位法

由幾何關(guān)系可知:

(1)

可得出M點(diǎn)坐標(biāo):

(2)

(3)

M的定位特征數(shù)據(jù)d、方向偏角θ分別為:

(4)

θ=arctan(y/x)

(5)

在工程應(yīng)用中,通常在被測物M上設(shè)置信號(hào)源,在測量物的A,B處設(shè)置信號(hào)接收節(jié)點(diǎn),信號(hào)接受節(jié)點(diǎn)A,B接收到信號(hào)源M發(fā)射的信號(hào)后,便可計(jì)算出距離la、lb,從而計(jì)算出M當(dāng)前的坐標(biāo)(x,y)及M到A、B中點(diǎn)O的距離d和方向偏角θ。

2 多信號(hào)源定位及避障模型

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

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

3 多信號(hào)源定位及避障算法

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

3.1 計(jì)算移動(dòng)目標(biāo)中心坐標(biāo)及特征數(shù)據(jù)

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

圖3 中心位置Mojk計(jì)算

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

(6)

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

(7)

由于信號(hào)源是順時(shí)針編號(hào)排布的,則:

(8)

即:

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

(9)

(10)

(11)

3.2 計(jì)算實(shí)時(shí)移動(dòng)姿態(tài)

圖4 移動(dòng)姿態(tài)計(jì)算

(12)

3.3 遇障分析

當(dāng)信號(hào)源由M1到MN切換發(fā)送信號(hào)完成后,信號(hào)接收節(jié)點(diǎn)A、B都沒有接收到任意信號(hào)源的信號(hào)或者其中一個(gè)信號(hào)接受節(jié)點(diǎn)沒有接收到任意信號(hào)源的信號(hào),自主跟隨機(jī)器人將會(huì)啟動(dòng)測距模塊C、D、E,測距模塊會(huì)進(jìn)行測距,根據(jù)測距結(jié)果,自主跟隨機(jī)器人重新調(diào)整運(yùn)動(dòng)速度和方向。在自主跟隨機(jī)器人與移動(dòng)目標(biāo)之間的障礙有如圖5所示5種方式。

圖5 遇障方式

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

4 多信號(hào)源定位及避障實(shí)驗(yàn)

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

為了測試本文方法的定位精度和定位效果,本文設(shè)計(jì)了一個(gè)自主跟隨定位實(shí)驗(yàn)。將自主跟隨機(jī)器人固定不動(dòng),使移動(dòng)目標(biāo)在距離兩信號(hào)接受節(jié)點(diǎn)所連成的直線50 0 mm、1 000 mm、1 500 mm、2 000 mm、2 500 mm的平行直線上移動(dòng),起點(diǎn)和終點(diǎn)在垂直方向上距離O點(diǎn)1 000 mm,如圖6所示。

圖6 定位實(shí)驗(yàn)示意圖

從起點(diǎn)開始,每隔100 mm記錄一個(gè)數(shù)據(jù),將測量結(jié)果用MATLAB繪制成圖,其軌跡結(jié)果如圖7所示。所記錄的數(shù)據(jù)x坐標(biāo)誤差、y坐標(biāo)誤差如圖8所示。移動(dòng)姿態(tài)方向偏差如圖9所示。

圖7 測量軌跡

圖8 x、y坐標(biāo)誤差曲線

圖9 移動(dòng)姿態(tài)方向偏差

由圖7可知,自主跟隨機(jī)器人測量移動(dòng)目標(biāo)的軌跡與實(shí)際軌跡偏差很小,由圖8、圖9可知,x軸誤差和y軸誤差在±30 mm以內(nèi),方向偏角在±15°以內(nèi),能夠滿足跟隨的精度要求。

為了驗(yàn)證本方法的避障能力,使自主跟隨機(jī)器人隨移動(dòng)目標(biāo)前進(jìn),并在其跟隨過程中添加如圖5所示障礙,實(shí)驗(yàn)結(jié)果顯示,障礙在圖5(a)、5(b)、5(d)、5(e)狀態(tài)下,自主跟隨機(jī)器人能避開障礙物,及時(shí)跟隨移動(dòng)目標(biāo)前進(jìn),在圖5(c)狀態(tài)下,自主跟隨機(jī)器人丟失跟隨目標(biāo)。

5 結(jié)論

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