陳躍躍林海翔王 侃徐孝東胡 超馮忠晴,2
1(浙江大學(xué)寧波理工學(xué)院信息科學(xué)與工程學(xué)院 寧波 315100)
2(太原科技大學(xué)電子信息工程學(xué)院 太原 030024)
一種簡化的室內(nèi)機(jī)器人電磁定位算法與系統(tǒng)
陳躍躍1林海翔1王 侃1徐孝東1胡 超1馮忠晴1,2
1(浙江大學(xué)寧波理工學(xué)院信息科學(xué)與工程學(xué)院 寧波 315100)
2(太原科技大學(xué)電子信息工程學(xué)院 太原 030024)
文章提出了一種簡化的室內(nèi)機(jī)器人的電磁定位算法。在定位空間內(nèi)布置一個(gè)發(fā)射線圈和一個(gè)三軸接收線圈,形成電磁耦合系統(tǒng)。以接收線圈三軸為參考建立空間直角坐標(biāo)系,并對(duì)發(fā)射線圈加載正弦電信號(hào)作為激勵(lì)信號(hào),產(chǎn)生交變電磁場。接收線圈感應(yīng)到磁場的變化,通過測量計(jì)算感應(yīng)耦合的強(qiáng)度特征值,確定移動(dòng)目標(biāo)的位置參數(shù)。本系統(tǒng)將三軸接收線圈固定,而將水平發(fā)射線圈置于平面移動(dòng)機(jī)器人目標(biāo)之上,這樣可將定位算法簡化。根據(jù)磁偶極子模型,提出了解析計(jì)算方法。通過仿真和實(shí)驗(yàn),證明該方法能夠滿足室內(nèi)機(jī)器人的定位要求,是可行且有效的。
移動(dòng)機(jī)器人;電磁定位;磁偶極子模型
近年來,機(jī)器人的應(yīng)用在社會(huì)各行業(yè)飛速發(fā)展,如軍事、服務(wù)、娛樂、醫(yī)療等[1]。移動(dòng)機(jī)器人是機(jī)器人的一個(gè)重要分枝。在室內(nèi),移動(dòng)機(jī)器人一般在平面上運(yùn)動(dòng)。為了對(duì)移動(dòng)機(jī)器人進(jìn)行高效可靠的控制,需知道其所處位置,即定位。近年來,對(duì)定位技術(shù)的研究很受人們的重視,產(chǎn)生了很多方法,包括GPS定位[2]、超聲定位[3]、機(jī)器視覺定位[4,5]和電磁定位[6]等。GPS定位技術(shù)的原理是測量GPS接收器與衛(wèi)星間的距離,綜合多顆不同衛(wèi)星的距離數(shù)據(jù)來計(jì)算接收器的位置坐標(biāo)。由于該技術(shù)要依賴于衛(wèi)星信號(hào),室內(nèi)往往難以檢測,且定位精度相對(duì)較低,不適合室內(nèi)移動(dòng)機(jī)器人的定位。超聲定位在軍事、醫(yī)學(xué)、工業(yè)和農(nóng)業(yè)上都有廣泛的應(yīng)用。該技術(shù)的精度較高,但是超聲信號(hào)在傳輸過程中衰減很大,影響了可測量的范圍。此外,室內(nèi)空間環(huán)境復(fù)雜,可能存在多個(gè)超聲反射物體,影響定位精度,因此不太適用于室內(nèi)定位?;趫D像和機(jī)器視覺的定位技術(shù)定位精度很高,但需要采集和處理的信息數(shù)據(jù)量較大,處理速度要求高,計(jì)算過程復(fù)雜耗時(shí)[5],且也有光線遮擋問題,使定位失敗或精度受到嚴(yán)重影響。電磁定位技術(shù)是一種比較適于室內(nèi)定位的技術(shù)。該技術(shù)的實(shí)現(xiàn)首先通過向發(fā)射線圈加載交變的電流信號(hào),在周圍定位空間內(nèi)產(chǎn)生變化的電磁場;接著作為跟蹤目標(biāo)的接收線圈感應(yīng)到磁場變化,產(chǎn)生相應(yīng)的交變電信號(hào);最后根據(jù)該信號(hào),利用定位算法計(jì)算接收線圈相對(duì)于發(fā)射線圈的位置與方向[7]。電磁定位技術(shù)的優(yōu)點(diǎn)是可以使用特定頻率的電信號(hào)以及合理的信號(hào)處理方法來降低環(huán)境的干擾以提高定位精度,實(shí)現(xiàn)較大范圍的定位[8,9]。
通常情況下,接收線圈的信號(hào)與發(fā)射線圈之間位置和方向呈非線性關(guān)系。電磁定位系統(tǒng)中,接收線圈與發(fā)射線圈均可采用三軸正交線圈,將接收線圈固定在移動(dòng)體上,通過尋找發(fā)射線圈與接收線圈的耦合關(guān)系可得到接收線圈在空間的位置信息。然而,該系統(tǒng)確定的非線性位置信息方程求解算法復(fù)雜[10],此外,實(shí)驗(yàn)布置條件苛刻,不易實(shí)現(xiàn),且容易受到周圍環(huán)境的影響[11,12]。對(duì)于平面運(yùn)動(dòng)的移動(dòng)機(jī)器人,可根據(jù)其坐標(biāo)變化的特殊性,利用磁偶極子模型將算法進(jìn)行簡化。本文中提出了一種電磁定位方法,將單軸激勵(lì)線圈置于移動(dòng)機(jī)器人上,另用二軸正交或三軸正交線圈作為接收線圈,檢測接收磁場強(qiáng)度,即可計(jì)算移動(dòng)機(jī)器人上的發(fā)射線圈的位置。系統(tǒng)布置簡單,且用解析計(jì)算方法,具有高效和簡單的特點(diǎn),且能夠?qū)崿F(xiàn)較大范圍的平面移動(dòng)機(jī)器人的定位。
室內(nèi)機(jī)器人的移動(dòng)可認(rèn)為是在平面上的運(yùn)動(dòng)。定義一個(gè)空間直角坐標(biāo)系Oxyz,則機(jī)器人的位置可通過三維坐標(biāo)(a,b,c)表示。若使xOy平面與機(jī)器人運(yùn)動(dòng)的平面平行,則機(jī)器人所處位置高度可知,即c已知。本文中提出的室內(nèi)機(jī)器人的定位算法,電磁耦合定位是在直角坐標(biāo)系的xOy平面進(jìn)行,由于需要確定機(jī)器人的旋轉(zhuǎn)角度,至少需要3個(gè)接收線圈(或一個(gè)三軸接收線圈)確定移動(dòng)機(jī)器人的坐標(biāo)位置(a,b)以及角度θ。試驗(yàn)中將單軸發(fā)射線圈安裝在移動(dòng)機(jī)器人的非鐵磁材料體上,使其垂直于運(yùn)動(dòng)平面,并將接收線圈固定在墻壁或天花板上,如圖1所示。其中,Q1和Q2表示接收線圈位置。
通常,求取定位參數(shù)采用非線性求解方法,但是非線性算法需要初始假定值,可能存在局部最小問題,當(dāng)定位范圍較大時(shí),算法甚至?xí)?。在本文試?yàn)中,由于高度已經(jīng)確定,且機(jī)器人不存在俯仰和側(cè)翻姿態(tài),因此問題簡化為3自由度的平面問題。同時(shí),由于單軸發(fā)射線圈的主軸與運(yùn)動(dòng)平面垂直,不提供偏轉(zhuǎn)(即角θ)信息,因此問題進(jìn)一步簡化為2個(gè)未知數(shù)的平面定位問題?;谶@種關(guān)系,得到了解析計(jì)算的電磁定位算法,簡化了計(jì)算。
圖1 移動(dòng)機(jī)器人定位方式Fig.1 Mobile robot localization mode
2.1 算法模型
給發(fā)射線圈加載激勵(lì)信號(hào)后,發(fā)射線圈成為信號(hào)發(fā)射源,可視為磁偶極子,如圖2所示。發(fā)射線圈(即磁偶極子)的位置可以通過其坐標(biāo)(a,b,c)來確定。這里移動(dòng)機(jī)器人的高度c為一個(gè)已知量。發(fā)射線圈安裝在移動(dòng)機(jī)器人上,且與地面平行,即其軸向向量平行于坐標(biāo)系z軸,因此可以將軸向向量表示為,根據(jù)磁偶極子模型,可以得到在接收線圈位置(x,y,z)處的磁感應(yīng)強(qiáng)度公式:
其中,Bx、By和Bz分別是磁感應(yīng)強(qiáng)度在x、y和z軸上的分量;μ0是空氣磁導(dǎo)率;I是電磁線圈通入的電流大小;r是電磁線圈的半徑;BT是一個(gè)由發(fā)射線圈的尺寸大小、線圈匝數(shù)和通入電流確定的常量,其大小為;P是由發(fā)射線圈中心位置(a,b,c)和接收線圈位置(x,y,z)確定的向量,;R是發(fā)射線圈中心與接收線圈的距離,其大小為。
圖2 磁偶極子模型Fig.2 Magnetic dipole model
由公式(4)可知,運(yùn)動(dòng)空間內(nèi),磁感應(yīng)強(qiáng)度在z軸的分量Bz是關(guān)于兩線圈之間的距離R的函數(shù)。在定點(diǎn)(x,y,z)處,c為已知量,則Bz僅與R有關(guān)。這樣就可以通過測量接收線圈在該點(diǎn)處的磁感應(yīng)強(qiáng)度在z軸的分量Bz來確定R的值。根據(jù)R值,求解公式(2)(3)可得到(a,b)的值,即得到裝有發(fā)射線圈的移動(dòng)機(jī)器人的位置。
2.2 算法介紹
根據(jù)式(4)進(jìn)行變化可以得到:
通過求解方程(5),可以得到R的5個(gè)根,對(duì)所有根進(jìn)行約束區(qū)間的判斷,可以得到正確的R值。假設(shè)接收線圈被固定在已知坐標(biāo)的位置(x,y,z)處,其三軸正交各線圈的軸向向量分別與坐標(biāo)系x、y、z軸平行,三軸磁感應(yīng)強(qiáng)度分量為Bx、By和Bz,采樣得到的電信號(hào)值分別為Vx、Vy和Vz。采樣值與磁感應(yīng)強(qiáng)度之間是線性關(guān)系:
其中,BTx、BTy和BTz為常數(shù),可以通過實(shí)驗(yàn)標(biāo)定得到。求得R后,再根據(jù)公式(6)可求得發(fā)射線圈x、y軸位置的解析式:
即可得到移動(dòng)目標(biāo)發(fā)射線圈的位置坐標(biāo)(a,b)。
根據(jù)上述算法的思路,使用Matlan進(jìn)行仿真計(jì)算,并對(duì)有噪聲干擾的情況進(jìn)行軌跡跟蹤和性能評(píng)價(jià)。
3.1 算法仿真舉例
假定發(fā)射線圈位置在(a,b,c)=(6,5.5,0.8),線圈軸向向量為;另外一方三軸正交接收線圈位置固定在(x,y,z)=(4,3,3)處,通過仿真計(jì)算得到:
因R為正實(shí)根,取第三個(gè)11.104與第四個(gè)根3.8846。當(dāng)R=3.8846時(shí),根據(jù)公式(9)得到a=6;而當(dāng)R=11.104時(shí),根據(jù)公式(6),得a=385.68,根據(jù)定位邊界條件,a=385.68超過定位范圍,應(yīng)舍棄。因此,正確的根應(yīng)該為R= 3.8846。然后通過公式(10)得到b1=5.5和b2= 0.5,再根據(jù)公式(8),取b1=5.5,即得到發(fā)射線圈的位置為(6,5.5)。
3.2 混有噪聲時(shí)算法性能評(píng)估
為了對(duì)這種簡化的定位算法進(jìn)行性能評(píng)估,在樣本值Bx、By和Bz中加入不同水平的隨機(jī)噪聲,噪聲的水平與樣本值成正比。假定接收線圈的位置在室內(nèi)的中心位置處,定義為(xr,yr)=(0, 0),高度為zr=3 m,發(fā)射線圈的位置分別在A= (3,2,0.8)和B=(6,4,0.8)。定位誤差情況如圖3和圖4。
由圖3和圖4可看出,定位誤差的大小與噪聲水平基本成正比,且當(dāng)發(fā)射線圈與接收線圈距離較近時(shí),誤差較小。
圖3 A點(diǎn)定位誤差與噪聲水平(磁感應(yīng)強(qiáng)度的百分比)的關(guān)系圖Fig.3 The localization error with respect to noise level(percentage of the magnetic intensity)
圖4 B點(diǎn)定位誤差與噪聲水平(磁感應(yīng)強(qiáng)度的百分比)的關(guān)系圖Fig.4 The localization error with respect to noise level(percentage of the magnetic intensity)
3.3 混有噪聲時(shí)軌跡跟蹤情況
圖5所示為加入噪聲大小為Bz的2.5%時(shí)對(duì)機(jī)器人位置移動(dòng)的跟蹤情況。此時(shí),接收線圈在(xr,yr,zr)=(0,0,3),裝載有發(fā)射線圈的移動(dòng)機(jī)器人在以(3,2,0.8)為中心,半徑為4 m的圓形(圖中藍(lán)線表示)軌跡上運(yùn)動(dòng),72個(gè)位置點(diǎn)用紅色的*表示。由圖可知,計(jì)算得到的位置坐標(biāo)在距離接收線圈位置(xr,yr)較近時(shí)誤差較小,在兩者距離較遠(yuǎn)時(shí),誤差較大。x軸平均定位誤差為0.052 m,y軸平均定位誤差為0.046 m,在xOy平面的距離定位誤差為0.079 m。
圖5 2.5%的噪聲時(shí)移動(dòng)機(jī)器人軌跡跟蹤Fig.5 Tracking locus of the mobile robot on noise with 2.5% magnetic intensity
根據(jù)提出的簡化的室內(nèi)機(jī)器人電磁定位算法搭建實(shí)驗(yàn)平臺(tái),并采集數(shù)據(jù)對(duì)其進(jìn)行性能評(píng)估。實(shí)驗(yàn)中使用單軸發(fā)射線圈,三軸正交接收線圈。將發(fā)射線圈固定在移動(dòng)體上,且其軸向向量與建立的直角坐標(biāo)系z軸平行,發(fā)射線圈在z=1 m的平面上運(yùn)動(dòng),接收線圈為三軸正交線圈,固定在直角坐標(biāo)系的(0,0,2.2 m)處。
實(shí)驗(yàn)中采用發(fā)射線圈與接收線圈如圖6和圖7所示,發(fā)射線圈是使用0.35 mm的漆包線繞制300圈制成,線圈半徑為30 cm,激勵(lì)信號(hào)是頻率為2 kHz的正弦波信號(hào)。接收線圈由0.08 mm的漆包線繞制1600匝制成,線圈半徑為0.5 cm,為了增加磁導(dǎo)率,線圈中心放置磁芯。激勵(lì)信號(hào)產(chǎn)生的變化的磁場在三軸接收線圈上產(chǎn)生3個(gè)電壓信號(hào),采集信號(hào)進(jìn)行定位計(jì)算。采集數(shù)據(jù)如表1所示。表中,xs和ys表示發(fā)射線圈放置位置;Ex、Ey和Ez為三個(gè)接收線圈的采樣信號(hào);xr和yr表示系統(tǒng)測量得到的位置。
圖6 三軸正交接收線圈Fig.6 3-axis orthogonal receiving coils
圖7 發(fā)射線圈Fig.7 Transmitting coil
首先通過標(biāo)定反推得到公式(6)、(7)和(8)中的BTx、BTy和BTz值,其中BTx=17.35,BTy= 24.99,BTz=21.35。根據(jù)搭建的定位系統(tǒng)測得接收線圈上的電壓即可得到線圈在該點(diǎn)處的磁感應(yīng)強(qiáng)度,然后根據(jù)本文方法求解得到發(fā)射線圈的位置,即可得到移動(dòng)機(jī)器人的位置。在平面上選取22個(gè)位置進(jìn)行定位實(shí)驗(yàn),計(jì)算得到系統(tǒng)所測位置與真實(shí)位置的實(shí)驗(yàn)結(jié)果如圖8所示。
圖8 固定位置點(diǎn)定位實(shí)驗(yàn)Fig.8 Localization results for fixed positions
表1 實(shí)驗(yàn)數(shù)據(jù)Table 1 Experimental results
定位的目標(biāo)是在普通室內(nèi)環(huán)境10 m×10 m范圍內(nèi),定位精度要求為x和y軸平均誤差小于10 cm。由上述實(shí)驗(yàn)數(shù)據(jù)以及實(shí)驗(yàn)仿真結(jié)果可以看出,實(shí)驗(yàn)結(jié)果在x軸上的平均定位誤差為7.72 cm,在y軸上的平均定位誤差為5.89 cm。由于實(shí)驗(yàn)中系統(tǒng)所處環(huán)境,信號(hào)受到噪聲的干擾以及操作的影響,有一定誤差,但是定位誤差在允許的范圍內(nèi)。
我們還對(duì)算法的計(jì)算速度進(jìn)行了測試。電磁定位常用的算法是非線性最小化尋優(yōu)算法Levennerg-Marquardt(LM)算法。與其他非線性算法相比,LM算法具有較高的效率,因此將本文算法與LM算法進(jìn)行比較。Matlan環(huán)境下,采用本文方法計(jì)算的平均耗時(shí)為6 ms,而LM算法在18 ms左右。同時(shí),非線性算法需要初始定位值,當(dāng)定位范圍增大,初始值誤差超過0.5 m時(shí),LM算法會(huì)因?yàn)橄萑刖植孔钚栴}而失效;而本文方法則不存在這一問題。因此可認(rèn)為本文方法具有求解簡單,計(jì)算速度快,定位范圍大的特點(diǎn)。
本文提出的簡化的室內(nèi)機(jī)器人定位算法是一種基于磁偶極子模型的電磁定位技術(shù),適用于平面定位。實(shí)驗(yàn)采用單軸發(fā)射線圈和三軸正交接收線圈。根據(jù)接收線圈的三軸建立直角坐標(biāo)系,將發(fā)射線圈固定在移動(dòng)機(jī)器人上,且其軸向向量與坐標(biāo)系的z軸平行,接收線圈固定在空間中已知位置,采集接收線圈輸出信號(hào),利用接收線圈與發(fā)射線圈的耦合關(guān)系進(jìn)行定位計(jì)算。算法利用求解出的解析式計(jì)算發(fā)射線圈的位置坐標(biāo)(a,b),與非線性尋優(yōu)算法相比,具有求解簡單,計(jì)算耗時(shí)少,定位范圍大的特點(diǎn),能滿足移動(dòng)機(jī)器人在室內(nèi)的實(shí)時(shí)定位要求;同時(shí)由于發(fā)射和接收線圈減少,相對(duì)于其他電磁定位系統(tǒng),結(jié)構(gòu)與電路系統(tǒng)也大為簡化。由于實(shí)驗(yàn)材料為自制、實(shí)驗(yàn)過程中人工操作以及環(huán)境的干擾等原因,實(shí)驗(yàn)結(jié)果有一定誤差。后期可以在上述方面進(jìn)行改進(jìn),減小實(shí)驗(yàn)誤差,使該方法的精度得到提高。
[1] 王衛(wèi)華.移動(dòng)機(jī)器人定位技術(shù)研究[D].武漢:華中科技大學(xué),2005.
[2] 盧望.基于ARM的GPS定位系統(tǒng)的研究與實(shí)現(xiàn)[D].成都:西南交通大學(xué),2009.
[3] Shen JY,Molisch AF,Salmi J.Accurate passive location estimation using TOA measurements[J]. IEEE Transactions on Wireless Communication, 2012,11(6):2182-2192.
[4] 王連朝.微創(chuàng)手術(shù)機(jī)器人視覺定位系統(tǒng)的研究[D].武漢:武漢理工大學(xué),2006.
[5] Wu CS,Yang Z,Liu YH,et al.WILL:wireless indoor localization without site survey[J].IEEE Transactions on Parallel and Distrinuted Systems, 2013,24(4):839-847.
[6] Song S,Qiao W,Li BP,et al.An efficient magnetic tracking method using uniaxial sensing coil[J]. IEEE Transactions on Magnetics,2014,50(1): 4003707.
[7] Hu C,Song S,Wang XJ,et al.A novel positioning and orientation system nased on three-axis magnetic coils[J].IEEE Transactions on Magnetics,2012, 48(7):2211-2219.
[8] Song S,Hu C,Li BP,et al.An electromagnetic localization and orientation method nased on rotating magnetic dipole[J].IEEE Transactions on Magnetics,2013,49(3):1274-1277.
[9] 宋霜,胡超,李抱樸,等.面向機(jī)器人運(yùn)動(dòng)跟蹤的電磁定位系統(tǒng)[J].集成技術(shù),2013,2(2):8-15.
[10]包建孟,胡超,林衛(wèi)星,等.平面電磁定位跟蹤系統(tǒng)[J].傳感技術(shù)學(xué)報(bào),2014,4:518-523.
[11]Shin H,Chon Y,Cha H.Unsupervised construction of an indoor floor plan using a smartphone [J].IEEE Transactions on System,Man,and Cynernetics—Part C:Applications and Reviews, 2012,42(6):889-898.
[12]Cheng L,Wu CD,Zhang YZ.Indoor ronot localization nased on wireless sensor networks[J]. IEEE Transactions on Consumer Electronics,2011, 57(3):1099-1104.
A Simplified Magnetic Localization Algorithm and System for Indoor Mobile Robot
CHEN Yueyue1LIN Haixiang1WANG Kan1XU Xiaodong1HU Chao1FENG Zhongqing1,2
1(School of Information Science and Engineering,Ningbo Institute of Technology,Zhejiang University,Ningbo315100,China)
2(School of Information Science and Engineering,Taiyuan University of Science and Technology,Taiyuan030024,China)
In this paper, a simplified magnetic localization method and system for indoor ronot was proposed. A transmitter coil and a set of three axis receiver coils were ranged in the localization space to form the magnetic coupling system. The Cartesian coordinate system was nuilt according to the three-axis orthogonal receiving coils. A magnetic field was setup ny the transmitter coil which was activated ny a sinusoidal current signal. The 3-axis receiver coils sensed the change of the magnetic signal around, and their intensities could ne calculated to extract the characteristic parameters for computing the position of the transmitter coil. Here, the transmitting coil was fixed on the monile ronot, while the 3-axis receiver coil was located in a certain position of the nuilding. The characteristic parameters could ne calculated nased on the magnetic dipole model. According to the simulation and experimental results, this method proves to have high efficiency and satisfactory accuracy which are suitanle for monile ronot localization.
monile ronot; electromagnetic positioning; magnetic dipole model
TP 212.9
A
2015-01-13
:2015-01-21
國家自然科學(xué)基金(61273332)
陳躍躍,本科生,研究方向?yàn)樽詣?dòng)化;林海翔,本科生,研究方向?yàn)樽詣?dòng)化;王侃,本科生,研究方向?yàn)樽詣?dòng)化;徐孝東,本科生,研究方向?yàn)樽詣?dòng)化;胡超(通訊作者),博士生導(dǎo)師,研究方向?yàn)樽詣?dòng)化、智能信息處理和機(jī)器人控制,E-mail:huchao@nit.net.cn;馮忠晴,碩士研究生,研究方向?yàn)樽詣?dòng)化、嵌入式控制系統(tǒng)與應(yīng)用。