CN104864865B - 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 - Google Patents
一种面向室内行人导航的ahrs/uwb无缝组合导航方法 Download PDFInfo
- Publication number
- CN104864865B CN104864865B CN201510292083.0A CN201510292083A CN104864865B CN 104864865 B CN104864865 B CN 104864865B CN 201510292083 A CN201510292083 A CN 201510292083A CN 104864865 B CN104864865 B CN 104864865B
- Authority
- CN
- China
- Prior art keywords
- msup
- mtr
- mtd
- coordinate system
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种面向室内行人导航的AHRS/UWB无缝组合导航方法，包括：将整个导航方法分为培训和预估两个阶段，在UWB信号正常的情况下，将AHRS采集到的位置和速度信息与UWB采集到的位置和速度信息的差值作为主数据融合滤波器的观测量，对当前时刻最优的位置、速度和姿态误差进行预估；一旦UWB信号出现失锁，AHRS中输出的位置、速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对主数据融合滤波器失锁的观测向量进行预估，保证主数据融合滤波器的正常工作，进而保证整个导航系统的无缝导航。
Description
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及一种面向室内行人导航的AHRS/UWB无缝组合导航方法。
背景技术
近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
在现有的定位方式中，全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。
近年来，WSN以其低成本、低功耗和低系统复杂度的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于WSN的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是如在如下不足：
1、由于室内环境复杂多变，WSN信号十分容易受到干扰而导致定位精度下降甚至失锁；
2、目前的导航定位精度为米级，不能保证对室内行人高精度的导航需求；
3、由于WSN采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。
因此现阶段基于WSN的目标跟踪在室内导航领域仍旧面临很多挑战。
AHRS具有全自主、运动信息全面、短时、高精度的优点，适用于小区域目标跟踪领域；虽然其可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航精度的严重下降。
对于现有的组合式导航技术，大多完全依靠人工智能算法直接替代信号失锁时不能工作的数据融合滤波器对位置、速度和姿态等导航信息的解算误差进行预估，并没有充分有效的利用前一时刻的测量值，产生野值的概率非常高。
发明内容
本发明的目的就是解决上述问题，提供一种面向室内行人导航的AHRS/UWB无缝组合导航方法，该方法克服了单一导航技术存在的不同程度的缺陷，保证整个导航系统的无缝导航。
为实现上述目的，本发明采用下述技术方案，包括：
一种面向室内行人导航的AHRS/UWB无缝组合导航方法，包括以下步骤：
(1)分别将AHRS与UWB无线发送模块安装在行人的鞋上，将UWB无线发送模块固定在设定位置；
(2)通过AHRS对鞋子的运行状态进行识别，在鞋子处于静止状态时，通过局部数据融合滤波器对AHRS解算的导航信息误差进行预估和补偿，得到当前时刻鞋子的位置、速度和姿态的导航信息的最优估计；
(3)根据UWB信号是否正常，将整个导航算法分为培训和预估两个阶段；
(4)培训阶段，UWB信号正常，通过对AHRS与UWB两种模块之间的距离和距离变化率的测量，完成对行人鞋子位置和速度的预估；
(5)将步骤(4)中采集到的位置和速度信息与步骤(2)中采集到的位置和速度信息做差，所述差值作为主数据融合滤波器的观测量，通过主数据融合滤波器对当前时刻最优的位置、速度和姿态误差进行预估，并对AHRS采集到的数据进行误差补偿，最终得到当前时刻最优的导航信息预估；
(6)将AHRS采集到的位置、速度和姿态信息与主数据融合滤波器输出的最优误差预估分别作为人工智能算法的培训输入和目标输入，通过人工智能算法对两者之间的映射关系进行构建；
(7)一旦UWB信号出现失锁，整个导航算法进入预估阶段；在这一阶段，将步骤(2)中输出的当前时刻鞋子的导航信息作为预估输入传送到人工智能算法中，利用步骤(6)中构建的映射关系，对UWB采集到的位置和速度信息进行预估；
(8)返回步骤(5)，将对UWB采集到的位置和速度信息的预估值与步骤(2)中采集到的位置和速度信息做差，所述差值作为主数据融合滤波器的观测量，保证主数据融合滤波器的正常工作，进而保证整个导航系统的无缝导航。
所述步骤(2)中通过AHRS对鞋子的运行状态进行识别的具体方法为：
根据AHRS的3个加速度传感器采集到的数据，得到当前时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。
通过加速度模值的大小判断鞋子的运动状态的具体方法为：
其中，加速度模值：ax、ay、az分别为在载体坐标系下的x、y和z三个方向的加速度值。
所述步骤(2)中当前时刻鞋子处于静止状态时，所采用的局部滤波器的状态方程具体为：
其中，φE、φN、φU分别为导航坐标系下的东向、北向和天向的角度误差；
δVn为IMU测量得到的导航坐标系下东北天三个方向的速度误差；φ为IMU测量得到的导航坐标系下东北天三个方向的姿态误差；δPn为IMU测量得到的导航坐标系下东北天三个方向的位置误差；▽n为导航坐标系下东北天三个方向的加速度误差；εn为导航坐标系下东北天三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；为导航坐标系下东北天三个方向的速度误差；为导航坐标系下东北天三个方向的姿态误差；为导航坐标系下东北天三个方向的位置误差；φ为导航坐标系下东北天三个方向的姿态角误差；为导航坐标系下东北天三个方向的加速度误差；为导航坐标系下东北天三个方向的陀螺仪误差。
所述步骤(2)中当前时刻鞋子处于静止状态时，局部滤波器的观测方程具体为：
其中，为当前时刻测量到的导航坐标系下IMU解算的东北天三个方向速度误差；为当前时刻测量到的导航坐标系下IMU解算东北天三个方向的姿态误差；δVn为IMU测量得到的导航坐标系下东北天三个方向的速度误差的理论值；φ为IMU测量得到的导航坐标系下的东北天三个方向的姿态误差的理论值；vloc为滤波器的观测噪声矩阵。
IMU测量得到的速度为IMU解算的速度误差；IMU测量得到的姿态角为IMU解算的姿态误差。
所述步骤(5)中主数据融合滤波器的状态方程具体为：
其中，φE、φN、φU分别为导航坐标系下的东向、北向和天向的角度误差；
δVn为低成本IMU测量得到的导航坐标系下的东北天三个方向的速度误差；φ为低成本IMU测量得到的导航坐标系下的东北天三个方向的姿态误差；δPn为低成本IMU测量得到的导航坐标系下的东北天三个方向的位置误差；▽n为导航坐标系下的东北天三个方向的加速度误差；εn为导航坐标系下的东北天三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；为导航坐标系下东北天三个方向的速度误差；为导航坐标系下东北天三个方向的姿态误差；为导航坐标系下东北天三个方向的位置误差；φ为导航坐标系下东北天三个方向的姿态角误差；为导航坐标系下东北天三个方向的加速度误差；为导航坐标系下东北天三个方向的陀螺仪误差。
所述步骤(5)中主数据融合滤波器的观测方程具体为：
其中，为当前时刻测量到的低成本IMU解算的速度误差；为当前时刻测量到的低成本IMU解算的姿态误差；δPn为IMU测量得到的导航坐标系下的东北天三个方向的位置误差的理论值；φ为IMU测量得到的导航坐标系下的x、y和z轴三个方向的姿态误差；v为滤波器的观测噪声矩阵。
IMU测量得到的速度为IMU解算的速度误差；IMU测量得到的姿态角为IMU解算的姿态误差。
本发明有益效果：
1、本发明方法适用于室内环境下的智能鞋子的中高精度无缝定位和定向；在AHRS中采用了局部滤波器，有效的提高了传感器所采集数据的精度，有助于后续主数据融合滤波器进行数据融合过程中精度的提升；
2、在UWB信号正常的情况下，将AHRS采集到的位置和速度信息与UWB采集到的位置和速度信息的差值作为主数据融合滤波器的观测量；一旦UWB信号出现失锁，对UWB采集到的位置和速度信息进行预估，进而得到主数据融合滤波器失锁的观测向量，保证主数据融合滤波器的正常工作，进而保证整个导航系统的无缝导航；
3、本发明方法充分利用前一时刻的测量值，相比于传统的组合式导航技术，不完全依靠人工智能算法对导航信息的预估，导航精度更高，能够实现整个导航系统的无缝导航。
附图说明
图1为一种面向室内行人导航的AHRS/UWB无缝组合导航系统示意图；
图2为UWB信号正常情况下面向室内行人导航的AHRS/UWB组合导航方法的示意图；
图3为UWB信号失锁情况下面向室内行人导航的AHRS/UWB组合导航方法的示意图。
具体实施方式
下面结合附图与实施例对本发明做进一步说明：
如图1所示，用于一种面向室内行人导航的AHRS/UWB无缝组合导航系统，包括UWB无线发送模块和导航鞋两个部分。其中导航鞋包括AHRS模块、UWB无线接收模块和数据处理模块。AHRS模块和UWB无线接收模块分别与数据处理模块连接。
如图2、3所示，在一种面向室内行人导航的AHRS/UWB无缝组合导航方法使用局部滤波器、主数据融合滤波器和人工智能算法共同完成系统的数据融合，具体步骤如下：
1、一种面向室内行人导航的AHRS/UWB无缝组合导航方法，属于复杂环境下组合定位技术领域，其特征在于包括下列步骤：
(1)将低成本AHRS与UWB无线发送模块安装在行人的鞋上，将UWB无线发送模块固定在已知位置；
(2)鞋上的AHRS通过将低成本IMU的3个加速度传感器所采集到的数据通过下式进行计算，得到当前鞋子的加速度模值：
通过加速度模值完成对鞋子运动状态的判断，即：
在完成对鞋子运动状态的判断之后，当鞋子处于静止状态时，所采用的局部数据融合滤波器的状态方程具体为：
其中，δVn为低成本IMU测量得到的导航坐标系下的东北天三个方向的速度误差；φ为低成本IMU测量得到的导航坐标系下的东北天三个方向的姿态误差；δPn为低成本IMU测量得到的导航坐标系下的东北天三个方向的位置误差；▽n为导航坐标系下的东北天三个方向的加速度误差；εn为导航坐标系下的东北天三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；
局部滤波器的观测方程具体为：
其中，为当前时刻测量到的低成本IMU解算的速度误差，由于本局部滤波器工作时，鞋子处于静止状态，因此当前时刻的理论值为零，故低成本IMU测量得到的速度均为IMU的解算误差；为当前时刻测量到的低成本IMU解算的姿态误差，由于本局部滤波器工作时，鞋子处于静止状态，因此当前时刻的理论值为零，故低成本IMU测量得到的姿态角均为IMU的解算误差；vloc为滤波器的观测噪声矩阵；
(3)根据UWB信号是否正常，将整个导航算法分为培训和预估两个阶段；
(4)培训阶段，UWB信号正常，通过UWB无线接收/发送模块完成对两种模块之间的距离和距离变化率的测量，继而完成对行人鞋子位置和速度的预估；
(5)将(4)中采集到的位置和速度信息与(2)中采集导航未知和速度信息做差，差值作为主数据融合滤波器的观测量，通过数据融合滤波器对当前时刻最优的位置、速度和姿态误差进行预估，并对AHRS采集到的数据进行误差补偿，最终得到当前时刻最优的导航信息预估；
主数据融合滤波器的状态方程具体为：
其中，δVn为低成本IMU测量得到的导航坐标系下的东北天三个方向的速度误差；φ为低成本IMU测量得到的导航坐标系下东北天三个方向的姿态误差；δPn为低成本IMU测量得到的导航坐标系下的东北天三个方向的位置误差；▽n为导航坐标系下的东北天三个方向的加速度误差；εn为导航坐标系下的东北天三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；
局部滤波器的观测方程具体为：
其中，为当前时刻测量到的低成本IMU解算的速度误差，由于本局部滤波器工作时，鞋子处于静止状态，因此当前时刻的理论值为零，故低成本IMU测量得到的速度均为IMU的解算误差；为当前时刻测量到的低成本IMU解算的姿态误差，由于本局部滤波器工作时，鞋子处于静止状态，因此当前时刻的理论值为零，故低成本IMU测量得到的姿态角均为IMU的解算误差；v为滤波器的观测噪声矩阵；
(6)将AHRS采集到的位置、速度和姿态信息与数据融合滤波器输出的最优误差预估分别作为人工智能算法的培训输入和目标输入，通过人工智能算法对两者之间的映射关系进行构建；
(7)一旦UWB信号出现失锁，整个导航算法进入预估阶段；在这一阶段，AHRS中输出的位置、速度和姿态信息作为预估输入传送到人工智能算法中，通过(6)中构建的映射关系，对主数据融合滤波器失锁的观测向量进行预估，保证主数据融合滤波器的正常工作，进而保证整个导航系统的无缝导航。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
Claims (9)
1.一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，包括以下步骤：
(1)分别将AHRS与UWB无线发送模块安装在行人的鞋上，将UWB无线发送模块固定在设定位置；
(2)通过AHRS对鞋子的运行状态进行识别，在鞋子处于静止状态时，通过局部数据融合滤波器对AHRS解算的导航信息误差进行预估和补偿，得到当前时刻鞋子的位置、速度和姿态的导航信息的最优估计；
(3)根据UWB信号是否正常，将整个导航算法分为培训和预估两个阶段；
(4)培训阶段，UWB信号正常，通过对AHRS与UWB两种模块之间的距离和距离变化率的测量，完成对行人鞋子位置和速度的预估；
(5)将步骤(4)中采集到的位置和速度信息与步骤(2)中采集到的位置和速度信息做差，所述差值作为主数据融合滤波器的观测量，通过主数据融合滤波器对当前时刻最优的位置、速度和姿态误差进行预估，并对AHRS采集到的数据进行误差补偿，最终得到当前时刻最优的导航信息预估；
(6)将AHRS采集到的位置、速度和姿态信息与主数据融合滤波器输出的最优误差预估分别作为人工智能算法的培训输入和目标输入，通过人工智能算法对两者之间的映射关系进行构建；
(7)一旦UWB信号出现失锁，整个导航算法进入预估阶段；在这一阶段，将步骤(2)中输出的当前时刻鞋子的导航信息作为预估输入传送到人工智能算法中，利用步骤(6)中构建的映射关系，对UWB采集到的位置和速度信息进行预估；
(8)返回步骤(5)，将对UWB采集到的位置和速度信息的预估值与步骤(2)中采集到的位置和速度信息做差，所述差值作为主数据融合滤波器的观测量，保证主数据融合滤波器的正常工作，进而保证整个导航系统的无缝导航。
2.如权利要求1所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，所述步骤(2)中通过AHRS对鞋子的运行状态进行识别的具体方法为：
根据AHRS的3个加速度传感器采集到的数据，得到当前时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。
3.如权利要求2所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，通过加速度模值的大小判断鞋子的运动状态的具体方法为：
其中，加速度模值：ax、ay、az分别为在载体坐标系下的x、y和z三个方向的加速度值。
4.如权利要求1所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，所述步骤(2)中当前时刻鞋子处于静止状态时，所采用的局部滤波器的状态方程具体为：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<msup>
<mover>
<mi>V</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mo>-</mo>
<msup>
<mi>&Theta;</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<msup>
<mi>n</mi>
<mo>&prime;</mo>
</msup>
</msubsup>
<msup>
<mover>
<mi>f</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
</msup>
<mo>+</mo>
<msup>
<mo>&dtri;</mo>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&phi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>-</mo>
<msup>
<mi>&epsiv;</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<msup>
<mi>&delta;V</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mover>
<mo>&dtri;</mo>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
其中，φE、φN、φU分别为导航坐标系下的东向、北向和天向的角度误差；
δVn为低成本IMU测量得到的导航坐标系下东北天三个方向的速度误差；φ为低成本IMU测量得到的导航坐标系下东北天三个方向的姿态误差；δPn为低成本IMU测量得到的导航坐标系下东北天三个方向的位置误差；▽n为导航坐标系下东北天三个方向的加速度误差；εn为导航坐标系下东北天三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；为导航坐标系下东北天三个方向的速度误差；为导航坐标系下东北天三个方向的姿态误差；为导航坐标系下东北天三个方向的位置误差；为导航坐标系下东北天三个方向的加速度误差；为导航坐标系下东北天三个方向的陀螺仪误差。
5.如权利要求1所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，所述步骤(2)中当前时刻鞋子处于静止状态时，局部滤波器的观测方程具体为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<msup>
<mover>
<mi>V</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mover>
<mi>&phi;</mi>
<mo>^</mo>
</mover>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<msup>
<mi>V</mi>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&phi;</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<msub>
<mi>v</mi>
<mrow>
<mi>l</mi>
<mi>o</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
其中，为当前时刻测量到的导航坐标系下低成本IMU解算的速度误差；为当前时刻测量到的导航坐标系下低成本IMU解算的姿态误差；δVn为低成本IMU测量得到的导航坐标系下的东北天三个方向的速度误差；φ为低成本IMU测量得到的导航坐标系下的东北天三个方向的姿态误差；vloc为滤波器的观测噪声矩阵。
6.如权利要求5所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，低成本IMU测量得到的速度为低成本IMU解算的速度误差；低成本IMU测量得到的姿态角为低成本IMU解算的姿态误差。
7.如权利要求1所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，所述步骤(5)中主数据融合滤波器的状态方程具体为：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<msup>
<mover>
<mi>V</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mo>-</mo>
<msup>
<mi>&Theta;</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<msup>
<mi>n</mi>
<mo>&prime;</mo>
</msup>
</msubsup>
<msup>
<mover>
<mi>f</mi>
<mo>^</mo>
</mover>
<mi>b</mi>
</msup>
<mo>+</mo>
<msup>
<mo>&dtri;</mo>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&phi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>-</mo>
<msup>
<mi>&epsiv;</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<msup>
<mi>&delta;V</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mover>
<mo>&dtri;</mo>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
2
其中，φE、φN、φU分别为导航坐标系下的东向、北向和天向的角度误差；
δVn为低成本IMU测量得到的导航坐标系下的东北天三个方向的速度误差；φ为低成本IMU测量得到的导航坐标系下的东北天三个方向的姿态误差；δPn为低成本IMU测量得到的导航坐标系下的东北天三个方向的位置误差；▽n为导航坐标系下的东北天三个方向的加速度误差；εn为导航坐标系下的东北天三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；为导航坐标系下东北天三个方向的速度误差；为导航坐标系下东北天三个方向的姿态误差；为导航坐标系下东北天三个方向的位置误差；为导航坐标系下东北天三个方向的加速度误差；为导航坐标系下东北天三个方向的陀螺仪误差。
8.如权利要求1所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，所述步骤(5)中主数据融合滤波器的观测方程具体为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<msup>
<mover>
<mi>P</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mover>
<mi>&phi;</mi>
<mo>^</mo>
</mover>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<msup>
<mi>P</mi>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&phi;</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mi>v</mi>
</mrow>
其中，为当前时刻低成本IMU测量得到的导航坐标系下的东北天三个方向的位置误差；为当前时刻测量到的低成本IMU解算的姿态误差；δPn为理论上低成本IMU测量得到的导航坐标系下的东北天三个方向的位置误差；φ为低成本IMU测量得到的导航坐标系下的东北天三个方向的姿态误差；v为滤波器的观测噪声矩阵。
9.如权利要求8所述的一种面向室内行人导航的AHRS/UWB无缝组合导航方法，其特征是，低成本IMU测量得到的速度为低成本IMU解算的速度误差；低成本IMU测量得到的姿态角为低成本IMU解算的姿态误差。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510292083.0A CN104864865B (zh) | 2015-06-01 | 2015-06-01 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510292083.0A CN104864865B (zh) | 2015-06-01 | 2015-06-01 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104864865A CN104864865A (zh) | 2015-08-26 |
| CN104864865B true CN104864865B (zh) | 2017-09-22 |
Family
ID=53910867
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510292083.0A Active CN104864865B (zh) | 2015-06-01 | 2015-06-01 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104864865B (zh) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105509739B (zh) * | 2016-02-04 | 2018-04-06 | 济南大学 | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 |
| CN105928518B (zh) * | 2016-04-14 | 2018-10-19 | 济南大学 | 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法 |
| CN106052684B (zh) * | 2016-06-16 | 2023-07-11 | 济南大学 | 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法 |
| CN106093858B (zh) * | 2016-06-22 | 2019-02-05 | 山东大学 | 一种基于uwb、rfid、ins多源联合定位技术的定位系统及定位方法 |
| CN106908759A (zh) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | 一种基于uwb技术的室内行人导航方法 |
| CN108680167B (zh) * | 2018-05-16 | 2020-10-16 | 深迪半导体(上海)有限公司 | 基于uwb和激光测距的室内航位推算定位方法及系统 |
| CN109141412B (zh) * | 2018-08-06 | 2020-09-22 | 济南大学 | 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统 |
| CN108759825B (zh) * | 2018-08-06 | 2020-05-05 | 济南大学 | 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统 |
| CN109655060B (zh) * | 2019-02-19 | 2021-07-06 | 济南大学 | 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统 |
| CN109916395B (zh) * | 2019-04-04 | 2023-06-23 | 山东智翼航空科技有限公司 | 一种姿态自主冗余组合导航算法 |
| CN110926460B (zh) * | 2019-10-29 | 2021-03-02 | 广东工业大学 | 一种基于IMU的uwb定位异常值处理方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102279404A (zh) * | 2010-06-13 | 2011-12-14 | 上海伽利略导航有限公司 | 一种无缝定位方法及装置 |
| CN102753992A (zh) * | 2009-06-30 | 2012-10-24 | 高通股份有限公司 | 基于轨迹的位置确定 |
| EP2549286A1 (en) * | 2011-07-18 | 2013-01-23 | Thales Alenia Space Italia S.p.A. Con Unico Socio | Navigation system for exploring and/or monitoring unknown and/or difficult environments. |
| CN103471594A (zh) * | 2013-09-24 | 2013-12-25 | 成都市星达通科技有限公司 | 基于ahrs的精对准算法 |
| CN103499348A (zh) * | 2013-09-24 | 2014-01-08 | 成都市星达通科技有限公司 | Ahrs高精度姿态数据计算方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101806544B1 (ko) * | 2011-01-14 | 2017-12-07 | 엘지이노텍 주식회사 | 경로 정보 제공 방법, 그를 위한 시스템 및 단말 |
-
2015
- 2015-06-01 CN CN201510292083.0A patent/CN104864865B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102753992A (zh) * | 2009-06-30 | 2012-10-24 | 高通股份有限公司 | 基于轨迹的位置确定 |
| CN102279404A (zh) * | 2010-06-13 | 2011-12-14 | 上海伽利略导航有限公司 | 一种无缝定位方法及装置 |
| EP2549286A1 (en) * | 2011-07-18 | 2013-01-23 | Thales Alenia Space Italia S.p.A. Con Unico Socio | Navigation system for exploring and/or monitoring unknown and/or difficult environments. |
| CN103471594A (zh) * | 2013-09-24 | 2013-12-25 | 成都市星达通科技有限公司 | 基于ahrs的精对准算法 |
| CN103499348A (zh) * | 2013-09-24 | 2014-01-08 | 成都市星达通科技有限公司 | Ahrs高精度姿态数据计算方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104864865A (zh) | 2015-08-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104864865B (zh) | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 | |
| CN105509739B (zh) | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 | |
| CN105928518B (zh) | 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法 | |
| CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
| CN102692223B (zh) | 用于wsn/ins组合导航的多级非线性滤波器的控制方法 | |
| CN103759730B (zh) | 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法 | |
| CN102494685B (zh) | 用于wsn/ins高精度实时组合导航的h无穷信息融合方法 | |
| CN102636166B (zh) | 基于航向角的wsn/ins组合导航系统的控制方法 | |
| CN106052684A (zh) | 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法 | |
| CN104181573B (zh) | 北斗惯导深组合导航微系统 | |
| CN106871893A (zh) | 分布式ins/uwb紧组合导航系统及方法 | |
| CN106680765A (zh) | 基于分布式组合滤波ins/uwb行人导航系统及方法 | |
| CN104121905A (zh) | 一种基于惯性传感器的航向角获取方法 | |
| CN107966143A (zh) | 一种基于多窗口的自适应efir数据融合方法 | |
| CN102809377A (zh) | 飞行器惯性/气动模型组合导航方法 | |
| CN107966142A (zh) | 一种基于多窗口的室内行人自适应ufir数据融合方法 | |
| CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
| CN104374389B (zh) | 一种面向室内移动机器人的imu/wsn组合导航方法 | |
| CN104296741B (zh) | 采用距离平方和距离平方变化率的wsn/ahrs紧组合方法 | |
| CN108387236A (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN104697520A (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
| CN104197935A (zh) | 一种基于移动智能终端的室内定位方法 | |
| CN105115507B (zh) | 一种基于双imu的双模式室内个人导航系统及方法 | |
| CN104316058A (zh) | 一种采用交互多模型的移动机器人wsn/ins组合导航方法 | |
| CN103033836A (zh) | 一种车载导航指向装置及方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| EXSB | Decision made by sipo to initiate substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |