CN116182867A - 复杂室内环境下基于紧组合的ins/uwb无人车定位方法 - Google Patents

复杂室内环境下基于紧组合的ins/uwb无人车定位方法 Download PDF

Info

Publication number
CN116182867A
CN116182867A CN202310150395.2A CN202310150395A CN116182867A CN 116182867 A CN116182867 A CN 116182867A CN 202310150395 A CN202310150395 A CN 202310150395A CN 116182867 A CN116182867 A CN 116182867A
Authority
CN
China
Prior art keywords
ins
uwb
noise
error
expressed
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
CN202310150395.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 CN202310150395.2A priority Critical patent/CN116182867A/zh
Publication of CN116182867A publication Critical patent/CN116182867A/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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor 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/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
    • G01C21/165—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 combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提出复杂室内环境下基于紧组合的INS/UWB无人车定位方法，涉及室内高精度定位领域。首先基于捷联惯性导航系统的误差模型，设计了基于紧组合的INS/UWB组合定位系统；其次，为了提高UWB异常值时INS单独定位的精度，使用Allan方差对惯性传感器的误差项进行辨识，从而建立惯性传感器的误差模型，精确了INS的误差状态模型；最后，采用自适应扩展卡尔曼滤波算法，对噪声矩阵进行自适应调整，从而增强了对环境的适应性。

Description

复杂室内环境下基于紧组合的INS/UWB无人车定位方法
技术领域
本发明涉及无人车定位技术领域，具体涉及复杂室内环境下基于紧组合的INS/UWB无人车定位方法。
背景技术
自航空运输服务产生以及各大机场建设运营以来，机场地面服务作为航班保障的重要环节，与航空运作息息相关且不可或缺。从飞机进入停机坪，到离开停机坪进入滑行道，飞机停泊期间的所有地面服务，包括飞机引导、给油、给水、旅客下机登机、行李搬运、飞机餐点装载、机身清洁、废弃物处理等，皆为机场地面设备服务内容。无人地面车辆(Unmanned Ground Vehicle，UGV)的出现为机场的地面设备服务提供了便捷。其中，UGV的低成本和高精度定位技术发挥着关键作用。全球导航卫星系统(GNSS)可以提供高精度的位置信息，基本可以满足高精度位置信息的需要。目前已完成和在建的全球卫星导航系统有GPS导航系统、BDS导航系统、GLONAS导航系统以及Galileo导航系统。然而，由于在航站楼、地下通道这些区域，卫星定位信号容易受到干扰而导致无法提供有效的位置估计。因此，实现低成本、高精度的定位已经成为航站楼、地下通道这些室内领域的研究热点。
超宽带(Ultra Wide Band，UWB)具有高分辨率、低功耗和高测距精度等优点被广泛应用在室内定位中。UWB是一种利用纳秒级的非正弦波窄脉冲传输数据的通信技术，可以达到厘米级的测距精度。除了具有良好的精度，UWB在遭受多径效应时也具有更好的鲁棒性。然而，在非视距(Non Line of Sight，NLOS)情况下，由于UWB的高频带，测距精度将大大降低，并且单一的UWB定位系统不能提供定位对象的方向信息。惯性导航系统(InertialNavigation System，INS)不需要任何外来信息，也不会向外辐射任何信息，完全依靠运动载体设备自主完成导航任务(自主性好)，且能够提供比较齐全的导航参数(参数齐全)。同时，无需预先部署任何参考基站(BS)可以直接计算载体的姿态、速度和位置。为了在复杂的室内环境中实现低成本、高精度定位，将UWB和INS以互补的方式进行结合是一种高效、低成本的方案。
基于微机电系统(Micro-Electro-Mechanical System，MEMS)技术的惯性传感器以较低的成本广泛应用于INS/UWB导航系统，但其精度受到复杂随机误差的显著影响。另一方面，在非视距的影响下，UWB的导航和定位精度也遭到严重下降。为了进一步提高INS/UWB系统的导航性能，设计了基于紧组合的INS/UWB组合导航定位系统。
目前现有技术如下：
申请号：CN202010034636.3，申请名称：基于粒子滤波算法的UWB/INS组合室内定位方法，针对非视距环境下UWB(超宽带)测距结果易受复杂环境干扰进而影响定位精度的情况，提出基于粒子滤波算法的UWB/INS组合室内定位方法。通过UWB系统获取待定位人员到各参考基站的距离，通过UWB位置解算单元计算出人员的东方向位置和北方向位置。通过INS(惯性导航系统)获取人员在行走过程中的三轴加速度、三轴角速度、以及三轴磁场强度，通过INS解算单元计算出人员行进时的步长、东方向步速、北方向步速、迈步时间以及姿态角。通过粒子滤波算法，将UWB系统计算结果与INS系统计算结果进行数据融合。最终达到降低非视距复杂环境的影响，提高定位精度的目的。
其是室内高精度人员定位。而我们的目标是实现复杂室内环境下无人车的低成本、高精度定位，采用的是粒子滤波算法对UWB和INS的数据进行融合。而本申请采用的是基于紧组合的自适应扩展卡尔曼滤波来进行两者之间的数据融合。相对于CN111256695A采用的算法具有更简单，有效的特点，同时能够对量测噪声进行自适应调整，能够适用于更复杂的室内环境。
申请号：CN202110382193.1，申请名称：基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，涉及一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，包括：建立UWB-INS融合定位的运动模型；采用超宽带UWB定位技术确定待定位点的第一位置和第一速度；采用惯性导航系统INS获取待定位点的第二位置数据和第二速度；计算以上两种定位方式的实际测量误差；采用动态鲁棒容积卡尔曼滤波算法对实际测量误差进行修正处理，得到最终位置误差；通过最终位置误差对第二位置数据进行修正，得到准确位置s。本发明方法可以缓解外界噪声对系统估计的影响，又可以追踪系统的运动误差模型状态，降低运动不确定性对目标轨迹预测的影响，从而实现精确、稳定的室内导航。
其是室内高精度人员定位，根据运动学规律构建室内行人的状态方程和测量方程。而我们的针对的复杂室内环境下无人车的低成本、高精度定位，根据捷联惯性导航系统的误差模型构建状态方程，其采用了基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，而我们采用的是基于自适应扩展卡尔曼滤波的UWB/INS融合定位方法。相对于CN113074739B采用的算法具有更简单，有效的特点。
申请号：CN201711108074.7，申请名称：一种室内定位方法、导航方法及系统，用以解决现有技术中累计误差/非视距误差大的问题。该方法应用于自动导引运输车AGV，AGV包括主控制器、惯性导航系统INS和至少2个UWB电子标签，包括：主控制器从INS获取第一定位数据和第一姿态数据，并分别从至少两个超宽带UWB电子标签中获取UWB电子标签与各UWB基站间的距离数据；利用获取到的距离数据确定AGV的第二定位数据和第二姿态数据；采用卡尔曼滤波器对第一和第二定位数据、第一和第二姿态数据进行自回归数据处理，得到最优定位数据和最优姿态数据。利用本定位方法可获得准确的定位数据获得导航信息,能够使用较为廉价的器件得到理想的导航精度。其采用的卡尔曼滤波器对INS和UWB的数据进行融合，而我们采用的是扩展卡尔曼滤波算法来融合两者之间的数据。相对于CN108036784A采用的滤波算法，可以提高在UWB异常值INS/组合系统的精度
在对INS的数据进行处理时，没有考虑惯性传感器中有色噪声的影响。若此时直接采用卡尔曼滤波算法，将导致卡尔曼滤波算法中的计算协方差矩阵与实际估计误差之间严重不匹配，从而使得综合导航精度受到极大限制。而我们的专利通过对惯性传感器中的随机误差项进行建模，精确了INS的误差模型，消除了这一影响。
发明内容
为解决上述技术问题，本发明提出复杂室内环境下基于紧组合的INS/UWB无人车定位方法，涉及室内高精度定位领域。首先基于捷联惯性导航系统的误差模型，设计了基于紧组合的INS/UWB组合定位系统；其次，为了提高UWB异常值时INS单独定位的精度，使用Allan方差对惯性传感器的误差项进行辨识，从而建立惯性传感器的误差模型，精确了INS的误差状态模型；最后，采用自适应扩展卡尔曼滤波算法，对噪声矩阵进行自适应调整，从而增强了对环境的适应性。
为实现上述目的，本发明采取的技术方案是：
复杂室内环境下基于紧组合的INS/UWB无人车定位方法，其特征在于：具体过程如下：
步骤1：选取地理坐标系作为导航坐标系，采用捷联惯导算法；
步骤2：建立惯性传感器的误差模型，选取姿态、速度和位置误差以及陀螺与加速度计零偏误差作为状态向量，以此建立INS/UWB组合导航定位系统的状态方程；
使用阿仑方差建立惯性传感器误差的随机模型，表示为
Figure BDA0004090557260000031
其中，
Figure BDA0004090557260000032
和/>
Figure BDA0004090557260000033
分别表示角速率和加速度计的量化噪声分量，/>
Figure BDA0004090557260000034
是角速率随机游走，零偏不稳定性和速率斜坡的组合，即/>
Figure BDA0004090557260000035
同理，/>
Figure BDA0004090557260000036
表示加速度计的有色噪声分量，即/>
Figure BDA0004090557260000041
基于KF理论，白噪声直接用作系统的驱动噪声，而无需建模,量化噪声通过INS的误差模型转换为等效白噪声，其中状态转化定义为
Figure BDA0004090557260000042
Figure BDA0004090557260000043
和/>
Figure BDA0004090557260000044
分别为新定义的姿态误差和速度误差矢量。/>
Figure BDA0004090557260000045
是陀螺仪的角速率量化噪声，/>
Figure BDA0004090557260000046
是加速度计的速度量化噪声，这两者分别满足
Figure BDA0004090557260000047
因此，得到经过量化噪声增强的INS的误差方程，表示为
Figure BDA0004090557260000048
为了补偿卡尔曼滤波中有色噪声引起的惯性传感器漂移，需要对有色噪声进行建模，通过成形滤波器传递函数的傅里叶逆变换得到有色噪声的随机微分方程；
零偏不稳定性的随机微分方程表示为
Figure BDA0004090557260000049
角速率随机游走的随机微分方程表示为
Figure BDA00040905572600000410
速率斜坡的随机微分方程可以表示为
Figure BDA00040905572600000411
步骤3：以UWB距离测量与INS推算距离等差值作为系统量测，从而建立系统的量测方程，不需要进行UWB独立解算；
步骤4：在UWB与IMU组合定位中，采用卡尔曼滤波算法进行数据融合；
扩展卡尔曼滤波算法进行数据融合，具体方法为：
假设目前有N个可用的UWB基站，则UWB测量的距离与INS计算的距离之间的差值为
Figure BDA00040905572600000412
其中
Figure BDA00040905572600000413
表示n系中UWB基站的坐标，/>
Figure BDA00040905572600000414
是INS估计的位置，ri表示表示标签和第i个基站之间的距离；
系统的量测向量为
z＝[z1 z2 … zN]T,
量测方程表示为
zk＝f(xk)+vk,
其中
Figure BDA0004090557260000051
vk表示为量测噪声,即UWB距离测量噪声。INS的位置误差为
xINS,k＝xk+1+δxk+1,yINS,k＝yk+1+δyk+1,zINS,k＝zk+1+δzk+1,
将量测方程代入f(xk)表达式，然后在(0,0,0)处进行泰勒级数展开，以获得线性化的测量方程；
zk＝Hkxk+vk,
其中
Hk＝[0N×3 J(0,0,0) 0N×3 0N×3 0N×3],
J(0,0,0)是(43)at(0,0,0)的雅可比矩阵，表示为
Figure BDA0004090557260000052
步骤5：在滤波级自适应地调整噪声，增强系统对环境的适应性。
作为本发明进一步改进，所述步骤1中捷联惯性导航算法，表示为
Figure BDA0004090557260000053
其中，φ＝[φE φN φU]T表示失准角误差矢量，δvn＝[δvE δvN δvU]T表示速度误差矢量，δpn＝[δpE δpN δpU]T表示位置误差矢量。
作为本发明进一步改进，所述步骤2中惯性传感器中存在五种主要的随机误差，即量化噪声、角度随机游走、角速率随机游走、零偏不稳定性、速率斜坡，其中角速率随机游走、零偏不稳定性、速率斜坡属于有色噪声。
作为本发明进一步改进，在EKF算法中，假设系统噪声和测量噪声为高斯分布，然而，当系统受到非视距影响时，系统的状态估计将受到干扰，为了评估非视距对当前位置的影响，设计了一种基于自适应的EKF算法，该算法使用基于高斯分布的概率密度函数来自适应地调整噪声；
定义非视距辨别指标DI为
Figure BDA0004090557260000061
其中
Figure BDA0004090557260000062
Figure BDA0004090557260000063
表示在k时刻基站的坐标，/>
Figure BDA0004090557260000064
表示在k时刻INS估计的位置，/>
Figure BDA0004090557260000065
表示基站和标签之间的实际距离，Δr是系统误差项，如非视距误差，/>
Figure BDA0004090557260000066
表示测距误差；
假设
Figure BDA0004090557260000067
不受非视距的影响，基站坐标是固定的，因此，当DI的值太小时，UWB信号在传输期间受到NLOS干扰，假设DI满足均值为0和方差为/>
Figure BDA0004090557260000068
的高斯分布，即/>
Figure BDA0004090557260000069
因此，DI的概率密度函数用于评估当前UWB信号受NLOS影响的程度；
Figure BDA00040905572600000610
基于上式，自适应系统噪声表示为
Figure BDA00040905572600000611
类似地，自适应测量噪声表示为
Figure BDA00040905572600000612
其中th表示阈值，由经验值确定。
与现有技术相比，本发明的有益效果为：
1.设计了适用于复杂室内环境下低成本、高精度的INS/UWB组合定位系统，利用UWB的量测值抑制SINS的误差累积，从而提供可靠的姿态、速度和位置信息；
2.对惯性传感器的随机误差项进行建模，精确了INS的误差状态模型，从而提高了UWB异常值时INS单独定位的精度；
4.采用紧组合的方式，以UWB距离测量与SINS推算距离等差值作为系统量测，不需要进行UWB独立解算；
5.采用自适应扩展卡尔曼滤波算法，可以自适应调整噪声矩阵，从而增强了对环境的适应性。
附图说明
图1是本发明算法框架；
图2是本发明系统框图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
作为本发明一种具体实施例，本申请提供复杂室内环境下基于紧组合的INS/UWB无人车定位方法，包括以下步骤：
步骤1：选取地理坐标系作为导航坐标系(n系)，采用捷联惯导算法(如图1所示)
在室内定位中，由于MEMS陀螺仪的低精度，无法感知地球旋转信息以及忽略载体低速引起的地球自转和曲率的影响，因此采用简化的捷联惯性导航(S-SINS)算法，可以表示为
Figure BDA0004090557260000071
其中，φ＝[φE φN φU]T表示失准角误差矢量，δvn＝[δvE δvN δvU]T表示速度误差矢量，δpn＝[δpE δpN δpU]T表示位置误差矢量。
步骤2：建立惯性传感器的误差模型，选取姿态、速度和位置误差以及陀螺与加速度计零偏误差作为状态向量，以此建立INS/UWB组合导航定位系统的状态方程
根据惯性传感器的IEEE标准，惯性传感器中存在五种主要的随机误差，即量化噪声(QN)、角度随机游走(ARW)、角速率随机游走(RRW)、零偏不稳定性(BI)、速率斜坡(RR)，其中角速率随机游走、零偏不稳定性、速率斜坡属于有色噪声(CN)。因此，使用阿仑方差(AV)建立惯性传感器误差的随机模型，可以表示为
Figure BDA0004090557260000072
其中，
Figure BDA0004090557260000073
和/>
Figure BDA0004090557260000074
分别表示角速率和加速度计的量化噪声分量，/>
Figure BDA0004090557260000075
是角速率随机游走，零偏不稳定性和速率斜坡的组合，即/>
Figure BDA0004090557260000076
同理，/>
Figure BDA0004090557260000077
表示加速度计的有色噪声分量，即/>
Figure BDA0004090557260000078
基于KF理论，白噪声(角度随机游走)可以直接用作系统的驱动噪声，而无需建模。量化噪声可以通过INS的误差模型转换为等效白噪声，其中状态转化定义为
Figure BDA0004090557260000079
Figure BDA0004090557260000081
和/>
Figure BDA0004090557260000082
分别为新定义的姿态误差和速度误差矢量。/>
Figure BDA0004090557260000083
是陀螺仪的角速率量化噪声，/>
Figure BDA0004090557260000084
是加速度计的速度量化噪声，这两者分别满足
Figure BDA0004090557260000085
因此，可以得到经过量化噪声增强的INS的误差方程，表示为
Figure BDA0004090557260000086
为了补偿卡尔曼滤波中有色噪声引起的惯性传感器漂移，需要对有色噪声进行建模，通过成形滤波器传递函数的傅里叶逆变换可以得到有色噪声的随机微分方程。
零偏不稳定性的随机微分方程可以表示为
Figure BDA0004090557260000087
角速率随机游走的随机微分方程可以表示为
Figure BDA0004090557260000088
速率斜坡的随机微分方程可以表示为
Figure BDA0004090557260000089
步骤3：以UWB距离测量与INS推算距离等差值作为系统量测，从而建立系统的量测方程
设计了UWB和IMU的紧组合系统，以UWB距离测量与SINS推算距离等差值作为系统量测，不需要进行UWB独立解算，系统框图如图2所示；
步骤4：在UWB与IMU组合定位中，采用卡尔曼滤波算法(Kalman filter,KF)进行数据融合；
在UWB与IMU组合定位中，采用扩展卡尔曼滤波算法(Extended Kalman Filter,EKF)进行数据融合。
假设目前有N个可用的UWB基站，则UWB测量的距离与INS计算的距离之间的差值为
Figure BDA00040905572600000810
其中
Figure BDA00040905572600000811
表示n系中UWB基站的坐标，/>
Figure BDA00040905572600000812
是INS估计的位置，ri表示表示标签和第i个基站之间的距离。
系统的量测向量为
z＝[z1 z2 ... zN]T, (10)
量测方程可以表示为
zk＝f(xk)+vk, (11)
其中
Figure BDA0004090557260000091
vk表示为量测噪声,即UWB距离测量噪声。INS的位置误差为
xINS,k＝xk+1+δxk+1,yINS,k＝yk+1+δyk+1,zINS,k＝zk+1+δzk+1, (13)
将(11)代入(12)，然后在(0,0,0)处进行泰勒级数展开，以获得线性化的测量方程。
zk＝Hkxk+vk, (14)
其中
Hk＝[0N×3 J(0,0,0) 0N×3 0N×3 0N×3], (15)
J(0,0,0)是(43)at(0,0,0)的雅可比矩阵，表示为
Figure BDA0004090557260000092
步骤5：在滤波级自适应地调整噪声，增强系统对环境的适应性
根据从UWB和INS获得的距离差概率密度函数可以自适应地调整噪声参数，从而增强系统对环境的适应性
在EKF算法中，假设系统噪声和测量噪声为高斯分布。然而，当系统受到非视距影响时，系统的状态估计将受到干扰。为了评估非视距对当前位置的影响，设计了一种基于自适应的EKF算法，该算法可以使用基于高斯分布的概率密度函数来自适应地调整噪声。
定义非视距辨别指标DI为
Figure BDA0004090557260000093
其中
Figure BDA0004090557260000094
Figure BDA0004090557260000095
表示在k时刻基站的坐标，/>
Figure BDA0004090557260000096
表示在k时刻INS估计的位置，/>
Figure BDA0004090557260000097
表示基站和标签之间的实际距离，Δr是系统误差项，如非视距误差，/>
Figure BDA0004090557260000098
表示测距误差。
假设
Figure BDA0004090557260000101
不受非视距的影响，基站坐标是固定的。因此，当DI的值太小时，UWB信号在传输期间受到NLOS干扰。假设DI满足均值为0和方差为/>
Figure BDA0004090557260000102
的高斯分布，即/>
Figure BDA0004090557260000103
因此，DI的概率密度函数可用于评估当前UWB信号受NLOS影响的程度。
Figure BDA0004090557260000104
基于(19)，自适应系统噪声可以表示为
Figure BDA0004090557260000105
类似地，自适应测量噪声表示为
Figure BDA0004090557260000106
其中th表示阈值，由经验值确定。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (4)

1.复杂室内环境下基于紧组合的INS/UWB无人车定位方法，其特征在于：具体过程如下：
步骤1：选取地理坐标系作为导航坐标系，采用捷联惯导算法；
步骤2：建立惯性传感器的误差模型，选取姿态、速度和位置误差以及陀螺与加速度计零偏误差作为状态向量，以此建立INS/UWB组合导航定位系统的状态方程；
使用阿仑方差建立惯性传感器误差的随机模型，表示为
Figure FDA0004090557250000011
其中，
Figure FDA0004090557250000012
和/>
Figure FDA0004090557250000013
分别表示角速率和加速度计的量化噪声分量，/>
Figure FDA0004090557250000014
是角速率随机游走，零偏不稳定性和速率斜坡的组合，即/>
Figure FDA0004090557250000015
同理，/>
Figure FDA0004090557250000016
表示加速度计的有色噪声分量，即
Figure FDA0004090557250000017
基于KF理论，白噪声直接用作系统的驱动噪声，而无需建模,量化噪声通过INS的误差模型转换为等效白噪声，其中状态转化定义为
Figure FDA0004090557250000018
Figure FDA0004090557250000019
和/>
Figure FDA00040905572500000110
分别为新定义的姿态误差和速度误差矢量,/>
Figure FDA00040905572500000111
是陀螺仪的角速率量化噪声，/>
Figure FDA00040905572500000112
是加速度计的速度量化噪声，这两者分别满足
Figure FDA00040905572500000113
因此，得到经过量化噪声增强的INS的误差方程，表示为
Figure FDA00040905572500000114
为了补偿卡尔曼滤波中有色噪声引起的惯性传感器漂移，需要对有色噪声进行建模，通过成形滤波器传递函数的傅里叶逆变换得到有色噪声的随机微分方程；
零偏不稳定性的随机微分方程表示为
Figure FDA00040905572500000115
角速率随机游走的随机微分方程表示为
Figure FDA00040905572500000116
速率斜坡的随机微分方程可以表示为
Figure FDA00040905572500000117
步骤3：以UWB距离测量与INS推算距离等差值作为系统量测，从而建立系统的量测方程，不需要进行UWB独立解算；
步骤4：在UWB与IMU组合定位中，采用卡尔曼滤波算法进行数据融合；
扩展卡尔曼滤波算法进行数据融合，具体方法为：
假设目前有N个可用的UWB基站，则UWB测量的距离与INS计算的距离之间的差值为
Figure FDA0004090557250000021
其中
Figure FDA0004090557250000022
表示n系中UWB基站的坐标，/>
Figure FDA0004090557250000023
是INS估计的位置，ri表示表示标签和第i个基站之间的距离；
系统的量测向量为
z＝[z1 z2 … zN]T,
量测方程表示为
zk＝f(xk)+vk,
其中
Figure FDA0004090557250000024
vk表示为量测噪声,即UWB距离测量噪声。INS的位置误差为
xINS,k＝xk+1+δxk+1,yINS,k＝yk+1+δyk+1,zINS,k＝zk+1+δzk+1,
将量测方程代入f(xk)表达式，然后在(0,0,0)处进行泰勒级数展开，以获得线性化的测量方程；
zk＝Hkxk+vk,
其中
Hk＝[0N×3 J(0,0,0) 0N×3 0N×3 0N×3],
J(0,0,0)是(43)at(0,0,0)的雅可比矩阵，表示为
Figure FDA0004090557250000025
步骤5：在滤波级自适应地调整噪声，增强系统对环境的适应性。
2.根据权利要求1所述的复杂室内环境下基于紧组合的INS/UWB无人车定位方法，其特征在于，所述步骤1中捷联惯性导航算法，表示为
Figure FDA0004090557250000031
其中，φ＝[φE φN φU]T表示失准角误差矢量，δvn＝[δvE δvN δvU]T表示速度误差矢量，δpn＝[δpE δpN δpU]T表示位置误差矢量。
3.根据权利要求1所述的复杂室内环境下基于紧组合的INS/UWB无人车定位方法，其特征在于，所述步骤2中惯性传感器中存在五种主要的随机误差，即量化噪声、角度随机游走、角速率随机游走、零偏不稳定性、速率斜坡，其中角速率随机游走、零偏不稳定性、速率斜坡属于有色噪声。
4.根据权利要求1所述的复杂室内环境下基于紧组合的INS/UWB无人车定位方法，其特征在于，
在EKF算法中，假设系统噪声和测量噪声为高斯分布，然而，当系统受到非视距影响时，系统的状态估计将受到干扰，为了评估非视距对当前位置的影响，设计了一种基于自适应的EKF算法，该算法使用基于高斯分布的概率密度函数来自适应地调整噪声；
定义非视距辨别指标DI为
Figure FDA0004090557250000032
其中
Figure FDA0004090557250000033
Figure FDA0004090557250000034
表示在k时刻基站的坐标，/>
Figure FDA0004090557250000035
表示在k时刻INS估计的位置，/>
Figure FDA0004090557250000036
表示基站和标签之间的实际距离，Δr是系统误差项，如非视距误差，/>
Figure FDA0004090557250000037
表示测距误差；
假设
Figure FDA0004090557250000038
不受非视距的影响，基站坐标是固定的，因此，当DI的值太小时，UWB信号在传输期间受到NLOS干扰，假设DI满足均值为0和方差为/>
Figure FDA0004090557250000039
的高斯分布，即/>
Figure FDA00040905572500000310
因此，DI的概率密度函数用于评估当前UWB信号受NLOS影响的程度；
Figure FDA00040905572500000311
基于上式，自适应系统噪声表示为
Figure FDA00040905572500000312
类似地，自适应测量噪声表示为
Figure FDA0004090557250000041
其中th表示阈值，由经验值确定。
CN202310150395.2A 2023-02-22 2023-02-22 复杂室内环境下基于紧组合的ins/uwb无人车定位方法 Pending CN116182867A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310150395.2A CN116182867A (zh) 2023-02-22 2023-02-22 复杂室内环境下基于紧组合的ins/uwb无人车定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310150395.2A CN116182867A (zh) 2023-02-22 2023-02-22 复杂室内环境下基于紧组合的ins/uwb无人车定位方法

Publications (1)

Publication Number Publication Date
CN116182867A true CN116182867A (zh) 2023-05-30

Family

ID=86437980

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310150395.2A Pending CN116182867A (zh) 2023-02-22 2023-02-22 复杂室内环境下基于紧组合的ins/uwb无人车定位方法

Country Status (1)

Country Link
CN (1) CN116182867A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117367431A (zh) * 2023-10-23 2024-01-09 哈尔滨工业大学（威海） 带未知量测偏置的mems与uwb紧组合定位方法及系统
CN117433517A (zh) * 2023-10-27 2024-01-23 青岛威弗德自动化系统有限公司 一种基于多传感器融合的四向穿梭车高精度定位算法
CN117848331A (zh) * 2024-03-06 2024-04-09 河北美泰电子科技有限公司 基于视觉标签地图的定位方法及装置

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117367431A (zh) * 2023-10-23 2024-01-09 哈尔滨工业大学（威海） 带未知量测偏置的mems与uwb紧组合定位方法及系统
CN117367431B (zh) * 2023-10-23 2024-05-14 哈尔滨工业大学（威海） 带未知量测偏置的mems与uwb紧组合定位方法及系统
CN117433517A (zh) * 2023-10-27 2024-01-23 青岛威弗德自动化系统有限公司 一种基于多传感器融合的四向穿梭车高精度定位算法
CN117848331A (zh) * 2024-03-06 2024-04-09 河北美泰电子科技有限公司 基于视觉标签地图的定位方法及装置

