WO2021082790A1 - 一种基于IMU的uwb定位异常值处理方法 - Google Patents

一种基于IMU的uwb定位异常值处理方法 Download PDF

Info

Publication number
WO2021082790A1
WO2021082790A1 PCT/CN2020/116100 CN2020116100W WO2021082790A1 WO 2021082790 A1 WO2021082790 A1 WO 2021082790A1 CN 2020116100 W CN2020116100 W CN 2020116100W WO 2021082790 A1 WO2021082790 A1 WO 2021082790A1
Authority
WO
WIPO (PCT)
Prior art keywords
uwb
value
time
abnormal
data
Prior art date
Application number
PCT/CN2020/116100
Other languages
English (en)
French (fr)
Inventor
鲁仁全
孟伟
连仕康
薛佳文
Original Assignee
广东工业大学
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 广东工业大学 filed Critical 广东工业大学
Priority to JP2020554405A priority Critical patent/JP7055484B2/ja
Publication of WO2021082790A1 publication Critical patent/WO2021082790A1/zh

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
    • 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
    • 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
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Definitions

  • the present invention relates to the technical field of uwb positioning, and in particular to an IMU-based method for processing abnormal values of uwb positioning.
  • Uwb positioning technology is a wireless carrier communication technology that uses a frequency bandwidth of 1 GHz or more. It does not use a sinusoidal carrier, but uses nanosecond non-sine wave narrow pulses to transmit data, so it occupies a large spectrum range, although it uses Wireless communication, but its data transmission rate can reach hundreds of megabits per second. Because uwb has the characteristics of high-speed data transmission, low power consumption, high security, and high positioning accuracy, it is widely used in indoor positioning in various fields.
  • the application background of uwb of this patent is to provide high-precision positioning data for indoor drones. Due to the complexity of the indoor environment and the differences of each uwb hardware, the original data of the uwb ranging input value jumps, is lost, and loses the real data. At this time, these abnormal data values need to be processed and filled to provide continuous positioning for uwb. And more accurate measurement data value.
  • the current way to deal with outliers is to delete data containing outliers, treat the outliers as missing values, hand them over to the missing value processing method, use the average to correct them, or not to deal with them, etc.
  • the value method has certain defects in the process of processing uwb positioning data. For example, the method of using the mean value to correct. When the drone is flying with a large acceleration, at this time, when the abnormal value is replaced by the mean value, the data is distorted.
  • the processing method is the estimation of the latter algorithm, which brings the influence of error.
  • the purpose of the present invention is to overcome the shortcomings of the prior art and provide an IMU-based uwb positioning abnormal value processing method, taking four uwb base stations for uwb positioning as an example, to solve the existing abnormal value discrimination in the original data preprocessing process Problems such as inaccuracy and the accuracy of outlier correction improve the accuracy of measurement hardware data.
  • the invention does not require additional hardware equipment, enhances the stability of the uwb sensor's measurement data in a complex environment, and at the same time provides effective and accurate measurement output data for the subsequent uwb positioning solution, and improves the stability, rapidity and real-time of the uwb positioning Sex.
  • An IMU-based uwb location abnormal value processing method including the following steps:
  • step S3 According to the attitude angle of the UAV obtained in step S2, calculate the acceleration of the X, Y, and Z axes and the displacement on the X, Y, and Z axes within ⁇ t, so as to estimate the UAV's displacement value Q;
  • step S4 Perform abnormal detection on the uwb measurement value according to the displacement value Q obtained in step S3 and the distance data of the uwb sensor; if the uwb measurement value is abnormal, proceed to step S5;
  • step S2 is as follows:
  • the drift error b g is subtracted, and the white noise measured by the gyroscope w g is subtracted.
  • the drift noise can be modeled as Gaussian white noise, Is the noise of the gyroscope;
  • T s is the sampling time
  • E(X 0 ) represents the mathematical expectation value at zero time
  • E[(X 0 -E(X 0 ))(X 0 -E(X 0 )) T ] represents the error covariance at zero time
  • k-1 ⁇ k-1 P k-1
  • H k [O 3 ⁇ 3 -gI]
  • R T is the measurement noise variance matrix
  • k X k
  • Z is the observation data
  • I is the identity matrix, and the precise attitude angle is obtained according to the state vector X k.
  • step S3 the specific process of X, Y, Z axis acceleration calculation in step S3 is as follows:
  • x k (i,j) represents the data in the i-th row and j-th column of x k , and the pitch angle ⁇ and roll angle ⁇ can be obtained; since the flying speed of the UAV is very small, its instantaneous acceleration can be ignored Don't count, that is, the measured acceleration is near g gravitational acceleration, according to the measured acceleration modulus a b :
  • P t is the position calculated by the uwb positioning system at time t
  • the average speed at the previous moment is regarded as the current The speed of time; and because of Approaching zero within ⁇ t time, that is:
  • a is the acceleration in the navigation coordinate system of the precise acceleration obtained by the IMU solution, and t is the system running time. According to the above formula, the estimated displacement s t + ⁇ s at time t+ ⁇ t can be obtained;
  • ⁇ s 1 , ⁇ s 2 , and ⁇ s 3 are the displacements of the X, Y, and Z axes in the navigation coordinate system within the sampling time ⁇ t, respectively.
  • calculation formula for the displacement value Q of the drone in the step S3 is as follows:
  • x, y, z are the positioning values measured last time, Is the position value of the *th base station.
  • the formula is among them They are the current measured distance value of uwb and the measured value at the last moment, Q is the displacement value within ⁇ t, mean is the standard deviation of n measured data, and k is the specific gravity parameter, which needs to be adjusted according to specific environmental parameters; Satisfaction See To measure outliers.
  • step S5 corrects the uwb measurement value
  • the formula is as follows:
  • std is the average value of n measurement data.
  • This scheme integrates the UAV's own IMU sensor to predict the displacement within ⁇ t of the UAV, which can well predict the fluctuation range of the distance value measured by uwb, and can accurately determine whether the data is abnormal.
  • this solution can more accurately correct the abnormal value and better restore the original data value.
  • Fig. 1 is a flow chart of the principle of a UWB positioning abnormal value processing method based on IMU of the present invention.
  • the method for processing abnormal values of uwb positioning based on IMU in this embodiment includes the following steps:
  • the drift error b g is subtracted, and the white noise measured by the gyroscope w g is subtracted.
  • the drift noise can be modeled as Gaussian white noise, Is the noise of the gyroscope;
  • T s is the sampling time
  • E(X 0 ) represents the mathematical expectation value at the zero time
  • E[(X 0 -E(X 0 ))(X 0 -E(X 0 )) T ] represents the calculation of the error covariance at the zero time
  • k-1 ⁇ k-1 P k-1
  • H k [O 3 ⁇ 3 -gI]
  • R T is the measurement noise variance matrix
  • k X k
  • Z is the observation data
  • I is the identity matrix, and the precise attitude angle is obtained according to the state vector X k.
  • step S3 Calculate the acceleration of the X, Y, and Z axes according to the attitude angle of the UAV obtained in step S2:
  • x k (i,j) represents the data in the i-th row and j-th column of x k , and the pitch angle ⁇ and roll angle ⁇ can be obtained; since the flying speed of the UAV is very small, its instantaneous acceleration can be ignored Don't count, that is, the measured acceleration is near g gravitational acceleration, according to the measured acceleration modulus a b :
  • P t is the position calculated by the uwb positioning system at time t
  • the average speed at the previous moment is regarded as the current The speed of time; and because of Approaching zero within ⁇ t time, that is:
  • a is the acceleration in the navigation coordinate system of the precise acceleration obtained by the IMU solution, and t is the system running time. According to the above formula, the estimated displacement s t + ⁇ s at time t+ ⁇ t can be obtained;
  • ⁇ s 1 , ⁇ s 2 , and ⁇ s 3 are the displacements of the X, Y, and Z axes in the navigation coordinate system within the sampling time ⁇ t, respectively.
  • x, y, z are the positioning values measured last time, Is the position value of the *th base station.
  • step S4 Perform abnormal detection on the uwb measurement value according to the displacement value Q obtained in step S3 and the distance data of the uwb sensor; if the uwb measurement value is abnormal, proceed to step S5;
  • the formula is among them They are the current measured distance value of uwb and the measured value at the last moment, Q is the displacement value within ⁇ t, mean is the standard deviation of n measured data, and k is the specific gravity parameter, which needs to be adjusted according to specific environmental parameters; Satisfaction See To measure outliers.
  • std is the average value of n measurement data.
  • This embodiment uses four uwb base stations to perform uwb positioning as an example, which solves the problems of inaccurate determination of abnormal values and accuracy of abnormal value correction in the existing raw data preprocessing process, and improves the accuracy of measurement hardware data. Moreover, this embodiment does not require additional hardware equipment, which enhances the stability of the uwb sensor's measurement data in a complex environment, and at the same time provides effective and accurate measurement output data for the subsequent uwb positioning solution, and improves the stability and rapidity of uwb positioning. And real-time.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

一种基于IMU的uwb定位异常值处理方法，包括步骤：S1.获取无人机的IMU数据；S2.将加速度计和陀螺仪进行融合，计算无人机的姿态角度；S3.依据步骤S2得到的无人机的姿态角度，计算X、Y、Z轴的加速度以及△t时间内X、Y、Z轴上的位移，估计出无人机的位移值Q；S4.依据步骤S3得到的位移值Q以及uwb传感器的距离数据，对uwb测量值进行异常检测；若uwb测量值异常，进入步骤S5；S5.对异常的uwb测量值进行数据融合，修正uwb测量值。可解决现有的原始数据预处理过程中的异常值判别不准确以及异常值修正的精度地等问题，提高测量硬件数据准确度。同时不需要外加额外硬件设备，增强了uwb传感器在复杂环境下测量数据的稳定性，同时为后面的uwb定位解算提供有效精确的测量输出数据，提高uwb定位的稳定性，快速性以及实时性。

Description

一种基于IMU的uwb定位异常值处理方法
本申请要求2019年10月29日提交中国专利局、申请号为201911038648.7、发明名称为“一种基于IMU的uwb定位异常值处理方法”的发明专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本发明涉及uwb定位的技术领域，尤其涉及到一种基于IMU的uwb定位异常值处理方法。
背景技术
Uwb定位技术是一种使用1GHz以上频率带宽的无线载波通信技术，它不采用正弦载波，而是利用纳秒级的非正弦波窄脉冲传输数据，因此其所占的频谱范围很大，尽管使用无线通信，但其数据传输速率可以达到几百兆比特每秒以上。由于uwb具有高速的数据传输、低功耗、安全性高、定位精度高的特点被广泛应用于各个领域室内定位中，本专利uwb应用背景在于给室内无人机提供高精度的定位数据。由于室内环境的复杂以及各个uwb硬件差异性，导致uwb测距输入值的原始数据发生跳变，丢失，失去真实数据，此时即需要对这些异常数据值进行处理、填充，为uwb定位提供连续而且较为准确的测量数据值。
目前对异常值常处理的方式是删除含有异常值的数据、将异常值视为缺失值，交给缺失值处理方法来处理、用平均值来修正或者是不做处理等，然而以上这些处理异常值的方法在处理uwb定位数据过程中均有定的缺陷，例如使用均值来修正的方法，当无人机进行较大加 速度飞行，此时当异常值用均值代替明显地到时数据失真而其他处理方法均为后面算法作估计带来误差影响。
发明内容
本发明的目的在于克服现有技术的不足，提供一种基于IMU的uwb定位异常值处理方法，以四个uwb基站进行uwb定位为例，解决现有的原始数据预处理过程中的异常值判别不准确以及异常值修正的精度地等问题，提高了测量硬件数据准确度。本发明不需要外加额外硬件设备，增强了uwb传感器在复杂环境下测量数据的稳定性，同时为后面的uwb定位解算提供有效精确的测量输出数据，提高uwb定位的稳定性，快速性以及实时性。
为实现上述目的，本发明所提供的技术方案为：
一种基于IMU的uwb定位异常值处理方法，包括以下步骤：
S1.获取无人机的IMU数据；
S2.将加速度计和陀螺仪进行融合，计算无人机的姿态角度；
S3.依据步骤S2得到的无人机的姿态角度，计算X、Y、Z轴的加速度以及△t时间内X、Y、Z轴上的位移，从而估计出无人机的位移值Q；
S4.依据步骤S3得到的位移值Q以及uwb传感器的距离数据，对uwb测量值进行异常检测；若uwb测量值异常，则进入步骤S5；
S5.对异常的uwb测量值进行数据融合，修正uwb测量值。
进一步地，所述步骤S2的具体过程如下：
估计姿态角度以及陀螺仪的漂移，即令：
Figure PCTCN2020116100-appb-000001
其中，
Figure PCTCN2020116100-appb-000002
b g,k为陀螺仪的漂移量,θ,φ分别为俯仰角，横滚角；
又由于
Figure PCTCN2020116100-appb-000003
ω b为无人机角速度，R为机体坐标系到大地坐标系的旋转矩阵，提取出第三列，即：
Figure PCTCN2020116100-appb-000004
又
Figure PCTCN2020116100-appb-000005
其中，减去漂移误差b g，再减去陀螺仪测量白噪声w g，漂移噪声建模可为高斯白噪声，
Figure PCTCN2020116100-appb-000006
为陀螺仪的噪声；
通过离散化后,得到：
Figure PCTCN2020116100-appb-000007
其中，T s为采样时间；
通过简化分离出状态变量得：
Figure PCTCN2020116100-appb-000008
简化：X k＝Φ k-1X k-1+Γ k-1w k-1；
其中：
k-1时刻的系统转移矩阵：
Figure PCTCN2020116100-appb-000009
k-1时刻系统噪声矩阵：
Figure PCTCN2020116100-appb-000010
k-1时刻状态向量：
Figure PCTCN2020116100-appb-000011
k-1时刻系统噪声：
Figure PCTCN2020116100-appb-000012
观测模型：
Figure PCTCN2020116100-appb-000013
其中：
Figure PCTCN2020116100-appb-000014
Figure PCTCN2020116100-appb-000015
K时刻观测矩阵：
Figure PCTCN2020116100-appb-000016
观测噪声：
Figure PCTCN2020116100-appb-000017
综上所述，简化后的过程模型与观测模型如下：
Figure PCTCN2020116100-appb-000018
得到过程模型与观测模型后，进行初始化
Figure PCTCN2020116100-appb-000019
其中，E(X 0)表示零时刻的数学期望值，E[(X 0-E(X 0))(X 0-E(X 0)) T]表示计算零时刻的误差协方差；
状态估计预测：
X k|k-1＝Φ k-1X k-1；
误差协方差预测：
P k|k-1＝Φ k-1P k-1|k-1Φ k-1 T+Γ k-1Q k-1Γ k-1 T，Q k-1表示系统噪声方差矩阵；
卡尔曼增益矩阵：
Figure PCTCN2020116100-appb-000020
其中：H k＝[O 3×3 -gI]，R T为测量噪声方差矩阵；
状态估计更新：
X k|k＝X k|k-1+K k(Z-H kX k|k-1)；
其中：Z为观测数据；
误差协方差更新：
P k|k＝(I-K kH k)P k|k-1；
I为单位矩阵，根据状态向量X k从而得到精确的姿态角度。
进一步地，所述步骤S3中X、Y、Z轴加速度计算的具体过程如下：
由于
Figure PCTCN2020116100-appb-000021
又
Figure PCTCN2020116100-appb-000022
即：
Figure PCTCN2020116100-appb-000023
其中，x k(i,j)表示x k的第i行第j列的数据，即可求得俯仰角θ和横滚角φ；由于无人机飞行速度很小，即其瞬时加速度可忽略不计，即所测加速度在g重力加速度附近，根据测量的加速度模值a b：
即得
Figure PCTCN2020116100-appb-000024
其中
Figure PCTCN2020116100-appb-000025
为机体坐标系转到大地坐标系下的旋转矩阵。
进一步地，所述步骤S3中△t时间内X、Y、Z轴上的位移的计算过程如下：
根据公式：
Figure PCTCN2020116100-appb-000026
即在采样时间为△t时刻的位移：
Figure PCTCN2020116100-appb-000027
其中v t为当前速度：v t＝(P t-P t-1)/Δt为三维向量，P t为t时刻的uwb定位系统经过位置解算的位置，视前一时刻的平均速度为当前时间的速度；又由于
Figure PCTCN2020116100-appb-000028
在Δt时间内趋近于零，即：
Δs＝v tΔt+atΔt；
式中，a为通过IMU解算所得的精确加速度在导航坐标系下的加速度，t为系统运行时间，根据上式可得t+Δt时刻的估算位移s t+Δs；
其中，
Figure PCTCN2020116100-appb-000029
Δs 1，Δs 2，Δs 3分别为采样时间Δt内导航坐标系下X，Y，Z轴的位移。
进一步地，所述步骤S3中无人机的位移值Q的计算公式如下：
Figure PCTCN2020116100-appb-000030
其中，
Figure PCTCN2020116100-appb-000031
x,y,z为上次所测的定位值，
Figure PCTCN2020116100-appb-000032
为第*个基站的位置值。
进一步地，所述步骤S4中，检测判断异常值时，公式为
Figure PCTCN2020116100-appb-000033
其中
Figure PCTCN2020116100-appb-000034
分别为uwb的当前测量距离值和上一时刻的测量值，Q为Δt时间内的位移值，mean为n个测量数据的标准方差值，k为比重参数，需要根据具体环境参数调整；当满足式子
Figure PCTCN2020116100-appb-000035
则视
Figure PCTCN2020116100-appb-000036
为测量异常值。
进一步地，所述步骤S5修正uwb测量值
Figure PCTCN2020116100-appb-000037
的公式如下：
Figure PCTCN2020116100-appb-000038
其中，std为n个测量数据的平均值。
与现有技术相比，本方案原理及优点如下：
1.本方案融合无人机自带的IMU传感器来预测无人机Δt时间内的 位移，能很好地预测到uwb测量的距离值的波动范围，能准确地判别出数据是否为异常。
2.本方案在判断出异常值数据以及估计出无人机在Δt时间内的位移的基础上，能更加准确地对异常值进行修正，更好地恢复原始数据值。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的服务作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一种基于IMU的uwb定位异常值处理方法的原理流程图。
具体实施方式
下面结合具体实施例对本发明作进一步说明：
如图1所示，本实施例所述的一种基于IMU的uwb定位异常值处理方法，包括以下步骤：
S1.获取无人机的IMU数据；
S2.将加速度计和陀螺仪进行融合，计算无人机的姿态角度，具体如下：
估计姿态角度以及陀螺仪的漂移，即令：
Figure PCTCN2020116100-appb-000039
其中，
Figure PCTCN2020116100-appb-000040
b g,k为陀螺仪的漂移量,θ,φ分别为俯仰角，横滚角；
又由于
Figure PCTCN2020116100-appb-000041
ω b为无人机角速度，R为机体坐标系到大地坐标系的旋转矩阵，提取出第三列，即：
Figure PCTCN2020116100-appb-000042
又
Figure PCTCN2020116100-appb-000043
其中，减去漂移误差b g，再减去陀螺仪测量白噪声w g，漂移噪声建模可为高斯白噪声，
Figure PCTCN2020116100-appb-000044
为陀螺仪的噪声；
通过离散化后,得到：
Figure PCTCN2020116100-appb-000045
其中，T s为采样时间；
通过简化分离出状态变量得：
Figure PCTCN2020116100-appb-000046
简化：X k＝Φ k-1X k-1+Γ k-1w k-1；
其中：
k-1时刻的系统转移矩阵：
Figure PCTCN2020116100-appb-000047
k-1时刻系统噪声矩阵：
Figure PCTCN2020116100-appb-000048
k-1时刻状态向量：
Figure PCTCN2020116100-appb-000049
k-1时刻系统噪声：
Figure PCTCN2020116100-appb-000050
观测模型：
Figure PCTCN2020116100-appb-000051
其中：
Figure PCTCN2020116100-appb-000052
Figure PCTCN2020116100-appb-000053
K时刻观测矩阵：
Figure PCTCN2020116100-appb-000054
观测噪声：
Figure PCTCN2020116100-appb-000055
综上所述，简化后的过程模型与观测模型如下：
Figure PCTCN2020116100-appb-000056
得到过程模型与观测模型后，进行初始化
Figure PCTCN2020116100-appb-000057
其中：E(X 0)表示零时刻的数学期望值，E[(X 0-E(X 0))(X 0-E(X 0)) T]表示计算零时刻的误差协方差；
状态估计预测：
X k|k-1＝Φ k-1X k-1；
误差协方差预测：
P k|k-1＝Φ k-1P k-1|k-1Φ k-1 T+Γ k-1Q k-1Γ k-1 T，Q k-1表示系统噪声方差矩阵；
卡尔曼增益矩阵：
Figure PCTCN2020116100-appb-000058
其中：H k＝[O 3×3 -gI]，R T为测量噪声方差矩阵；
状态估计更新：
X k|k＝X k|k-1+K k(Z-H kX k|k-1)；
其中：Z为观测数据；
误差协方差更新：
P k|k＝(I-K kH k)P k|k-1；
I为单位矩阵，根据状态向量X k从而得到精确的姿态角度。
S3.依据步骤S2得到的无人机的姿态角度，计算X、Y、Z轴的加速度：
由于
Figure PCTCN2020116100-appb-000059
又
Figure PCTCN2020116100-appb-000060
即：
Figure PCTCN2020116100-appb-000061
其中，x k(i,j)表示x k的第i行第j列的数据，即可求得俯仰角θ和横滚角φ；由于无人机飞行速度很小，即其瞬时加速度可忽略不计，即所测加速度在g重力加速度附近，根据测量的加速度模值a b：
即得
Figure PCTCN2020116100-appb-000062
其中
Figure PCTCN2020116100-appb-000063
为机体坐标系转到大地坐标系下的旋转矩阵。
计算△t时间内X、Y、Z轴上的位移：
根据公式：
Figure PCTCN2020116100-appb-000064
即在采样时间为△t时刻的位移：
Figure PCTCN2020116100-appb-000065
其中v t为当前速度：v t＝(P t-P t-1)/Δt为三维向量，P t为t时刻的uwb定位系统经过位置解算的位置，视前一时刻的平均速度为当前时间的速度；又由于
Figure PCTCN2020116100-appb-000066
在Δt时间内趋近于零，即：
Δs＝v tΔt+atΔt；
式中，a为通过IMU解算所得的精确加速度在导航坐标系下的加速度，t为系统运行时间，根据上式可得t+Δt时刻的估算位移s t+Δs；
其中，
Figure PCTCN2020116100-appb-000067
Δs 1，Δs 2，Δs 3分别为采样时间Δt内导航坐标系下X，Y，Z轴的位移。
估计出无人机的位移值Q，公式如下：
Figure PCTCN2020116100-appb-000068
其中，
Figure PCTCN2020116100-appb-000069
x,y,z为上次所测的定位值，
Figure PCTCN2020116100-appb-000070
为第*个基站的位置值。
S4.依据步骤S3得到的位移值Q以及uwb传感器的距离数据，对uwb测量值进行异常检测；若uwb测量值异常，则进入步骤S5；
检测判断异常值时，公式为
Figure PCTCN2020116100-appb-000071
其中
Figure PCTCN2020116100-appb-000072
分别为uwb的当前测量距离值和上一时刻的测量值，Q为Δt时间内的位移值，mean为n个测量数据的标准方差值，k为比重参数，需要根据具体环境参数调整；当满足式子
Figure PCTCN2020116100-appb-000073
则视
Figure PCTCN2020116100-appb-000074
为测量异常值。
S5.对异常的uwb测量值进行数据融合，修正uwb测量值；修正uwb测量值
Figure PCTCN2020116100-appb-000075
的公式如下：
Figure PCTCN2020116100-appb-000076
其中，std为n个测量数据的平均值。
本实施例以四个uwb基站进行uwb定位为例，解决了现有的原始数据预处理过程中的异常值判别不准确以及异常值修正的精度地等问题，提高了测量硬件数据准确度。而且本实施例不需要外加额外硬件设备，增强了uwb传感器在复杂环境下测量数据的稳定性，同时为后面的uwb定位解算提供有效精确的测量输出数据，提高uwb定位的稳定性，快速性以及实时性。
以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (7)

  1. 一种基于IMU的uwb定位异常值处理方法，其特征在于，包括以下步骤：
    S1.获取无人机的IMU数据；
    S2.将加速度计和陀螺仪进行融合，计算无人机的姿态角度；
    S3.依据步骤S2得到的无人机的姿态角度，计算X、Y、Z轴的加速度以及△t时间内X、Y、Z轴上的位移，从而估计出无人机的位移值Q；
    S4.依据步骤S3得到的位移值Q以及uwb传感器的距离数据，对uwb测量值进行异常检测；若uwb测量值异常，则进入步骤S5；
    S5.对异常的uwb测量值进行数据融合，修正uwb测量值。
  2. 根据权利要求1所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S2的具体过程如下：
    估计姿态角度以及陀螺仪的漂移，即令：
    Figure PCTCN2020116100-appb-100001
    其中，
    Figure PCTCN2020116100-appb-100002
    b g,k为陀螺仪的漂移量,θ,φ分别为俯仰角，横滚角；
    又由于
    Figure PCTCN2020116100-appb-100003
    ω b为无人机角速度，R为机体坐标系到大地坐标系的旋转矩阵，提取出第三列，即：
    Figure PCTCN2020116100-appb-100004
    又
    Figure PCTCN2020116100-appb-100005
    其中，减去漂移误差b g，再减去陀螺仪测量白噪声w g，漂移噪 声建模可为高斯白噪声，
    Figure PCTCN2020116100-appb-100006
    为陀螺仪的噪声；
    通过离散化后,得到：
    Figure PCTCN2020116100-appb-100007
    其中，T s为采样时间；
    通过简化分离出状态变量得：
    Figure PCTCN2020116100-appb-100008
    简化：X k＝Φ k-1X k-1+Γ k-1w k-1；
    其中：
    k-1时刻的系统转移矩阵：
    Figure PCTCN2020116100-appb-100009
    k-1时刻系统噪声矩阵：
    Figure PCTCN2020116100-appb-100010
    k-1时刻状态向量：
    Figure PCTCN2020116100-appb-100011
    k-1时刻系统噪声：
    Figure PCTCN2020116100-appb-100012
    观测模型：
    Figure PCTCN2020116100-appb-100013
    其中：
    Figure PCTCN2020116100-appb-100014
    Figure PCTCN2020116100-appb-100015
    K时刻观测矩阵：
    Figure PCTCN2020116100-appb-100016
    观测噪声：
    Figure PCTCN2020116100-appb-100017
    综上所述，简化后的过程模型与观测模型如下：
    Figure PCTCN2020116100-appb-100018
    得到过程模型与观测模型后，进行初始化
    Figure PCTCN2020116100-appb-100019
    其中，E(X 0)表示零时刻的数学期望值，E[(X 0-E(X 0))(X 0-E(X 0)) T]表示计算零时刻的误差协方差；
    状态估计预测：
    X k|k-1＝Φ k-1X k-1；
    误差协方差预测：
    P k|k-1＝Φ k-1P k-1|k-1Φ k-1 T+Γ k-1Q k-1Γ k-1 T，Q k-1表示系统噪声方差矩阵；
    卡尔曼增益矩阵：
    Figure PCTCN2020116100-appb-100020
    其中：H k＝[O 3×3-gI]，R T为测量噪声方差矩阵；
    状态估计更新：
    X k|k＝X k|k-1+K k(Z-H kX k|k-1)；
    其中：Z为观测数据；
    误差协方差更新：
    P k|k＝(I-K kH k)P k|k-1；
    I为单位矩阵，根据状态向量X k从而得到精确的姿态角度。
  3. 根据权利要求2所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S3中X、Y、Z轴加速度计算的具体过程如下：
    由于
    Figure PCTCN2020116100-appb-100021
    又
    Figure PCTCN2020116100-appb-100022
    即：
    Figure PCTCN2020116100-appb-100023
    其中，x k(i,j)表示x k的第i行第j列的数据，即可求得俯仰角θ和横滚角φ；由于无人机飞行速度很小，即其瞬时加速度可忽略不计，即所测加速度在g重力加速度附近，根据测量的加速度模值a b：
    即得
    Figure PCTCN2020116100-appb-100024
    其中
    Figure PCTCN2020116100-appb-100025
    为机体坐 标系转到大地坐标系下的旋转矩阵。
  4. 根据权利要求3所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S3中△t时间内X、Y、Z轴上的位移的计算过程如下：
    根据公式：
    Figure PCTCN2020116100-appb-100026
    即在采样时间为△t时刻的位移：
    Figure PCTCN2020116100-appb-100027
    其中v t为当前速度：v t＝(P t-P t-1)/Δt为三维向量，P t为t时刻的uwb定位系统经过位置解算的位置，视前一时刻的平均速度为当前时间的速度；又由于
    Figure PCTCN2020116100-appb-100028
    在Δt时间内趋近于零，即：
    Δs＝v tΔt+atΔt；
    式中，a为通过IMU解算所得的精确加速度在导航坐标系下的加速度，t为系统运行时间，根据上式可得t+Δt时刻的估算位移s t+Δs；
    其中，
    Figure PCTCN2020116100-appb-100029
    Δs 1，Δs 2，Δs 3分别为采样时间Δt内导航坐标系下X，Y，Z轴的位移。
  5. 根据权利要求4所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S3中无人机的位移值Q的计算公式如下：
    Figure PCTCN2020116100-appb-100030
    Figure PCTCN2020116100-appb-100031
    其中，
    Figure PCTCN2020116100-appb-100032
    x,y,z为上次所测的定位值，(a *1，a *2，a *3)为第*个基站的位置值。
  6. 根据权利要求5所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S4中，检测判断异常值时，公式为
    Figure PCTCN2020116100-appb-100033
    其中
    Figure PCTCN2020116100-appb-100034
    分别为uwb的当前测量距离值和上一时刻的测量值，Q为Δt时间内的位移值，mean为n个测量数据的标准方差值，k为比重参数，需要根据具体环境参数调整；当满足式子
    Figure PCTCN2020116100-appb-100035
    则视
    Figure PCTCN2020116100-appb-100036
    为测量异常值。
  7. 根据权利要求6所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S5修正uwb测量值
    Figure PCTCN2020116100-appb-100037
    的公式如下：
    Figure PCTCN2020116100-appb-100038
    其中，std为n个测量数据的平均值。
PCT/CN2020/116100 2019-10-29 2020-09-18 一种基于IMU的uwb定位异常值处理方法 WO2021082790A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2020554405A JP7055484B2 (ja) 2019-10-29 2020-09-18 ＩＭＵに基づくｕｗｂ測位異常値処理方法

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201911038648.7A CN110926460B (zh) 2019-10-29 2019-10-29 一种基于IMU的uwb定位异常值处理方法
CN201911038648.7 2019-10-29

Publications (1)

Publication Number Publication Date
WO2021082790A1 true WO2021082790A1 (zh) 2021-05-06

Family

ID=69849748

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/116100 WO2021082790A1 (zh) 2019-10-29 2020-09-18 一种基于IMU的uwb定位异常值处理方法

Country Status (3)

Country Link
JP (1) JP7055484B2 (zh)
CN (1) CN110926460B (zh)
WO (1) WO2021082790A1 (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113418493A (zh) * 2021-07-23 2021-09-21 广东工业大学 一种基于陀螺仪辅助测量伺服电机角度的方法
CN113569430A (zh) * 2021-08-31 2021-10-29 中国西安卫星测控中心 一种仅外测观测下再入飞行转弯方向辨识方法
CN113608166A (zh) * 2021-08-04 2021-11-05 燕山大学 一种基于多源信息融合的动物行为监测方法
CN113640738A (zh) * 2021-06-24 2021-11-12 西南科技大学 一种结合imu与单组uwb的旋转式目标定位方法
CN114166221A (zh) * 2021-12-08 2022-03-11 中国矿业大学 动态复杂矿井环境中辅助运输机器人定位方法及系统
CN115967971A (zh) * 2023-03-16 2023-04-14 长沙迪迈数码科技股份有限公司 井下uwb定位基站安装异常识别方法、装置及存储介质

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110926460B (zh) * 2019-10-29 2021-03-02 广东工业大学 一种基于IMU的uwb定位异常值处理方法
CN111890373A (zh) * 2020-09-29 2020-11-06 常州唯实智能物联创新中心有限公司 车载机械臂的感知定位方法
CN113120713B (zh) * 2021-04-28 2023-09-08 上海有个机器人有限公司 电梯定位方法
CN116321418B (zh) * 2023-03-02 2024-01-02 中国人民解放军国防科技大学 一种基于节点构型优选的集群无人机融合估计定位方法
CN116592880B (zh) * 2023-07-06 2023-11-17 中国科学院空天信息创新研究院 一种uwb_ins组合定位系统自主完好性检测方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104864865A (zh) * 2015-06-01 2015-08-26 济南大学 一种面向室内行人导航的ahrs/uwb无缝组合导航方法
CN109100768A (zh) * 2018-08-01 2018-12-28 南京科远自动化集团股份有限公司 一种综合定位方法及定位标签
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
CN109916410A (zh) * 2019-03-25 2019-06-21 南京理工大学 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN110926460A (zh) * 2019-10-29 2020-03-27 广东工业大学 一种基于IMU的uwb定位异常值处理方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010096647A (ja) 2008-10-17 2010-04-30 Mitsubishi Electric Corp 航法装置及び推定方法
CN107980100B (zh) 2015-03-07 2022-05-27 维里蒂工作室股份公司 分布式定位系统和方法以及自定位设备
CN108981689B (zh) * 2018-08-07 2022-06-14 河南工业大学 基于dsp tms320c6748的uwb/ins组合导航系统
CN109764865B (zh) * 2019-01-25 2022-11-04 北京交通大学 一种基于mems和uwb的室内定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104864865A (zh) * 2015-06-01 2015-08-26 济南大学 一种面向室内行人导航的ahrs/uwb无缝组合导航方法
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
CN109100768A (zh) * 2018-08-01 2018-12-28 南京科远自动化集团股份有限公司 一种综合定位方法及定位标签
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN109916410A (zh) * 2019-03-25 2019-06-21 南京理工大学 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN110926460A (zh) * 2019-10-29 2020-03-27 广东工业大学 一种基于IMU的uwb定位异常值处理方法

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113640738A (zh) * 2021-06-24 2021-11-12 西南科技大学 一种结合imu与单组uwb的旋转式目标定位方法
CN113418493A (zh) * 2021-07-23 2021-09-21 广东工业大学 一种基于陀螺仪辅助测量伺服电机角度的方法
CN113418493B (zh) * 2021-07-23 2024-02-27 广东工业大学 一种基于陀螺仪辅助测量伺服电机角度的方法
CN113608166A (zh) * 2021-08-04 2021-11-05 燕山大学 一种基于多源信息融合的动物行为监测方法
CN113569430A (zh) * 2021-08-31 2021-10-29 中国西安卫星测控中心 一种仅外测观测下再入飞行转弯方向辨识方法
CN113569430B (zh) * 2021-08-31 2023-07-04 中国西安卫星测控中心 一种仅外测观测下再入飞行转弯方向辨识方法
CN114166221A (zh) * 2021-12-08 2022-03-11 中国矿业大学 动态复杂矿井环境中辅助运输机器人定位方法及系统
CN115967971A (zh) * 2023-03-16 2023-04-14 长沙迪迈数码科技股份有限公司 井下uwb定位基站安装异常识别方法、装置及存储介质
CN115967971B (zh) * 2023-03-16 2023-05-12 长沙迪迈数码科技股份有限公司 井下uwb定位基站安装异常识别方法、装置及存储介质

Also Published As

Publication number Publication date
JP2022500617A (ja) 2022-01-04
CN110926460B (zh) 2021-03-02
JP7055484B2 (ja) 2022-04-18
CN110926460A (zh) 2020-03-27

Similar Documents

Publication Publication Date Title
WO2021082790A1 (zh) 一种基于IMU的uwb定位异常值处理方法
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN105589064B (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
US10322819B2 (en) Autonomous system for taking moving images from a drone, with target tracking and improved target location
CN106708066A (zh) 基于视觉/惯导的无人机自主着陆方法
CN111288989B (zh) 一种小型无人机视觉定位方法
CN105973238B (zh) 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN111983660A (zh) Gnss拒止环境下四旋翼无人机定位系统和方法
CN111890373A (zh) 车载机械臂的感知定位方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
CN114018254B (zh) 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN114397642A (zh) 一种基于图优化的三维激光雷达与imu外参标定方法
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
Pudovkin et al. Development and research of the rangefinder of the information and measurement system of air traffic control based on data from on-board sensors of the aircraft
EP3120164A1 (en) Producing data describing target measurements
Zhang et al. Research on UAV attitude data fusion algorithm based on quaternion gradient descent
Girrbach et al. Adaptive compensation of measurement delays in multi-sensor fusion for inertial motion tracking using moving horizon estimation

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2020554405

Country of ref document: JP

Kind code of ref document: A

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20882884

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20882884

Country of ref document: EP

Kind code of ref document: A1