本發(fā)明涉及機(jī)器人室內(nèi)定位技術(shù)領(lǐng)域，具體涉及基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)及方法。

背景技術(shù)：

超聲定位系統(tǒng)中比較典型的系統(tǒng)是activebat和cricket系統(tǒng)。其中前者接收節(jié)點(diǎn)發(fā)射rf信號和us信號，位置固定的信標(biāo)節(jié)點(diǎn)接收rf信號和us信號；cricket系統(tǒng)則相反，信標(biāo)節(jié)點(diǎn)作為信號發(fā)射節(jié)點(diǎn)，待定位的接收節(jié)點(diǎn)用于信號的接收，然后根據(jù)rf信號和us信號的到達(dá)時間差進(jìn)行分布式目標(biāo)定位。

activebat是低功耗，無線室內(nèi)定位系統(tǒng)，精度可達(dá)3cm。利用三角定位法，利用在天花板上嵌入的超聲接收器，測量tof。

activebat系統(tǒng)中，中心控制主機(jī)集中控制多個接收節(jié)點(diǎn)，而待定的接收節(jié)點(diǎn)作為發(fā)射節(jié)點(diǎn)是不可控的，尤其是當(dāng)接收節(jié)點(diǎn)數(shù)量較多的情況下，必然會引起各組rf和us信號之間的串?dāng)_。從而導(dǎo)致錯誤的定位，影響系統(tǒng)的穩(wěn)定性。cricket系統(tǒng)具有很好的可擴(kuò)展性，但其信標(biāo)節(jié)點(diǎn)采用隨機(jī)發(fā)射信號的方式，仍然不能有效解決信號串?dāng)_的問題。

在洪林的碩士畢業(yè)論文《移動機(jī)器人超聲波室內(nèi)定位系統(tǒng)研究》中提出了十字陣單發(fā)射多接收系統(tǒng)，該系統(tǒng)依然要通過同步來解決測距問題。

通過分析目前系統(tǒng)中存在的主要問題包括：

1)系統(tǒng)定位過程中需要同步，一般采用射頻同步的方法，增加了系統(tǒng)的復(fù)雜度；

2)系統(tǒng)定位采用的是三角定位法，因此需要至少三個信標(biāo)節(jié)點(diǎn)和一個接收節(jié)點(diǎn)才能完成定位，增加了系統(tǒng)中信標(biāo)節(jié)點(diǎn)個數(shù)，也就意味著成本的增加；

3)射頻同步的模式，在公共頻段，與wifi等同頻，增加了電磁干擾，同時，系統(tǒng)也容易受到電磁干擾的影響。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明所要解決的技術(shù)問題是克服以上技術(shù)問題，目的在于提供基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)及方法，實(shí)現(xiàn)一種結(jié)構(gòu)簡單、計(jì)算量低，能準(zhǔn)確定位移動物體在室內(nèi)的位置，能避免電磁干擾的系統(tǒng)及方法的目的。

本發(fā)明通過下述技術(shù)方案實(shí)現(xiàn)：

基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)，包括位于天花板上的信標(biāo)節(jié)點(diǎn)和位于機(jī)器人上的接收節(jié)點(diǎn)，所述信標(biāo)節(jié)點(diǎn)的數(shù)量至少為一個，每個信標(biāo)節(jié)點(diǎn)由一個超聲波發(fā)射探頭組成，每個接收節(jié)點(diǎn)包括至少三個超聲波接收探頭，在機(jī)器人上還設(shè)置有地磁傳感器，所述超聲波接收探頭和地磁傳感器均與接收控制系統(tǒng)連接，其中：

超聲波發(fā)射探頭：發(fā)射超聲波信號給超聲波接收探頭；

地磁傳感器：探測地磁方向信號，并將地磁方向信號傳輸給接收控制系統(tǒng)；

超聲波接收探頭：接收超聲波發(fā)射探頭發(fā)射的超聲波信號，并將超聲波信號傳輸給接收控制系統(tǒng)；

接收控制系統(tǒng)：接收超聲波接收探頭傳輸?shù)某暡ㄐ盘柡偷卮艂鞲衅鱾鬏數(shù)牡卮欧较蛐盘枴?/p>

進(jìn)一步的，本發(fā)明開創(chuàng)性的設(shè)計(jì)了一種用于機(jī)器人的超聲波室內(nèi)定位系統(tǒng)，在天花板上均勻安裝多個信標(biāo)節(jié)點(diǎn)，每個信標(biāo)節(jié)點(diǎn)只包括一個超聲波發(fā)射探頭，超聲波發(fā)射探頭和超聲波接收探頭通信并利用現(xiàn)有的列陣測向技術(shù)可得到機(jī)器人的水平角，可得到機(jī)器人在室內(nèi)的相對位置，然后再利用地磁傳感器測向可得到機(jī)器人在水平面的偏離角度，以此矯正相對位置，通過這種兩維測角陣列最終能機(jī)器人的實(shí)際位置，相比現(xiàn)有技術(shù)，本發(fā)明結(jié)構(gòu)更加簡單，減少了系統(tǒng)中信標(biāo)節(jié)點(diǎn)個數(shù)，有效降低了定位成本。并且，本發(fā)明無需發(fā)射射頻信號，有效避免了系統(tǒng)定位過程中需要同步，采用射頻同步的方法增加了系統(tǒng)的復(fù)雜度的問題，同時也避免了電磁干擾。本發(fā)明安裝在機(jī)器人身上的地磁傳感器為一種專用于探測地磁方向的探測器，類似指南針，可時刻測量地磁方向信號。超聲波發(fā)射探頭與發(fā)射控制系統(tǒng)連接。超聲波接收探頭與接收控制系統(tǒng)，由接收控制系統(tǒng)處理定位信息，該接收控制系統(tǒng)為現(xiàn)有結(jié)構(gòu)，包括與超聲波接收探頭依次連接的放大器、bpf、mcu模塊等，放大器將超聲波接收探頭傳輸?shù)某暡ㄐ盘栠M(jìn)行功率放大后傳輸給bpf；bpf接收放大器傳輸?shù)某暡ㄐ盘?，按其所在頻率濾波后傳輸給mcu模塊進(jìn)行處理，mcu模塊將超聲波信號進(jìn)行數(shù)模轉(zhuǎn)換和回波信號處理后輸出移動設(shè)備的定位信息。

優(yōu)選的，當(dāng)每個接收節(jié)點(diǎn)由三個超聲波接收探頭組成時，三個超聲波接收探頭圍成一個三角形。進(jìn)一步的，三個超聲波接收探頭可形成任意形狀三陣元平面，三陣元平面在相同孔徑的陣列中，具有線陣有較好的定向精度；當(dāng),陣列孔徑越大，其定距精度越高。在無任何目標(biāo)位置的先驗(yàn)信息時，為避免整個平面的定位精度不會出現(xiàn)某些點(diǎn)特別大的問題，可采用正三角陣。

優(yōu)選的，當(dāng)每個接收節(jié)點(diǎn)由四個超聲波接收探頭組成時，四個超聲波接收探頭位于同一平面，且四個探頭中相對的兩個探頭的連線與另外兩個相對的探頭的連線相互垂直。進(jìn)一步的，該結(jié)構(gòu)排列成十字陣列，也可稱為正方陣列或圓形陣列，這種陣列的距離參數(shù)只有一個，相比現(xiàn)有技術(shù)，數(shù)據(jù)處理起來更加簡單。

優(yōu)選的，所述接收控制系統(tǒng)內(nèi)的存儲器采用fifo存儲器。本發(fā)明采用的fifo存儲器用于存儲超聲波接收探頭接收的數(shù)據(jù)，當(dāng)數(shù)據(jù)存儲滿了之后，自動覆蓋掉最初始的數(shù)據(jù)。進(jìn)一步的，采用這樣的方式，接收端不用一直進(jìn)行信號處理，而是等接收到信號超過一定門限之后，表示有發(fā)射信號接收到，同時開始進(jìn)行處理。

基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位方法，包括以下步驟：

a：接收節(jié)點(diǎn)接收信標(biāo)節(jié)點(diǎn)發(fā)射的超聲波信號，根據(jù)陣列測向理論，測得信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的俯仰角水平角θ；

b：測量出信標(biāo)節(jié)點(diǎn)所在平面到接收節(jié)點(diǎn)所在平面的垂直高度h，根據(jù)信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的俯仰角通過計(jì)算得到接收節(jié)點(diǎn)距離信標(biāo)節(jié)點(diǎn)在水平面上的投影長度l；

c：地磁傳感器探測地磁方向信號，獲得機(jī)器人在水平面的偏離角度θ0；設(shè)定兩個超聲波接收探頭的基線方向與地磁傳感器的探測方向重合時的方向?yàn)槌跏挤较颍鶕?jù)信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ，可計(jì)算出機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度θ′＝θ0±θ；

d：由于信標(biāo)節(jié)點(diǎn)的坐標(biāo)已知，設(shè)為(x0,y0)，再根據(jù)方程組x1＝x0+l·cosθ′，y1＝y(tǒng)0+l·sinθ′，可計(jì)算得到機(jī)器人的坐標(biāo)為(x1,y1)。

進(jìn)一步的，本發(fā)明的定位系統(tǒng)的具體定位方法如上述所示，利用陣列測向理論，可測得信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的俯仰角水平角θ；通常室內(nèi)的天花板與地面平行，那么機(jī)器人上的接收節(jié)點(diǎn)距離天花板的垂直高度固定不變，本利用高度已知信息、俯仰角水平角θ和信標(biāo)節(jié)點(diǎn)的坐標(biāo)就可獲得機(jī)器人的坐標(biāo)位置，但由于機(jī)器人在移動的過程中會接收到多個信標(biāo)節(jié)點(diǎn)發(fā)射的超聲波信號，所以所測得水平角并不是絕對水平角，根據(jù)該水平角計(jì)算得出的坐標(biāo)位置為一種相對位置，即機(jī)器人相對每個一個信標(biāo)節(jié)點(diǎn)的位置，系統(tǒng)無法從若干個相對位置點(diǎn)判別出機(jī)器人的實(shí)際位置在哪。所以本發(fā)明再利用地磁傳感器來對機(jī)器人的相對水平角進(jìn)行校正，測量出機(jī)器人在水平面的偏離角度θ0，通過θ′＝θ0±θ計(jì)算得出θ′，獲得機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)水平角度，最終計(jì)算出機(jī)器人的實(shí)際坐標(biāo)。相比現(xiàn)有技術(shù)，本發(fā)明采用的方法計(jì)算量低，且不需要系統(tǒng)同步，降低了系統(tǒng)復(fù)雜度。

具體位置校正原理如下：1、選取接收節(jié)點(diǎn)中任意兩個超聲波接收探頭，設(shè)定兩個超聲波接收探頭的基線(基線指兩個超聲波接收探頭的連線)方向與地磁傳感器的探測方向重合時的方向?yàn)槌跏挤较?，此時所述的信標(biāo)節(jié)點(diǎn)位于兩個超聲波接收探頭的基線的中垂線上，此時測得的信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ為0°，地磁傳感器的測量方向與初始方向重合，測得的偏離角度θ0也為0°，其機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度θ′＝θ0±θ為0°，該時刻信標(biāo)節(jié)點(diǎn)所測得水平角θ就是機(jī)器人實(shí)際位置測得的水平角。2、隨著機(jī)器人的移動，兩個超聲波接收探頭的基線的中垂線會再次經(jīng)過信標(biāo)節(jié)點(diǎn)，此時測得的信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ仍為0°，該水平角θ不是絕對水平角，所以需要地磁傳感器進(jìn)行校正，由于地磁傳感器探測的地磁方向是一直不變的，此時地磁傳感器測得的相對于基線方向的偏離角度θ0為45°，則機(jī)器人實(shí)際位置測得的水平角θ′＝θ0±θ為45°。3、由于機(jī)器人是隨意移動的，當(dāng)機(jī)器人的兩個超聲波接收探頭的基線的中垂線不經(jīng)過信標(biāo)節(jié)點(diǎn)時，此刻根據(jù)陣列測向理論測得的信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ就不為0°，地磁傳感器測得的偏離角度θ0也不為0°，實(shí)際得到的機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度就為θ′＝θ0±θ。

本發(fā)明與現(xiàn)有技術(shù)相比，具有如下的優(yōu)點(diǎn)和有益效果：

1、本發(fā)明基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)及方法，利用超聲波探頭和地磁傳感器同時測向，獲得機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)水平角度實(shí)現(xiàn)了對機(jī)器人的最終定位，相比現(xiàn)有技術(shù)，結(jié)構(gòu)更加簡單，減少了系統(tǒng)中信標(biāo)節(jié)點(diǎn)個數(shù)，有效降低了定位成本；

2、本發(fā)明基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)及方法，無需發(fā)射射頻信號，有效避免了系統(tǒng)定位過程中采用射頻同步的方法增加了系統(tǒng)的復(fù)雜度的問題，同時也避免了電磁干擾；

3、本發(fā)明基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)及方法，定位方法計(jì)算量低，可實(shí)現(xiàn)對機(jī)器人的準(zhǔn)確定位。

附圖說明

此處所說明的附圖用來提供對本發(fā)明實(shí)施例的進(jìn)一步理解，構(gòu)成本申請的一部分，并不構(gòu)成對本發(fā)明實(shí)施例的限定。在附圖中：

圖1為本發(fā)明超聲波室內(nèi)定位位置示意圖；

圖2為本發(fā)明的位置校正測向示意圖；

圖3為本發(fā)明十字接收陣列示意圖；

圖4為本發(fā)明的接收控制系統(tǒng)結(jié)構(gòu)圖。

附圖中標(biāo)記及對應(yīng)的零部件名稱：

1-天花板，2-地面，3-超聲波發(fā)射探頭，4-超聲波接收探頭，5-地磁傳感器。

具體實(shí)施方式

為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚明白，下面結(jié)合實(shí)施例和附圖，對本發(fā)明作進(jìn)一步的詳細(xì)說明，本發(fā)明的示意性實(shí)施方式及其說明僅用于解釋本發(fā)明，并不作為對本發(fā)明的限定。

實(shí)施例1

如圖1、4所示，本發(fā)明基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)，包括位于天花板1上的信標(biāo)節(jié)點(diǎn)和位于機(jī)器人上的接收節(jié)點(diǎn)，所述信標(biāo)節(jié)點(diǎn)的數(shù)量至少為一個，每個信標(biāo)節(jié)點(diǎn)由一個超聲波發(fā)射探頭3組成，每個接收節(jié)點(diǎn)包括至少三個超聲波接收探頭4，在機(jī)器人上還設(shè)置有地磁傳感器5，所述超聲波接收探頭4和地磁傳感器5均與接收控制系統(tǒng)連接，其中：

超聲波發(fā)射探頭3：發(fā)射超聲波信號給超聲波接收探頭4；

地磁傳感器5：探測地磁方向信號，并將地磁方向信號傳輸給接收控制系統(tǒng)；

超聲波接收探頭4：接收超聲波發(fā)射探頭3發(fā)射的超聲波信號，并將超聲波信號傳輸給接收控制系統(tǒng)；

接收控制系統(tǒng)：接收超聲波接收探頭4傳輸?shù)某暡ㄐ盘柡偷卮艂鞲衅?傳輸?shù)牡卮欧较蛐盘枴Ｋ鼋邮湛刂葡到y(tǒng)內(nèi)的存儲器采用fifo存儲器；接收控制系統(tǒng)包括依次連接的放大器、bpf、mcu模塊，放大器與超聲波接收探頭4連接，且放大器、bpf的數(shù)量均與超聲波接收探頭4的數(shù)量一一對應(yīng)，其中：超聲波接收探頭4接收信標(biāo)節(jié)點(diǎn)發(fā)送的超聲波信號，并將超聲波信號傳輸給放大器；放大器接收超聲波接收探頭4傳輸?shù)某暡ㄐ盘?，將功率放大后的超聲波信號傳輸給bpf；bpf接收放大器傳輸?shù)某暡ㄐ盘?，并將超聲波信號傳輸給mcu模塊；mcu模塊接收所有的bpf傳輸?shù)某暡ㄐ盘枺瑢⒊暡ㄐ盘栠M(jìn)行數(shù)模轉(zhuǎn)換和回波信號處理后輸出移動設(shè)備的定位信息。

基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位方法，包括以下步驟：

a：接收節(jié)點(diǎn)接收信標(biāo)節(jié)點(diǎn)發(fā)射的超聲波信號，根據(jù)陣列測向理論，測得信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的俯仰角水平角θ；

b：測量出信標(biāo)節(jié)點(diǎn)所在平面到接收節(jié)點(diǎn)所在平面的垂直高度h，根據(jù)信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的俯仰角通過計(jì)算得到接收節(jié)點(diǎn)距離信標(biāo)節(jié)點(diǎn)在水平面上的投影長度l；

c：地磁傳感器探測地磁方向信號，獲得機(jī)器人在水平面的偏離角度θ0；設(shè)定兩個超聲波接收探頭的基線方向與地磁傳感器的探測方向重合時的方向?yàn)槌跏挤较?，根?jù)信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ，可計(jì)算出機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度θ′＝θ0±θ；

d：由于信標(biāo)節(jié)點(diǎn)的坐標(biāo)已知，設(shè)為(x0,y0)，再根據(jù)方程組x1＝x0+l·cosθ′，y1＝y(tǒng)0+l·sinθ′，可計(jì)算得到機(jī)器人的坐標(biāo)為(x1,y1)。

實(shí)施例2

如圖1～4所示，本發(fā)明基于超聲和磁力計(jì)的機(jī)器人室內(nèi)定位系統(tǒng)，當(dāng)每個接收節(jié)點(diǎn)由四個超聲波接收探頭4組成時，四個超聲波接收探頭4位于同一平面，且四個探頭中相對的兩個探頭的連線與另外兩個相對的探頭的連線相互垂直。四個接收探頭排列成十字形狀，其中為機(jī)器人正前方向，為機(jī)器人正前方向垂直的軸。每個軸中的兩個陣元間距為d，中心交于一點(diǎn)，假設(shè)中心點(diǎn)即為機(jī)器人的位置奇點(diǎn)，如圖3所示。

當(dāng)信標(biāo)節(jié)點(diǎn)在天花板1上發(fā)射信號時，四個探頭可以接收到同一個發(fā)射信號，根據(jù)陣列測向理論，四個超聲波接收探頭4可以測量出發(fā)射信號的俯仰角水平角θ。由于天花板1的高度h已知，根據(jù)俯仰角可以求出目標(biāo)距離超聲波發(fā)射探頭3在地面2上的投影長度為

地磁傳感器5的輸出為目標(biāo)在水平面的偏離角度θ0，假設(shè)以北向?yàn)槌跏挤较?，則根據(jù)陣列獲得的水平角可以得到目標(biāo)與信源的真實(shí)角度θ′。由于超聲波發(fā)射信源的坐標(biāo)已知，設(shè)為(x0,y0)，則機(jī)器人的位置為(x1,y1)，利用x1＝x0+l·cosθ′，y1＝y(tǒng)0+l·sinθ′可計(jì)算得出。

以上所述機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度θ′具體計(jì)算過程如下，具體示意圖見圖2，該圖為天花板指向地面的俯視圖：首先在十字陣列的四個超聲波接收探頭4中選取任意兩個超聲波接收探頭，設(shè)定兩個超聲波接收探頭的基線方向與地磁傳感器的探測方向重合時的方向?yàn)槌跏挤较?，在圖2中表示為l1，此時所述的信標(biāo)節(jié)點(diǎn)位于兩個超聲波接收探頭的基線的中垂線上，此時測得的信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ為0°，地磁傳感器的測量方向與初始方向重合，測得的偏離角度θ0也為0°，其機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度θ′＝θ0±θ為0°，該時刻信標(biāo)節(jié)點(diǎn)所測得水平角θ就是機(jī)器人實(shí)際位置測得的水平角。隨著機(jī)器人的移動，兩個超聲波接收探頭的基線的中垂線會再次經(jīng)過信標(biāo)節(jié)點(diǎn)，在圖2中表示為l2，此時測得的信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ仍為0°，該水平角θ不是絕對水平角，所以需要地磁傳感器進(jìn)行校正，由于地磁傳感器探測的地磁方向是一直不變的，此時地磁傳感器測得的相對于基線方向的偏離角度θ0為45°，則機(jī)器人實(shí)際位置測得的水平角θ′＝θ0±θ為45°。由于機(jī)器人是隨意移動的，當(dāng)機(jī)器人的兩個超聲波接收探頭的基線的中垂線不經(jīng)過信標(biāo)節(jié)點(diǎn)時，在圖2中表示為l3，此刻根據(jù)陣列測向理論測得的信標(biāo)節(jié)點(diǎn)到接收節(jié)點(diǎn)的水平角θ為15°，地磁傳感器測得的偏離角度θ0為45°，實(shí)際得到的機(jī)器人與信標(biāo)節(jié)點(diǎn)之間的真實(shí)角度就為θ′＝θ0±θ為60°或30度。

以上所述的具體實(shí)施方式，對本發(fā)明的目的、技術(shù)方案和有益效果進(jìn)行了進(jìn)一步詳細(xì)說明，所應(yīng)理解的是，以上所述僅為本發(fā)明的具體實(shí)施方式而已，并不用于限定本發(fā)明的保護(hù)范圍，凡在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。