CN114923481A - 复杂环境下基于惯性传感器的行人自主定位方法 - Google Patents

复杂环境下基于惯性传感器的行人自主定位方法 Download PDF

Info

Publication number
CN114923481A
CN114923481A CN202210535329.2A CN202210535329A CN114923481A CN 114923481 A CN114923481 A CN 114923481A CN 202210535329 A CN202210535329 A CN 202210535329A CN 114923481 A CN114923481 A CN 114923481A
Authority
CN
China
Prior art keywords
inertial
pedestrian
matrix
navigation system
state
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.)
Pending
Application number
CN202210535329.2A
Other languages
English (en)
Inventor
王庆
米静
刘鹏飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN202210535329.2A priority Critical patent/CN114923481A/zh
Publication of CN114923481A publication Critical patent/CN114923481A/zh
Pending legal-status Critical Current

Links

Images

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
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/15—Correlation function computation including computation of convolution operations
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Navigation (AREA)

Abstract

复杂环境下基于惯性传感器的行人自主定位方法，1)考虑单个陀螺的相应参数，确定冗余惯性传感器的数量。2)确定惯性传感器数量后通过对冗余惯性传感器的配置方案进行研究，得到能够同时使行人导航系统的导航性能和故障检测与隔离性能达到最优的惯性传感器配置方案。3)通过数据融合算法将将步骤1、步骤2得到的惯性传感器配置方案所测得的数据进行最优融合。4)将步骤3得到的融合之后的惯性运动信息输入到惯性导航系统解算模块，以计算行人的各项运动参数。本发明基于微机电系统的惯性测量单元，实现复杂环境下对行人的高精度自主定位，采用惯性传感器的冗余技术，解决惯性传感器的噪声多次累计从而使得轨迹快速发散的问题。

Description

复杂环境下基于惯性传感器的行人自主定位方法
技术领域
本发明属于惯性传感器的行人自主定位领域，具体为复杂环境下基于惯性传感器的行人自主定位方法。
背景技术
智能位置服务已成为当今国内外研究的重要方向，在人工智能、应急救援、智慧城市等领域起着至关重要的作用。全球导航系统(Global Navigation Satellite System，GNSS)可以提供精确的定位能力，基本可以满足室外开阔场景对高精度位置信息的需要。然而，在仓库、高楼内部、地下室等室内环境中，GNSS由于卫星信号受阻和多径效应的影响难以完成定位。因此，研究不依靠GNSS的室内定位技术具有很高的实际意义。
目前室内定位大多采用无线射频识别(radio frequency identification,RFID)、紫蜂(Zig Bee)、蓝牙(Bluetooth)、超宽带(Ultra-wideband,UWB)、超声波(Ultrasound)、Wi-Fi、伪卫星等技术。这些室内定位方案都是基于一定基础设施的无线定位系统，而且需要提前安装各种传感器来辅助定位，后期也需要专人进行维护，需要花费大量人力物力，对现实环境要求也很高。因此，无需提前部署复杂设备的独立定位系统成为了人们的迫切需求。
惯性导航系统(Inertial Navigation System，INS)不需要接受外部的信息，且隐蔽性强，不会向外界发射任何信号，也不受到气候条件的制约，可以在全球范围内全天候运行，实现完全独立自主的导航。由于惯性导航主要依靠自身惯性传感器，不依赖任何外界信息。基于微机电系统(Micro Electro Mechanical System,MEMS)的惯性测量单元(Inertial Measurement Unit，IMU)具有体积小、结构简单、成本低等优点，在行人惯性导航系统中得到了广泛的应用。行人惯性导航系统可以提供行人的位置和方向(滚转、俯仰、偏航)。然而，长时间的运行会导致惯性传感器的噪声多次累计，使得轨迹快速发散，降低行人惯性导航系统的定位精度。因此，惯性传感器精度的提高对于行人惯性导航系统来说十分重要。
发明内容
为解决上述技术问题，本发明提出了复杂环境下基于惯性传感器的行人自主定位方法，为实现复杂环境下对行人的高精度自主定位，采用基于微机电系统(Micro ElectroMechanical System,MEMS)的惯性测量单元。为解决惯性传感器的噪声多次累计从而使得轨迹快速发散的问题，采用惯性传感器的冗余技术。
为达到上述目的，本发明的技术方案如下：
复杂环境下基于惯性传感器的行人自主定位方法，包括以下步骤：
步骤1：考虑单个陀螺的平均故障间隔时间MTBF、相对MTBF、相对MTBF变化量、行人惯性导航系统的体积、重量、成本，确定冗余惯性传感器的数量；
步骤2：在步骤1确定惯性传感器数量的基础上，通过对冗余惯性传感器的配置方案进行研究，从而得到能够同时使行人导航系统的导航性能和故障检测与隔离性能达到最优的惯性传感器配置方案。
步骤3：通过数据融合算法将步骤1、步骤2所测得的数据进行最优融合，以达到提升行人惯性导航系统精度的目的；
在基于标准卡尔曼滤波算法的最优融合算法中，将标准卡尔曼滤波算法用带系统噪声估计器的自适应滤波算法代替，即形成自适应最优融合算法；
采用增强型自适应最优融合算法，该算法将渐消因子引入了协方差矩阵的预测过程中，提高了对状态突变的应对能力，在一定程度上抑制了滤波器随时间的发散，并提高了滤波算法的精度；
步骤4：将步骤3得到的融合之后的惯性运动信息输入到惯性导航系统解算模块，以计算行人的各项运动参数(包括三维姿态、速度和位置信息)。
作为本发明进一步改进，步骤1具体步骤如下：
在行人惯性导航系统内，对惯性空间的角速率或加速度进行量测，需要至少三个陀螺仪或加速度计，假设有同一可靠性Re的n个惯性器件进行配置，则系统可靠性Ra为
Figure BDA0003647697990000021
因此，整个行人惯性导航系统的MTBF表示为
Figure BDA0003647697990000031
通过计算可得，单个陀螺仪的MTBF是1/λ，惯性传感器的数量越多，惯性导航系统的可靠性越高，通过计算可得，单个陀螺仪的MTBF是1/λ，三个惯性传感器沿正交坐标系安装的无余度系统的MTBF3是1/3λ，定义：
Figure BDA0003647697990000032
Figure BDA0003647697990000035
Figure BDA0003647697990000033
其中，
Figure BDA0003647697990000038
为惯性器件斜置构成的冗余导航系统的MTBFn与无余度系统的MTBF3比值，也称为相对MTBF；
Figure BDA0003647697990000036
为相对MTBF的变化量；F称为惯性导航系统的可靠性性能指标；
通过改变惯性器件的数量来分别计算
Figure BDA0003647697990000037
和可靠性性能指标F。
作为本发明进一步改进，步骤2具体步骤如下：
惯性传感器使用6个同类传感器的冗余惯性传感器，含有6个同类传感器的冗余惯性传感器采用正十二面体配置方式时，惯性导航系统的导航性能与故障检测隔离性能FDI同时达到最优；
在该配置方案中，要想测量X、Y、Z轴的运动信息至少需要三个传感器，该方案中相应的冗余配置矩阵为：
Figure BDA0003647697990000034
式中，α＝31.72°为，则具体的配置矩阵为：
Figure BDA0003647697990000041
当惯性导航系统的量测矩阵满足下列等式时，认为导航系统的导航性能和FPI达到最优；
Figure BDA0003647697990000042
式中，hi为冗余配置矩阵H的行向量，冗余配置矩阵H满足
Figure BDA0003647697990000043
Figure BDA0003647697990000044
通过公式(8)得，当采用正十二面体配置方式时，Hop的值为0.4472，因此，当含有6个同类传感器的冗余导航系统采用正十二面体配置方式时，系统的导航性能与故障检测隔离性能FDI同时达到最优。
作为本发明进一步改进，步骤3自适应最优融合算法具体如下：
将冗余惯性传感器的输出矢量向配置矩阵的左零空间上进行投影，以得到融合算法的冗余观测，通过对冗余观测的最优估计即可实现各传感器性能的最大化利用；
在该行人惯性导航系统中，选取惯性传感器的误差作为状态向量，惯性传感器的输出作为观测量，即
X＝[x1 x2…xn]T
Z＝[y1 y2…yn]T (9)
其中，xi表示第i个惯性传感器的误差，yi表示第i个惯性传感器的量测输出；
因此系统的状态方程和观测方程表示为：
Xk＝Ak/k-1Xk-1+Bk/k-1Wk-1 (10)
Zk＝CkXk+Huk+Vk (11)
式中Ak/k-1，Bk/k-1，Ck为系数矩阵，H为安装矩阵，Wk-1和Vk为噪声矩阵；
令
Figure BDA0003647697990000051
其中TkH＝0，称Tk是H的左零空间基，
Figure BDA0003647697990000052
是Tk正交补空间，即
Figure BDA0003647697990000053
将T左乘方程(11)得：
TZk＝TCkXk+THuk+TVk＝TCkXk+TVk (12)
所以将系统模型表示为
Figure BDA0003647697990000054
用卡尔曼滤波器对该模型进行估计，递推公式如下：
状态一步预测：
Figure BDA0003647697990000055
状态一步预测均方误差：
Figure BDA0003647697990000056
滤波增益：
Kk＝Pk/k-1(TCk)T((TCk)Pk/k-1(TCk)T+Rk)-1 (16)
状态估计：
Figure BDA0003647697990000057
状态估计均方误差：
Pk＝(I-KkTCk)Pk/k-1 (18)
在进行卡尔曼滤波的更新过程中，首先需要给定初始状态量X0和初始方差P0，方程(14)与(15)称为时间更新；方程(16)、(17)与(18)包含的过程称为量测更新，在对时间更新完之后，检测是否有量测信息，如果有，则进行量测更新以及状态估计，从而获得最优估计输出；反之，将量测信息作为最优估计输出，根据卡尔曼滤波可以将
Figure BDA0003647697990000061
求解出来，在方程(14)中，Vk满足高斯分布，即Vk～(0,Rk)，冗余配置矩阵H满秩。因可通过加权最小二乘估计uk，得
Figure BDA0003647697990000062
作为本发明进一步改进，步骤3中通过将自适应滤波算法与Huber广义最大似然估计方法进行结合，解决了因非高斯量测噪声引起滤波模型失真的问题；
通过将自适应最优算法中的量测更新转化为状态预测与量测值之间的一种线性回归问题来得到增强型自适应滤波算法；
设状态的真实值为Xk，观测值为
Figure BDA0003647697990000063
则状态误差表示为
Figure BDA0003647697990000064
线性回归问题描述为
Figure BDA0003647697990000065
定义下列变量
Figure BDA0003647697990000066
Figure BDA0003647697990000067
Figure BDA0003647697990000068
Figure BDA0003647697990000069
因此，线性回归问题转化为
yk＝MkXk+ζk (26)
式中，
Figure BDA00036476979900000610
为单位矩阵。上式可通过广义最大似然估计算法求解，其解可通过对代价函数进行求解得到，该代价函数为
Figure BDA0003647697990000071
其中，ξi为ξ的第I个分量，称为残差向量，该向量满足ξ＝Mk-yk，n为残差向量ξ的维数，并且p函数是著名的Huber凸函数，具有如下形式：
Figure BDA0003647697990000072
其中，r为调节参数，通过使用上述形式的p函数得到的估计值具有很强的鲁棒性，如果p函数是可微的，则符合公式(27)；
因此通过对公式(27)进行偏微分得到线性回归问题的解：
Figure BDA0003647697990000073
Figure BDA0003647697990000074
为了对方程(29)进行求解，首先定义
Figure BDA0003647697990000075
Ψ(ξi)＝diag[ψ(ξi)] (32)
将ξi＝(MkXk-yk)i代入式(29)，得到
MkΨ(MkXk-yk)＝0 (33)
通过式(33)可得
Figure BDA0003647697990000076
其中，上标(j)表示迭代次数。采用最小二乘方法进行初始化，即为
Figure BDA0003647697990000077
迭代过程的收敛值可被认为是在进行测量更新后的状态估计值
Figure BDA0003647697990000078
最后状态误差协方差更新矩阵采用如下公式计算
Figure BDA0003647697990000081
上式利用了使得系统状态估计值达到收敛时的Ψ矩阵。
作为本发明进一步改进，步骤4具体的实现过程如下：
(1)初始参数设置：
Figure BDA0003647697990000082
k＝0；
(2)时间更新：采用标准的卡尔曼滤波算法更新等式，更新状态预测值
Figure BDA0003647697990000083
和状态误差协方差矩阵
Figure BDA0003647697990000084
(3)线性回归问题：通过式(22)-(25)定义的变量，构造如等式(26)的线性回归问题；
量测更新：定义代价函数J(Xk)，然后利用最小二乘算法求解，最后，状态更新值
Figure BDA0003647697990000085
和状态误差更新阵
Figure BDA0003647697990000086
分别由式(34)和(36)求得。
有益效果：
本发明公开了复杂环境下基于惯性传感器的行人自主定位方法。由于在冗余行人惯性导航系统中，导航系统的导航性能和可靠性随惯性传感器数目的增加而提高。但是，当器件过多时，系统的集成难度将会大大增加，而且成本也会显著提高。因此，本申请定义了一种与系统可靠性相关的性能指标，用来定量描述冗余传感器数目和行人惯性导航系统可靠性之间的关系。通过对可靠性性能指标的分析和计算，得到了能够使系统可靠性和经济性最优时的传感器数量。在确定传感器数量的基础上，通过对冗余传感器的配置方案进行研究，从而得到了能够同时使导航系统的导航性能和故障检测与隔离性能(FPI)达到最优的冗余传感器配置方案。
本发明将惯性传感器配置方案所测得的数据通过数据融合算法进行最优融合。为解决在标准卡尔曼滤波算法的系统噪声特性参数未知和量测噪声不满足高斯分布而导致滤波性能急剧下降的问题，采用一种增强型自适应最优融合算法。该算法将渐消因子引入了协方差矩阵的预测过程中，提高了对状态突变的应对能力，在一定程度上抑制了滤波器随时间的发散，并提高了滤波算法的精度；通过将自适应滤波算法与Huber广义最大似然估计方法进行结合，解决了因非高斯量测噪声引起滤波模型失真的问题。
附图说明
图1是本发明相对MTBF变化曲线；
图2是本发明相对MTBF变化量；
图3是本发明可靠性性能指标的变化曲线；
图4是本发明正十二面体配置方案；
图5是本发明自适应最优算法流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
作为本发明一种具体实施例，本发明提供一种复杂环境下基于惯性传感器的行人自主定位方法，包括以下步骤：
步骤1：考虑单个陀螺的平均故障间隔时间(MTBF)、相对MTBF、相对MTBF变化量、行人惯性导航系统的体积、重量、成本，确定冗余惯性传感器的数量。
定义一种与系统可靠性相关的性能指标，用来定量描述冗余惯性传感器的数目和行人惯性导航系统可靠性之间的关系。通过对可靠性性能指标的分析和计算，得到了能够使系统可靠性和经济性最优时的传感器数量。
在行人惯性导航系统内，要对惯性空间的角速率或加速度进行量测，至少需要三个陀螺仪或加速度计。假设有同一可靠性Re的n个惯性器件进行配置，则系统可靠性Ra为
Figure BDA0003647697990000091
因此，整个行人惯性导航系统的MTBF可以表示为
Figure BDA0003647697990000101
通过上式可以看出，惯性传感器的数量越多，惯性导航系统的可靠性越高。通过计算可得，单个陀螺仪的MTBF是1/λ，惯性传感器的数量越多，惯性导航系统的可靠性越高。通过计算可得，单个陀螺仪的MTBF是1/λ，三个惯性传感器沿正交坐标系安装的无余度系统的MTBF3是1/3λ。为进一步研究惯性传感器的数量对导航系统的影响，定义：
Figure BDA0003647697990000102
Figure BDA0003647697990000104
Figure BDA0003647697990000103
其中，
Figure BDA0003647697990000105
个惯性器件斜置构成的冗余导航系统的MTBFn与无余度系统的MTBF3比值，也称为相对MTBF；
Figure BDA0003647697990000106
为相对MTBF的变化量；F称为惯性导航系统的可靠性性能指标。
通过改变惯性器件的数量来分别计算
Figure BDA0003647697990000107
和可靠性性能指标F，变化曲线如图1、2和3所示。
通过分析上述图形可知，当有6个惯性传感器时，可靠性性能指标F达到最大值，即系统的可靠性和经济性达到最优。
步骤2：在步骤1确定惯性传感器数量的基础上，通过对冗余惯性传感器的配置方案进行研究，从而得到能够同时使行人导航系统的导航性能和故障检测与隔离性能(Faultdetection and isolation performance，FPI)达到最优的惯性传感器配置方案。
含有6个同类传感器的冗余惯性传感器采用正十二面体配置方式时，惯性导航系统的导航性能与故障检测隔离性能(FDI)同时达到最优。
正十二面体配置方案如图4所示。在该配置方案中，要想测量X、Y、Z轴的运动信息至少需要三个传感器，该方案中相应的冗余配置矩阵为：
Figure BDA0003647697990000111
式中，α＝31.72°为。则具体的配置矩阵为：
Figure BDA0003647697990000112
当惯性导航系统的量测矩阵满足下列等式时，可以认为导航系统的导航性能和FPI达到最优。
Figure BDA0003647697990000113
式中，hi为冗余配置矩阵H的行向量，冗余配置矩阵H满足
Figure BDA0003647697990000114
k＝1,2,3,…。
通过公式(8)得，当采用正十二面体配置方式时，Hop的值为0.4472。因此，当含有6个同类传感器的冗余导航系统采用正十二面体配置方式时，系统的导航性能与故障检测隔离性能(FDI)同时达到最优。
步骤3：通过数据融合算法将将步骤1、步骤2得到的惯性传感器配置方案所测得的数据进行最优融合，以达到提升行人惯性导航系统精度的目的。
为了解决复杂环境下标准卡尔曼滤波算法的系统噪声特性参数未知，采用一种增强型自适应最优融合算法。该算法将渐消因子引入了协方差矩阵的预测过程中，提高了对状态突变的应对能力，在一定程度上抑制了滤波器随时间的发散，并提高了滤波算法的精度。
在基于标准卡尔曼滤波算法的最优融合算法中，将标准卡尔曼滤波算法用带系统噪声估计器的自适应滤波算法代替，即形成一种自适应最优融合算法。该算法的流程图如图5所示：
将冗余惯性传感器的输出矢量向配置矩阵的左零空间上进行投影，以得到融合算法的冗余观测，通过对冗余观测的最优估计即可实现各传感器性能的最大化利用。
在该行人惯性导航系统中，选取惯性传感器的误差作为状态向量，惯性传感器的输出作为观测量，即
X＝[x1 x2…xn]T
Z＝[y1 y2…yn]T (9)
其中，xi表示第i个惯性传感器的误差，yi表示第i个惯性传感器的量测输出。
因此系统的状态方程和观测方程可以表示为
Xk＝Ak/k-1Xk-1+Bk/k-1Wk-1 (10)
Zk＝CkXk+Huk+Vk (11)
式中Ak/k-1，Bk/k-1，Ck为系数矩阵，H为安装矩阵，Wk-1和Vk为噪声矩阵。
令
Figure BDA0003647697990000121
其中TkH＝0，称Tk是H的左零空间基，
Figure BDA0003647697990000122
是Tk正交补空间，即
Figure BDA0003647697990000123
将T左乘方程(11)可得
TZk＝TCkXk+THuk+TVk＝TCkXk+TVk (12)
所以可将系统模型表示为
Figure BDA0003647697990000124
用卡尔曼滤波器对该模型进行估计，递推公式如下：
状态一步预测：
Figure BDA0003647697990000131
状态一步预测均方误差：
Figure BDA0003647697990000132
滤波增益：
Kk＝Pk/k-1(TCk)T((TCk)Pk/k-1(TCk)T+Rk)-1 (16)
状态估计：
Figure BDA0003647697990000133
状态估计均方误差：
Pk＝(I-KkTCk)Pk/k-1 (18)
在进行卡尔曼滤波的更新过程中，首先需要给定初始状态量X0和初始方差P0。方程(14)与(15)称为时间更新；方程(16)、(17)与(18)包含的过程称为量测更新。在对时间更新完之后，检测是否有量测信息，如果有，则进行量测更新以及状态估计，从而获得最优估计输出；反之，将量测信息作为最优估计输出。根据卡尔曼滤波可以将
Figure BDA0003647697990000134
求解出来。在方程(14)中，Vk满足高斯分布，即Vk～(0,Rk)，冗余配置矩阵H满秩。因可通过加权最小二乘估计uk，得
Figure BDA0003647697990000135
为了解决复杂环境下标准卡尔曼滤波算法的量测噪声不满足高斯分布而导致滤波性能急剧下降的问题，通过将自适应滤波算法与Huber广义最大似然估计方法进行结合，解决了因非高斯量测噪声引起滤波模型失真的问题。
标准的姿态估计测量传感器存在非高斯特性并且具有不确定的噪声特性，当误差/噪声统计分布服从非高斯分布，自适应最优融合算法的估计性能会迅速降低。因此需对该算法进行改进。通过将自适应最优算法中的量测更新转化为状态预测与量测值之间的一种线性回归问题来得到增强型自适应滤波算法。
设状态的真实值为Xk，观测值为
Figure BDA0003647697990000141
则状态误差可以表示为
Figure BDA0003647697990000142
线性回归问题可以描述为
Figure BDA0003647697990000143
定义下列变量
Figure BDA0003647697990000144
Figure BDA0003647697990000145
Figure BDA0003647697990000146
Figure BDA0003647697990000147
因此，线性回归问题可以转化为
yk＝MkXk+ζk (26)
式中，
Figure BDA0003647697990000148
为单位矩阵。上式可通过广义最大似然估计算法求解，
其解可通过对代价函数进行求解得到，该代价函数为
Figure BDA0003647697990000149
其中，ξi为ξ的第I个分量，称为残差向量。该向量满足ξ＝Mk-yk，n为残差向量ξ的维数，并且p函数是著名的Huber凸函数，具有如下形式：
Figure BDA0003647697990000151
其中，r为调节参数，通过使用上述形式的p函数得到的估计值具有很强的鲁棒性。如果p函数是可微的，则符合公式(27)。因此可以通过对公式(27)进行偏微分得到线性回归问题的解：
Figure BDA0003647697990000152
Figure BDA0003647697990000153
为了对方程(29)进行求解，首先定义
Figure BDA0003647697990000154
Ψ(ξi)＝diag[ψ(ξi)] (32)
将ξi＝(MkXk-yk)i代入式(29)，得到
MkΨ(MkXk-yk)＝0 (33)
通过式(33)可得
Figure BDA0003647697990000155
其中，上标(j)表示迭代次数。采用最小二乘方法进行初始化，即为
Figure BDA0003647697990000156
迭代过程的收敛值可被认为是在进行测量更新后的状态估计值
Figure BDA0003647697990000157
最后状态误差协方差更新矩阵可采用如下公式计算
Figure BDA0003647697990000158
上式利用了使得系统状态估计值达到收敛时的Ψ矩阵。
步骤4：将步骤3得到的融合之后的惯性运动信息输入到惯性导航系统解算模块，以计算行人的各项运动参数(三维姿态、速度、位置信息)。
具体的实现过程如下：
(1)初始参数设置：
Figure BDA0003647697990000161
k＝0；
(2)时间更新：采用标准的卡尔曼滤波算法更新等式，更新状态预测值
Figure BDA0003647697990000162
和状态误差协方差矩阵
Figure BDA0003647697990000163
(3)线性回归问题：通过式(22)-(25)定义的变量，构造如等式(26)的线性回归问题；
量测更新：定义代价函数J(Xk)，然后利用最小二乘算法求解。最后，状态更新值
Figure BDA0003647697990000164
和状态误差更新阵
Figure BDA0003647697990000165
分别由式(34)和(36)求得。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (6)

1.复杂环境下基于惯性传感器的行人自主定位方法，其特征在于，包括以下步骤：
步骤1：考虑单个陀螺的平均故障间隔时间MTBF、相对MTBF、相对MTBF变化量、行人惯性导航系统的体积、重量、成本，确定冗余惯性传感器的数量；
步骤2：在步骤1确定惯性传感器数量的基础上，通过对冗余惯性传感器的配置方案进行研究，从而得到能够同时使行人导航系统的导航性能和故障检测与隔离性能达到最优的惯性传感器配置方案。
步骤3：通过数据融合算法将步骤1、步骤2中所测得的数据进行最优融合，以达到提升行人惯性导航系统定位精度的目的；
在基于标准卡尔曼滤波算法的最优融合算法中，将标准卡尔曼滤波算法用带系统噪声估计器的自适应滤波算法代替，即形成自适应最优融合算法；
采用增强型自适应最优融合算法，该算法将渐消因子引入了协方差矩阵的预测过程中，提高了对状态突变的应对能力，在一定程度上抑制了滤波器随时间的发散，并提高了滤波算法的精度；
步骤4：将步骤3得到的融合之后的惯性运动信息输入到惯性导航系统解算模块，以计算行人的各项运动参数(包括三维姿态、速度和位置信息)。
2.根据权利要求1所述的复杂环境下基于惯性传感器的行人自主定位方法，其特征在于：步骤1具体步骤如下：
在行人惯性导航系统内，对惯性空间的角速率或加速度进行量测，需要至少三个陀螺仪或加速度计，假设有同一可靠性Re的n个惯性器件进行配置，则系统可靠性Ra为
Figure FDA0003647697980000011
因此，整个行人惯性导航系统的MTBF表示为
Figure FDA0003647697980000012
通过计算可得，单个陀螺仪的MTBF是1/λ，惯性传感器的数量越多，惯性导航系统的可靠性越高，通过计算可得，单个陀螺仪的MTBF是1/λ，三个惯性传感器沿正交坐标系安装的无余度系统的MTBF3是1/3λ，定义：
Figure FDA0003647697980000021
Δθ＝θn-θn-1 (4)
Figure FDA0003647697980000022
其中，θn为n，n≥3个惯性器件斜置构成的冗余导航系统的MTBFn与无余度系统的MTBF3比值，也称为相对MTBF；Δθ为相对MTBF的变化量；F称为惯性导航系统的可靠性性能指标；
通过改变惯性器件的数量来分别计算θn、Δθ和可靠性性能指标F。
3.根据权利要求1所述的复杂环境下基于惯性传感器的行人自主定位方法，其特征在于：步骤2具体步骤如下：
惯性传感器使用6个同类传感器的冗余惯性传感器，含有6个同类传感器的冗余惯性传感器采用正十二面体配置方式时，惯性导航系统的导航性能与故障检测隔离性能FDI同时达到最优；
在该配置方案中，要想测量X、Y、Z轴的运动信息至少需要三个传感器，该方案中相应的冗余配置矩阵为：
Figure FDA0003647697980000023
式中，α＝31.72°为，则具体的配置矩阵为：
Figure FDA0003647697980000031
当惯性导航系统的量测矩阵满足下列等式时，认为导航系统的导航性能和FPI达到最优；
Figure FDA0003647697980000032
式中，hi为冗余配置矩阵H的行向量，冗余配置矩阵H满足
Figure FDA0003647697980000033
Figure FDA0003647697980000034
通过公式(8)得，当采用正十二面体配置方式时，Hop的值为0.4472，因此，当含有6个同类传感器的冗余导航系统采用正十二面体配置方式时，系统的导航性能与故障检测隔离性能FDI同时达到最优。
4.根据权利要求1所述的复杂环境下基于惯性传感器的行人自主定位方法，其特征在于：
步骤3自适应最优融合算法具体如下：
将冗余惯性传感器的输出矢量向配置矩阵的左零空间上进行投影，以得到融合算法的冗余观测，通过对冗余观测的最优估计即可实现各传感器性能的最大化利用；
在该行人惯性导航系统中，选取惯性传感器的误差作为状态向量，惯性传感器的输出作为观测量，即
Figure FDA0003647697980000035
其中，xi表示第i个惯性传感器的误差，yi表示第i个惯性传感器的量测输出；
因此系统的状态方程和观测方程表示为：
Xk＝Ak/k-1Xk-1+Bk/k-1Wk-1 (10)
Zk＝CkXk+Huk+Vk (11)
式中Ak/k-1，Bk/k-1，Ck为系数矩阵，H为安装矩阵，Wk-1和Vk为噪声矩阵；
令
Figure FDA0003647697980000041
其中TkH＝0，称Tk是H的左零空间基，
Figure FDA0003647697980000042
是Tk正交补空间，即
Figure FDA0003647697980000043
将T左乘方程(11)得：
Figure FDA0003647697980000044
所以将系统模型表示为
Figure FDA0003647697980000045
用卡尔曼滤波器对该模型进行估计，递推公式如下：
状态一步预测：
Figure FDA0003647697980000046
状态一步预测均方误差：
Figure FDA0003647697980000047
滤波增益：
Kk＝Pk/k-1(TCk)T((TCk)Pk/k-1(TCk)T+Rk)-1 (16)
状态估计：
Figure FDA0003647697980000048
状态估计均方误差：
Pk＝(I-KkTCk)Pk/k-1 (18)
在进行卡尔曼滤波的更新过程中，首先需要给定初始状态量X0和初始方差P0，方程(14)与(15)称为时间更新；方程(16)、(17)与(18)包含的过程称为量测更新，在对时间更新完之后，检测是否有量测信息，如果有，则进行量测更新以及状态估计，从而获得最优估计输出；反之，将量测信息作为最优估计输出，根据卡尔曼滤波可以将
Figure FDA0003647697980000051
求解出来，在方程(14)中，Vk满足高斯分布，即Vk～(0,Rk)，冗余配置矩阵H满秩。因可通过加权最小二乘估计uk，得
Figure FDA0003647697980000052
5.根据权利要求1所述的复杂环境下基于惯性传感器的行人自主定位方法，其特征在于：
步骤3中通过将自适应滤波算法与Huber广义最大似然估计方法进行结合，解决了因非高斯量测噪声引起滤波模型失真的问题；
通过将自适应最优算法中的量测更新转化为状态预测与量测值之间的一种线性回归问题来得到增强型自适应滤波算法；
设状态的真实值为Xk，观测值为
Figure FDA0003647697980000053
则状态误差表示为
Figure FDA0003647697980000054
线性回归问题描述为
Figure FDA0003647697980000055
定义下列变量
Figure FDA0003647697980000056
Figure FDA0003647697980000057
Figure FDA0003647697980000058
Figure FDA0003647697980000059
因此，线性回归问题转化为
yk＝MkXk+ζk (26)
式中，
Figure FDA0003647697980000067
为单位矩阵。上式可通过广义最大似然估计算法求解，其解可通过对代价函数进行求解得到，该代价函数为
Figure FDA0003647697980000061
其中，ξi为ξ的第I个分量，称为残差向量，该向量满足ξ＝Mk-yk，n为残差向量ξ的维数，并且p函数是著名的Huber凸函数，具有如下形式：
Figure FDA0003647697980000062
其中，r为调节参数，通过使用上述形式的p函数得到的估计值具有很强的鲁棒性，如果p函数是可微的，则符合公式(27)；
因此通过对公式(27)进行偏微分得到线性回归问题的解：
Figure FDA0003647697980000063
Figure FDA0003647697980000064
为了对方程(29)进行求解，首先定义
Figure FDA0003647697980000065
Ψ(ξi)＝diag[ψ (ξi)] (32)
将ξi＝(MkXk-yk)i代入式(29)，得到
MkΨ(MkXk-yk)＝0 (33)
通过式(33)可得
Figure FDA0003647697980000066
其中，上标(j)表示迭代次数。采用最小二乘方法进行初始化，即为
Figure FDA0003647697980000071
迭代过程的收敛值可被认为是在进行测量更新后的状态估计值
Figure FDA0003647697980000072
最后状态误差协方差更新矩阵采用如下公式计算
Figure FDA0003647697980000073
上式利用了使得系统状态估计值达到收敛时的Ψ矩阵。
6.根据权利要求1所述的复杂环境下基于惯性传感器的行人自主定位方法，其特征在于：
步骤4具体的实现过程如下：
(1)初始参数设置：
Figure FDA0003647697980000074
k＝0；
(2)时间更新：采用标准的卡尔曼滤波算法更新等式，更新状态预测值
Figure FDA0003647697980000075
和状态误差协方差矩阵
Figure FDA0003647697980000076
(3)线性回归问题：通过式(22)-(25)定义的变量，构造如等式(26)的线性回归问题；
量测更新：定义代价函数J(Xk)，然后利用最小二乘算法求解，最后，状态更新值
Figure FDA0003647697980000077
和状态误差更新阵
Figure FDA0003647697980000078
分别由式(34)和(36)求得。
CN202210535329.2A 2022-05-17 2022-05-17 复杂环境下基于惯性传感器的行人自主定位方法 Pending CN114923481A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210535329.2A CN114923481A (zh) 2022-05-17 2022-05-17 复杂环境下基于惯性传感器的行人自主定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210535329.2A CN114923481A (zh) 2022-05-17 2022-05-17 复杂环境下基于惯性传感器的行人自主定位方法

Publications (1)

Publication Number Publication Date
CN114923481A true CN114923481A (zh) 2022-08-19

Family

ID=82808300

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210535329.2A Pending CN114923481A (zh) 2022-05-17 2022-05-17 复杂环境下基于惯性传感器的行人自主定位方法

Country Status (1)

Country Link
CN (1) CN114923481A (zh)

Similar Documents

Publication Publication Date Title
CN110244715B (zh) 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
Jourdan et al. Monte Carlo localization in dense multipath environments using UWB ranging
US8374624B2 (en) Location measurement method based on predictive filter
CN103776453B (zh) 一种多模型水下航行器组合导航滤波方法
CN109459019A (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN108120438B (zh) 一种基于imu和rfid信息融合的室内目标快速跟踪方法
CN113252033B (zh) 基于多传感器融合的定位方法、定位系统及机器人
Nguyen et al. Flexible and resource-efficient multi-robot collaborative visual-inertial-range localization
CN106679672A (zh) 一种基于dbn和卡尔曼滤波算法的agv定位算法
CN112729301A (zh) 一种基于多源数据融合的室内定位方法
Khan et al. Investigating effective methods for integration of building's map with low cost inertial sensors and wifi-based positioning
KR101390776B1 (ko) 퍼지 확장 칼만 필터를 이용한 위치인식 장치, 방법 및 이동로봇
Malleswaran et al. IMM-UKF-TFS model-based approach for intelligent navigation
CN114706421B (zh) 一种多无人机编队穿越框形障碍物方法
CN116908777A (zh) 基于显式通信带标签伯努利的多机器人随机组网协同导航方法
CN115900708A (zh) 基于gps引导式粒子滤波的机器人多传感器融合定位方法
Chen et al. An optimal selection of sensors in multi-sensor fusion navigation with factor graph
Martinelli et al. A resilient solution to range-only SLAM based on a decoupled landmark range and bearing reconstruction
JP7275553B2 (ja) 移動体、移動体の制御方法及びプログラム
US20220306089A1 (en) Relative Position Tracking Using Motion Sensor With Drift Correction
CN117451032A (zh) 一种低算力与松耦合的激光雷达和imu的slam方法及系统
CN109945864B (zh) 室内行车定位融合方法、装置、存储介质及终端设备
CN114923481A (zh) 复杂环境下基于惯性传感器的行人自主定位方法
Fabresse et al. Active perception for 3D range-only simultaneous localization and mapping with UAVs
CN115790603A (zh) 一种用于信息拒止环境下的无人机动态目标估计方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination