組合導航系統(tǒng)的制作方法
【專利摘要】本發(fā)明提供一種包括室內(nèi)空間測量定位系統(tǒng)、圖像記憶測量系統(tǒng)、以及捷聯(lián)慣導系統(tǒng)的室內(nèi)組合導航系統(tǒng)。在該室內(nèi)組合導航系統(tǒng)中，可通過空間測量定位系統(tǒng)（wMPS）或圖像記憶測量系統(tǒng)輸出的高精度的位置信息對捷聯(lián)慣導系統(tǒng)（MEMS-SINS）輸出的姿態(tài)信息進行校正。本發(fā)明可廣泛應用于例如機器人、車輛等移動物體的定位以及姿勢輸出。
【專利說明】組合導航系統(tǒng)
【技術領域】
[0001]本發(fā)明涉及一種通過wMPS (Workspace Measuring Position System:室內(nèi)空間測量定位系統(tǒng))的測量值、或者搭載在載體(例如，機器人)上的視覺定位系統(tǒng)的測量值對MEMS-SINS (微機電-捷聯(lián)慣導系統(tǒng))的測量值進行校正的組合導航系統(tǒng)。
【背景技術】
[0002]目前,慣性導航基本上分為平臺式(Gimbaled)和捷聯(lián)式(Strapdown)兩大類。在捷聯(lián)式慣導中，慣性傳感器直接固連在載體上，并跟隨載體在空間一起轉動，整個系統(tǒng)以計算機為中心的數(shù)學平臺來替代平臺式慣導中的常規(guī)型物理平臺。上述慣性傳感器分為兩大類，一類為距離傳感器，這是一類可用來測量加速度、速度或位移的傳感器，例如加速度計、速度計、里程表、多普勒雷達和氣壓計等；另一類為角度傳感器，這是一類可用來測量角度變化或方位的傳感器，例如陀螺儀和磁羅盤等。其中，陀螺儀和加速度計通常作為慣性導航系統(tǒng)的慣性傳感器。
[0003]另外，最近，隨著通過硅加工以極小尺寸制造以往由金屬等制成的機械系統(tǒng)的技術(MEMS)的發(fā)展，例如，在硅上制成μ m單位的超小型的齒輪或凸輪、彈簧等成為可能。進而，出現(xiàn)了采用MEMS技術的陀螺儀、加速度計即MEMS陀螺儀、MEMS加速度計等。由于通過將MEMS陀螺儀、MEMS加速度計等慣性傳感器直接固連在載體(例如，機器人等)上，能夠大大減輕載體的載重量，因此MEMS這一技術得到了廣泛的應用。在此，將應用MEMS陀螺儀、MEMS加速度計等慣性傳感器的捷聯(lián)式慣導簡稱為MEMS-SINS。
[0004]但是，在MEMS-SINS測量而得的測量值中包含各種誤差，這些誤差會通過積分運算逐漸在定位結果中得到積累，若定位結果未被及時校正、重置，則定位誤差隨時間逐漸積累，從而導致MEMS-SINS最終會失去使用價值。尤其是，例如，搭載有MEMS-SINS的機器人在室內(nèi)從事對定位精度有極高要求的情況下，上述補正便變得尤為重要。
【發(fā)明內(nèi)容】
[0005]本發(fā)明便是鑒于上述問題而提出的，其目的在于提供一種組合導航系統(tǒng)，能夠利用從wMPS (Workspace Measuring Position System:室內(nèi)空間測量定位系統(tǒng))或者視覺定位系統(tǒng)輸出的位置信息、與MEMS-SINS (微機電-捷聯(lián)慣導系統(tǒng))輸出的位置信息對MEMS-SINS輸出的姿態(tài)測量值進行校正。
[0006]根據(jù)本發(fā)明，提供一種室內(nèi)組合導航系統(tǒng)包括室內(nèi)空間測量定位系統(tǒng)、捷聯(lián)慣導系統(tǒng)、比較器、卡爾曼濾波器。該室內(nèi)空間測量定位系統(tǒng)具備:光信號接收器，安裝在載體上，用于接收光信號發(fā)射器所發(fā)射的扇面激光信號；載體位置算出部，基于所述光信號接收器所接收到的所述扇面激光信號算出所述載體的位置；載體速度算出部，通過對所述載體位置算出部算出的所述位置進行擬合算出所述載體的速度。捷聯(lián)慣導系統(tǒng)，安裝在所述載體上，由微機電構成，輸出該載體的位置、速度以及姿態(tài)。比較器，基于從所述載體位置算出部輸入的所述載體的位置與從所述捷聯(lián)慣導系統(tǒng)輸入的所述載體的位置計算第I位置差，基于從所述載體速度算出部輸入的所述載體的速度與從所述捷聯(lián)慣導系統(tǒng)輸入的所述載體的速度計算速度差。卡爾曼濾波器，基于從所述比較器輸入的所述第I位置差與速度差，算出對所述姿態(tài)的第I姿態(tài)校正量。所述捷聯(lián)慣導系統(tǒng)基于所述第I姿態(tài)校正量對所述姿態(tài)進行校正，并輸出校正后的姿態(tài)。
[0007] 據(jù)此，能夠利用從wMPS輸出的位置信息、速度信息與MEMS-SINS^iW1-捷聯(lián)慣導系統(tǒng))輸出的位置信息、速度信息對MEMS-SINS輸出的姿態(tài)測量值進行校正。
[0008]在上述室內(nèi)組合導航系統(tǒng)中，所述光信號發(fā)射器發(fā)射兩束扇面激光信號，且在室內(nèi)空間中至少配置2臺。
[0009]據(jù)此，能夠使WMPS所輸出的位置具有更高的精度，因此將該位置作為室內(nèi)組合導航系統(tǒng)的位置進行輸出，能夠實現(xiàn)高精度的導航。
[0010]在上述室內(nèi)組合導航系統(tǒng)中，所述載體位置算出部基于所述光信號接收器所接收到的所述扇面激光信號以及所述光信號發(fā)射器的位置算出所述載體的位置。
[0011]據(jù)此，能夠使WMPS所輸出的位置具有更高的精度，因此將該位置作為室內(nèi)組合導航系統(tǒng)的位置進行輸出，能夠實現(xiàn)高精度的導航。
[0012]在上述室內(nèi)組合導航系統(tǒng)中，還包括圖像記憶測量系統(tǒng)，該圖像記憶測量系統(tǒng)利用對拍攝而得的圖像中的多個特征點坐標，通過雙目交會原理，反解算所述載體的位置、姿態(tài)。所述比較器基于從所述圖像記憶測量系統(tǒng)輸入的所述載體的位置、姿態(tài)以及從所述捷聯(lián)慣導系統(tǒng)輸入的所述載體的位置、姿態(tài)分別計算第2位置差、姿態(tài)差。所述卡爾曼濾波器基于從所述比較器輸入的第2位置差以及姿態(tài)差，算出對所述姿態(tài)的第2姿態(tài)校正量；所述捷聯(lián)慣導系統(tǒng)基于所述第2姿態(tài)校正量對所述姿態(tài)進行校正，并輸出校正后的姿態(tài)。
[0013]據(jù)此，能夠不局限于利用空間測量定位系統(tǒng)輸出的位置信息、速度信息與MEMS-SINS (微機電-捷聯(lián)慣導系統(tǒng))輸出的位置信息、速度信息對MEMS-SINS輸出的姿態(tài)測量值進行校正，也能夠通過視覺定位系統(tǒng)輸出的位置信息、姿態(tài)信息與MEMS-SINS (微機電-捷聯(lián)慣導系統(tǒng))輸出的位置信息、姿態(tài)信息對MEMS-SINS輸出的姿態(tài)測量值進行校正。
[0014]在上述室內(nèi)組合導航系統(tǒng)中，還包括選通開關以及選通控制部，該選通開關選通所述室內(nèi)空間測量定位系統(tǒng)或者所述圖像記憶測量系統(tǒng)中的一方。該選通控制部通過判斷所述光信號接收器所接收的所述扇面激光信號的誤碼率，對所述選通開關進行控制，使所述選通開關選通所述室內(nèi)空間測量定位系統(tǒng)或者所述圖像記憶測量系統(tǒng)中的一方。
[0015]據(jù)此，能夠通過誤碼率實現(xiàn)是選通室內(nèi)空間測量定位系統(tǒng)還是選通圖像記憶測量系統(tǒng)。由于判斷誤碼率是通過軟體進行的，因此不會增加硬件上的成本，從而實現(xiàn)選通成本的降低。
[0016]在上述室內(nèi)組合導航系統(tǒng)中，在所述選通控制部判斷為所述光信號接收器所接收的所述扇面激光信號的誤碼率為規(guī)定閾值以上時，所述選通控制部控制所述選通開關使所述圖像記憶測量系統(tǒng)處于選通狀態(tài)，而使所述室內(nèi)空間測量定位系統(tǒng)處于未選通狀態(tài)。
[0017]據(jù)此，在通過誤碼率判斷光信號接收器所接收的扇面激光信號的信號品質差時，能夠選通輸出的位置的精度相對較高的圖像記憶測量系統(tǒng)。從而實現(xiàn)了連續(xù)地高精度的導航。
[0018]在上述室內(nèi)組合導航系統(tǒng)中，在所述選通控制部判斷為所述光信號接收器所接收的所述扇面激光信號的誤碼率小于所述規(guī)定閾值時，所述選通控制部控制所述選通開關使所述室內(nèi)空間測量定位系統(tǒng)處于選通狀態(tài)，而使所述圖像記憶測量系統(tǒng)處于未選通狀態(tài)。
[0019]據(jù)此，在通過誤碼率判斷光信號接收器所接收的扇面激光信號的信號品質高時，能夠重新選通輸出的位置的精度高的室內(nèi)空間測量定位系統(tǒng)。由此能夠避免因使用圖像記憶測量系統(tǒng)而帶來的高負荷的數(shù)據(jù)運算，以及降低了對周圍環(huán)境(例如，光照條件等)的依賴性。從而實現(xiàn)了受外部條件影響小地連續(xù)地高精度的導航。
[0020]在所述組合導航系統(tǒng)中，所述的組合導航系統(tǒng)還包括:信號強度檢測部，用于檢測由所述光信號接收器所接收的所述光信號的信號強度，并輸出所述信號強度；選通控制部，在判斷輸入的所述信號強度大于信號強度比較用閾值時，使所述室內(nèi)空間測量定位系統(tǒng)處于選通狀態(tài)，而使所述圖像記憶測量系統(tǒng)處于未選通狀態(tài)。
[0021]據(jù)此，能夠通過對光信號發(fā)射器所發(fā)射的扇面激光信號的信號強度進行判斷，來實現(xiàn)室內(nèi)空間測量定位系統(tǒng)的選通。
[0022]在所述組合導航系統(tǒng)中，所述的組合導航系統(tǒng)還包括:照度值取得部，取得外界的照度值；所述選通控制部，在判斷輸入的所述信號強度不大于信號強度比較用閾值、且照度值大于照度值比較用閾值時，使所述圖像記憶測量系統(tǒng)處于選通狀態(tài)，而使所述室內(nèi)空間測量定位系統(tǒng)處于未選通狀態(tài)；在判斷輸入的所述信號強度不大于信號強度比較用閾值、照度值不大于照度值比較用閾值、且信號強度偏差率大于照度值偏差率時，使所述圖像記憶測量系統(tǒng)處于選通狀態(tài)，而使所述室內(nèi)空間測量定位系統(tǒng)處于未選通狀態(tài)；在判斷輸入的所述信號強度不大于信號強度比較用閾值、照度值不大于照度值比較用閾值、且信號強度偏差率不大于照度值偏差率時，使所述室內(nèi)空間測量定位系統(tǒng)處于選通狀態(tài)，而使所述圖像記憶測量系統(tǒng)處于未選通狀態(tài)。
[0023]據(jù)此，能夠從wMPS或圖像記憶測量系統(tǒng)中選通輸出的位置的精度較高一方。這樣一來，即使在wMPS與圖像記憶測量系統(tǒng)的輸出的位置的精度都差的情況下，也能夠通過選通二者之中輸出的位置的精度相對較高的一方，從而最大限度地實現(xiàn)高精度的姿態(tài)校正。
【專利附圖】
【附圖說明】
[0024]圖1是表示涉及本發(fā)明的第I實施方式的框圖。
[0025]圖2是表示涉及本發(fā)明的第I實施方式的流程圖。
[0026]圖3是表示涉及本發(fā)明的第2實施方式的框圖。
[0027]圖4是表示涉及本發(fā)明的第2實施方式的流程圖。
[0028]圖5是表示涉及本發(fā)明的第3實施方式的框圖。
[0029]圖6是表示涉及本發(fā)明的第3實施方式的流程圖。
[0030]圖7是表示涉及本發(fā)明的第2實施方式的另一流程圖。
[0031]圖8是表示涉及本發(fā)明的第2實施方式的另一流程圖。圖中，
[0032]1:MEMS-SINS ;2、2X:wMPS ；3:比較器；4:卡爾曼濾波器；5、5X:圖像記憶測量系統(tǒng)；6:選通控制部；7:選通開關。
【具體實施方式】
[0033]第I實施方式
[0034]下面，對照圖1以及圖2具體說明本發(fā)明的第I實施方式。[0035]首先，對照圖1說明本實施方式的構成。圖1是表示涉及本發(fā)明的第I實施方式的框圖。
[0036]在圖1中，室內(nèi)組合導航系統(tǒng)S由MEMS-SINS1、WMPS2、比較器3、卡爾曼濾波器4等構成。
[0037]其中，MEMS-SINS1具備MEMS陀螺儀(未圖示)、MEMS加速度計等(未圖示)，安裝于需要定位導航的載體(例如，室內(nèi)機器人)上。MEMS-SINS1通過對MEMS陀螺儀、MEMS加速度計取得的載體的運動信息(角速率、加速度等)進行解算，得到該載體的位置(三維坐標)、速度(三維速度)、姿態(tài)等信息并向后述的比較器3輸出。
[0038]wMPS2具備光信號接收器21、載體位置模塊22以及載體速度算出模塊23等。
[0039]光信號接收器21安裝在上述載體上，用于接收由2臺以上的光信號發(fā)射器(未圖示)按照一定周期發(fā)射出的扇面激光信號。在此，每臺光信號發(fā)射器邊旋轉邊發(fā)射兩束扇束(扇面激光信號)。
[0040]載體位置模塊(載體位置算出部)22利用光信號接收器21所接收的扇面激光信號來確定相對于光信號發(fā)射器的位置。在此，由于扇面以預定角度彼此交叉，因此用于接收扇束的光信號接收器21的三維坐標值(即，光信號接收器21的位置或者高度)能夠被載體位置模塊22解算出。在通常的wMPS中，載體位置模塊22可按照數(shù)十Hz的頻率輸出位置。
[0041]載體速度算出模塊23通過對上述位置信息的擬合，能夠得到近似的速度信息，向后述的比較器3輸出。
[0042]在本實施方式中，設定光信號接收器21的位置與載體的位置相一致，即光信號接收器21的位置即為載體的位置。另外，在本發(fā)明的其他實施方式中也采用這樣的設定。不言自明的是，若光信號接收器21的位置載體的位置不一致，則需要進行坐標系變換求解載體的位置。
[0043]另外，多臺光信號發(fā)射器被分別事先配置在室內(nèi)的多個固定位置上。每臺光信號發(fā)射器所發(fā)射出的扇面激光信號中包含有能夠識別該光信號發(fā)射器的識別信息(例如，脈沖列)，并且光信號發(fā)射器彼此的識別信息(脈沖列)均不相同。載體位置模塊(載體位置算出部)22在解算光信號接收器21時，可以根據(jù)需要，利用該識別信息。
[0044]比較器3對從MEMS-SINS1輸入的位置、速度以及從wMPS2輸入的位置、速度分別求出位置差與速度差，并將該位置差與速度差輸入后述的卡爾曼濾波器4。
[0045]卡爾曼濾波器4利用上述差值即位置差與速度差，算出姿態(tài)校正量，并將該姿態(tài)校正量輸出至MEMS-SINS1。MEMS-SINS1利用該姿態(tài)校正量對由其解算得到的姿態(tài)進行校正，并將校正后的姿態(tài)作為室內(nèi)組合導航系統(tǒng)S的姿態(tài)輸出。
[0046]下面，對照圖2說明本實施方式的動作。圖2為表示本發(fā)明所述方式的動作的流程圖。
[0047]S201:首先，WMPS2算出載體當前的位置、并擬合出速度輸入比較器3。
[0048]S202:比較器3求出從MEMS-SINS1輸入的位置與wMPS2輸入的位置之差(與三維坐標值對應的差，以下稱為“位置差”)，同時求出從MEMS-SINS1輸入的速度與wMPS2輸入的速度之差(與三維速度值對應的差，以下稱為“速度差”)，并輸入卡爾曼濾波器4。
[0049]S203:卡爾曼濾波器4局域輸入的位置差以及速度差，算出姿態(tài)的校正量并輸入至 MEMS-SINS1。[0050]S204:MEMS-SINS1利用輸入的姿態(tài)的校正量對由其解算得到的載體的姿態(tài)進行校正，并將校正后的姿態(tài)向外部輸出。
[0051]第2實施方式
[0052]下面，對照圖3、圖4說明本發(fā)明的第2實施方式。
[0053]首先，對照圖3說明本實施方式的構成。圖3為表示涉及本發(fā)明的第2實施方式的框圖。在圖3中，對與圖1中相同的構成要素賦予相同的符號，并省略其說明。
[0054]圖3 (室內(nèi)組合導航系統(tǒng)SI)與圖1 (室內(nèi)組合導航系統(tǒng)S)的不同之處在于，在圖3中還包括WMPS2X、圖像記憶測量系統(tǒng)5、選通控制部6、以及選通開關7。
[0055]WMPS2X與wMPS2的不同之處在于其增加了信號強度檢測器(未圖示)，該信號強度檢測器用于檢測來自信號發(fā)射器的紅外激光信號的信號強度，并將檢測出的信號強度輸入至后述的選通控制部6。在此，要指出的是，該信號強度檢測器的檢測對象是WMPS2X用于計算載體的位置的扇面激光信號的信號強度。
[0056]圖像記憶測量系統(tǒng)5與MEMS-SINS1搭載于同一載體上，包括:可對載體四周環(huán)境全方位進行拍攝的CCD照相機(雙目攝像機)；對CCD照相機拍攝的圖像進行數(shù)字處理的DSP,該DSP還能夠從圖像中選擇特征點，根據(jù)WMPS2得出的載體的坐標以及相機坐標，通過雙目視覺交會原理能夠求出特征點在世界坐標系中的坐標，并存儲圖像、特征點以及坐標信息的存儲部。另外，DSP能夠將CCD照相機現(xiàn)在拍照而得的圖像與所存儲的圖像、特征點進行匹配，從而從CCD照相機現(xiàn)在拍照而得的圖像中提取之前存儲的特征點，并根據(jù)存儲的該特征點的坐標，反解算出載體在坐標系中的坐標(位置)、姿態(tài)。
[0057]選通控制部6具備存儲比較用規(guī)定閾值的存儲部，選通控制部6基于來自信號強度檢測器的信號強度，控制后述的選通開關7的選通對象。具體而言，選通控制部6將來自信號強度檢測器的信號強度與比較用規(guī)定閾值進行比較，當比較結果是來自信號強度檢測器的信號強度大于比較用規(guī)定閾值時，選通控制部6控制選通開關7只選通WMPS2X，當比較結果是來自信號強度檢測器的信號強度不大于比較用規(guī)定閾值時，選通控制部6控制選通開關7只選通圖像記憶測量系統(tǒng)5。
[0058]選通開關7基于選通控制部6的控制從WMPS2X以及圖像記憶測量系統(tǒng)5中選通其一，并使來自選通的選通對象(WMPS2X或圖像記憶測量系統(tǒng)5)的信號輸入比較器3。
[0059]下面，對照圖4說明本發(fā)明的第2實施方式的動作。圖4是表示涉及本發(fā)明的第2實施方式的動作的流程圖。
[0060]S401:首先，CXD照相機對載體四周環(huán)境全方位進行拍攝并將拍攝而得的映像數(shù)據(jù)存儲于圖像記憶測量系統(tǒng)5的存儲部中。
[0061]S402 =DSP從存儲于圖像記憶測量系統(tǒng)5的存儲部中讀出映像數(shù)據(jù)，并從映像數(shù)據(jù)中提取圖像的特征點并將該特征點在世界坐標系中的空間位置坐標也一同存儲至該存儲部。
[0062]S403:選通控制部6判斷來自信號強度檢測器的信號強度是否大于比較用規(guī)定閾值。
[0063]在S403的判斷結果為來自信號強度檢測器的信號強度連續(xù)規(guī)定時間大于比較用規(guī)定閾值時(S403:是)，選通控制部6向選通開關7發(fā)送選通WMPS2X的指令，選通開關7基于該指令選通wMPS2X (S404)。然后，wMPS2X解算出載體當前的位置(三維坐標值)并擬合出速度輸入至比較器3(S405)。接著，進入S202?S204的處理，有關S202?S204的處理參考前述。
[0064]在S403的判斷結果為來自信號強度檢測器的信號強度連續(xù)規(guī)定時間不大于比較用規(guī)定閾值時(S403:否)，選通控制部6向選通開關7發(fā)送選通圖像記憶測量系統(tǒng)5的指令，選通開關7根據(jù)該指令選通圖像記憶測量系統(tǒng)5 (S406)。
[0065]S407 =DSP能夠將CXD照相機現(xiàn)在拍照而得的圖像與所存儲的圖像、特征點進行匹配。
[0066]S408 =DSP通過上述圖像匹配從CXD照相機現(xiàn)在拍照而得的圖像中提取之前存儲的特征點，并根據(jù)存儲的該特征點的坐標，反解算出載體在坐標系中的坐標(位置)和姿態(tài)。進而言之，利用至少3個不共線的特征點，通過雙目交會原理反解算姿態(tài)和位置。
[0067]S409:在S408反解算出的載體的姿態(tài)和位置輸入比較器3，與由MEMS-SINS1輸入的姿態(tài)、位置進行比較，求對應的姿態(tài)差、位置差，并將這些差值輸入至卡爾曼濾波器4。
[0068]S410:卡爾曼濾波器4利用輸入的差值(位置差、姿態(tài)差)生成姿態(tài)校正量，并向MEMS-SINS1輸出。MEMS-SINS1利用由卡爾曼濾波器4輸入的姿態(tài)校正量對其解算而得的姿態(tài)進行校正，并輸出校正后的姿態(tài)(S204)。
[0069]第3實施方式
[0070]下面，對照圖5、圖6說明本發(fā)明的第3實施方式。
[0071]首先，說明本實施方式的構成。圖5表示涉及本發(fā)明的第3實施方式的框圖。在圖5中，對與第2實施方式的圖3中相同的構成要素賦予相同的符號，并省略其說明。
[0072]圖5 (室內(nèi)組合導航系統(tǒng)S2)與圖3 (室內(nèi)組合導航系統(tǒng)SI)的不同之處在于，在圖5中還包括圖像記憶測量系統(tǒng)5X。
[0073]圖像記憶測量系統(tǒng)5X與圖像記憶測量系統(tǒng)5的不同之處在于其增加了照度值取得部(未圖示)，該照度值取得部用于取得CCD照相機拍攝時的照度值，并將照度值輸入至后述的選通控制部6。
[0074]選通控制部6基于來自信號強度檢測器的信號強度、來自照度值取得部的照度值，以及存儲于選通控制部6的存儲部的比較用規(guī)定閾值以及照度值比較用閾值，對WMPS2X以及圖像記憶測量系統(tǒng)5X進行選通。
[0075]下面，對照圖6說明本發(fā)明的第3實施方式的動作。圖6表示涉及本發(fā)明的第3實施方式的流程圖。
[0076]S601:信號強度檢測器檢測信號強度并輸入選通控制部6，與此同時，照度值取得部取得環(huán)境的照度值并輸入選通控制部6。
[0077]S602:選通控制部6將信號強度、照度值分別與比較用規(guī)定閾值以及照度值比較用閾值。
[0078]S603:選通控制部6判斷輸入的信號強度是否大于比較用規(guī)定閾值。
[0079]在選通控制部6判斷信號強度大于比較用規(guī)定閾值的情況下(S603:是)，則不管照度值與照度值比較用閾值的比較結果，選通控制部6選通wMPS2X(S404)。然后依次執(zhí)行S405、S202、S203、S204,有關 S405、S202、S203、S204 參考前述。
[0080]在選通控制部6判斷信號強度不大于比較用規(guī)定閾值時(S603:否)，選通控制部6判斷輸入的照度值是否大于照度值比較用閾值(S604)。在選通控制部6判斷照度值大于照度值比較用閾值的情況下(S604:是)，選通控制部6向選通開關7發(fā)送選通圖像記憶測量系統(tǒng)5X的指令，從而選通圖像記憶測量系統(tǒng)5X (S606)。然后依次執(zhí)行S407?S410，接著執(zhí)行S204。有關S407?S410以及S204請參考前述。
[0081]在選通控制部6判斷照度值不大于照度值比較用閾值的情況下(S604:否)，選通控制部6接著判斷信號強度偏差率是否大于照度值偏差率(S605)。其中，信號強度偏差率=(信號強度-比較用規(guī)定閾值)/比較用規(guī)定閾值* 100%，照度值偏差率=(照度值-照度值比較用閾值)/照度值比較用閾值* 100%。在選通控制部6判斷信號強度偏差率大于照度值偏差率(S605:是)的情況下，進入S606，接著依次執(zhí)行S407?S410繼而執(zhí)行S204 ；。在選通控制部6判斷信號強度偏差率不大于照度值偏差率的情況下(S605:否)，進入步驟S404繼而順次執(zhí)行S405、S202?S204。
[0082]《補足I》
[0083]在上述第2實施方式中，選通控制部6利用來自信號強度檢測器的信號強度信息，選通圖像記憶測量系統(tǒng)5。但不限于此，在wMPS中也可不設置該信號強度檢測器，取而代之，設置可檢測光信號接收器所接收到的扇面激光信號的誤碼率選通圖像記憶測量系統(tǒng)5亦可。具體而言，在選通控制部6的存儲器中存儲有誤碼率用規(guī)定閾值，在選通控制部6判斷為光信號接收器所接收的扇面激光信號的誤碼率為誤碼率用規(guī)定閾值以上時，選通控制部6控制選通開關7使圖像記憶測量系統(tǒng)5處于選通狀態(tài)，而使室內(nèi)空間測量定位系統(tǒng)2X處于未選通狀態(tài)，即選通控制部6向選通開關7發(fā)送選通圖像記憶測量系統(tǒng)5的指令，選通開關7執(zhí)行該指令。另一方面，在選通控制部6判斷為光信號接收器所接收的扇面激光信號的誤碼率小于誤碼率用規(guī)定閾值時，選通控制部6控制選通開關7使室內(nèi)空間測量定位系統(tǒng)2X處于選通狀態(tài)，而使圖像記憶測量系統(tǒng)5X處于未選通狀態(tài)。
[0084]《補足2》
[0085]在上述第2實施方式以及第3實施方式中，選通控制部與選通開關分別獨立地設置，但對選通控制部與選通開關實施模塊化亦可，即通過一模塊實現(xiàn)選通控制部與選通開關的功能亦可。再有，選通控制部與選通開關集成至wMPS、圖像記憶測量系統(tǒng)、或者比較器之一中亦可。據(jù)此，零部件的表面安裝變得簡單。
[0086]《補足3》
[0087]圖7是表示涉及本發(fā)明的第2實施方式的另一流程圖。
[0088]其中，符號7A中的處理與上述各實施方式中的MEMS-SINS的處理的一部分相當。
[0089]S701:陀螺儀、加計(加速度計)采集IMU數(shù)據(jù)(慣性導航數(shù)據(jù)，例如，角速度、加速度)。
[0090]S702:由于在S701中所采集到的MU數(shù)據(jù)中存在噪聲，因此對在S701中采集到的數(shù)據(jù)進行低通濾波處理，從而去除上述噪聲。
[0091]S703:對在S702中得到的去除噪聲后的IMU數(shù)據(jù)進行誤差補償，在此，該誤差補償主要是對陀螺儀、加計進行零偏、溫度補償和刻度系數(shù)誤差補償。
[0092]S704:將經(jīng)S702以及S703處理后的IMU數(shù)據(jù)發(fā)送至導航解算CPU。
[0093]另外，與符號7A中的處理同時,執(zhí)行符號7B中的處理。符號7B中的處理與上述wMPS中的處理的一部分相當。
[0094]S705:采集測站脈沖，這里的“站”是指基站，即上述各實施方式中的光信號發(fā)射器。
[0095]S706:脈沖匹配，即通過脈沖中所包含的信息(脈沖列)認識發(fā)射該脈沖的基站的位置。
[0096]S707:對所接收的脈沖進行交會解算，從而得到載體的位置。
[0097]S708:將wMPS數(shù)據(jù)(S707中得到的位置)發(fā)送至導航解算CPU。
[0098]另外，符號7C與上述第2實施方式中的選通控制部、wMPS、圖像記憶測量系統(tǒng)、選通開關的各功能的一部分相當。要指出的是，在符號7C的處理中，作為判斷所接收的扇面激光信號的品質的標準，選用了《補足I》中所言及的方法，即使用誤碼率作為判斷的基準，而非如第2或3實施方式中的利用信號強度檢測器所檢測出的扇面激光信號的信號強度。為了便于說明，在本補足中，將執(zhí)行符號7C的處理的硬件以及軟件統(tǒng)和后，稱為“導航解算CPU”。
[0099]S709:首先，導航解算CPU進行參數(shù)初始化。
[0100]S710:導航解算CPU基于來自7A的IMU數(shù)據(jù)(慣性敏感器數(shù)據(jù))與從S711載入的初始導航參數(shù)來進行粗對準。據(jù)此，能夠對MEMS-SINS的初始姿態(tài)進行初步估計。
[0101]S712:導航解算CPU進行精對準。
[0102]S713:導航解算CPU檢測來自7B的wMPS數(shù)據(jù)(wMPS系統(tǒng)解算數(shù)據(jù))的誤碼率，從而判斷wMPS數(shù)據(jù)是否有效。
[0103]S714:在S713的判斷結果為wMPS數(shù)據(jù)無效(S713:否)時，導航解算CPU進行特征
點記憶/慣性組合導航解算。
[0104]S715:在S713的判斷結果為wMPS數(shù)據(jù)有效(S713:是)時，導航解算CPU進行組合
導航解算。
[0105]S716:導航解算CPU將在S515以及S714中解算而得的導航參數(shù)進行輸出。并上傳導航數(shù)據(jù)(S717)
[0106]在此，該導航解算CPU可視為集成在MEMS_SINS中。
[0107]《補足4》
[0108]圖8是表不涉及本發(fā)明的第2實施方式的另一流程圖。另外，圖8也是對圖7中的S714的詳述。其動作的執(zhí)行均由圖像記憶測量系統(tǒng)中的圖像特征標記CPU來完成。
[0109]S801:兩臺的CXD照相機進行雙目拍攝。
[0110]S802:從拍攝而得的圖像中，提取并識別特征點。
[0111]S803:圖像特征標記CPU檢測來自7B的wMPS數(shù)據(jù)(wMPS系統(tǒng)解算數(shù)據(jù))的誤碼率，從而判斷wMPS數(shù)據(jù)是否有效。
[0112]S804:在S803的判斷結果為wMPS數(shù)據(jù)有效(S803:是)時，圖像特征標記CPU進行特征點記憶標記與定位。
[0113]S805:存儲在S804中處理而得的特征點信息(位置信息等)。
[0114]S806:在S803的判斷結果為wMPS數(shù)據(jù)無效(S803:否)時，圖像特征標記CPU搜索并跟蹤存儲特征區(qū)域。
[0115]S807:圖像特征標記CPU對當前拍攝而得的圖像與事先存儲的圖像(特征點)進行匹配。
[0116]S808:圖像特征標記CPU提取特征。[0117]S809:圖像特征標記CPU反解算載體空間坐標。
[0118]S810:圖像特征標記CPU輸出載體坐標值。
[0119]在此，要指明的是，在S810輸出的載體坐標值具有極高的精度，除了可用于對MEMS-SINS所輸出的姿態(tài)的校正之外，也可直接作為室內(nèi)組合導航系統(tǒng)的輸出值輸出。
[0120]《補足5》
[0121]在上述各實施方式以及各補足中，在判斷wMPS信號是否有效時，使用了信號強度以及誤碼率，不限于此，也可考慮錯誤發(fā)生次數(shù)。具體而言，例如，判斷信號強度低于閾值且持續(xù)規(guī)定期間時，錯誤發(fā)生次數(shù)計數(shù)為1，隨后，判斷誤碼率高于閾值時，錯誤發(fā)生次數(shù)累計計數(shù)為2，如此累計，例如，在規(guī)定時間內(nèi)，該錯誤發(fā)生次數(shù)計數(shù)達到規(guī)定閾值時，則判斷wMPS信號無效。這樣一來，考慮信號強度的同時也考慮誤碼率，從而能夠更高精度地判斷wMPS信號是否有效。
[0122]另外，本發(fā)明可應用于例如機器人、車輛等移動物體的定位以及姿勢輸出。
【權利要求】
1.一種室內(nèi)組合導航系統(tǒng)，其特征在于，包括: 室內(nèi)空間測量定位系統(tǒng)，具備: 光信號接收器，其安裝在載體上，用于接收光信號發(fā)射器所發(fā)射的扇面激光信號； 載體位置算出部，其基于所述光信號接收器所接收到的所述扇面激光信號算出所述載體的位置； 載體速度算出部，其通過對所述載體位置算出部算出的所述位置進行擬合算出所述載體的速度； 捷聯(lián)慣導系統(tǒng)，其安裝在所述載體上，由微機電構成，輸出該載體的位置、速度以及姿態(tài)； 比較器，其基于從所述載體位置算出部輸入的所述載體的位置與從所述捷聯(lián)慣導系統(tǒng)輸入的所述載體的位置計算第I位置差，基于從所述載體速度算出部輸入的所述載體的速度與從所述捷聯(lián)慣導系統(tǒng)輸入的所述載體的速度計算速度差； 卡爾曼濾波器，其基于從所述比較器輸入的所述第I位置差與速度差，算出對所述姿態(tài)的第I姿態(tài)校正量； 所述捷聯(lián)慣導系統(tǒng)，其基于所述第I姿態(tài)校正量對所述姿態(tài)進行校正，并輸出校正后的姿態(tài)。
2.根據(jù)權利要求1所述的室內(nèi)組合導航系統(tǒng)，其特征在于， 所述光信號發(fā)射器，其在室內(nèi)空間中至少配置2臺，每臺發(fā)射兩束扇面激光信號。
3.根據(jù)權利要求1所述的室內(nèi)組合導航系統(tǒng)，其特征在于， 所述載體位置算出部，其基于所述光信號接收器所接收到的所述扇面激光信號以及所述光信號發(fā)射器的位置算出所述載體的位置。
4.根據(jù)權利要求1所述的室內(nèi)組合導航系統(tǒng)，其特征在于，還包括: 圖像記憶測量系統(tǒng)，其利用對拍攝而得的圖像中的多個特征點坐標，通過雙目交會原理，反解算所述載體的位置、姿態(tài)； 所述比較器，其基于從所述圖像記憶測量系統(tǒng)輸入的所述載體的位置、姿態(tài)以及從所述捷聯(lián)慣導系統(tǒng)輸入的所述載體的位置、姿態(tài)分別計算第2位置差、姿態(tài)差； 所述卡爾曼濾波器，其基于從所述比較器輸入的第2位置差以及姿態(tài)差，算出對所述姿態(tài)的第2姿態(tài)校正量； 所述捷聯(lián)慣導系統(tǒng)，其基于所述第2姿態(tài)校正量對所述姿態(tài)進行校正，并輸出校正后的姿態(tài)。
5.根據(jù)權利要求4所述的室內(nèi)組合導航系統(tǒng)，其特征在于，還包括: 選通開關，其選通所述室內(nèi)空間測量定位系統(tǒng)或者所述圖像記憶測量系統(tǒng)中的一方，選通控制部，其通過判斷所述光信號接收器所接收的所述扇面激光信號的誤碼率，對所述選通開關進行控制，使所述選通開關選通所述室內(nèi)空間測量定位系統(tǒng)或者所述圖像記憶測量系統(tǒng)中的一方。
6.根據(jù)權利要求5所述的室內(nèi)組合導航系統(tǒng)，其特征在于， 所述選通控制部，其在所述選通控制部判斷為所述光信號接收器所接收的所述扇面激光信號的誤碼率為規(guī)定閾值以上時，控制所述選通開關使所述圖像記憶測量系統(tǒng)處于選通狀態(tài)，而使所述室內(nèi)空間測量定位系統(tǒng)處于未選通狀態(tài)。
7.根據(jù)權利要求5所述的室內(nèi)組合導航系統(tǒng)，其特征在于， 所述選通控制部，其在所述選通控制部判斷為所述光信號接收器所接收的所述扇面激光信號的誤碼率小于所述規(guī)定閾值時，控制所述選通開關使所述室內(nèi)空間測量定位系統(tǒng)處于選通狀態(tài)，而使所述圖像記憶測量系統(tǒng)處于未選通狀態(tài)。
8.根據(jù)權利要求5所述的室內(nèi)組合導航系統(tǒng)，其特征在于，還包括: 信號強度檢測部，其用于檢測由所述光信號接收器所接收的所述光信號的信號強度，并講所述信號強度輸出至所述選通控制部； 所述選通控制部，其在判斷輸入的所述信號強度大于信號強度比較用閾值時，控制所述選通開關，使所述室內(nèi)空間測量定位系統(tǒng)處于選通狀態(tài)，而使所述圖像記憶測量系統(tǒng)處于未選通狀態(tài)。
9.根據(jù)權利要求8所述的室內(nèi)組合導航系統(tǒng)，其特征在于，還包括: 照度值取得部，其取得外界的照度值； 所述選通控制部， 其在判斷輸入的所述信號強度不大于信號強度比較用閾值、且照度值大于照度值比較用閾值時，使所述圖像記憶測量系統(tǒng)處于選通狀態(tài)，而使所述室內(nèi)空間測量定位系統(tǒng)處于未選通狀態(tài)； 其在判斷輸入的所述信號強度不大于信號強度比較用閾值、照度值不大于照度值比較用閾值、且信號強度偏差率大于照度值偏差率時，使所述圖像記憶測量系統(tǒng)處于選通狀態(tài)，而使所述室內(nèi)空間測量定位系統(tǒng)處于未選通狀態(tài)； 其在判斷輸入的所述信號強度不大于信號強度比較用閾值、照度值不大于照度值比較用閾值、且信號強度偏差率不大于照度值偏差率時，使所述室內(nèi)空間測量定位系統(tǒng)處于選通狀態(tài)，而使所述圖像記憶測量系統(tǒng)處于未選通狀態(tài)。
10.根據(jù)權利要求1~9所述的室內(nèi)組合導航系統(tǒng)，其特征在于， 所述載體為機器人、車輛。
【文檔編號】G01C21/00GK103591955SQ201310607301
【公開日】2014年2月19日 申請日期:2013年11月21日 優(yōu)先權日:2013年11月21日
【發(fā)明者】林俤, 吳易明, 趙曉進, 梁選森, 姚震, 王永旺 申請人:西安中科光電精密工程有限公司