CN103591955B - 组合导航系统 - Google Patents

Abstract

本发明提供一种包括室内空间测量定位系统、图像记忆测量系统、以及捷联惯导系统的室内组合导航系统。在该室内组合导航系统中，可通过空间测量定位系统（wMPS）或图像记忆测量系统输出的高精度的位置信息对捷联惯导系统（MEMS-SINS）输出的姿态信息进行校正。本发明可广泛应用于例如机器人、车辆等移动物体的定位以及姿势输出。

Description

组合导航系统

技术领域

本发明涉及一种通过wMPS（WorkspaceMeasuringPositionSystem：室内空间测量定位系统）的测量值、或者搭载在载体（例如，机器人）上的视觉定位系统的测量值对MEMS-SINS（微机电-捷联惯导系统）的测量值进行校正的组合导航系统。

背景技术

目前，惯性导航基本上分为平台式（Gimbaled）和捷联式（Strapdown）两大类。在捷联式惯导中，惯性传感器直接固连在载体上，并跟随载体在空间一起转动，整个系统以计算机为中心的数学平台来替代平台式惯导中的常规型物理平台。上述惯性传感器分为两大类，一类为距离传感器，这是一类可用来测量加速度、速度或位移的传感器，例如加速度计、速度计、里程表、多普勒雷达和气压计等；另一类为角度传感器，这是一类可用来测量角度变化或方位的传感器，例如陀螺仪和磁罗盘等。其中，陀螺仪和加速度计通常作为惯性导航系统的惯性传感器。

另外，最近，随着通过硅加工以极小尺寸制造以往由金属等制成的机械系统的技术（MEMS）的发展，例如，在硅上制成μm单位的超小型的齿轮或凸轮、弹簧等成为可能。进而，出现了采用MEMS技术的陀螺仪、加速度计即MEMS陀螺仪、MEMS加速度计等。由于通过将MEMS陀螺仪、MEMS加速度计等惯性传感器直接固连在载体（例如，机器人等）上，能够大大减轻载体的载重量，因此MEMS这一技术得到了广泛的应用。在此，将应用MEMS陀螺仪、MEMS加速度计等惯性传感器的捷联式惯导简称为MEMS-SINS。

但是，在MEMS-SINS测量而得的测量值中包含各种误差，这些误差会通过积分运算逐渐在定位结果中得到积累，若定位结果未被及时校正、重置，则定位误差随时间逐渐积累，从而导致MEMS-SINS最终会失去使用价值。尤其是，例如，搭载有MEMS-SINS的机器人在室内从事对定位精度有极高要求的情况下，上述补正便变得尤为重要。

发明内容

本发明便是鉴于上述问题而提出的，其目的在于提供一种组合导航系统，能够利用从wMPS（WorkspaceMeasuringPositionSystem：室内空间测量定位系统）或者视觉定位系统输出的位置信息、与MEMS-SINS（微机电-捷联惯导系统）输出的位置信息对MEMS-SINS输出的姿态测量值进行校正。

根据本发明，提供一种室内组合导航系统包括室内空间测量定位系统、捷联惯导系统、比较器、卡尔曼滤波器。该室内空间测量定位系统具备：光信号接收器，安装在载体上，用于接收光信号发射器所发射的扇面激光信号；载体位置算出部，基于所述光信号接收器所接收到的所述扇面激光信号算出所述载体的位置；载体速度算出部，通过对所述载体位置算出部算出的所述位置进行拟合算出所述载体的速度。捷联惯导系统，安装在所述载体上，由微机电构成，输出该载体的位置、速度以及姿态。比较器，基于从所述载体位置算出部输入的所述载体的位置与从所述捷联惯导系统输入的所述载体的位置计算第1位置差，基于从所述载体速度算出部输入的所述载体的速度与从所述捷联惯导系统输入的所述载体的速度计算速度差。卡尔曼滤波器，基于从所述比较器输入的所述第1位置差与速度差，算出对所述姿态的第1姿态校正量。所述捷联惯导系统基于所述第1姿态校正量对所述姿态进行校正，并输出校正后的姿态。

据此，能够利用从wMPS输出的位置信息、速度信息与MEMS-SINS（微机电-捷联惯导系统）输出的位置信息、速度信息对MEMS-SINS输出的姿态测量值进行校正。

在上述室内组合导航系统中，所述光信号发射器发射两束扇面激光信号，且在室内空间中至少配置2台。

据此，能够使WMPS所输出的位置具有更高的精度，因此将该位置作为室内组合导航系统的位置进行输出，能够实现高精度的导航。

在上述室内组合导航系统中，所述载体位置算出部基于所述光信号接收器所接收到的所述扇面激光信号以及所述光信号发射器的位置算出所述载体的位置。

据此，能够使WMPS所输出的位置具有更高的精度，因此将该位置作为室内组合导航系统的位置进行输出，能够实现高精度的导航。

在上述室内组合导航系统中，还包括图像记忆测量系统，该图像记忆测量系统利用对拍摄而得的图像中的多个特征点坐标，通过双目交会原理，反解算所述载体的位置、姿态。所述比较器基于从所述图像记忆测量系统输入的所述载体的位置、姿态以及从所述捷联惯导系统输入的所述载体的位置、姿态分别计算第2位置差、姿态差。所述卡尔曼滤波器基于从所述比较器输入的第2位置差以及姿态差，算出对所述姿态的第2姿态校正量；所述捷联惯导系统基于所述第2姿态校正量对所述姿态进行校正，并输出校正后的姿态。

据此，能够不局限于利用空间测量定位系统输出的位置信息、速度信息与MEMS-SINS（微机电-捷联惯导系统）输出的位置信息、速度信息对MEMS-SINS输出的姿态测量值进行校正，也能够通过视觉定位系统输出的位置信息、姿态信息与MEMS-SINS（微机电-捷联惯导系统）输出的位置信息、姿态信息对MEMS-SINS输出的姿态测量值进行校正。

在上述室内组合导航系统中，还包括选通开关以及选通控制部，该选通开关选通所述室内空间测量定位系统或者所述图像记忆测量系统中的一方。该选通控制部通过判断所述光信号接收器所接收的所述扇面激光信号的误码率，对所述选通开关进行控制，使所述选通开关选通所述室内空间测量定位系统或者所述图像记忆测量系统中的一方。

据此，能够通过误码率实现是选通室内空间测量定位系统还是选通图像记忆测量系统。由于判断误码率是通过软体进行的，因此不会增加硬件上的成本，从而实现选通成本的降低。

在上述室内组合导航系统中，在所述选通控制部判断为所述光信号接收器所接收的所述扇面激光信号的误码率为规定阈值以上时，所述选通控制部控制所述选通开关使所述图像记忆测量系统处于选通状态，而使所述室内空间测量定位系统处于未选通状态。

据此，在通过误码率判断光信号接收器所接收的扇面激光信号的信号品质差时，能够选通输出的位置的精度相对较高的图像记忆测量系统。从而实现了连续地高精度的导航。

在上述室内组合导航系统中，在所述选通控制部判断为所述光信号接收器所接收的所述扇面激光信号的误码率小于所述规定阈值时，所述选通控制部控制所述选通开关使所述室内空间测量定位系统处于选通状态，而使所述图像记忆测量系统处于未选通状态。

据此，在通过误码率判断光信号接收器所接收的扇面激光信号的信号品质高时，能够重新选通输出的位置的精度高的室内空间测量定位系统。由此能够避免因使用图像记忆测量系统而带来的高负荷的数据运算，以及降低了对周围环境（例如，光照条件等）的依赖性。从而实现了受外部条件影响小地连续地高精度的导航。

在所述组合导航系统中，所述的组合导航系统还包括：信号强度检测部，用于检测由所述光信号接收器所接收的所述光信号的信号强度，并输出所述信号强度；选通控制部，在判断输入的所述信号强度大于信号强度比较用阈值时，使所述室内空间测量定位系统处于选通状态，而使所述图像记忆测量系统处于未选通状态。

据此，能够通过对光信号发射器所发射的扇面激光信号的信号强度进行判断，来实现室内空间测量定位系统的选通。

在所述组合导航系统中，所述的组合导航系统还包括：照度值取得部，取得外界的照度值；所述选通控制部，在判断输入的所述信号强度不大于信号强度比较用阈值、且照度值大于照度值比较用阈值时，使所述图像记忆测量系统处于选通状态，而使所述室内空间测量定位系统处于未选通状态；在判断输入的所述信号强度不大于信号强度比较用阈值、照度值不大于照度值比较用阈值、且信号强度偏差率大于照度值偏差率时，使所述图像记忆测量系统处于选通状态，而使所述室内空间测量定位系统处于未选通状态；在判断输入的所述信号强度不大于信号强度比较用阈值、照度值不大于照度值比较用阈值、且信号强度偏差率不大于照度值偏差率时，使所述室内空间测量定位系统处于选通状态，而使所述图像记忆测量系统处于未选通状态。

据此，能够从wMPS或图像记忆测量系统中选通输出的位置的精度较高一方。这样一来，即使在wMPS与图像记忆测量系统的输出的位置的精度都差的情况下，也能够通过选通二者之中输出的位置的精度相对较高的一方，从而最大限度地实现高精度的姿态校正。

附图说明

图1是表示涉及本发明的第1实施方式的框图。

图2是表示涉及本发明的第1实施方式的流程图。

图3是表示涉及本发明的第2实施方式的框图。

图4是表示涉及本发明的第2实施方式的流程图。

图5是表示涉及本发明的第3实施方式的框图。

图6是表示涉及本发明的第3实施方式的流程图。

图7是表示涉及本发明的第2实施方式的另一流程图。

图8是表示涉及本发明的第2实施方式的另一流程图。图中，

1:MEMS-SINS；2、2X:wMPS；3:比较器；4:卡尔曼滤波器；5、5X：图像记忆测量系统；6：选通控制部；7：选通开关。

具体实施方式

第1实施方式

下面，对照图1以及图2具体说明本发明的第1实施方式。

首先，对照图1说明本实施方式的构成。图1是表示涉及本发明的第1实施方式的框图。

在图1中，室内组合导航系统S由MEMS-SINS1、wMPS2、比较器3、卡尔曼滤波器4等构成。

其中，MEMS-SINS1具备MEMS陀螺仪（未图示）、MEMS加速度计等（未图示），安装于需要定位导航的载体（例如，室内机器人）上。MEMS-SINS1通过对MEMS陀螺仪、MEMS加速度计取得的载体的运动信息（角速率、加速度等）进行解算，得到该载体的位置（三维坐标）、速度（三维速度）、姿态等信息并向后述的比较器3输出。

wMPS2具备光信号接收器21、载体位置模块22以及载体速度算出模块23等。

光信号接收器21安装在上述载体上，用于接收由2台以上的光信号发射器（未图示）按照一定周期发射出的扇面激光信号。在此，每台光信号发射器边旋转边发射两束扇束（扇面激光信号）。

载体位置模块（载体位置算出部）22利用光信号接收器21所接收的扇面激光信号来确定相对于光信号发射器的位置。在此，由于扇面以预定角度彼此交叉，因此用于接收扇束的光信号接收器21的三维坐标值（即，光信号接收器21的位置或者高度）能够被载体位置模块22解算出。在通常的wMPS中，载体位置模块22可按照数十Hz的频率输出位置。

载体速度算出模块23通过对上述位置信息的拟合，能够得到近似的速度信息，向后述的比较器3输出。

在本实施方式中，设定光信号接收器21的位置与载体的位置相一致，即光信号接收器21的位置即为载体的位置。另外，在本发明的其他实施方式中也采用这样的设定。不言自明的是，若光信号接收器21的位置载体的位置不一致，则需要进行坐标系变换求解载体的位置。

另外，多台光信号发射器被分别事先配置在室内的多个固定位置上。每台光信号发射器所发射出的扇面激光信号中包含有能够识别该光信号发射器的识别信息（例如，脉冲列），并且光信号发射器彼此的识别信息（脉冲列）均不相同。载体位置模块（载体位置算出部）22在解算光信号接收器21时，可以根据需要，利用该识别信息。

比较器3对从MEMS-SINS1输入的位置、速度以及从wMPS2输入的位置、速度分别求出位置差与速度差，并将该位置差与速度差输入后述的卡尔曼滤波器4。

卡尔曼滤波器4利用上述差值即位置差与速度差，算出姿态校正量，并将该姿态校正量输出至MEMS-SINS1。MEMS-SINS1利用该姿态校正量对由其解算得到的姿态进行校正，并将校正后的姿态作为室内组合导航系统S的姿态输出。

下面，对照图2说明本实施方式的动作。图2为表示本发明所述方式的动作的流程图。

S201：首先，wMPS2算出载体当前的位置、并拟合出速度输入比较器3。

S202：比较器3求出从MEMS-SINS1输入的位置与wMPS2输入的位置之差（与三维坐标值对应的差，以下称为“位置差”），同时求出从MEMS-SINS1输入的速度与wMPS2输入的速度之差（与三维速度值对应的差，以下称为“速度差”），并输入卡尔曼滤波器4。

S203：卡尔曼滤波器4局域输入的位置差以及速度差，算出姿态的校正量并输入至MEMS-SINS1。

S204：MEMS-SINS1利用输入的姿态的校正量对由其解算得到的载体的姿态进行校正，并将校正后的姿态向外部输出。

第2实施方式

下面，对照图3、图4说明本发明的第2实施方式。

首先，对照图3说明本实施方式的构成。图3为表示涉及本发明的第2实施方式的框图。在图3中，对与图1中相同的构成要素赋予相同的符号，并省略其说明。

图3（室内组合导航系统S1）与图1（室内组合导航系统S）的不同之处在于，在图3中还包括wMPS2X、图像记忆测量系统5、选通控制部6、以及选通开关7。

wMPS2X与wMPS2的不同之处在于其增加了信号强度检测器（未图示），该信号强度检测器用于检测来自信号发射器的红外激光信号的信号强度，并将检测出的信号强度输入至后述的选通控制部6。在此，要指出的是，该信号强度检测器的检测对象是wMPS2X用于计算载体的位置的扇面激光信号的信号强度。

图像记忆测量系统5与MEMS-SINS1搭载于同一载体上，包括：可对载体四周环境全方位进行拍摄的CCD照相机（双目摄像机）；对CCD照相机拍摄的图像进行数字处理的DSP，该DSP还能够从图像中选择特征点，根据wMPS2得出的载体的坐标以及相机坐标，通过双目视觉交会原理能够求出特征点在世界坐标系中的坐标，并存储图像、特征点以及坐标信息的存储部。另外，DSP能够将CCD照相机现在拍照而得的图像与所存储的图像、特征点进行匹配，从而从CCD照相机现在拍照而得的图像中提取之前存储的特征点，并根据存储的该特征点的坐标，反解算出载体在坐标系中的坐标（位置）、姿态。

选通控制部6具备存储比较用规定阈值的存储部，选通控制部6基于来自信号强度检测器的信号强度，控制后述的选通开关7的选通对象。具体而言，选通控制部6将来自信号强度检测器的信号强度与比较用规定阈值进行比较，当比较结果是来自信号强度检测器的信号强度大于比较用规定阈值时，选通控制部6控制选通开关7只选通wMPS2X，当比较结果是来自信号强度检测器的信号强度不大于比较用规定阈值时，选通控制部6控制选通开关7只选通图像记忆测量系统5。