Similar Documents

Publication Publication Date Title
CN116182867A (zh) 复杂室内环境下基于紧组合的ins/uwb无人车定位方法
CN109946731B (zh) 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
Fan et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN109682375B (zh) 一种基于容错决策树的uwb辅助惯性定位方法
US6947880B2 (en) Method for improving accuracy of a velocity model
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN111983660A (zh) Gnss拒止环境下四旋翼无人机定位系统和方法
CN114018273A (zh) 煤矿井下自动驾驶车辆精确定位系统及其定位方法
Jiang et al. Improved decentralized multi-sensor navigation system for airborne applications
CN114111767B (zh) 基于多信息融合对线路设计线型进行优化的方法
Zhou et al. Wi-Fi RTT/encoder/INS-based robot indoor localization using smartphones
CN110968113A (zh) 一种无人机自主跟踪起降系统及跟踪定位方法
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
Javed et al. A gyroless attitude estimation for ground vehicle under severe dynamic conditions
Yuan et al. MIAKF: Motion inertia estimated adaptive Kalman filtering for underground mine tunnel positioning
CN114035154B (zh) 一种运动参数辅助的单站射频信号定位方法
CN112762936B (zh) 一种运用于长航时无人机载荷的多源定位信息融合方法
CN114993299A (zh) 一种基于超宽带的无人机辅助定位方法及系统
Zahran et al. Augmented radar odometry by nested optimal filter aided navigation for UAVS in GNSS denied environment
Song An integrated GPS/vision UAV navigation system based on Kalman filter
Luo et al. Crawler Robot Indoor Positioning Based on a Combination of Bluetooth and IMU
Yao et al. Tightly coupled indoor positioning using UWB/mmWave radar/IMU
Džunda et al. Influence of mutual position of communication network users on accuracy of positioning by telemetry method

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