專利名稱：一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種對運(yùn)動(dòng)載體進(jìn)行室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)。
背景技術(shù)：
對運(yùn)動(dòng)載體的定位包括室外環(huán)境的定位和室內(nèi)環(huán)境的定位。目前主要針對室外環(huán)境進(jìn)行定位的系統(tǒng)有采用蜂窩網(wǎng)絡(luò)技術(shù)定位的系統(tǒng)和GPS全球定位系統(tǒng)。GPS全球定位系統(tǒng)目前能提供的室外定位精度約為:3m 5m，蜂窩網(wǎng)絡(luò)定位系統(tǒng)的精度遠(yuǎn)差于此。這些定位系統(tǒng)受到建筑物對信號的遮擋、反射和衍射等影響，在室內(nèi)環(huán)境下使用往往存在更大的定位誤差，不能滿足用戶對室內(nèi)定位的精度需求。室內(nèi)定位系統(tǒng)與室外定位系統(tǒng)不同，用戶更關(guān)心的是自己在室內(nèi)平面的二維坐標(biāo)和運(yùn)動(dòng)的航向信息。由于室內(nèi)環(huán)境相對較小，用戶對定位精度的要求也比室外環(huán)境更高。 目前，室內(nèi)定位技術(shù)在室內(nèi)移動(dòng)探測器、救災(zāi)機(jī)器人車、倉庫搬運(yùn)機(jī)器人車的跟蹤和運(yùn)動(dòng)導(dǎo)航，以及汽車防撞檢測、自動(dòng)泊車等領(lǐng)域有廣泛的應(yīng)用需求。根據(jù)導(dǎo)航定位使用的設(shè)備儀器不同，目前室內(nèi)定位的方式主要有電磁導(dǎo)航、無線電導(dǎo)航、無線網(wǎng)絡(luò)導(dǎo)航、激光導(dǎo)航、視覺導(dǎo)航、超聲波導(dǎo)航和多種傳感器融合導(dǎo)航等。這些導(dǎo)航方式主要的優(yōu)缺點(diǎn)對比如下電磁導(dǎo)航是讓低頻電流流過埋設(shè)在路徑下的電纜，然后用安裝在機(jī)器人上的線圈檢測電纜周圍產(chǎn)生的磁場，并根據(jù)該信息計(jì)算位置，并控制方向，以實(shí)現(xiàn)沿規(guī)定路徑的導(dǎo)航控制。主要優(yōu)點(diǎn)是導(dǎo)引線隱蔽，不易污染和破損，導(dǎo)引原理簡單，便于控制和通信，對聲光無干擾；缺點(diǎn)是靈活性差，改變或擴(kuò)充路徑麻煩，對導(dǎo)引線附近的鐵磁物質(zhì)有干擾，電纜鋪設(shè)工作量大，維護(hù)困難。無線電導(dǎo)航是通過利用移動(dòng)通信網(wǎng)絡(luò)來實(shí)施定位導(dǎo)航的。具體有利用基站發(fā)射信號的到達(dá)時(shí)間差技術(shù)(TDOA)、信號到達(dá)角技術(shù)(AOA)和信號衰減技術(shù)來確定用戶與基站之間的距離，利用球面三角算出移動(dòng)終端的位置。主要優(yōu)點(diǎn)是可以利用現(xiàn)有的移動(dòng)通信網(wǎng)絡(luò)的基站和用戶手持設(shè)備完成定位服務(wù)，無需額外開發(fā)新的設(shè)備；缺點(diǎn)是信號受建筑物和周圍環(huán)境的影響，定位精度不高，一般在50米以上，通常只用于粗定位。無線網(wǎng)絡(luò)導(dǎo)航技術(shù)，利用Wi-Fi、藍(lán)牙(Bluetooth)等室內(nèi)無線網(wǎng)絡(luò)，通過用戶設(shè)備接收到的室內(nèi)無線信號的強(qiáng)弱(received signal strength, RSS)測定信號點(diǎn)與接收點(diǎn)的距離，進(jìn)而進(jìn)行定位計(jì)算的一種定位技術(shù)。無線網(wǎng)絡(luò)目前已經(jīng)開始廣泛普及，尤其是在醫(yī)院、學(xué)校、辦公大樓等地區(qū)，使其用于室內(nèi)定位成為可能，并且實(shí)施成本并不高。但是這種定位方式需要首先為使用環(huán)境建立一個(gè)精確的無線網(wǎng)絡(luò)傳播模型，當(dāng)室內(nèi)環(huán)境改變時(shí)，會(huì)影響無線信號強(qiáng)度的發(fā)布，需要重新修改模型；目前單獨(dú)是使用無線網(wǎng)絡(luò)實(shí)施室內(nèi)定位的精度最高只能達(dá)到米級。激光導(dǎo)航的一種方案是用一個(gè)指向標(biāo)發(fā)出旋轉(zhuǎn)掃描激光光束，再利用運(yùn)動(dòng)載體上的若干個(gè)傳感器來檢測，從而求出指向標(biāo)的方向和到指向標(biāo)的距離，用以確定運(yùn)動(dòng)載體的位置；另外還有和導(dǎo)向電纜方式類似的采用激光導(dǎo)引運(yùn)動(dòng)載體的方法。激光導(dǎo)航的優(yōu)點(diǎn)是傳感器發(fā)散小或沒有發(fā)散，并對大多數(shù)物體無鏡面反射現(xiàn)象；缺點(diǎn)是存在潛在的安全問題(首先是人眼安全問題)，且不適用于透明物質(zhì)。視覺導(dǎo)航成為近幾年室內(nèi)導(dǎo)航研究的熱點(diǎn)，運(yùn)動(dòng)載體利用視覺傳感器獲取的信息進(jìn)行定位，從而實(shí)現(xiàn)智能行駛。視覺導(dǎo)航中一般使用的方法如基于像素匹配的方法識別特征點(diǎn)、基于光流場分析的方法估計(jì)環(huán)境結(jié)構(gòu)等等。視覺導(dǎo)航方法的優(yōu)點(diǎn)是獲取信息量大，靈敏度高，成本低，并且可根據(jù)需要靈活地改變或擴(kuò)充路徑，具有很好的柔性；缺點(diǎn)是對環(huán)境光線有一定要求，并且由于計(jì)算復(fù)雜對導(dǎo)航的實(shí)時(shí)性有一定影響。超聲波測距作為一種常用的非接觸式測距方式，長期以來一直是室內(nèi)導(dǎo)航定位研究的熱點(diǎn)。常用的超聲波定位方法有反射式和超聲信標(biāo)式。反射式將超聲發(fā)射和接收做為一體，使用超聲波收發(fā)器發(fā)出和接收信號，完成距離的測量。在目前的應(yīng)用中，主要采用超聲波收發(fā)器與載體固定連接的方法。這種方法最主要的缺點(diǎn)是只能獲得超聲波收發(fā)器到與之垂直的障礙面的距離，測量角度范圍小(一般士5° )且難以控制，當(dāng)載體姿態(tài)發(fā)生改變導(dǎo)致與之固聯(lián)的超聲波收發(fā)器的測量角度范圍離開了障礙面時(shí)，將不能收到回波，無法完成測量。超聲信標(biāo)式，如L. Kleeman使用主動(dòng)超聲作為信標(biāo)，在運(yùn)動(dòng)載體身上裝有超聲波接收器，測得運(yùn)動(dòng)載體和信標(biāo)的距離及方位從而估計(jì)運(yùn)動(dòng)載體的位姿。這種方法擴(kuò)大了超聲波定位的使用范圍，但需在室內(nèi)預(yù)先鋪設(shè)很多的超聲波信標(biāo)，且超聲波接收器很難區(qū)分不同信標(biāo)發(fā)出的信號。綜合上述兩種方法，超聲波導(dǎo)航的優(yōu)點(diǎn)是硬件結(jié)構(gòu)簡單，價(jià)格低廉，容易操作；缺點(diǎn)是速度慢，傳感器存在較大的波束角(角度分辨率低)，單一傳感器的穩(wěn)定性不理想等。 在實(shí)際應(yīng)用中，往往采用其它傳感器來補(bǔ)償，或采用多傳感器融合技術(shù)提高檢測精度等。多傳感器融合導(dǎo)航組合了多種導(dǎo)航方式。已有的室內(nèi)多傳感器融合導(dǎo)航系統(tǒng)往往融合了無線網(wǎng)絡(luò)導(dǎo)航、激光導(dǎo)航、超聲波導(dǎo)航等，如AT&T Lab Cambridge的Active badge 和Bat室內(nèi)定位系統(tǒng)，MIT的Cricket系統(tǒng)。多傳感器融合導(dǎo)航系統(tǒng)彌補(bǔ)了的單一傳感器的許多固有缺點(diǎn)，提高了定位精度，但是系統(tǒng)復(fù)雜程度增加，運(yùn)行和維護(hù)成本更高。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，該二維導(dǎo)航系統(tǒng)能夠獲取運(yùn)動(dòng)載體相對于其初始位置的偏轉(zhuǎn)角度(定向)和位置坐標(biāo)(定位)，滿足在相對簡潔的室內(nèi)使用環(huán)境下對運(yùn)動(dòng)載體實(shí)施自主定位與定向的需求。在本發(fā)明中二維導(dǎo)航系統(tǒng)由MEMS陀螺傳感器、DSP數(shù)字信號處理器、數(shù)字舵機(jī)和超聲波收發(fā)器等部件組成。本發(fā)明的二維導(dǎo)航系統(tǒng)安裝在運(yùn)動(dòng)載體上，一方面DSP數(shù)字信號處理器處理MEMS陀螺傳感器的測量數(shù)據(jù)，獲得運(yùn)動(dòng)載體相對于其初始位置的偏轉(zhuǎn)角度 (即航向信息)；另一方面DSP數(shù)字信號處理器通過調(diào)節(jié)數(shù)字舵機(jī)搖臂的偏轉(zhuǎn)角度(即掃描角度)，對載體兩側(cè)固聯(lián)在舵機(jī)搖臂上的超聲波收發(fā)器的對準(zhǔn)方位實(shí)施可控調(diào)節(jié)，實(shí)現(xiàn)掃描測距和定向測距兩種測距模式，測得載體到障礙面的垂直距離，并利用收到回波時(shí)處理器內(nèi)控制數(shù)字舵機(jī)偏轉(zhuǎn)角度的變量換算出此時(shí)對應(yīng)的測距指向角度；最后根據(jù)位置更新矩陣更新當(dāng)前載體相對于其初始位置的位置坐標(biāo)，對運(yùn)動(dòng)載體實(shí)施室內(nèi)自主定位與定向，具有實(shí)用意義。本發(fā)明的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，該二維導(dǎo)航系統(tǒng)包括有DSP數(shù)字信號處理器(1)、MEMS陀螺傳感器( 、轉(zhuǎn)向舵機(jī)C3)、左超聲波收發(fā)器(4)、右超聲波收發(fā)器(7)、左掃描舵機(jī)( 和右掃描舵機(jī)(6)；其中左掃描舵機(jī)( 包括有左舵機(jī)本體 (5A)、左舵機(jī)搖臂(5B)和左舵機(jī)轉(zhuǎn)軸(5C)；左舵機(jī)搖臂(5B)為T形搖臂，左舵機(jī)搖臂(5B) 的一端繞左舵機(jī)轉(zhuǎn)軸(5C)旋轉(zhuǎn)，左舵機(jī)搖臂(5B)安裝在左舵機(jī)本體(5A)上；其中右掃描舵機(jī)(6)包括有右舵機(jī)本體(6A)、右舵機(jī)搖臂(6B)和右舵機(jī)轉(zhuǎn)軸(6C)；右舵機(jī)搖臂(6B) 為T形搖臂，右舵機(jī)搖臂(6B)的一端繞右舵機(jī)轉(zhuǎn)軸(6C)旋轉(zhuǎn)，右舵機(jī)搖臂(6B)安裝在右舵機(jī)本體(6A)上；所述的DSP數(shù)字信號處理器(1)安裝在運(yùn)動(dòng)載體(10)上；所述的MEMS陀螺傳感器(2)安裝在運(yùn)動(dòng)載體(10)上，且安裝方位需要保證MEMS 陀螺傳感器O)的三軸與運(yùn)動(dòng)載體(10)的坐標(biāo)系0b-、yb、的三軸分別重合；MEMS陀螺傳感器⑵用于測量運(yùn)動(dòng)載體(10)在運(yùn)動(dòng)狀態(tài)下的三軸角速率信息F21(cobx，coby，cobz)，cobx 表示在坐標(biāo)系ob-xbyb、下繞、軸轉(zhuǎn)動(dòng)的角速率分量，"by表示在坐標(biāo)系ob-Xby#b下繞yb軸轉(zhuǎn)動(dòng)的角速率分量，"bz表示在坐標(biāo)系obibyb、下繞、軸轉(zhuǎn)動(dòng)的角速率分量；所述的轉(zhuǎn)向舵機(jī)C3)安裝在運(yùn)動(dòng)載體10的前端，轉(zhuǎn)向舵機(jī)搖臂31與運(yùn)動(dòng)載體 (10)的轉(zhuǎn)向機(jī)構(gòu)相連；轉(zhuǎn)向舵機(jī)3用于操縱運(yùn)動(dòng)載體(10)實(shí)現(xiàn)轉(zhuǎn)向；所述的左超聲波收發(fā)器(4)安裝在左舵機(jī)搖臂(5B)上，且保證左超聲波收發(fā)器 (4)所發(fā)超聲波波束的方向與左舵機(jī)搖臂(5B)的指向相同；左超聲波收發(fā)器(4)根據(jù)收到的左超聲波測距脈沖信號F14后發(fā)射出超聲波，然后當(dāng)左超聲波收發(fā)器(4)收到回波后，則輸出左距離脈沖信號F41給DSP數(shù)字信號處理器(1)；所述的右超聲波收發(fā)器(7)安裝在右舵機(jī)搖臂(6B)上，且保證右超聲波收發(fā)器 (7)所發(fā)超聲波波束的方向與右舵機(jī)搖臂(6B)的指向相同；右超聲波收發(fā)器(7)根據(jù)收到的右超聲波測距脈沖信號F17后發(fā)射出超聲波，然后當(dāng)右超聲波收發(fā)器(7)收到回波后，則輸出右距離脈沖信號F71給DSP數(shù)字信號處理器(1)。本發(fā)明的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)的優(yōu)點(diǎn)在于①本發(fā)明中，超聲波收發(fā)器的指向受到DSP數(shù)字信號處理器的可控調(diào)節(jié)，能夠?qū)崿F(xiàn)掃描測距和定向測距兩種測距模式掃描測距模式在數(shù)字舵機(jī)搖臂旋轉(zhuǎn)掃描過程中，超聲波收發(fā)器檢測是否收到回波，當(dāng)收到回波時(shí)數(shù)字舵機(jī)搖臂停止旋轉(zhuǎn)，進(jìn)入定向測距模式。 使用這種方法可以保證不管載體如何運(yùn)動(dòng)，超聲波收發(fā)器總能自動(dòng)修改指向?qū)ふ艺系K面， 始終保持與障礙面的垂直，始終能完成測距，有效地克服了傳統(tǒng)的與載體固定連接的反射式測距方法的不足。②本發(fā)明中，組合使用了 MEMS慣性傳感器確定載體運(yùn)動(dòng)方向。DSP數(shù)字信號處理器通過處理MEMS陀螺傳感器的測量數(shù)據(jù)，能獲得運(yùn)動(dòng)載體相對于其初始位置的偏轉(zhuǎn)角度， 在輔助完成定位功能的同時(shí)還給用戶提供了載體的航向信息。③本發(fā)明的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)已在遙控電動(dòng)車自動(dòng)倒車入庫的應(yīng)用中通過了室內(nèi)試驗(yàn)驗(yàn)證。定位與定向精度高，方法正確可行，易于工程實(shí)現(xiàn)。。④本發(fā)明的二維導(dǎo)航系統(tǒng)能夠應(yīng)用于移動(dòng)探測器、移動(dòng)機(jī)器人車、救災(zāi)機(jī)器人車的室內(nèi)運(yùn)動(dòng)導(dǎo)航，以及在汽車防撞檢測、自動(dòng)泊車等領(lǐng)域都具有廣泛實(shí)用前景。


圖1是本發(fā)明室內(nèi)自主定位與定向的二圖IA是本發(fā)明室內(nèi)自主定位與定向的二圖2是本發(fā)明室內(nèi)自主定位與定向的二圖3是本發(fā)明室內(nèi)自主定位與定向的二圖4是本發(fā)明室內(nèi)自主定位與定向的二1.DSP數(shù)學(xué)信號處理器102.陀螺信息讀取單元104.左超聲波觸發(fā)脈沖生成單元106.左超聲波距離獲取單元108.左掃描舵機(jī)控制脈沖生成單元110.超聲波測距指向解析單元3.轉(zhuǎn)向舵機(jī)4.5A.左舵機(jī)本體5B.7.右超聲波收發(fā)器6.6B.右舵機(jī)搖臂6C.
維導(dǎo)航系統(tǒng)的硬件結(jié)構(gòu)圖。 二維導(dǎo)航系統(tǒng)的另一視角硬件結(jié)構(gòu)圖。 維導(dǎo)航系統(tǒng)中DSP處理器的功能框圖維導(dǎo)航系統(tǒng)中定位解算的情景示意圖維導(dǎo)航系統(tǒng)的工作流程框圖。 101.當(dāng)前位置坐標(biāo)解算單元 103.轉(zhuǎn)向舵機(jī)控制脈沖生成單元 105.右超聲波觸發(fā)脈沖生成單元 107.右超聲波距離獲取單元 109.右掃描舵機(jī)控制脈沖生成單元 2. MEMS陀螺傳感器左超聲波收發(fā)器左舵機(jī)搖臂右掃描舵機(jī)右舵機(jī)轉(zhuǎn)軸
5.左掃描舵機(jī) 5C.左舵機(jī)轉(zhuǎn)軸 6A.右舵機(jī)本體 10.運(yùn)動(dòng)載體
具體實(shí)施例方式下面將結(jié)合附圖對本發(fā)明做進(jìn)一步的詳細(xì)說明。參見圖1、圖IA所示，本發(fā)明的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其組成包括DSP數(shù)字信號處理器1、MEMS陀螺傳感器2、轉(zhuǎn)向舵機(jī)3、左超聲波收發(fā)器4、右超聲波收發(fā)器7、左掃描舵機(jī)5和右掃描舵機(jī)6 ；其中左掃描舵機(jī)5包括有左舵機(jī)本體5A、左舵機(jī)搖臂5B和左舵機(jī)轉(zhuǎn)軸5C ；左舵機(jī)搖臂5B為T形搖臂，左舵機(jī)搖臂5B的一端繞左舵機(jī)轉(zhuǎn)軸 5C旋轉(zhuǎn)，左舵機(jī)搖臂5B安裝在左舵機(jī)本體5A上。其中右掃描舵機(jī)6包括有右舵機(jī)本體 6A、右舵機(jī)搖臂6B和右舵機(jī)轉(zhuǎn)軸6C ；右舵機(jī)搖臂6B為T形搖臂，右舵機(jī)搖臂6B的一端繞右舵機(jī)轉(zhuǎn)軸6C旋轉(zhuǎn)，右舵機(jī)搖臂6B安裝在右舵機(jī)本體6A上。在本發(fā)明中，運(yùn)動(dòng)載體10的坐標(biāo)系記為ob_、yb、，也稱為本體坐標(biāo)系ob_、yb、；坐標(biāo)原點(diǎn)Ob保持與運(yùn)動(dòng)載體10的幾何中心重合，Ob-Xb7bZb坐標(biāo)系的、軸沿運(yùn)動(dòng)載體的縱向軸向前，Ob-Xb7bZb坐標(biāo)系的yb軸沿運(yùn)動(dòng)載體的橫向軸向左，Ob-Xb7bZb坐標(biāo)系的Ib軸垂直于運(yùn)動(dòng)載體向上，與、、yb構(gòu)成右手坐標(biāo)系。所述的DSP數(shù)字信號處理器1安裝在運(yùn)動(dòng)載體10上。所述的MEMS陀螺傳感器2安裝在運(yùn)動(dòng)載體10上，且安裝方位需要保證MEMS陀螺傳感器2的三軸與運(yùn)動(dòng)載體10的坐標(biāo)系ob_、yb、的三軸分別重合。MEMS陀螺傳感器2用于測量運(yùn)動(dòng)載體10在運(yùn)動(dòng)狀態(tài)下的三軸角速率信息F21(cobx，coby，cobz)，cobx表示在坐標(biāo)系 Ob-XbYbZb下繞、軸轉(zhuǎn)動(dòng)的角速率分量，ω by表示在坐標(biāo)系0b-、yb、下繞yb軸轉(zhuǎn)動(dòng)的角速率分量，cobz表示在坐標(biāo)系ob-、yb、下繞、軸轉(zhuǎn)動(dòng)的角速率分量。所述的轉(zhuǎn)向舵機(jī)3安裝在運(yùn)動(dòng)載體10的前端，轉(zhuǎn)向舵機(jī)搖臂31與運(yùn)動(dòng)載體10的轉(zhuǎn)向機(jī)構(gòu)相連；轉(zhuǎn)向舵機(jī)3用于操縱運(yùn)動(dòng)載體10實(shí)現(xiàn)轉(zhuǎn)向。所述的左超聲波收發(fā)器4安裝在左舵機(jī)搖臂5B上，且保證左超聲波收發(fā)器4所發(fā)超聲波波束的方向與左舵機(jī)搖臂5B的指向相同。左超聲波收發(fā)器4根據(jù)收到的左超聲波測距脈沖信號F14后發(fā)射出超聲波，然后當(dāng)左超聲波收發(fā)器4收到回波后，則輸出左距離脈沖信號I741給DSP數(shù)字信號處理器1。所述的右超聲波收發(fā)器7安裝在右舵機(jī)搖臂6B上，且保證右超聲波收發(fā)器7所發(fā)超聲波波束的方向與右舵機(jī)搖臂6B的指向相同。右超聲波收發(fā)器7根據(jù)收到的右超聲波測距脈沖信號F17后發(fā)射出超聲波，然后當(dāng)右超聲波收發(fā)器7收到回波后，則輸出右距離脈沖信號F71給DSP數(shù)字信號處理器1。所述的左舵機(jī)本體5A安裝在運(yùn)動(dòng)載體10上，其安裝方位需要保證在左舵機(jī)本體 5A復(fù)位時(shí)(上電初始狀態(tài))左舵機(jī)搖臂5B的指向沿著運(yùn)動(dòng)載體10的縱向軸(即ob-xbyb、 坐標(biāo)系的、軸)向前，且以后左舵機(jī)搖臂5B能向左旋轉(zhuǎn)不受阻礙。為了便于定位解算，左舵機(jī)本體5A安裝時(shí)同時(shí)保證左舵機(jī)轉(zhuǎn)軸5C垂直于運(yùn)動(dòng)載體10的左上頂點(diǎn)AA。所述的右舵機(jī)本體6A安裝在運(yùn)動(dòng)載體10上，其安裝方位需要保證在右舵機(jī)本體 6A復(fù)位時(shí)(上電初始狀態(tài))右舵機(jī)搖臂6B的指向沿著運(yùn)動(dòng)載體10的縱向軸(ob-Xbyb、坐標(biāo)系的&軸)向前，且以后右舵機(jī)搖臂6B能向右旋轉(zhuǎn)不受阻礙。為了便于定位解算，右舵機(jī)本體6A安裝時(shí)同時(shí)保證右舵機(jī)轉(zhuǎn)軸6C垂直于運(yùn)動(dòng)載體10的右上頂點(diǎn)BB。 在本發(fā)明中，左超聲波收發(fā)器4和左掃描舵機(jī)5組成左側(cè)掃描測距單元；右超聲波收發(fā)器7和右掃描舵機(jī)6組成右側(cè)掃描測距單元(參見圖2所示)。本發(fā)明中，DSP數(shù)字信號處理器1選用AnalogDevices公司生產(chǎn)的ADSP-BF537處理器。MEMS陀螺傳感器2選用hvenSense公司生產(chǎn)的ITG-3200陀螺儀。轉(zhuǎn)向舵機(jī)3選用輝盛公司的Tower Pro 9805BB舵機(jī)，其舵機(jī)搖臂31選取原裝的53. 3mm直型搖臂。左掃描舵機(jī)5選用ESKY公司的EK2-0508型數(shù)字舵機(jī)，其左舵機(jī)搖臂5B選取原裝的T形搖臂。 右掃描舵機(jī)6選用ESKY公司的EK2-0508型數(shù)字舵機(jī)，其右舵機(jī)搖臂6B選取原裝的T形搖臂。左超聲波收發(fā)器4選用上海芯源(中國)電子有限公司生產(chǎn)的SRF05超聲波傳感器， 其探測范圍Icm 細(xì)，精度可達(dá)3mm。右超聲波收發(fā)器7選用上海芯源(中國)電子有限公司生產(chǎn)的SRF05超聲波傳感器，其探測范圍Icm 細(xì)，精度可達(dá)3mm。參見圖2所示，用于實(shí)現(xiàn)本發(fā)明室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)的DSP數(shù)字信號處理器1，該DSP數(shù)字信號處理器1根據(jù)實(shí)現(xiàn)的功能包括有下列單元當(dāng)前位置坐標(biāo)解算單元101、陀螺信息讀取單元102、轉(zhuǎn)向舵機(jī)控制脈沖生成單元103、左超聲波觸發(fā)脈沖生成單元104、左超聲波距離獲取單元106、右超聲波觸發(fā)脈沖生成單元105、右超聲波距離獲取單元107、左掃描舵機(jī)控制脈沖生成單元108、右掃描舵機(jī)控制脈沖生成單元109、超聲波測距指向解析單元110。下面將詳細(xì)說明在DSP數(shù)字信號處理器1上各個(gè)單元實(shí)現(xiàn)的功能(1)陀螺信息讀取單元102陀螺信息讀取單元102第一方面用于讀取MEMS陀螺傳感器2測量得到的運(yùn)動(dòng)載體10在運(yùn)動(dòng)狀態(tài)下的三軸角速率信息F21(G)bx, COby，CObz)；第二方面對三軸角速率信息 F21(cobx，coby，ω J中的繞、軸轉(zhuǎn)動(dòng)的角速率分量《bz進(jìn)行初始時(shí)刻至當(dāng)前時(shí)刻角速率分
量的積分處理0 = Γ ，t0表示初始時(shí)刻，t表示當(dāng)前時(shí)刻，得到運(yùn)動(dòng)載體10相對于導(dǎo)航
nQ
坐標(biāo)系0-x。y。z。的偏轉(zhuǎn)角度θ (參見圖3所示)。在本發(fā)明中，用于確定運(yùn)動(dòng)載體10位置的坐標(biāo)系稱為導(dǎo)航坐標(biāo)系0-X。y。z。，導(dǎo)航坐標(biāo)系0-X。y。z。的坐標(biāo)原點(diǎn)0與初始位置時(shí)的運(yùn)動(dòng)載體10的幾何中心重合，χ。沿初始位置時(shí)的運(yùn)動(dòng)載體10的縱向軸向前，y0沿初始位置時(shí)的運(yùn)動(dòng)載體10的橫軸向左，Z0垂直運(yùn)動(dòng)載體10向上，與x。、y。構(gòu)成右手坐標(biāo)系。(2)轉(zhuǎn)向舵機(jī)控制脈沖生成單元103轉(zhuǎn)向舵機(jī)控制脈沖生成單元103第一方面用于產(chǎn)生控制轉(zhuǎn)向舵機(jī)3的脈沖信號 F13 (簡稱為轉(zhuǎn)向舵機(jī)脈沖信號F13)；第二方面以轉(zhuǎn)向舵機(jī)脈沖寬度變量F31控制轉(zhuǎn)向舵機(jī)脈
沖信號F13的脈沖寬度Wf13 (簡稱為轉(zhuǎn)向舵機(jī)脈沖寬度WF13)，且
權(quán)利要求
1.一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于該二維導(dǎo)航系統(tǒng)包括有 DSP數(shù)字信號處理器⑴、MEMS陀螺傳感器(2)、轉(zhuǎn)向舵機(jī)(3)、左超聲波收發(fā)器⑷、右超聲波收發(fā)器(7)、左掃描舵機(jī)( 和右掃描舵機(jī)(6)；其中左掃描舵機(jī)( 包括有左舵機(jī)本體 (5A)、左舵機(jī)搖臂(5B)和左舵機(jī)轉(zhuǎn)軸(5C)；左舵機(jī)搖臂(5B)為T形搖臂，左舵機(jī)搖臂(5B) 的一端繞左舵機(jī)轉(zhuǎn)軸(5C)旋轉(zhuǎn)，左舵機(jī)搖臂(5B)安裝在左舵機(jī)本體(5A)上；其中右掃描舵機(jī)(6)包括有右舵機(jī)本體(6A)、右舵機(jī)搖臂(6B)和右舵機(jī)轉(zhuǎn)軸(6C)；右舵機(jī)搖臂(6B) 為T形搖臂，右舵機(jī)搖臂(6B)的一端繞右舵機(jī)轉(zhuǎn)軸(6C)旋轉(zhuǎn)，右舵機(jī)搖臂(6B)安裝在右舵機(jī)本體(6A)上；所述的DSP數(shù)字信號處理器(1)安裝在運(yùn)動(dòng)載體(10)上；所述的MEMS陀螺傳感器(2)安裝在運(yùn)動(dòng)載體(10)上，且安裝方位需要保證MEMS陀螺傳感器O)的三軸與運(yùn)動(dòng)載體(10)的坐標(biāo)系ob_、yb、的三軸分別重合；MEMS陀螺傳感器 (2)用于測量運(yùn)動(dòng)載體(10)在運(yùn)動(dòng)狀態(tài)下的三軸角速率信息F21(cobx，coby，cobz)，cobx表示在坐標(biāo)系ob-xby#b下繞、軸轉(zhuǎn)動(dòng)的角速率分量，Ql3y表示在坐標(biāo)系ob-Xby#b下繞yb軸轉(zhuǎn)動(dòng)的角速率分量，"bz表示在坐標(biāo)系obibyb、下繞、軸轉(zhuǎn)動(dòng)的角速率分量；所述的轉(zhuǎn)向舵機(jī)C3)安裝在運(yùn)動(dòng)載體10的前端，轉(zhuǎn)向舵機(jī)搖臂31與運(yùn)動(dòng)載體(10)的轉(zhuǎn)向機(jī)構(gòu)相連；轉(zhuǎn)向舵機(jī)3用于操縱運(yùn)動(dòng)載體(10)實(shí)現(xiàn)轉(zhuǎn)向；所述的左超聲波收發(fā)器(4)安裝在左舵機(jī)搖臂(5B)上，且保證左超聲波收發(fā)器(4)所發(fā)超聲波波束的方向與左舵機(jī)搖臂(5B)的指向相同；左超聲波收發(fā)器(4)根據(jù)收到的左超聲波測距脈沖信號F14后發(fā)射出超聲波，然后當(dāng)左超聲波收發(fā)器(4)收到回波后，則輸出左距離脈沖信號F41給DSP數(shù)字信號處理器(1)；所述的右超聲波收發(fā)器(7)安裝在右舵機(jī)搖臂(6B)上，且保證右超聲波收發(fā)器(7)所發(fā)超聲波波束的方向與右舵機(jī)搖臂(6B)的指向相同；右超聲波收發(fā)器(7)根據(jù)收到的右超聲波測距脈沖信號F17后發(fā)射出超聲波，然后當(dāng)右超聲波收發(fā)器(7)收到回波后，則輸出右距離脈沖信號F71給DSP數(shù)字信號處理器(1)。
2.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于左超聲波收發(fā)器(4)和左掃描舵機(jī)( 組成左側(cè)掃描測距單元；右超聲波收發(fā)器(7)和右掃描舵機(jī)(6)組成右側(cè)掃描測距單元。
3.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于DSP數(shù)字信號處理器(1)選用AnalogDevices公司生產(chǎn)的ADSP-BF537處理器；MEMS陀螺傳感器(2) 選用hvenSense公司生產(chǎn)的ITG-3200陀螺儀；轉(zhuǎn)向舵機(jī)(3)選用輝盛公司的Tower Pro 9805BB舵機(jī)，其舵機(jī)搖臂選取原裝的53. 3mm直型搖臂；左掃描舵機(jī)(5)選用ESKY公司的 EK2-0508型數(shù)字舵機(jī)，其舵機(jī)搖臂選取原裝的T形搖臂；右掃描舵機(jī)(6)選用ESKY公司的 EK2-0508型數(shù)字舵機(jī)，其舵機(jī)搖臂選取原裝的T形搖臂；左超聲波收發(fā)器(4)選用上海芯源(中國)電子有限公司生產(chǎn)的SRF05超聲波傳感器，其探測范圍Icm 細(xì)，精度可達(dá)3mm ； 右超聲波收發(fā)器(7)選用上海芯源(中國)電子有限公司生產(chǎn)的SRF05超聲波傳感器，其探測范圍Icm 鈿，精度可達(dá)3mm。
4.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于DSP數(shù)字信號處理器(1)根據(jù)實(shí)現(xiàn)的功能包括有當(dāng)前位置坐標(biāo)解算單元(101)、陀螺信息讀取單元 (102)、轉(zhuǎn)向舵機(jī)控制脈沖生成單元(103)、左超聲波觸發(fā)脈沖生成單元(104)、左超聲波距離獲取單元(106)、右超聲波觸發(fā)脈沖生成單元(105)、右超聲波距離獲取單元(107)、左掃描舵機(jī)控制脈沖生成單元(108)、右掃描舵機(jī)控制脈沖生成單元(109)、超聲波測距指向解析單元(110)；陀螺信息讀取單元(10 第一方面用于讀取MEMS陀螺傳感器( 測量得到的運(yùn)動(dòng)載體(10)在運(yùn)動(dòng)狀態(tài)下的三軸角速率信息F21(cobx，coby，cobz)；第二方面對三軸角速率信息 F21(cobx，coby，ω J中的繞、軸轉(zhuǎn)動(dòng)的角速率分量《bz進(jìn)行初始時(shí)刻至當(dāng)前時(shí)刻角速率分量的積分處理
5.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于通過改變輸出的左舵機(jī)控制脈沖信號Fni的脈沖寬度Wn6能夠改變左偏轉(zhuǎn)角度α ；通過改變輸出的右舵機(jī)控制脈沖信號F19的脈沖寬度Wf19能夠改變右偏轉(zhuǎn)角度β ；利用改變左偏轉(zhuǎn)角度α 對固聯(lián)在左舵機(jī)搖臂5Β上的左超聲波收發(fā)器(4)的指向進(jìn)行實(shí)時(shí)可控調(diào)節(jié)；利用改變右偏轉(zhuǎn)角度β對固聯(lián)在右舵機(jī)搖臂(6Β)上的右超聲波收發(fā)器(7)的指向進(jìn)行實(shí)時(shí)可控調(diào)節(jié)。
6.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于超聲波測距指向解析單元(110)連續(xù)地增加或減小左舵機(jī)脈沖寬度Wn6和右舵機(jī)脈沖寬度Wf19，左偏轉(zhuǎn)角度α和右偏轉(zhuǎn)角度β將連續(xù)改變，從而實(shí)現(xiàn)兩個(gè)超聲波收發(fā)器的掃描測距；當(dāng)超聲波測距指向解析單元(110)不改變左舵機(jī)脈沖寬度Wn6和右舵機(jī)脈沖寬度Wf19，則左偏轉(zhuǎn)角度α和右偏轉(zhuǎn)角度β將固定不變，從而實(shí)現(xiàn)兩個(gè)超聲波收發(fā)器的定向測距。
7.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于轉(zhuǎn)向舵機(jī)控制脈沖信號F13是周期為19. 2ms的方波。
8.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于左舵機(jī)控制脈沖信號K6是周期為19. 2ms的方波；右舵機(jī)控制脈沖信號F19是周期為19. 2ms的方波。
9.根據(jù)權(quán)利要求1的一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，其特征在于左超聲波測距脈沖信號F14是一個(gè)脈沖寬度為10 μ S的單束脈沖；右超聲波測距脈沖信號F17是一個(gè)脈沖寬度為10 μ s的單束脈沖。
全文摘要
本發(fā)明公開了一種室內(nèi)自主定位與定向的二維導(dǎo)航系統(tǒng)，該二維導(dǎo)航系統(tǒng)由MEMS陀螺傳感器、DSP數(shù)字信號處理器、數(shù)字舵機(jī)和超聲波收發(fā)器等部件組成。本發(fā)明的二維導(dǎo)航系統(tǒng)安裝在運(yùn)動(dòng)載體上，DSP數(shù)字信號處理器一方面處理MEMS陀螺傳感器的測量數(shù)據(jù)，獲得運(yùn)動(dòng)載體相對于其初始位置的偏轉(zhuǎn)角度。另一方面通過調(diào)節(jié)數(shù)字舵機(jī)搖臂的偏轉(zhuǎn)角度，對分布在載體兩側(cè)、固聯(lián)在舵機(jī)搖臂上的超聲波收發(fā)器的對準(zhǔn)方位實(shí)施可控調(diào)節(jié)，實(shí)現(xiàn)掃描測距和定向測距兩種測距模式，測得載體到障礙面的垂直距離；并利用收到回波時(shí)處理器內(nèi)控制數(shù)字舵機(jī)偏轉(zhuǎn)角度的變量換算出此時(shí)對應(yīng)的測距指向角度；最后根據(jù)位置更新矩陣更新當(dāng)前載體相對于其初始位置的位置坐標(biāo)，完成對運(yùn)動(dòng)載體的一次局部自主定位與定向。
文檔編號G01C21/16GK102436260SQ20111025630
公開日2012年5月2日 申請日期2011年9月1日 優(yōu)先權(quán)日2011年9月1日
發(fā)明者劉創(chuàng), 舒磊正, 陳培, 韓潮 申請人:北京航空航天大學(xué)