选通开关7基于选通控制部6的控制从wMPS2X以及图像记忆测量系统5中选通其一，并使来自选通的选通对象（wMPS2X或图像记忆测量系统5）的信号输入比较器3。

下面，对照图4说明本发明的第2实施方式的动作。图4是表示涉及本发明的第2实施方式的动作的流程图。

S401：首先，CCD照相机对载体四周环境全方位进行拍摄并将拍摄而得的映像数据存储于图像记忆测量系统5的存储部中。

S402：DSP从存储于图像记忆测量系统5的存储部中读出映像数据，并从映像数据中提取图像的特征点并将该特征点在世界坐标系中的空间位置坐标也一同存储至该存储部。

S403：选通控制部6判断来自信号强度检测器的信号强度是否大于比较用规定阈值。

在S403的判断结果为来自信号强度检测器的信号强度连续规定时间大于比较用规定阈值时（S403：是），选通控制部6向选通开关7发送选通wMPS2X的指令，选通开关7基于该指令选通wMPS2X（S404）。然后，wMPS2X解算出载体当前的位置（三维坐标值）并拟合出速度输入至比较器3（S405）。接着，进入S202～S204的处理，有关S202～S204的处理参考前述。

在S403的判断结果为来自信号强度检测器的信号强度连续规定时间不大于比较用规定阈值时（S403：否），选通控制部6向选通开关7发送选通图像记忆测量系统5的指令，选通开关7根据该指令选通图像记忆测量系统5（S406）。

S407：DSP能够将CCD照相机现在拍照而得的图像与所存储的图像、特征点进行匹配。

S408：DSP通过上述图像匹配从CCD照相机现在拍照而得的图像中提取之前存储的特征点，并根据存储的该特征点的坐标，反解算出载体在坐标系中的坐标（位置）和姿态。进而言之，利用至少3个不共线的特征点，通过双目交会原理反解算姿态和位置。

S409：在S408反解算出的载体的姿态和位置输入比较器3，与由MEMS-SINS1输入的姿态、位置进行比较，求对应的姿态差、位置差，并将这些差值输入至卡尔曼滤波器4。

S410：卡尔曼滤波器4利用输入的差值（位置差、姿态差）生成姿态校正量，并向MEMS-SINS1输出。MEMS-SINS1利用由卡尔曼滤波器4输入的姿态校正量对其解算而得的姿态进行校正，并输出校正后的姿态（S204）。

第3实施方式

下面，对照图5、图6说明本发明的第3实施方式。

首先，说明本实施方式的构成。图5表示涉及本发明的第3实施方式的框图。在图5中，对与第2实施方式的图3中相同的构成要素赋予相同的符号，并省略其说明。

图5（室内组合导航系统S2）与图3（室内组合导航系统S1）的不同之处在于，在图5中还包括图像记忆测量系统5X。

图像记忆测量系统5X与图像记忆测量系统5的不同之处在于其增加了照度值取得部（未图示），该照度值取得部用于取得CCD照相机拍摄时的照度值，并将照度值输入至后述的选通控制部6。

选通控制部6基于来自信号强度检测器的信号强度、来自照度值取得部的照度值，以及存储于选通控制部6的存储部的比较用规定阈值以及照度值比较用阈值，对wMPS2X以及图像记忆测量系统5X进行选通。

下面，对照图6说明本发明的第3实施方式的动作。图6表示涉及本发明的第3实施方式的流程图。

S601：信号强度检测器检测信号强度并输入选通控制部6，与此同时，照度值取得部取得环境的照度值并输入选通控制部6。

S602：选通控制部6将信号强度、照度值分别与比较用规定阈值以及照度值比较用阈值。

S603：选通控制部6判断输入的信号强度是否大于比较用规定阈值。

在选通控制部6判断信号强度大于比较用规定阈值的情况下（S603：是），则不管照度值与照度值比较用阈值的比较结果，选通控制部6选通wMPS2X（S404）。然后依次执行S405、S202、S203、S204，有关S405、S202、S203、S204参考前述。

在选通控制部6判断信号强度不大于比较用规定阈值时（S603：否），选通控制部6判断输入的照度值是否大于照度值比较用阈值（S604）。在选通控制部6判断照度值大于照度值比较用阈值的情况下（S604：是），选通控制部6向选通开关7发送选通图像记忆测量系统5X的指令，从而选通图像记忆测量系统5X（S606）。然后依次执行S407～S410，接着执行S204。有关S407～S410以及S204请参考前述。

在选通控制部6判断照度值不大于照度值比较用阈值的情况下（S604：否），选通控制部6接着判断信号强度偏差率是否大于照度值偏差率（S605）。其中，信号强度偏差率=（信号强度-比较用规定阈值）/比较用规定阈值＊100%，照度值偏差率=（照度值-照度值比较用阈值）/照度值比较用阈值＊100%。在选通控制部6判断信号强度偏差率大于照度值偏差率（S605：是）的情况下，进入S606，接着依次执行S407～S410继而执行S204；。在选通控制部6判断信号强度偏差率不大于照度值偏差率的情况下（S605：否），进入步骤S404继而顺次执行S405、S202～S204。

《补足1》

在上述第2实施方式中，选通控制部6利用来自信号强度检测器的信号强度信息，选通图像记忆测量系统5。但不限于此，在wMPS中也可不设置该信号强度检测器，取而代之，设置可检测光信号接收器所接收到的扇面激光信号的误码率选通图像记忆测量系统5亦可。具体而言，在选通控制部6的存储器中存储有误码率用规定阈值，在选通控制部6判断为光信号接收器所接收的扇面激光信号的误码率为误码率用规定阈值以上时，选通控制部6控制选通开关7使图像记忆测量系统5处于选通状态，而使室内空间测量定位系统2X处于未选通状态，即选通控制部6向选通开关7发送选通图像记忆测量系统5的指令，选通开关7执行该指令。另一方面，在选通控制部6判断为光信号接收器所接收的扇面激光信号的误码率小于误码率用规定阈值时，选通控制部6控制选通开关7使室内空间测量定位系统2X处于选通状态，而使图像记忆测量系统5X处于未选通状态。

《补足2》

在上述第2实施方式以及第3实施方式中，选通控制部与选通开关分别独立地设置，但对选通控制部与选通开关实施模块化亦可，即通过一模块实现选通控制部与选通开关的功能亦可。再有，选通控制部与选通开关集成至wMPS、图像记忆测量系统、或者比较器之一中亦可。据此，零部件的表面安装变得简单。

《补足3》

图7是表示涉及本发明的第2实施方式的另一流程图。

其中，符号7A中的处理与上述各实施方式中的MEMS-SINS的处理的一部分相当。

S701：陀螺仪、加计（加速度计）采集IMU数据（惯性导航数据，例如，角速度、加速度）。

S702：由于在S701中所采集到的IMU数据中存在噪声，因此对在S701中采集到的数据进行低通滤波处理，从而去除上述噪声。

S703：对在S702中得到的去除噪声后的IMU数据进行误差补偿，在此，该误差补偿主要是对陀螺仪、加计进行零偏、温度补偿和刻度系数误差补偿。

S704：将经S702以及S703处理后的IMU数据发送至导航解算CPU。

另外，与符号7A中的处理同时，执行符号7B中的处理。符号7B中的处理与上述wMPS中的处理的一部分相当。

S705：采集测站脉冲，这里的“站”是指基站，即上述各实施方式中的光信号发射器。

S706：脉冲匹配，即通过脉冲中所包含的信息（脉冲列）认识发射该脉冲的基站的位置。

S707：对所接收的脉冲进行交会解算，从而得到载体的位置。

S708：将wMPS数据（S707中得到的位置）发送至导航解算CPU。

另外，符号7C与上述第2实施方式中的选通控制部、wMPS、图像记忆测量系统、选通开关的各功能的一部分相当。要指出的是，在符号7C的处理中，作为判断所接收的扇面激光信号的品质的标准，选用了《补足1》中所言及的方法，即使用误码率作为判断的基准，而非如第2或3实施方式中的利用信号强度检测器所检测出的扇面激光信号的信号强度。为了便于说明，在本补足中，将执行符号7C的处理的硬件以及软件统和后，称为“导航解算CPU”。

S709：首先，导航解算CPU进行参数初始化。

S710：导航解算CPU基于来自7A的IMU数据（惯性敏感器数据）与从S711载入的初始导航参数来进行粗对准。据此，能够对MEMS-SINS的初始姿态进行初步估计。

S712：导航解算CPU进行精对准。

S713：导航解算CPU检测来自7B的wMPS数据（wMPS系统解算数据）的误码率，从而判断wMPS数据是否有效。

S714：在S713的判断结果为wMPS数据无效（S713：否）时，导航解算CPU进行特征点记忆/惯性组合导航解算。

S715：在S713的判断结果为wMPS数据有效（S713：是）时，导航解算CPU进行组合导航解算。

S716：导航解算CPU将在S515以及S714中解算而得的导航参数进行输出。并上传导航数据（S717）

在此，该导航解算CPU可视为集成在MEMS_SINS中。

《补足4》

图8是表示涉及本发明的第2实施方式的另一流程图。另外，图8也是对图7中的S714的详述。其动作的执行均由图像记忆测量系统中的图像特征标记CPU来完成。

S801：两台的CCD照相机进行双目拍摄。

S802：从拍摄而得的图像中，提取并识别特征点。

S803：图像特征标记CPU检测来自7B的wMPS数据（wMPS系统解算数据）的误码率，从而判断wMPS数据是否有效。

S804：在S803的判断结果为wMPS数据有效（S803：是）时，图像特征标记CPU进行特征点记忆标记与定位。

S805：存储在S804中处理而得的特征点信息（位置信息等）。

S806：在S803的判断结果为wMPS数据无效（S803：否）时，图像特征标记CPU搜索并跟踪存储特征区域。

S807：图像特征标记CPU对当前拍摄而得的图像与事先存储的图像（特征点）进行匹配。

S808：图像特征标记CPU提取特征。

S809：图像特征标记CPU反解算载体空间坐标。

S810：图像特征标记CPU输出载体坐标值。

在此，要指明的是，在S810输出的载体坐标值具有极高的精度，除了可用于对MEMS-SINS所输出的姿态的校正之外，也可直接作为室内组合导航系统的输出值输出。

《补足5》

在上述各实施方式以及各补足中，在判断wMPS信号是否有效时，使用了信号强度以及误码率，不限于此，也可考虑错误发生次数。具体而言，例如，判断信号强度低于阈值且持续规定期间时，错误发生次数计数为1，随后，判断误码率高于阈值时，错误发生次数累计计数为2，如此累计，例如，在规定时间内，该错误发生次数计数达到规定阈值时，则判断wMPS信号无效。这样一来，考虑信号强度的同时也考虑误码率，从而能够更高精度地判断wMPS信号是否有效。

另外，本发明可应用于例如机器人、车辆等移动物体的定位以及姿势输出。

Claims (10)

1.一种室内组合导航系统，其特征在于，包括室内空间测量定位系统、捷联惯导系统、比较器、卡尔曼滤波器，其中，

所述室内空间测量定位系统具备：

光信号接收器，其安装在载体上，用于接收光信号发射器所发射的扇面激光信号；

载体位置算出部，其基于所述光信号接收器所接收到的所述扇面激光信号算出所述载体的位置；

载体速度算出部，其通过对所述载体位置算出部算出的所述位置进行拟合算出所述载体的速度；

所述捷联惯导系统，其安装在所述载体上，由微机电构成，输出该载体的位置、速度以及姿态；

所述比较器，其基于从所述载体位置算出部输入的所述载体的位置与从所述捷联惯导系统输入的所述载体的位置计算第1位置差，基于从所述载体速度算出部输入的所述载体的速度与从所述捷联惯导系统输入的所述载体的速度计算速度差；

所述卡尔曼滤波器，其基于从所述比较器输入的所述第1位置差与速度差，算出对所述姿态的第1姿态校正量；

所述捷联惯导系统，其基于所述第1姿态校正量对所述姿态进行校正，并输出校正后的姿态。

2.根据权利要求1所述的室内组合导航系统，其特征在于，

所述光信号发射器，其在室内空间中至少配置2台，每台发射两束扇面激光信号。

3.根据权利要求1所述的室内组合导航系统，其特征在于，

所述载体位置算出部，其基于所述光信号接收器所接收到的所述扇面激光信号以及所述光信号发射器的位置算出所述载体的位置。

4.根据权利要求1所述的室内组合导航系统，其特征在于，还包括：

图像记忆测量系统，其利用对拍摄而得的图像中的多个特征点坐标，通过双目交会原理，反解算所述载体的位置、姿态；

所述比较器，其基于从所述图像记忆测量系统输入的所述载体的位置、姿态以及从所述捷联惯导系统输入的所述载体的位置、姿态分别计算第2位置差、姿态差；

所述卡尔曼滤波器，其基于从所述比较器输入的第2位置差以及姿态差，算出对所述姿态的第2姿态校正量；

所述捷联惯导系统，其基于所述第2姿态校正量对所述姿态进行校正，并输出校正后的姿态。

5.根据权利要求4所述的室内组合导航系统，其特征在于，还包括：

选通开关，其选通所述室内空间测量定位系统或者所述图像记忆测量系统中的一方，

选通控制部，其通过判断所述光信号接收器所接收的所述扇面激光信号的误码率，对所述选通开关进行控制，使所述选通开关选通所述室内空间测量定位系统或者所述图像记忆测量系统中的一方。

6.根据权利要求5所述的室内组合导航系统，其特征在于，

所述选通控制部，其在所述选通控制部判断为所述光信号接收器所接收的所述扇面激光信号的误码率为规定阈值以上时，控制所述选通开关使所述图像记忆测量系统处于选通状态，而使所述室内空间测量定位系统处于未选通状态。

7.根据权利要求5所述的室内组合导航系统，其特征在于，

所述选通控制部，其在所述选通控制部判断为所述光信号接收器所接收的所述扇面激光信号的误码率小于规定阈值时，控制所述选通开关使所述室内空间测量定位系统处于选通状态，而使所述图像记忆测量系统处于未选通状态。

8.根据权利要求5所述的室内组合导航系统，其特征在于，还包括：

信号强度检测部，其用于检测由所述光信号接收器所接收的所述光信号的信号强度，并讲所述信号强度输出至所述选通控制部；

所述选通控制部，其在判断输入的所述信号强度大于信号强度比较用阈值时，控制所述选通开关，使所述室内空间测量定位系统处于选通状态，而使所述图像记忆测量系统处于未选通状态。

9.根据权利要求8所述的室内组合导航系统，其特征在于，还包括：

照度值取得部，其取得外界的照度值；

所述选通控制部，

其在判断输入的所述信号强度不大于信号强度比较用阈值、且照度值大于照度值比较用阈值时，使所述图像记忆测量系统处于选通状态，而使所述室内空间测量定位系统处于未选通状态；

其在判断输入的所述信号强度不大于信号强度比较用阈值、照度值不大于照度值比较用阈值、且信号强度偏差率大于照度值偏差率时，使所述图像记忆测量系统处于选通状态，而使所述室内空间测量定位系统处于未选通状态；

其在判断输入的所述信号强度不大于信号强度比较用阈值、照度值不大于照度值比较用阈值、且信号强度偏差率不大于照度值偏差率时，使所述室内空间测量定位系统处于选通状态，而使所述图像记忆测量系统处于未选通状态。

10.根据权利要求1～9任一所述的室内组合导航系统，其特征在于，

所述载体为机器人、车辆。