CN113900061A - 基于uwb无线定位与imu融合的导航定位系统及方法 - Google Patents

基于uwb无线定位与imu融合的导航定位系统及方法 Download PDF

Info

Publication number
CN113900061A
CN113900061A CN202110597733.8A CN202110597733A CN113900061A CN 113900061 A CN113900061 A CN 113900061A CN 202110597733 A CN202110597733 A CN 202110597733A CN 113900061 A CN113900061 A CN 113900061A
Authority
CN
China
Prior art keywords
uwb
positioning
imu
label
data
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.)
Granted
Application number
CN202110597733.8A
Other languages
English (en)
Other versions
CN113900061B (zh
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.)
Shenzhen Yiaider Intelligent Technology Co ltd
Original Assignee
Shenzhen Yiaider Intelligent Technology Co ltd
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 Shenzhen Yiaider Intelligent Technology Co ltd filed Critical Shenzhen Yiaider Intelligent Technology Co ltd
Priority to CN202110597733.8A priority Critical patent/CN113900061B/zh
Priority claimed from CN202110597733.8A external-priority patent/CN113900061B/zh
Publication of CN113900061A publication Critical patent/CN113900061A/zh
Application granted granted Critical
Publication of CN113900061B publication Critical patent/CN113900061B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/0257—Hybrid positioning
    • G01S5/0258—Hybrid positioning by combining or switching between measurements derived from different systems
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • 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/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于UWB无线定位与IMU融合的导航定位系统及方法,该系统包括：安装于指定工作区域内的至少四个UWB基站；由两个UWB定位标签和两个IMU组成的标签装置；其中一个UWB定位标签与一个IMU以刚体连接方式安装在一起组成第一复合标签，另外一个UWB定位标签和IMU以刚体连接安装在一起组成第二复合标签，第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成所述标签装置；主控装置，用于接收标签装置数据，采用卡尔曼滤波器紧耦合的方式对UWB标签数据和IMU数据进行实时融合，得到当前位置数据和航向角数据；由标签装置及主控装置组成定位装置。本发明可以同时提供精确的位置信息和航向角信息，可广泛应用移动机器人的室内外导航。

Description

基于UWB无线定位与IMU融合的导航定位系统及方法
技术领域
本发明属于导航定位技术领域，尤其涉及一种基于UWB无线定位与IMU融合的导航定位系统及方法。
背景技术
全球卫星导航系统(Global Navigation Satellite System,GNSS) 可以为用户提供全球范围内的自主定位导航服务，如今已广泛的应用于各种领域。但由于受卫星时钟误差、星历数据误差、电离层影响、多径影响等诸多干扰，传统的卫星定位只能达到10m左右的定位精度，远无法满足一些对定位精度要求高的场合。载波相位差分技术即RTK(RealTime Kinematic)的出现，将卫星定位精度提高到厘米级，进一步拓宽了卫星定位技术的使用场景，但RTK成本较高，另外也无法在室内环境使用。随着区域内定位服务需求的日益增加，局部定位服务( Location Position Server )成为了研究的热点，常见的有WiFi指纹技术、UWB技术、蓝牙技术等。其中UWB技术以其高传输率，高穿透力，及较高的定位精度等优势已经在仓库，隧道等地方的人员、物资定位取得广泛的应用，但由于其定位效果在非视距环境下较差，极大的限制了其使用范围如各种移动机器人的导航定位，存在遮挡、多径干扰、信号不稳定导致定位误差较大的问题。
发明内容
本发明的目的是提供一种基于UWB无线定位与IMU融合的导航定位系统及方法，采用拓展卡尔曼滤波器紧耦合的方式融合UWB标签数据和IMU数据，有效减小UWB非视距误差对定位精度产生的影响，，并且采用双定位标签融合陀螺仪得到全局可靠航向角数据，为各类室内外移动机器人导航定位提供可靠的位置和航向角数据支持，解决单一标签没有角度信息，无法为移动机器人提供航向角的问题。
本发明解决其技术问题是通过以下的技术方案实现的：
本发明提供了一种基于UWB无线定位与IMU融合的导航定位系统,包括：
安装于指定工作区域内的至少四个UWB基站；
由两个UWB定位标签和两个IMU组成的标签装置；其中一个所述UWB定位标签与一个IMU以刚体连接方式安装在一起组成第一复合标签，另外一个所述UWB定位标签和IMU以刚体连接安装在一起组成第二复合标签，所述第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成所述标签装置；
主控装置，用于接收所述标签装置数据，采用卡尔曼滤波器紧耦合的方式对UWB标签数据和IMU数据进行实时融合，得到当前位置数据和航向角数据；
由所述标签装置及主控装置组成的定位装置。
进一步地，所述第一复合标签与第二复合标签之间的距离为0.5m~1.0m。
本发明还提供了一种应用上述导航定位系统进行导航定位的方法，包括如下步骤：
步骤1，在多个UWB基站中选取其中一个作为参考坐标原点，精确测量其他基站的相对位置坐标，从而建立参考坐标系；
步骤2，获取UWB定位标签到各基站的UWB测距信息；获取IMU的加速度计和陀螺仪信息进行姿态解算，得到标签装置的IMU线性加速度信息；
步骤3，采用紧耦合的方式构建卡尔曼滤波器模型，对UWB测距信息和IMU线性加速度信息进行融合；所述卡尔曼滤波器模型包括位置卡尔曼滤波器模型及角度卡尔曼滤波器模型；
步骤4，启动布置于工作区域内的定位装置，进行位置卡尔曼滤波器模型初始化及角度卡尔曼滤波器模型初始化，得到初始位置坐标和航向角，并进入位置跟踪状态及角度跟踪状态；
步骤5，初始化完成后，实时获取UWB定位标签和IMU的测量数据，并对位置卡尔曼滤波器模型和角度卡尔曼滤波器模型进行实时更新，得到当前位置数据和航向角数据。
进一步地，所述卡尔曼滤波器模型用于：
系统状态预测，计算公式为：
Figure 100002_1
U(k)
上式中，A为系统的状态转移矩阵，
Figure 100002_3
表示本次系统的最优估计值,在本系统构建的公式中指由位置、速度、加速度计误差一同构成的矩阵，
Figure 100002_2
表示系统的控制矩阵，
Figure 100002_4
表示系统的控制量，本系统中指加速度矩阵；
系统协方差预预测，计算公式为:
Figure 100002_5
上式中，A为系统的状态转移矩阵，
Figure 100002_6
为本次系统先验协方差最优化估计值，
Figure 100002_7
为上一次系统的先验协方差最优化估计值，Ｑ为系统的过程噪声协方差矩阵；
系统的卡尔曼增益计算，计算公式为:
Figure 100002_8
上式中，
Figure RE-395877DEST_PATH_IMAGE009
表示k时刻系统的卡尔曼增益，H为系统的测量矩阵，R为测量噪声协方差矩阵，
Figure RE-326923DEST_PATH_IMAGE010
；
滤波器更新，最优状态估计公式为:
Figure RE-81253DEST_PATH_IMAGE011
上式中，
Figure 100002_9
表示k时刻的系统最优状态估计值，
Figure 100002_10
表示k时刻的系统测量值，
Figure 100002_11
为k-1时刻的系统最优状态估计值,
Figure 100002_12
为k-2时刻系统的最优状态估计值，
Figure 100002_13
为k时刻系统的卡尔曼增益，H为系统的测量矩阵；
滤波器更新，最优状态协方差公式：
Figure 100002_14
上式中，
Figure 100002_15
为k时刻系统后验估计协方差，
Figure 100002_16
为k时刻系统的卡尔曼增益，H为系统的测量矩阵，
Figure 100002_17
为k-1时刻系统的后验协方差最优化估计结果。
借由上述方案，通过基于UWB无线定位与IMU融合的导航定位系统及方法，可以同时提供精确的位置信息和航向角信息，可广泛应用移动机器人的室内外导航。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例详细说明如后。
附图说明
图1是本发明基于UWB无线定位与IMU融合的导航定位系统的结构示意图；
图2是本发明基于UWB无线定位与IMU融合的导航定位系统的架构图；
图3是本发明基于UWB无线定位与IMU融合的导航定位方法的流程图。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实例用于说明本发明，但不用来限制本发明的范围。
参图1、图2所示，本实施例提供了一种基于UWB无线定位与IMU融合的导航定位系统,包括：
安装于指定工作区域内的至少四个UWB基站11（本实施例分别为UWB基站1、UWB基站2、UWB基站3、UWB基站4）；
由两个UWB定位标签和两个IMU组成的标签装置；其中第一UWB定位标21签与第一IMU23以刚体连接方式安装在一起组成第一复合标签，第二UWB定位标签22和第二IMU24以刚体连接安装在一起组成第二复合标签，第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成标签装置；
主控装置3，用于接收标签装置数据，采用卡尔曼滤波器紧耦合的方式对UWB标签数据和IMU数据进行实时融合，得到当前位置数据和航向角数据；
由标签装置及主控装置3组成的定位装置。
该基于UWB无线定位与IMU融合的导航定位系统，使用UWB测距信息作为观测量，使用IMU信息作为运动估计量，以紧耦合的方式设计拓展卡尔曼滤波器，对标签的位置进行最优化估计。可以有效解决UWB定位受遮挡、多路劲干扰产生测距误差，影响定位精度的问题。标签装置采用双定位标签设计，利用标签与基站之间的向量关系，可以测量标签装置的航向角，设计卡尔曼滤波器融合该航向角数据和陀螺仪数据可以有效消除陀螺仪测量产生的累积误差，从而提供一个全局精确可靠的航向角。该系统可以同时提供精确的位置信息和航向角信息，可广泛应用移动机器人的室内外导航。
在本实施例中，第一复合标签与第二复合标签之间的距离为0.8m。
参图3所示，本实施例还提供了一种应用上述导航定位系统进行导航定位的方法，包括如下步骤：
步骤1，在多个UWB基站中选取其中一个作为参考坐标原点，精确测量其他基站的相对位置坐标，从而建立参考坐标系。
参图1所示，按照UWB基站布置原则，在指定的场地范围内安装至少４个UWB基站，获取所有基站的坐标,在场地范围内设置坐标原点，以及坐标轴方向，求取坐标转移矩阵R,建立基站参考坐标系R(x,y,z)。
其中基站的安装应当遵守以下几个原则：
1）基站的布置形状应尽量接近长宽比为1:1的矩形，最大不应超过３: 1。
2）基站安装点离墙角距离应该大于1.0m，离墙面距离应该大于0.5m。
3）基站尽量安装于同一平面，并确保基站平面和标签工作平面之间的距离大于1.0m。
在基站安装完成之后，需要对基站坐标进行精确的测绘。一般采用全站仪来完成该工作，在测量得到所有基站的坐标点之后,在场地范围内设置坐标原点，以及坐标轴方向，求取坐标转移矩阵R,建立基站参考坐标系R(x,y,z)。
步骤2，获取UWB定位标签到各基站的UWB测距信息；获取IMU的加速度计和陀螺仪信息进行姿态解算，得到标签装置的IMU线性加速度信息。
接通所有基站的电源，确保基站进入测距工作状态，将标签装置置于基站包络范围内，启动标签装置，此时系统将自动开启TWR测距功能，标签装置将会依次测量其到所有信号范围内的基站的距离，并通过串口将距离信息传输到主控装置。
采取TWR(Two-Way_Ranging)方式创建距离测量系统，获取UWB标签和各个基站之间的距离,其基本原理如下。基站A和标签是B两个UWB模块。测距首先由A发起，B收到之后再发回一个相应(Responds)，A再接收这个相应，完成一次测距，在A和B每一次发送数据和收到数据的时候。都要记录当前时间戳。这样，通过时间戳相减，就可以得到传输时间差,公式如下:
Figure RE-263939DEST_PATH_IMAGE020
其中
Figure 35
即为电磁波从基站A到标签B飞行时间的，有了该值，再乘以光速c就可以得到距离D。
步骤3，采用紧耦合的方式构建卡尔曼滤波器模型，对UWB测距信息和IMU线性加速度信息进行融合；所述卡尔曼滤波器模型包括位置卡尔曼滤波器模型及角度卡尔曼滤波器模型。
建立UWB距离伪观测方程、建立IMU运动模型方程、构建拓展卡尔曼滤波器，对UWB测距信息和IMU得到的线性加速度信息进行紧耦合。
当接收到串口传回来的距离信息之后，主控装置将读取IMU的实时数据，并对UWB测距信息和IMU线性加速度信息进行融合。
步骤4，启动布置于工作区域内的定位装置，进行位置卡尔曼滤波器模型初始化及角度卡尔曼滤波器模型初始化，得到初始位置坐标和航向角，并进入位置跟踪状态及角度跟踪状态。
将定位装置放于待定区域内，启动装置，待主控启动完成，系统将会自动进入初始化模式，计算初始位置坐标和航向角；初始化完成后，即可进行动态数据输出。
通常情况下主控装置将先初始化位置卡尔曼滤波器模型，此时应该保持定位装置不动，待初始化完成，定位装置即可输出初始位置，并进入位置跟踪状态。
待位置初始化完成之后，主控装置将加载角度卡尔曼滤波器模型，建立双标签角度观测方程、陀螺仪测量方程，并融合标签观测角和陀螺仪测量得到的旋转角，融合标签观测角和陀螺仪测量得到的旋转角，得出当前方向角。
主控装置初始化角度卡尔曼滤波器模型，此时应该保持定位装置不动，待初始化完成，定位装置即可输出初始角度，并进入角度跟踪状态。
步骤5，初始化完成后，实时获取UWB定位标签和IMU的测量数据，并对位置卡尔曼滤波器模型和角度卡尔曼滤波器模型进行实时更新，得到当前位置数据和航向角数据。（对上述测量系统数据和卡尔曼滤波器输出进行实时更新，并输出当前融合得到的位置数据和航向角数据）
在位置和角度都初始化完成之后，定位装置将进入更新状态。此时定位装置将实时接收UWB标签和IMU的测量数据，并对位置卡尔曼滤波器和角度卡尔曼滤波器进行实时更新，并通过串口输出当前融合解算得到的位置数据和航向角数据。
在本实施例中，所述卡尔曼滤波器模型用于：
系统状态预测，计算公式为：
Figure 18
U(k)
上式中，A为系统的状态转移矩阵，
Figure 20
表示本次系统的最优估计值,在本系统构建的公式中指由位置、速度、加速度计误差一同构成的矩阵，
Figure 19
表示系统的控制矩阵，
Figure 21
表示系统的控制量，本系统中指加速度矩阵；
系统协方差预预测，计算公式为:
Figure 22
上式中，A为系统的状态转移矩阵，
Figure 23
为本次系统先验协方差最优化估计值，
Figure 24
为上一次系统的先验协方差最优化估计值,Ｑ为系统的过程噪声协方差矩阵；
系统的卡尔曼增益计算，计算公式为:
Figure 25
上式中，
Figure RE-321653DEST_PATH_IMAGE009
表示k时刻系统的卡尔曼增益，H为系统的测量矩阵，R为测量噪声协方差矩阵，
Figure RE-583483DEST_PATH_IMAGE010
；
滤波器更新，最优状态估计公式为:
Figure RE-602255DEST_PATH_IMAGE011
上式中，
Figure 26
表示k时刻的系统最优状态估计值，
Figure 27
表示k时刻的系统测量值，
Figure 28
为k-1时刻的系统最优状态估计值,
Figure 29
为k-2时刻系统的最优状态估计值，
Figure 30
为k时刻系统的卡尔曼增益，H为系统的测量矩阵;
滤波器更新，最优状态协方差公式：
Figure 31
上式中，
Figure 32
为k时刻系统的后验估计协方差，
Figure 34
为k时刻系统的卡尔曼增益，H为系统的测量矩阵，
Figure 33
为k-1时刻系统的后验协方差最优化估计结果。
本发明采取紧耦合的方式对IMU数据和UWB数据进行实时融合，IMU瞬时性能的优越性可以在极大程度上逆补UWB易受非视距误差影响的先天不足，有效提高系统的定位精度和鲁棒性。本发明创造式地采用双标签系统构成标签装置的整体结构，由两个标签位置构成的向量以及位置状态约束关系可以得出标签装置的观测方位角，再采用卡尔曼滤波器融合和陀螺仪数据便可以得到一个实时性高，全局精确可靠的方位角，弥补了传统的UWB定位系统无方位角信息，无法为移动机器人提供航向角的缺陷。
以上所述仅是本发明的优选实施方式，并不用于限制本发明，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。

Claims (4)

1.一种基于UWB无线定位与IMU融合的导航定位系统,其特征在于，包括：
安装于指定工作区域内的至少四个UWB基站；
由两个UWB定位标签和两个IMU组成的标签装置；其中一个所述UWB定位标签与一个IMU以刚体连接方式安装在一起组成第一复合标签，另外一个所述UWB定位标签和IMU以刚体连接安装在一起组成第二复合标签，所述第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成所述标签装置；
主控装置，用于接收所述标签装置数据，采用卡尔曼滤波器紧耦合的方式对UWB标签数据和IMU数据进行实时融合，得到当前位置数据和航向角数据；
由所述标签装置及主控装置组成的定位装置。
2.根据权利要求1所述的基于UWB无线定位与IMU融合的导航定位系统,其特征在于，所述第一复合标签与第二复合标签之间的距离为0.5m~1.0m。
3.一种应用权利要求1或2所述导航定位系统进行导航定位的方法，其特征在于，包括如下步骤：
步骤1，在多个UWB基站中选取其中一个作为参考坐标原点，精确测量其他基站的相对位置坐标，从而建立参考坐标系；
步骤2，获取UWB定位标签到各基站的UWB测距信息；获取IMU的加速度计和陀螺仪信息进行姿态解算，得到标签装置的IMU线性加速度信息；
步骤3，采用紧耦合的方式构建卡尔曼滤波器模型，对UWB测距信息和IMU线性加速度信息进行融合；所述卡尔曼滤波器模型包括位置卡尔曼滤波器模型及角度卡尔曼滤波器模型；
步骤4，启动布置于工作区域内的定位装置，进行位置卡尔曼滤波器模型初始化及角度卡尔曼滤波器模型初始化，得到初始位置坐标和航向角，并进入位置跟踪状态及角度跟踪状态；
步骤5，初始化完成后，实时获取UWB定位标签和IMU的测量数据，并对位置卡尔曼滤波器模型和角度卡尔曼滤波器模型进行实时更新，得到当前位置数据和航向角数据。
4.根据权利要求3所述的方法，其特征在于，所述卡尔曼滤波器模型用于：
系统状态预测，计算公式为：
Figure 2
U(k)
上式中，A为系统的状态转移矩阵，
Figure 1
表示本次系统的最优估计值,在本系统构建的公式中指由位置、速度、加速度计误差一同构成的矩阵，
Figure 3
表示系统的控制矩阵，
Figure 4
表示系统的控制量，本系统中指加速度矩阵；
系统协方差预预测，计算公式为:
Figure 5
上式中，A为系统的状态转移矩阵，
Figure 6
为本次系统先验协方差最优化估计值，
Figure 7
为上一次系统的先验协方差最优化估计值，Ｑ为系统的过程噪声协方差矩阵；
系统的卡尔曼增益计算，计算公式为:
Figure 8
上式中，
Figure RE-679755DEST_PATH_IMAGE009
表示k时刻系统的卡尔曼增益，H为系统的测量矩阵，R为测量噪声协方差矩阵，
Figure RE-544943DEST_PATH_IMAGE010
；
滤波器更新，最优状态估计公式为:
Figure RE-621484DEST_PATH_IMAGE011
上式中，
Figure 9
表示k时刻的系统最优状态估计值，
Figure 10
表示k时刻的系统测量值，
Figure 11
为k-1时刻的系统最优状态估计值，
Figure 12
为k-2时刻系统的最优状态估计值，
Figure 13
为k时刻系统的卡尔曼增益，H为系统的测量矩阵;
滤波器更新，最优状态协方差公式：
Figure 14
上式中，
Figure 15
为k时刻系统后验估计协方差，
Figure 16
为k时刻系统的卡尔曼增益，H为系统的测量矩阵，
Figure 17
为k-1时刻系统的后验协方差最优化估计结果。
CN202110597733.8A 2021-05-31 基于uwb无线定位与imu融合的导航定位系统及方法 Active CN113900061B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110597733.8A CN113900061B (zh) 2021-05-31 基于uwb无线定位与imu融合的导航定位系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110597733.8A CN113900061B (zh) 2021-05-31 基于uwb无线定位与imu融合的导航定位系统及方法

Publications (2)

Publication Number Publication Date
CN113900061A true CN113900061A (zh) 2022-01-07
CN113900061B CN113900061B (zh) 2024-10-22

Family

ID=

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114088091A (zh) * 2022-01-21 2022-02-25 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统
CN114545327A (zh) * 2022-02-07 2022-05-27 四川中电昆辰科技有限公司 一种运动状态信息和uwb融合定位方法及定位系统
CN116019442A (zh) * 2022-12-12 2023-04-28 天津大学 基于uwb/imu融合的运动姿势评估系统
CN116321418A (zh) * 2023-03-02 2023-06-23 中国人民解放军国防科技大学 一种基于节点构型优选地集群无人机融合估计定位方法

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110025562A1 (en) * 2009-08-03 2011-02-03 Xsens Technologies, B.V. Tightly Coupled UWB/IMU Pose Estimation System and Method
KR20160143438A (ko) * 2015-06-05 2016-12-14 한국전자통신연구원 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치
CN106507302A (zh) * 2016-11-04 2017-03-15 南开大学 一种基于uwb的三维室内定位系统
CN106871893A (zh) * 2017-03-03 2017-06-20 济南大学 分布式ins/uwb紧组合导航系统及方法
CN109115211A (zh) * 2018-08-01 2019-01-01 南京科远自动化集团股份有限公司 一种厂区高精度人员定位方法及定位系统
CN109341683A (zh) * 2018-06-29 2019-02-15 中国人民解放军海军工程大学 基于uwb双标签的航向计算及其性能分析方法
US20190056422A1 (en) * 2016-01-12 2019-02-21 Bigmotion Technologies Inc. Systems and methods for human body motion capture
CN109443350A (zh) * 2018-12-27 2019-03-08 西安中科光电精密工程有限公司 基于神经网络的蓝牙/光电/ins组合导航装置及方法
CN109541534A (zh) * 2018-12-14 2019-03-29 长沙智能机器人研究院有限公司 一种结合惯性测量单元的超宽带无线通信室内定位系统
CN110081882A (zh) * 2019-04-24 2019-08-02 中南大学 四旋翼无人机航向测量器及控制方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110631576A (zh) * 2019-08-28 2019-12-31 南京理工大学 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法
CN111288990A (zh) * 2020-03-19 2020-06-16 云南电网有限责任公司电力科学研究院 一种架空检修机器人组合姿态测量方法
WO2020151663A1 (zh) * 2019-01-25 2020-07-30 长城汽车股份有限公司 车辆定位装置、系统、方法以及车辆
WO2021082790A1 (zh) * 2019-10-29 2021-05-06 广东工业大学 一种基于IMU的uwb定位异常值处理方法

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110025562A1 (en) * 2009-08-03 2011-02-03 Xsens Technologies, B.V. Tightly Coupled UWB/IMU Pose Estimation System and Method
KR20160143438A (ko) * 2015-06-05 2016-12-14 한국전자통신연구원 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치
US20190056422A1 (en) * 2016-01-12 2019-02-21 Bigmotion Technologies Inc. Systems and methods for human body motion capture
CN106507302A (zh) * 2016-11-04 2017-03-15 南开大学 一种基于uwb的三维室内定位系统
CN106871893A (zh) * 2017-03-03 2017-06-20 济南大学 分布式ins/uwb紧组合导航系统及方法
CN109341683A (zh) * 2018-06-29 2019-02-15 中国人民解放军海军工程大学 基于uwb双标签的航向计算及其性能分析方法
CN109115211A (zh) * 2018-08-01 2019-01-01 南京科远自动化集团股份有限公司 一种厂区高精度人员定位方法及定位系统
CN109541534A (zh) * 2018-12-14 2019-03-29 长沙智能机器人研究院有限公司 一种结合惯性测量单元的超宽带无线通信室内定位系统
CN109443350A (zh) * 2018-12-27 2019-03-08 西安中科光电精密工程有限公司 基于神经网络的蓝牙/光电/ins组合导航装置及方法
WO2020151663A1 (zh) * 2019-01-25 2020-07-30 长城汽车股份有限公司 车辆定位装置、系统、方法以及车辆
CN110081882A (zh) * 2019-04-24 2019-08-02 中南大学 四旋翼无人机航向测量器及控制方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110631576A (zh) * 2019-08-28 2019-12-31 南京理工大学 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法
WO2021082790A1 (zh) * 2019-10-29 2021-05-06 广东工业大学 一种基于IMU的uwb定位异常值处理方法
CN111288990A (zh) * 2020-03-19 2020-06-16 云南电网有限责任公司电力科学研究院 一种架空检修机器人组合姿态测量方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
RAMSEY FARAGHER: "Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation", IEEE SIGNAL PROCESSING MAGAZINE, vol. 29, no. 5, 30 September 2012 (2012-09-30), pages 128 - 132, XP011458705, DOI: 10.1109/MSP.2012.2203621 *
孙建峰: "基于超宽带定位的室内移动机器人控制算法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑（月刊）》, no. 12, 31 December 2020 (2020-12-31), pages 1 - 69 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114088091A (zh) * 2022-01-21 2022-02-25 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统
CN114545327A (zh) * 2022-02-07 2022-05-27 四川中电昆辰科技有限公司 一种运动状态信息和uwb融合定位方法及定位系统
CN116019442A (zh) * 2022-12-12 2023-04-28 天津大学 基于uwb/imu融合的运动姿势评估系统
CN116019442B (zh) * 2022-12-12 2024-05-14 天津大学 基于uwb/imu融合的运动姿势评估系统
CN116321418A (zh) * 2023-03-02 2023-06-23 中国人民解放军国防科技大学 一种基于节点构型优选地集群无人机融合估计定位方法
CN116321418B (zh) * 2023-03-02 2024-01-02 中国人民解放军国防科技大学 一种基于节点构型优选的集群无人机融合估计定位方法

