CN117518077A - 一种复杂地下环境机器人定位和误差缓解方法 - Google Patents

一种复杂地下环境机器人定位和误差缓解方法 Download PDF

Info

Publication number
CN117518077A
CN117518077A CN202311469705.3A CN202311469705A CN117518077A CN 117518077 A CN117518077 A CN 117518077A CN 202311469705 A CN202311469705 A CN 202311469705A CN 117518077 A CN117518077 A CN 117518077A
Authority
CN
China
Prior art keywords
value
robot
positioning
covariance
measurement
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
CN202311469705.3A
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.)
Central South University
Original Assignee
Central South 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 Central South University filed Critical Central South University
Priority to CN202311469705.3A priority Critical patent/CN117518077A/zh
Publication of CN117518077A publication Critical patent/CN117518077A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0205—Details
    • G01S5/021—Calibration, monitoring or correction
    • 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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Optimization (AREA)
  • Remote Sensing (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Operations Research (AREA)
  • Computing Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了一种复杂地下环境机器人定位和误差缓解方法，首先通过建立的系统状态方程对机器人下一时刻状态进行预测，其次采用方差对系统各信息源的置信度进行评估，并分配权重，以更准确地反映每个信息源的可信度，再次引入了Anderson darling检验和马氏距离细分判断的两步检测方法对非视距和累计误差造成的离群值进行检验，最后基于马氏距离和变方差模型建立抗差因子，以抵御不同环境下测量离群值的干扰，从而为复杂地下环境下机器人的定位与导航系统带来显著的性能提升，为精确导航提供了坚实的解决方案。

Description

一种复杂地下环境机器人定位和误差缓解方法
技术领域
发明涉及机器人控制领域，具体涉及一种复杂地下环境机器人定位和误差缓解方法。
背景技术
在复杂地下环境中进行机器人定位和误差缓解是一个具有挑战性的任务。传统的定位技术，如GPS，在这种地下环境中通常无法提供高精度和可靠性的位置信息。而超宽带技术因为高精度和强抗干扰性的特点被广泛用于巡检机器人的定位，即使存在多径效应和其他信号传播问题，也能够在复杂的地下环境中保持定位的可靠性。
然而，在复杂地下环境中，超宽带定位仍然存在一些挑战，例如非视距条件和地下环境中的障碍物、折射、散射以及信号吸收导致信号的到达时间和强度难以准确测量，降低了位置精度。此外，超宽带技术也可能引入定位误差，如多径效应和信号衰减。为了克服这些缺点，通常使用卡尔曼滤波器与惯性导航单元结合的方法，由于其计算效率高和易于实现的特点，所以在各类场景都得到了应用。为了在非线性非高斯系统中获得最优解，可以通过一阶泰勒级数修正卡尔曼滤波器，即扩展卡尔曼滤波器。由于扩展卡尔曼滤波器中线性误差存在，会使滤波器过于自信，降低定位精度；无迹卡尔曼滤波是另一常用手段，通过无迹变换适应非线性系统，其输出分布性能优于解析局部线性化。例如中车永济电机有限公司申请的“列车用BDS与INS组合导航的自定位方法”(公开号：CN 112710306)提出了一种基于新息马氏距离构建抗差因子的无迹卡尔曼滤波方法，其缺点是系统门限的判别仍依赖于系统观测和预测值之间的差距，无法对复杂地下环境中非视距的超宽带信号干扰进行修正，同样对于东南大学申请的“一种基于卡方检验的部分粗差抗差自适应滤波方法”(公开号：CN 110161543)提出的基于卡方检验的部分粗差抗差自适应滤波方法同样依赖于新息的误差检测，且必须要满足特定的噪声分布才能获得较高的异常值甄别效果。综上，只有当噪声行为是已知或者模型已知时，上述这些滤波器的误差才会保持在可接受的水平。
因此，亟需对现有技术进行改进。
发明内容
针对传统定位技术，如GPS在地下环境中无法提供高精度和可靠性的位置信息的问题，同时现有融合惯性导航单元的超宽带定位方法只有当噪声行为是已知或者模型已知时，定位误差才会保持在可接受的水平，融合深度学习的方法虽然精度有所提升，但是存在模型复杂和定位实时性不高的挑战。为应对这些问题，本发明提出了一种复杂地下环境机器人定位和误差缓解方法。该方法采用了两阶段的离群值检验方法进行非视距识别，通过引入变方差和置信度融合策略改进卡尔曼滤波器。与传统方法不同的是，它不需要对噪声进行提前测量和建模，能够实时评估融合信息的可信度，进而动态调整权重，从而显著提升了定位精度。
本发明解决其技术问题所采用的技术方案是：
一种复杂地下环境机器人定位和误差缓解方法，包括以下步骤：
S1：假设k时刻为当前时刻，假设机器人定位系统的状态向量为进一步得到考虑过程噪声的系统状态方程：
式中，xk和yk为机器人在k时刻的Xs轴和Ys坐标，Δt为机器人从上一时刻k-1运动至当前时刻k的时间间隔，和/>为机器人在k时刻的Xs轴和Ys轴的速度，ax,k-1和ay,k-1为机器人在k-1时刻沿Xs轴和Ys坐标的加速度，θk和θk-1为机器人在k时刻和k-1时刻的偏航角，ωk-1为机器人在k-1时刻偏航角的转速，Wk＝[w1k w2k w3k w4k w5k]T为过程噪声；
将式(1)改成矩阵形式：
Xk＝FXk-1+BUk-1+Wk (2)
式中，F为状态转移矩阵，Uk-1为k-1时刻惯性导航单元的量测值，/>B为控制矩阵，/> Qk为系统噪声方差矩阵；
S2：初始化：
式中，X0为系统状态的初始值，为系统状态的初始估计值，P0为误差协方差矩阵的初值；
S3：进行分布特征点采样和权重计算：
式中，χ0,k-1、χi,k-1和χi+n,k-1为采样的分布特征点，为k-1时刻系统状态向量估计，Px为k-1时刻的协方差矩阵，当k＝1时，Px＝P0，Wi m和Wi c为均值加权系数和方差加权系数，ρ为调整逼近精度的尺度参数，以降低预测误差，ρ＝α2(n+κ)-n，α决定分布特征点在均值附近的分布状态，调节高阶项的影响，经验范围在0～1之间，β为状态分布参数，可调节方差精度，对于高斯分布，β＝2最优，n为系统状态向量的维数，κ为待选尺度参数，需要使得(n+ρ)Px正定，α、β和κ三个参数的选择对应不同的分布特征点构造策略，以获得不同的转换精度和成本；
S4：通过非线性函数预测下一时刻的分布特征点：
式中，F(x)为非线性函数；
对应的加权预测状态和协方差分别为：
式中，为加权预测值，/>为加权预测值和预测值之间的协方差；
S5：测量预测分布特征点的值：
式中，为测量预测值，H(x)为量测函数；
对应的加权测量预测值以及对应的协方差可分别计算为：
式中，为加权测量预测值，/>为加权测量预测值和测量预测值之间的协方差，Rk为观测协方差；
S6：计算预测状态和预测测量值的互协方差：
S7：计算增益；
S8：得到最后的状态估计量和后验协方差：
S9：通过变方差模型，基于观测值和系统状态之间马氏距离的平方，本发明引入了抗差因子，以应对测量离群值的干扰。当观测值受到离群值的污染，即当超宽带测距值为非视距，本发明通过增加观察噪声协方差来控制离群值对定位结果的影响，抗差因子的表达如下：
式中，η观测值受离群值污染的分界线值，Yk为系统观测值，τ为常量，用于调整抗差因子精度，为系统观测值和加权测量预测值/>的协方差，/>
S10：根据变方差模型结果更新步骤S5中观测协方差：
本发明还提供了一种复杂地下环境机器人定位中滤波器观测值的置信度融合策略，采用方差对系统各信息源的置信度进行评估并分配权重，其中，系统观测值Yk的更新策略如下：
式中，p为超宽带观测值的融合权重，为k时刻超宽带的观测值，/>为k时刻惯性导航单元的观测值；
权重p的计算方式如下：
p＝σimu/(σuwb+σimu) (19)
式中，σuwb为超宽带设备定位结果的方差和σimu为惯性导航单元定位结果的方差；
所述超宽带设备定位结果的方差σuwb和惯性导航单元定位结果的方差σimu的计算方式如下：
式中，[xuwb,1,xuwb,2,...,xuwb,n]为时间间隔Δt内通过最小二乘算法得到的超宽带设备定位结果，为在时间间隔Δt内存储n个超宽带定位结果的均值，即/>[ximu,1,ximu,2,...,ximu,m]为时间间隔Δt内通过式(1)航迹推算得到的惯性导航单元定位结果，/>为在时间间隔Δt内采集m个航迹推算得到的惯性导航单元定位结果的均值，即
进一步，所述复杂地下环境机器人定位和误差缓解方法，其中步骤S9中观测值是否被离群值污染，即超宽带测距值属于非视距或者视距条件的判断，通过Andersondarling检验和马氏距离判断两步进行，具体包含以下步骤：
S91：假设标签从第l个基站接受到n个不同的测距值，得到测距值集为并在此基础上计算Anderson darling统计量AD_UWB：
式中， 代表标准的正态分布函数；
S92：得到初步的非视距测距值判断条件为：
式中，
S93：对初步判定为视距的测距值，结合惯性导航单元采集的数据进行进一步判断，首先求算系统状态观测值到测量预测值的统计值：
式中，是判定为视距部分的系统状态观测值/>到k时刻加权测量预测值/>的马氏距离的平方，/>为判定为视距部分的测距值与对应同一时刻惯性导航单元测量结果结合的系统状态观测值，/>为k时刻的加权测量预测值；
S94：对初步判定为视距的系统状态向量进行细分判断：
式中，η为估计分布模型中0.05显著性水平下的取值，与式(15)中的大小相同；
本发明的有益效果主要表现在：
(1)基于观测值与系统状态之间的马氏距离和变方差模型，构建了抗差因子，以有效抵御测量离群值的干扰，显著提升了机器人的位置准确性，同时减轻了误差的扩散，不仅使其适应不同环境，还显著提高了任务执行效率，这确保机器人能够在多样的地下环境中实现卓越的定位与导航；
(2)采用方差对系统各信息源的置信度进行评估并分配权重，可以更准确地反映每个信息源的可信度，对决策过程进行量化，使得可信度高的信息源分配更高的权重，有助于提高系统对信息的准确性，从而提高决策质量和系统的鲁棒性；
(3)通过Anderson darling检验和马氏距离细分判断的两步检测，提高了离群值识别的精度，成功应对了非视距情况下超宽带定位误差显著增大的挑战，同时有效化解了惯性导航单元误差逐渐积累的问题，为精确导航提供了坚实的解决方案。
附图说明
图1是本发明机器人定位和误差缓解方法的运行流程图。
图2是机器人定位和误差缓解系统的示例图。
图3是两步检测法的运行流程图。
图4是本发明机器人定位和误差缓解方法的测试场景。
图5是本发明机器人定位和误差缓解方法的实际性能图。
具体实施方式
为了便于理解本发明，下文将结合说明书附图和较佳的实施例对本发明作更全面、细致地描述，但本发明的保护范围并不限于以下具体的实施例。
实施例1，一种复杂地下环境机器人定位和误差缓解方法，如图1所示，包括以下步骤：
S1：假设k时刻为当前时刻，假设机器人定位系统的状态向量为进一步得到考虑过程噪声的系统状态方程：
式中，xk和yk为机器人在k时刻的Xs轴和Ys坐标，Δt为机器人从上一时刻k-1运动至当前时刻k的时间间隔，和/>为机器人在k时刻的Xs轴和Ys轴的速度，ax,k-1和ay,k-1为机器人在k-1时刻沿Xs轴和Ys坐标的加速度，θk和θk-1为机器人在k时刻和k-1时刻的偏航角，ωk-1为机器人在k-1时刻偏航角的转速，Wk＝[w1k w2k w3k w4k w5k]T为过程噪声；
将式(1)改成矩阵形式：
Xk＝FXk-1+BUk-1+Wk (2)
式中，F为状态转移矩阵，Uk-1为k-1时刻惯性导航单元的量测值，/>B为控制矩阵，/> Qk为系统噪声方差矩阵；
S2：初始化：
式中，X0为系统状态的初始值，为系统状态的初始估计值，P0为误差协方差矩阵的初值；
S3：进行分布特征点采样和权重计算：
式中，χ0,k-1、χi,k-1和χi+n,k-1为采样的分布特征点，为k-1时刻系统状态向量估计，Px为k-1时刻的协方差矩阵，当k＝1时，Px＝P0，Wi m和Wi c为均值加权系数和方差加权系数，ρ为调整逼近精度的尺度参数，以降低预测误差，ρ＝α2(n+κ)-n，α决定分布特征点在均值附近的分布状态，调节高阶项的影响，经验范围在0～1之间，β为状态分布参数，可调节方差精度，对于高斯分布，β＝2最优，n为系统状态向量的维数，κ为待选尺度参数，需要使得(n+ρ)Px正定，α、β和κ三个参数的选择对应不同的分布特征点构造策略，以获得不同的转换精度和成本；
S4：通过非线性函数预测下一时刻的分布特征点：
式中，F(x)为非线性函数；
对应的加权预测状态和协方差分别为：
式中，为加权预测值，/>为加权预测值和预测值之间的协方差；
S5：测量预测分布特征点的值：
式中，H(x)为量测函数，为测量预测值；
对应的加权测量预测值以及对应的协方差可分别计算为：
式中，为加权测量预测值，/>为加权测量预测值和测量预测值之间的协方差，Rk为观测噪声；
S6：计算预测状态和预测测量值的互协方差：
S7：计算增益；
S8：得到最后的状态估计量和后验协方差：
S9：通过变方差模型，基于观测值和系统状态之间的马氏距离的平方，本发明引入了抗差因子，以应对测量离群值的干扰。当观测值受到离群值的污染，即当超宽带测距值为非视距，本发明通过增加观察噪声协方差来控制离群值对定位结果的影响，抗差因子的表达如下：
式中，η观测值受离群值污染的分界线值，Yk为系统观测值，τ为常量，用于调整抗差因子精度，为系统观测值和加权测量预测值/>的协方差，/>
S10：根据变方差模型结果更新步骤S5中观测协方差：
本发明还提供了一种复杂地下环境机器人定位中滤波器观测值的置信度融合策略，如图2系统框图所示，采用方差对系统各信息源的置信度进行评估并分配权重，其中，系统观测值Yk的更新策略如下：
式中，p为超宽带观测值的融合权重，Yk UWB为k时刻超宽带的观测值，为k时刻惯性导航单元的观测值；
权重p的计算方式如下：
p＝σimu/(σuwb+σimu) (19)
式中，σuwb为超宽带设备定位结果的方差和σimu为惯性导航单元定位结果的方差；
所述超宽带设备定位结果的方差σuwb和惯性导航单元定位结果的方差σimu的计算方式如下：
式中，[xuwb,1,xuwb,2,...,xuwb,n]为时间间隔Δt内通过最小二乘算法得到的超宽带设备定位结果，为在时间间隔Δt内存储n个超宽带定位结果的均值，即/>[ximu,1,ximu,2,...,ximu,m]为时间间隔Δt内通过式(1)航迹推算得到的惯性导航单元定位结果，/>为在时间间隔Δt内采集m个航迹推算得到的惯性导航单元定位结果的均值，即
进一步，所述复杂地下环境机器人定位和误差缓解方法，其中步骤S9中观测值是否被离群值污染，即超宽带测距值属于非视距或者视距条件的判断，通过Andersondarling检验和马氏距离判断两步进行，如图3所示，具体包含以下步骤：
S91：假设标签从第l个基站接受到n个不同的测距值，得到测距值集为并在此基础上计算Anderson darling统计量AD_UWB：
式中， 代表标准的正态分布函数；
S92：得到初步的非视距测距值判断条件为：
式中，
S93：对初步判定为视距的测距值，结合惯性导航单元采集的数据进行进一步判断，首先求算系统状态观测值到测量预测值的统计值：
式中，是判定为视距部分的系统状态观测值/>到k时刻加权测量预测值/>的马氏距离的平方，/>为判定为视距部分的测距值与对应同一时刻惯性导航单元测量结果结合的系统状态观测值，/>为k时刻的加权测量预测值；
S94：对初步判定为视距的系统状态向量进行细分判断：
式中，η为估计分布模型中0.05显著性水平下的取值，与式(15)中的大小相同；
如图4所示为本实施例复杂地下环境机器人定位和误差缓解方法的测试场景，当基站l与标签测距值出现非视距情况时，令否则令/>测试得到21个定位点的定位结果如图5(a)所示，可以发现大部分估计结果都在真实标签位置附近，极少数出现较大偏移，对所有误差进行统计后，得到在95％情况下，定位的误差均小于0.156m，定位误差的求算公式如下：
式中，为估计标签坐标结果，[ut,x ut,y]为真实标签坐标。
在前述说明书与相关附图中存在的教导的帮助下，本发明所属领域的技术人员将会想到本发明的许多修改和其它实施方案。因此，要理解的是，本发明不限于公开的具体实施方案，修改和其它实施方案被认为包括在所附权利要求的范围内。尽管本文中使用了特定术语，它们仅以一般和描述性意义使用，而不用于限制。

Claims (3)

1.一种复杂地下环境机器人定位和误差缓解方法，其特征在于：通过系统状态方程对机器人未来状态进行预测，在离群值检测后，利用贝叶斯和变方差模型对测量值进行修正，具体包括以下步骤：
S1：假设k时刻为当前时刻，假设机器人定位系统的状态向量为进一步得到考虑过程噪声的系统状态方程：
式中，xk和yk为机器人在k时刻的Xs轴和Ys坐标，Δt为机器人从上一时刻k-1运动至当前时刻k的时间间隔，和/>为机器人在k时刻的Xs轴和Ys轴的速度，ax,k-1和ay,k-1为机器人在k-1时刻沿Xs轴和Ys坐标的加速度，θk和θk-1为机器人在k时刻和k-1时刻的偏航角，ωk-1为机器人在k-1时刻偏航角的转速，Wk＝[w1k w2k w3k w4k w5k]T为过程噪声；
将式(1)改成矩阵形式：
Xk＝FXk-1+BUk-1+Wk (2)
式中，F为状态转移矩阵，Uk-1为k-1时刻惯性导航单元的量测值，/>B为控制矩阵，/> Qk为系统噪声方差矩阵；
S2：初始化：
式中，X0为系统状态的初始值，为系统状态的初始估计值，P0为误差协方差矩阵的初值；
S3：进行分布特征点采样和权重计算：
式中，χ0,k-1、χi,k-1和χi+n,k-1为采样的分布特征点，为k-1时刻系统状态向量估计，Px为k-1时刻的协方差矩阵，当k＝1时，Px＝P0，Wi m和Wi c为均值加权系数和方差加权系数，ρ为调整逼近精度的尺度参数，以降低预测误差，ρ＝α2(n+κ)-n，α决定分布特征点在均值附近的分布状态，调节高阶项的影响，经验范围在0～1之间，β为状态分布参数，可调节方差精度，对于高斯分布，β＝2最优，n为系统状态向量的维数，κ为待选尺度参数，需要使得(n+ρ)Px正定，α、β和κ三个参数的选择对应不同的分布特征点构造策略，以获得不同的转换精度和成本；
S4：通过非线性函数预测下一时刻的分布特征点：
式中，Fa(x)为非线性函数；
对应的加权预测状态和协方差分别为：
式中，为加权预测值，/>为加权预测值和预测值之间的协方差；
S5：测量预测分布特征点的值：
式中，H(x)为量测函数，为测量预测值；
对应的加权测量预测值以及对应的协方差可分别计算为：
式中，为加权测量预测值，/>为加权测量预测值和测量预测值之间的协方差，Rk为观测协方差；
S6：计算预测状态和预测测量值的互协方差：
S7：计算增益；
S8：得到最后的状态估计量和后验协方差：
S9：通过变方差模型，基于观测值和系统状态之间马氏距离的平方，本发明引入了抗差因子，以应对测量离群值的干扰。当观测值受到离群值的污染，即当超宽带测距值为非视距，本发明通过增加观察噪声协方差来控制离群值对定位结果的影响，抗差因子的表达如下：
式中，η观测值受离群值污染的分界线值，Yk为系统观测值，τ为常量，用于调整抗差因子精度，为系统观测值和加权测量预测值/>的协方差，/>
S10：根据变方差模型结果更新步骤S5中观测协方差：
2.一种复杂地下环境机器人定位中滤波器观测值的置信度融合策略，其特征在于：采用方差对系统各信息源的置信度进行评估并分配权重，其中，系统观测值Yk的更新策略如下：
式中，p为超宽带观测值的融合权重，为k时刻超宽带的观测值，/>为k时刻惯性导航单元的观测值；
权重p的计算方式如下：
p＝σimu/(σuwb+σimu) (19)
式中，σuwb为超宽带设备定位结果的方差和σimu为惯性导航单元定位结果的方差；
所述超宽带设备定位结果的方差σuwb和惯性导航单元定位结果的方差σimu的计算方式如下：
式中，[xuwb,1,xuwb,2,...,xuwb,n]为时间间隔Δt内通过最小二乘算法得到的超宽带设备定位结果，为在时间间隔Δt内存储n个超宽带定位结果的均值，即/>[ximu,1,ximu,2,...,ximu,m]为时间间隔Δt内通过式(1)航迹推算得到的惯性导航单元定位结果，/>为在时间间隔Δt内采集m个航迹推算得到的惯性导航单元定位结果的均值，即
3.根据权利要求1所述的一种复杂地下环境机器人定位和误差缓解方法，其特征在于：步骤S9中观测值是否被离群值污染，即超宽带测距值属于非视距或者视距条件的判断，通过Anderson darling检验和马氏距离判断两步进行，具体包含以下步骤：
S91：假设标签从第个基站接受到n个不同的测距值，得到测距值集为并在此基础上计算Anderson darling统计量AD_UWB：
式中，代表标准的正态分布函数；
S92：得到初步的非视距测距值判断条件为：
式中，
S93：对初步判定为视距的测距值，结合惯性导航单元采集的数据进行进一步判断，首先求算系统状态观测值到测量预测值的统计值：
式中，是判定为视距部分的系统状态观测值/>到k时刻加权测量预测值/>的马氏距离的平方，/>为判定为视距部分的测距值与对应同一时刻惯性导航单元测量结果结合的系统状态观测值，/>为k时刻的加权测量预测值；
S94：对初步判定为视距的系统状态向量进行细分判断：
式中，η为估计分布模型中0.05显著性水平下的取值，与式(15)中的大小相同。
CN202311469705.3A 2023-11-07 2023-11-07 一种复杂地下环境机器人定位和误差缓解方法 Pending CN117518077A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311469705.3A CN117518077A (zh) 2023-11-07 2023-11-07 一种复杂地下环境机器人定位和误差缓解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311469705.3A CN117518077A (zh) 2023-11-07 2023-11-07 一种复杂地下环境机器人定位和误差缓解方法

Publications (1)

Publication Number Publication Date
CN117518077A true CN117518077A (zh) 2024-02-06

Family

ID=89754305

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311469705.3A Pending CN117518077A (zh) 2023-11-07 2023-11-07 一种复杂地下环境机器人定位和误差缓解方法

Country Status (1)

Country Link
CN (1) CN117518077A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118034304A (zh) * 2024-03-05 2024-05-14 广州市东鼎智能装备有限公司 一种基于实时建模的机器人路径规划方法及系统

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118034304A (zh) * 2024-03-05 2024-05-14 广州市东鼎智能装备有限公司 一种基于实时建模的机器人路径规划方法及系统

Similar Documents

Publication Publication Date Title
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统
CN110503071B (zh) 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法
US10605925B2 (en) Processing GPS drifting
CN112612862B (zh) 一种基于点云配准的栅格地图定位方法
CN108983271B (zh) 基于rtk-gps/ins列车组合定位方法
CN107255795A (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN117518077A (zh) 一种复杂地下环境机器人定位和误差缓解方法
CN109752690B (zh) 无人机定位nlos的消除方法、系统、装置及存储介质
US11047708B2 (en) Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder
CN112702699B (zh) 一种融合UWB和LiDAR的室内定位方法
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
CN110174907A (zh) 一种基于自适应卡尔曼滤波的人体目标跟随方法
CN113280821B (zh) 基于斜率约束和回溯搜索的水下多目标跟踪方法
CN112540345B (zh) 一种基于高斯分布检测uwb质量的双模型定位方法及系统
CN117518076B (zh) 一种管道内机器人定位的非视距误差识别和缓解方法
CN112710306B (zh) 列车用bds与ins组合导航的自定位方法
CN116772837A (zh) 基于交互式多模型的gnss/sins组合导航方法
CN115334447B (zh) 一种基于自适应imm的鲁棒室内行人跟踪uwb定位方法
CN114548159B (zh) 信号干扰下的超宽带精确定位方法
CN116047480A (zh) 一种激光雷达到姿态传感器的外参标定方法
CN113866754A (zh) 基于高斯分布波门的运动目标航迹关联方法
KR102124763B1 (ko) Wi-Fi 환경에서 칼만필터와 입자필터를 혼용한 이동물체 위치추적 방법
CN112859126B (zh) 一种基于ufir滤波器的gnss定位漂移处理方法
CN116456460B (zh) 用于tdoa室内定位的滤波预处理方法
CN114485639B (zh) 一种用于室内导航的uwb定位漂移校正方法

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