Similar Documents

Publication Publication Date Title
Yang et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system
CN105628026B (zh) 一种移动物体的定位定姿方法和系统
US11428822B2 (en) Methods and systems for location determination
CN109974694B (zh) 一种基于uwb/imu/气压计的室内行人3d定位方法
CN105424041A (zh) 一种基于bd/ins紧耦合的行人定位算法
CN104864865A (zh) 一种面向室内行人导航的ahrs/uwb无缝组合导航方法
CN114554392B (zh) 一种基于uwb与imu融合的多机器人协同定位方法
CN112923919A (zh) 基于图优化的行人定位方法及系统
CN115950418A (zh) 多传感器融合定位方法
CN104374389B (zh) 一种面向室内移动机器人的imu/wsn组合导航方法
CN109141412A (zh) 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN108759825A (zh) 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN104296741A (zh) 采用距离平方和距离平方变化率的wsn/ahrs紧组合方法
CN112556695B (zh) 室内定位与三维建模方法、系统、电子设备及存储介质
Romaniuk et al. Real time localization system with Extended Kalman Filter for indoor applications
CN113900061B (zh) 基于uwb无线定位与imu融合的导航定位系统及方法
CN113900061A (zh) 基于uwb无线定位与imu融合的导航定位系统及方法
CN110315540A (zh) 一种基于uwb和双目vo紧耦合的机器人定位方法及系统
CN110764126B (zh) 一种gps信息缺失情况下的无人车导航方法及系统
Ruan et al. Accurate 2D localization for mobile robot by multi-sensor fusion
Ai et al. Research of AGV Positioning and Navigation System Based on UWB
CN109916401A (zh) 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及系统
CN116202534B (zh) 一种隧道内定位方法、装置、设备及存储介质
Guan et al. Fusing Ultra-wideband Range Measurements with IMU for Mobile Robot Localization
CN116608851B (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
CB02 Change of applicant information

Address after: 518052 Zone V, floor 2, building a, junxiangda building, No. 9, Zhongshan Garden Road, Nantou street, Nanshan District, Shenzhen, Guangdong

Applicant after: Shenzhen Pengkun Zhike Technology Co.,Ltd.

Address before: 518052 Zone V, floor 2, building a, junxiangda building, No. 9, Zhongshan Garden Road, Nantou street, Nanshan District, Shenzhen, Guangdong

Applicant before: Shenzhen yiaider Intelligent Technology Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant