CN113721189A - 一种基于uwb和vio融合的多运动节点定位方法及系统 - Google Patents

一种基于uwb和vio融合的多运动节点定位方法及系统 Download PDF

Info

Publication number
CN113721189A
CN113721189A CN202111132327.0A CN202111132327A CN113721189A CN 113721189 A CN113721189 A CN 113721189A CN 202111132327 A CN202111132327 A CN 202111132327A CN 113721189 A CN113721189 A CN 113721189A
Authority
CN
China
Prior art keywords
uwb
vio
motion
data
node
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
CN202111132327.0A
Other languages
English (en)
Other versions
CN113721189B (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.)
Aerospace Information Research Institute of CAS
Original Assignee
Aerospace Information Research Institute of CAS
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 Aerospace Information Research Institute of CAS filed Critical Aerospace Information Research Institute of CAS
Priority to CN202111132327.0A priority Critical patent/CN113721189B/zh
Publication of CN113721189A publication Critical patent/CN113721189A/zh
Application granted granted Critical
Publication of CN113721189B publication Critical patent/CN113721189B/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本公开提供了一种基于UWB和VIO融合的多运动节点定位方法，包括：S1，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB‑VIO数据帧，并将UWB‑VIO数据帧进行初始化和状态实时预测处理，得到UWB‑VIO数据帧的预测结果；S2，重复S1步骤多次，得到多帧UWB‑VIO数据帧及多帧UWB‑VIO数据帧的预测结果；S3，根据多帧UWB‑VIO数据帧构建融合窗口，并根据多帧UWB‑VIO数据帧的预测结果及多帧UWB‑VIO数据帧中多运动节点的测距约束和里程计约束迭代优化融合窗口，得到优化后的多运动节点的运动状态，该运动状态表征多帧UWB‑VIO数据帧的最优定位估计信息。本公开还提供了一种基于UWB和VIO融合的多运动节点定位系统、电子设备及存储介质。

Description

一种基于UWB和VIO融合的多运动节点定位方法及系统
技术领域
本公开涉及复杂环境下组合定位技术领域，具体涉及一种基于UWB和VIO融合的多运动节点定位方法及系统。
背景技术
多运动节点(如机器人集群等)通过感知交互、信息传递、协同作业完成各种复杂的任务，这一颠覆性的智能集群技术克服了单个机器人活动区域小、搭载设备有限、任务功能单一等诸多问题。机器人集群协作的重要前提是确定各机器人的运动轨迹，以及机器人之间的相对位置和方向。目前室外开阔环境下主要依赖全球定位系统GlobalPositioningSystem，GPS)，而在GPS拒止环境下通常采用蓝牙、Wi-Fi、超声波、超宽带(Ultra-WideBand，UWB)、视觉惯性里程计(Visual Inertial Odometer，VIO)等定位技术。其中，UWB技术通过发射纳秒级脉冲信号进行节点间的通信和距离测量，具有带宽大、安全性高、时间分辨率高、系统复杂度低、测距精度较高等特点，其常规测距精度可达10厘米，因此广泛应用于对精度要求较高的定位系统之中。VIO技术融合了视觉定位和惯导定位，具有更新速率快、漂移小、鲁棒性高等优点，受到了业界广泛的关注和研究，越来越多的应用于定位导航系统之中。
传统利用UWB进行节点间相对定位的方法，通常基于三边定位完成节点网络拓扑的还原和相对位置的确定。然而这种方法只能得到各节点的相对位置，并不能确定节点间相对方向，并且三边定位只能利用已知位置的相邻节点进行定位，不能充分利用节点网络所有的测距约束信息，不利于提高整体定位精度。另外可以在单个机器人节点上安装多个UWB模块，利用基于优化的方法同时优化节点的位置和方向，得以克服传统方法只能直接定位的限制，并且可以充分利用节点网络所有的测距约束和几何约束，从而得到更加精确的定位和定向结果。然而仅利用UWB只能得到节点间的相对位置方向，不能确定节点的实时运动速度、轨迹等信息。
基于视觉的同时定位与建图技术(Simultaneous Localization and Mapping，SLAM)以其成本低、结构简单等优点受到了广大研究者的青睐。近几年，基于优化方法的SLAM系统达到了令人瞩目的性能，展示了其巨大的应用潜力。然而，视觉图像对环境信息依赖性较大并且快速运动时容易丢失，最新的研究中以紧耦合的方式融合惯性器件，实现了更加快速和鲁棒的运动估计，得到了更加完善的VIO系统。然而仅利用VIO不能获取各节点间的相对位置，难以进行协同工作。虽然有方法利用基于视觉的目标匹配或者地图匹配进行节点间相对定位，然而这些方法较为复杂并且难以适应复杂多变的环境。
发明内容
为解决现有技术中存在的问题，本公开实施例提供的一种基于UWB和VIO融合的多运动节点定位方法、系统、电子设备、存储介质及程序产品，通过融合UWB和VIO得到稳定精确的各运动节点移动轨迹以及各运动节点间相对位置方向，为多机器人协同系统提供准确的位置服务，旨在提出一种基于UWB和VIO融合的多机器人协作定位定向方法，提升多机器人协作定位的准确率。
本公开的第一个方面提供了一种基于UWB和VIO融合的多运动节点定位方法，包括：S1，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将UWB-VIO数据帧进行初始化和状态实时预测处理，得到UWB-VIO数据帧的预测结果；S2，重复S1步骤多次，得到多帧UWB-VIO数据帧及所述多帧UWB-VIO数据帧的预测结果；S3，根据多帧UWB-VIO数据帧构建融合窗口，并根据多帧UWB-VIO数据帧的预测结果及所述多帧UWB-VIO数据帧中多运动节点的测距约束和里程计约束迭代优化所述融合窗口，得到优化后的多运动节点的运动状态，该运动状态表征所述多帧UWB-VIO数据帧的最优定位估计信息。
进一步地，S3包括：S31，根据所述多帧UWB-VIO数据帧构建融合窗口；S32，将多帧UWB-VIO数据帧的预测结果作为所述融合窗口的初始值；S33，根据多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束来迭代优化所述融合窗口，直至约束残差不再减小为止，得到优化后的多运动节点的运动状态。
进一步地，S33包括：将多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束输入至所述融合窗口的优化器，利用高斯牛顿法优化所述融合窗口，直至约束残差不再减小为止，得到优化后的所述多运动节点的运动状态。
进一步地，S1包括：S11，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，其中，所述多运动节点包括一主运动节点和多个从运动节点；S12，以所述主运动节点的载体坐标系为全局坐标系原点，根据所述UWB测距数据得到所述多个从运动节点的全局状态，并将所述UWB-VIO数据帧作为当前参考数据帧；S13，根据所述当前参考数据帧中的VIO里程计数据、所述全局状态及当前时刻的VIO里程计数据，实时预测所述多运动节点的节点状态，得到所述UWB-VIO数据帧的预测结果。
进一步地，该方法还包括：S4，将融合窗口内的所述多帧UWB-VIO数据帧进行边缘化处理，并将边缘化处理时移除的最后一帧UWB-VIO数据帧作为当前最新参考数据帧，该当前最新参考数据帧用于下一周期中多帧UWB-VIO数据帧的预测参考帧。
进一步地，该方法还包括：S5，将最后一帧UWB-VIO数据帧替换所述S12中的当前参考数据帧，重复S11、S13、S2～S4步骤多次，得到多个不同时间段的所述多运动节点的运动状态，该多个不同时间段的所述多运动节点的运动状态表征多运动节点的最优估计运动轨迹。
进一步地，多运动节点的全局状态包括多运动节点的全局位置和方向。
进一步地，多运动节点的测距约束euwb表征不同运动节点间每两个UWB模块的测距关系，其满足以下关系：
Figure BDA00032790229500000417
Figure BDA0003279022950000041
其中，||||2表示欧式距离，pi和pj分别表示第i个和第j个运动节点的全局位置，Ri和Rj分别表示第i个和第j个运动节点的全局方向，
Figure BDA0003279022950000042
和
Figure BDA0003279022950000043
分别表示第i个和第j个运动节点的载体坐标系下UWB模块的位置，dobs表示两个UWB模块之间观测到的距离数据，Euwb表示测距约束残差函数，p表示鲁棒核函数，Σuwb表示UWB模块的测距数据信息量，
Figure BDA0003279022950000044
表示测距约束euwb的转置矩阵。
进一步地，多运动节点的里程计约束表征每个节点不同时刻状态之间的约束，其满足以下关系：
Figure BDA0003279022950000045
Figure BDA0003279022950000046
Figure BDA0003279022950000047
其中，evio_p表示某个运动节点不同时刻状态之间的位置约束，evio_R表示某个运动节点不同时刻状态之间的方向约束，Evio表示里程计约束残差函数，pt、Rt分别表示t时刻某个运动节点的全局位置和方向，
Figure BDA0003279022950000048
Figure BDA0003279022950000049
分别表示某个运动节点在参考时刻的VIO输出的局部位置和方向，pref、Rref分别表示参考时刻的全局位置和方向，
Figure BDA00032790229500000410
分别为某个运动节点在参考时刻的VIO输出的局部位置和方向，Rref-1为Rref的逆矩阵，
Figure BDA00032790229500000411
为
Figure BDA00032790229500000412
的逆矩阵，∑vio表示VIO里程计数据信息矩阵，
Figure BDA00032790229500000413
分别为evio_p、evio_R的转置矩阵。
进一步地，UWB-VIO数据帧的预测结果满足以下关系：
Figure BDA00032790229500000414
Figure BDA00032790229500000415
Figure BDA00032790229500000416
其中，vt表示某个运动节点在t时刻的全局速度，
Figure BDA0003279022950000051
表示某个运动节点在参考时刻的VIO输出的局部速度，
Figure BDA0003279022950000052
为
Figure BDA0003279022950000053
逆矩阵。
本公开的第二个方面提供了一种基于UWB和VIO融合的多运动节点定位系统，包括：第一数据处理模块，用于获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将所述UWB-VIO数据帧进行初始化和状态实时预测处理，得到所述UWB-VIO数据帧的预测结果；第二数据处理模块，用于重复所述第一数据处理模块多次，得到多帧UWB-VIO数据帧及所述多帧UWB-VIO数据帧的预测结果；第三数据处理模块，用于根据所述多帧UWB-VIO数据帧构建融合窗口，并根据所述多帧UWB-VIO数据帧的预测结果及所述多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束来迭代优化所述融合窗口，得到优化后的所述多运动节点的运动状态，该运动状态表征所述多帧UWB-VIO数据帧的最优定位估计信息。
本公开的第三个方面提供了一种电子设备，包括：存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时，实现本公开的第一个方面提供的基于UWB和VIO融合的多运动节点定位方法。
本公开的第四个方面提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，实现本公开的第一个方面提供的基于UWB和VIO融合的多运动节点定位方法。
本公开的第五个方面提供了一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现本公开的第一个方面提供的基于UWB和VIO融合的多运动节点定位方法。
本公开提供的一种基于UWB和VIO融合的多运动节点定位方法、系统、电子设备、存储介质及程序产品，利用实时预测和窗口融合优化的方式，充分利用UWB数据和VIO数据，既可以得到稳定精确的运动节点间相对位置方向，又可以获得各运动节点实时的运动状态，并且可以通过运动节点之间的约束减小各运动节点的里程计漂移，为多运动节点的协同任务提供了有力支撑。
附图说明
为了更完整地理解本公开及其优势，现在将参考结合附图的以下描述，其中：
图1示意性示出了根据本公开一实施例的基于UWB和VIO融合的多运动节点定位方法的流程图；
图2示意性示出了根据本公开一实施例的多运动节点的原理示意图；
图3示意性示出了根据本公开一实施例的VIO里程计数据仿真示意图；
图4示意性示出了根据本公开一实施例的初始化后相对定位结果示意图；
图5示意性示出了根据本公开一实施例的数据融合优化后的示意图；
图6示意性示出了根据本公开一实施例的基于UWB和VIO融合的多运动节点定位系统的方框图；
图7示意性示出了根据本公开一实施例的适于实现上文描述的方法的电子设备的方框图。
具体实施方式
以下，将参照附图来描述本公开的实施例。但是应该理解，这些描述只是示例性的，而并非要限制本公开的范围。在下面的详细描述中，为便于解释，阐述了许多具体的细节以提供对本公开实施例的全面理解。然而，明显地，一个或多个实施例在没有这些具体细节的情况下也可以被实施。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本公开的概念。
在此使用的术语仅仅是为了描述具体实施例，而并非意在限制本公开。在此使用的术语“包括”、“包含”等表明了所述特征、步骤、操作和/或部件的存在，但是并不排除存在或添加一个或多个其他特征、步骤、操作或部件。
在此使用的所有术语(包括技术和科学术语)具有本领域技术人员通常所理解的含义，除非另外定义。应注意，这里使用的术语应解释为具有与本说明书的上下文相一致的含义，而不应以理想化或过于刻板的方式来解释。
在使用类似于“A、B和C等中至少一个”这样的表述的情况下，一般来说应该按照本领域技术人员通常理解该表述的含义来予以解释(例如，“具有A、B和C中至少一个的系统”应包括但不限于单独具有A、单独具有B、单独具有C、具有A和B、具有A和C、具有B和C、和/或具有A、B、C的系统等)。在使用类似于“A、B或C等中至少一个”这样的表述的情况下，一般来说应该按照本领域技术人员通常理解该表述的含义来予以解释(例如，“具有A、B或C中至少一个的系统”应包括但不限于单独具有A、单独具有B、单独具有C、具有A和B、具有A和C、具有B和C、和/或具有A、B、C的系统等)。
附图中示出了一些方框图和/或流程图。应理解，方框图和/或流程图中的一些方框或其组合可以由计算机程序指令来实现。这些计算机程序指令可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器，从而这些指令在由该处理器执行时可以创建用于实现这些方框图和/或流程图中所说明的功能/操作的装置。本公开的技术可以硬件和/或软件(包括固件、微代码等)的形式来实现。另外，本公开的技术可以采取存储有指令的计算机可读存储介质上的计算机程序产品的形式，该计算机程序产品可供指令执行系统使用或者结合指令执行系统使用。
本公开实施例提供一种基于UWB和VIO融合的多运动节点定位方法，包括：S1，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将UWB-VIO数据帧进行初始化和状态实时预测处理，得到UWB-VIO数据帧的预测结果；S2，重复S1步骤多次，得到多帧UWB-VIO数据帧及多帧UWB-VIO数据帧的预测结果；S3，根据多帧UWB-VIO数据帧构建融合窗口，并根据多帧UWB-VIO数据帧的预测结果及多帧UWB-VIO数据帧中多运动节点的测距约束和里程计约束迭代优化融合窗口，得到优化后的多运动节点的运动状态，该运动状态表征多帧UWB-VIO数据帧的最优定位估计信息。
根据本公开的实施例，通过利用实时预测和窗口融合优化的方式，充分利用UWB数据和VIO数据，既可以得到稳定精确的运动节点间相对位置方向，又可以获得各运动节点实时的运动状态，并且可以通过运动节点之间的约束减小各运动节点的里程计漂移，为多运动节点的协同任务提供了准确的位置定位方法。此外，该方法对应的系统模型易于建立，具有较强的鲁棒性和通用性，可以为多机器人协同系统提供准确的位置服务。
图1示意性示出了根据本公开实施例的基于UWB和VIO融合的多运动节点定位方法的流程图。如图1所示，该方法包括：步骤S1～S3。
在操作S1，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将所述UWB-VIO数据帧进行初始化和状态实时预测处理，得到所述UWB-VIO数据帧的预测结果。
本公开的实施例中，多运动节点可以指的是机器人集群等，该机器人集群通过感知交互、信息传递、协同作业完成各种复杂的任务。
如图2所示，为了实现任意运动节点相对位置和方向的估计，本公开的实施例采用一主多从模式，其中，多运动节点包括：一主运动节点10和多个从运动节点20，每个主运动节点10上安装4个UWB模块30和1个视觉惯性模块40(即VIO模块)，每个从运动节点20上安装2个UWB模块30和1个视觉惯性模块40。需说明的是，从运动节点20的数量可以为任意个。这种一主多从模式下，主运动节点10保证了节点网络相对坐标系的唯一性，并且即使在仅有1个从运动节点20时仍可以解算相对位置方向。另外，每个从运动节点20上安装2个UWB模块，便可以在完成相对定位定向的基础上尽可能减少运动节点网络所用UWB模块的个数，即减轻了网络测距和计算负担，在一定开销下可以容纳尽可能多的运动节点。
需说明的是，每个运动节点上可以安装更多数量的UWB模块，并不影响本方法的应用。为了实现各运动节点的实时运动估计，在每个运动节点上安装相机和IMU组成的视觉惯性模块40，利用开源VINS-Fusion算法，将图像和惯性器件信息以紧耦合的方式融合得到视觉惯性里程计信息。
如图2所示，本公开的实施例中，每个运动节点上均皆建立载体坐标系。本公开提供的方法优化过程中以主运动节点10的载体坐标系初始状态为全局参考系，将各个从运动节点20的载体坐标系相对于参考系的位置和方向作为待优化状态，将各运动节点间的UWB模块之间的测距约束信息(包括主从运动节点测距约束12和从运动节点间的测距约束22)以及各运动节点视觉惯性融合的里程计信息作为优化约束项。
根据本公开的实施例，步骤S1具体包括：步骤S11～S13。
S11，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧。
具体地，多运动节点的节点网络使用同一套时间系统，以保证各数据可以根据时间戳同步。同步时以主运动节点10的UWB测距数据时间为基准，分别寻找时间最近的其余从运动节点20的UWB测距数据以及各节点VIO里程计数据，并一起组成UWB-VIO数据帧。所有UWB测距数据包括各运动节点间所有UWB模块两两之间的测距，由于同一运动节点之上的不同UWB模块间距离固定，故所有UWB测距数据不包括同一运动节点上不同UWB模块间的固定距离。每个运动节点的VIO里程计数据是指每个运动节点运行的VINS-Fusion算法输出的实时里程计信息，包括位置(x、y、z)、相对主节点的方向和当前运动速度。
S12，以主运动节点10的载体坐标系为全局坐标系原点，根据UWB测距数据得到多个从运动节点20的全局状态，并将UWB-VIO数据帧作为当前参考数据帧。
本公开的实施例中，多运动节点的全局状态包括多运动节点的全局位置和方向。步骤S12为数据初始化过程，其利用UWB模块的几何关系和UWB测距约束解算各节运动点的相对位置和方向，得到整个系统的初始全局状态。其中，UWB模块的几何关系为各个UWB模块相对于载体坐标系的位置先验知识pU，其可表示为：
pU＝p+R·pBU
其中，p表示运动节点的相对位置，R表示运动节点的相对方向，pBU为运动节点的载体坐标系下每个UWB模块的位置。以该形式提供同一载体坐标系之上UWB模块之间的几何约束关系，保证优化过程中几何关系保持固定，并且方便优化器的构建。其中，多运动节点的测距约束euwb表征不同运动节点间每两个UWB模块的测距关系，其满足以下关系：
Figure BDA0003279022950000106
Figure BDA0003279022950000101
其中，||||2表示欧式距离，pi和pj分别表示第i个和第j个运动节点的全局位置，Ri和Rj分别表示第i个和第j个运动节点的全局方向，
Figure BDA0003279022950000102
和
Figure BDA0003279022950000103
分别表示第i个和第j个运动节点的载体坐标系下UWB模块的位置，dobs表示两个UWB模块之间观测到的距离数据，Euwb表示测距约束残差函数，ρ表示鲁棒核函数，∑uwb表示UWB模块的测距数据信息量，
Figure BDA0003279022950000104
表示测距约束euwb的转置矩阵。
该数学描述方式便于优化器对变量的优化，并且保证了几何约束。需说明的是，运动节点间每两个UWB模块都会产生一个测距约束。该步骤初始化过程中以主运动节点10载体坐标系为全局参考系，并将该UWB-VIO数据帧设置为当前参考数据帧。
S13，根据当前参考数据帧中的VIO里程计数据、全局状态及当前时刻的VIO里程计数据，实时预测多运动节点的节点状态，得到UWB-VIO数据帧的预测结果。
本公开的实施例中，实时预测多运动节点的节点状态是基于VIO里程计数据输出频率较高，通常达到百赫兹级别，并且里程计短时间内可信度较高，可以用于节点状态的实时预测，以便于节点执行对实时性要求较高的任务。
根据当前参考数据帧中的VIO里程计数据、全局状态及当前时刻的VIO里程计数据，实时预测多运动节点的节点状态，得到UWB-VIO数据帧的预测结果满足以下关系：
Figure BDA0003279022950000105
Figure BDA0003279022950000111
Figure BDA0003279022950000112
其中，pt、Rt、vt分别表示某个运动节点在t时刻的全局位置、方向、速度，
Figure BDA0003279022950000113
分别表示某个运动节点在参考时刻的VIO输出的局部速度，pref、Rref分别表示参考时刻的全局位置和方向，
Figure BDA0003279022950000114
Figure BDA0003279022950000115
分别为某个运动节点在参考时刻的VIO输出的局部位置和方向，
Figure BDA0003279022950000116
分别为
Figure BDA0003279022950000117
和
Figure BDA0003279022950000118
的逆矩阵。
在操作S2，重复S1步骤多次，得到多帧UWB-VIO数据帧及所述多帧UWB-VIO数据帧的预测结果。
本公开的实施例中，假设步骤S1获取的是t1时刻的多运动节点的UWB测距数据和VIO里程计数据，并将UWB测距数据和VIO里程计数据组成一帧UWB-VIO数据帧，经过步骤S1处理后得到该帧UWB-VIO数据帧的预测结果。重复S1步骤多次，得到多帧UWB-VIO数据帧及多帧UWB-VIO数据帧的预测结果可以分别为t2时刻、t3时刻、t4时刻、t5时刻、......、tn时刻的UWB-VIO数据帧的预测结果，n为正整数。需说明的是，时刻t1～tn为连续不间断的时刻表示。
在操作S3，根据多帧UWB-VIO数据帧构建融合窗口，并根据多帧UWB-VIO数据帧的预测结果及所述多帧UWB-VIO数据帧中多运动节点的测距约束和里程计约束迭代优化所述融合窗口，得到优化后的多运动节点的运动状态，该运动状态表征多帧UWB-VIO数据帧的最优定位估计信息。
本公开的实施例中，假设经过步骤S1～S2中得到了t1～t10时刻对应的10帧UWB-VIO数据帧及该10帧UWB-VIO数据帧的预测结果。将该10帧UWB-VIO数据帧构建融合窗口。其中，10帧数据帧的帧数量即代表融合窗口的大小，需说明的是，本公开的实施例融合窗口的大小并不仅限10帧数据帧对应的帧数量，其可根据实际需要设定，且融合窗口内的多个数据帧可以是连续的帧，也可以是按一定间隔时间抽取的帧，这可以根据优化精度要求和设备算力设定。
根据上述实施例内容可知，融合窗口内所有的数据帧包含测距约束和里程计约束，步骤S3中优化融合窗口得到多运动节点的运动状态具体包括：
S31，根据多帧UWB-VIO数据帧构建融合窗口。
S32，将多帧UWB-VIO数据帧的预测结果作为融合窗口的初始值。
S33，根据多帧UWB-VIO数据帧中多运动节点的测距约束和里程计约束来迭代优化融合窗口，直至约束残差不再减小为止，得到优化后的多运动节点的运动状态。
其中，测距约束euwb表征不同运动节点间每两个UWB模块的测距关系见上述内容，多运动节点的VIO里程计约束表征每个节点不同时刻状态之间的约束，其构建满足以下关系：
Figure BDA0003279022950000121
Figure BDA0003279022950000122
Figure BDA0003279022950000123
其中，evio_p表示某个运动节点不同时刻状态之间的位置约束，evio_R表示某个运动节点不同时刻状态之间的方向约束，Evio表示里程计约束残差函数，
Figure BDA0003279022950000124
为Rref的逆矩阵，
Figure BDA0003279022950000125
为
Figure BDA0003279022950000126
的逆矩阵，∑vio表示VIO里程计数据信息矩阵，
Figure BDA0003279022950000127
分别为evio_p、evio_R的转置矩阵。
具体地，将多帧UWB-VIO数据帧中多运动节点的测距约束和里程计约束输入至融合窗口的优化器，利用高斯牛顿法优化融合窗口，直至约束残差不再减小为止，得到优化后的多运动节点的运动状态，该运动状态表征多帧UWB-VIO数据帧的最优定位估计信息。
本公开的实施例中，将多帧UWB-VIO数据帧的预测结果作为融合窗口的初始值，可以优化设置融合窗口的初始计算状态，从而减少计算过程中的误差，提高数据融合计算的准确度。
根据本公开的实施例，该方法还包括：步骤S4～S5。
S4，将融合窗口内的多帧UWB-VIO数据帧进行边缘化处理，并将边缘化处理时移除的最后一帧UWB-VIO数据帧作为当前最新参考数据帧，该当前最新参考数据帧用于下一周期中多帧UWB-VIO数据帧的预测参考帧。
本公开的实施例中，该当前最新参考数据帧代替步骤S12中的当前参考数据帧用于下一周期步骤S13中的实时状态预测。基于融合窗口优化后可以得到更为准确的各帧状态，适用于利用里程计数据进行状态预测。具体地，边缘化处理包括：将融合窗口内最早的1帧或多帧数据帧移除，便于加入新的数据帧持续进行窗口优化。
S5，将最后一帧UWB-VIO数据帧替换步骤S12中的当前参考数据帧，重复S11、S13、S2～S4步骤多次，得到多个不同时间段的多运动节点的运动状态，该多个不同时间段的所述多运动节点的运动状态表征多运动节点的最优估计运动轨迹。
根据本公开的实施例，沿用上述实施例，经过第一个周期的步骤S1～S4的处理，可得到t1～t10时刻的多运动节点的运动状态，该运动状态表征t1～t10时刻的10帧UWB-VIO数据帧的最优定位估计信息。将步骤S4中得到的前最新参考数据帧代替步骤S12中当前参考数据帧用于下一周期步骤S13中的实时状态预测，每进行一次S11、S13、S2～S4步骤循环，就可得到10帧UWB-VIO数据帧的最优定位估计信息，如第二个周期得到的是t2～t11时刻的10帧UWB-VIO数据帧的最优定位估计信息，第三个周期得到的是t3～t12时刻的10帧UWB-VIO数据帧的最优定位估计信息，依此类推下去，得到多个不同时间段的多运动节点的运动状态。
需说明的是，第二个周期处理得到的10帧UWB-VIO数据帧的最优定位估计信息也可以为t3～t12、t4～t13、t5～t14、…、t11～t20时刻的10帧UWB-VIO数据帧最优定位估计信息，其只需满足下一周期处理的数据帧与上一周期处理的数据帧存在部分重叠或无间断即可。
本公开的实施例中，根据步骤S5得到多个不同时间段的多运动节点的运动状态，将该多个不同时间段的多运动节点的运动状态对应的位置轨迹进行图形化，即可得到该多运动节点的相应时间段内的最优估计运动轨迹。
下面将结合一具体的实施例对本公开提供的基于UWB和VIO融合的多运动节点定位方法的技术方案进行详细说明。应当理解，图3～5中示出的原理图及结果示意图仅是示例性的，以帮助本领域的技术人员理解本公开的技术方案，并非用以限制本公开的保护范围。
本公开的实施例中，利用仿真数据对本公开所提出的技术方案进行定量分析。仿真过程中采用一主多从模式，选取从运动节点数量为9个。主运动节点上的4个UWB模块呈0.6m正方形分布，从运动节点上的2个UWB模块间距0.6m。主运动节点上的4个UWB模块在其载体坐标系下位置分别为：
Figure BDA0003279022950000141
其中，
Figure BDA0003279022950000142
分别表示主运动节点上的第一个、第二个、第三个及第四个UWB模块的载体坐标系下的位置。
从运动节点上的2个UWB模块在其载体坐标系下位置分别为：
Figure BDA0003279022950000143
其中，
Figure BDA0003279022950000144
分别表示从运动节点上的第一个和第二个UWB模块的载体坐标系下的位置。
设定UWB测距频率为50Hz，即在0.02s内完成所有UWB模块之间的测距，并设置UWB测距精度为10cm。各运动节点VIO里程计输出频率为200Hz，设置里程计位置噪声方差为1m/s，方向噪声方差为1rad/s。模拟实际运动生成仿真轨迹，为了探究运动节点轨迹优化和网络相对定位能力，设置主运动节点固定，每个从运动节点以3m/s速度绕半径10m的圆匀速圆周运动一周，得到的真实轨迹和VIO里程计数据轨迹如图3所示。
根据本公开提供的方法将收集到的所有运动节点的UWB模块之间的测距数据和各运动节点VIO里程计数据进行处理，初始化得到的1个主运动节点和9个从运动节点的相对定位结果如图4所示，可见初始化后各运动节点的相对定位结果与真实位置匹配度较高。
根据当前参考数据帧中的VIO里程计数据、全局状态及当前时刻的VIO里程计数据，实时预测多运动节点的节点状态。再利用多帧UWB-VIO数据帧构建融合窗口，窗口大小设置为10，帧间隔时间为0.2s，优化得到精确的节点状态。窗口优化后移出窗口内一半的数据帧，并设定参考帧用于下一周期的状态预测。
循环执行以上步骤直到任务结束，任务结束可根据人为手动结束，本公开对此不做限定。经过UWB和VIO融合优化后，各运动节点的轨迹位置误差为0.11m，方向误差为0.27°，运动节点间相对定位精度为0.02m，得到的仿真结果轨迹如图5所示，可以看出优化结果与真值符合较好，即得到了稳定精确的运动节点间相对位置方向，又获得了各运动节点实时运动状态，并且通过运动节点之间的约束减小了VIO里程计的漂移，验证了本公开提供的方法的可行性和有效性。
需说明的是，上述实施例所示的各运动节点上设置的UWB模块、视觉惯性模块以及一主四从、一主九从的模式等仅为实施例性的说明，其并不构成本公开的实施例的限定，在实际应用过程中，其可根据实际需求进行从运动节点以及运动节点上UWB模块数量等信息的设定。
图6示意性示出了根据本公开实施例的基于UWB和VIO融合的多运动节点定位系统的方框图。
如图6所示，该基于UWB和VIO融合的多运动节点定位系统600包括：第一数据处理模块610、第二数据处理模块620及第三数据处理模块630。该系统600可以用于实现参考图1所描述的基于UWB和VIO融合的多运动节点定位方法。
第一数据处理模块610，用于获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将所述UWB-VIO数据帧进行初始化和状态实时预测处理，得到所述UWB-VIO数据帧的预测结果。根据本公开的实施例，该第一数据处理模块610例如可以用于执行上文参考图1所描述的S1步骤，在此不再赘述。
第二数据处理模块620，用于重复所述第一数据处理模块多次，得到多帧UWB-VIO数据帧及所述多帧UWB-VIO数据帧的预测结果。根据本公开的实施例，该第二数据处理模块620例如可以用于执行上文参考图1所描述的S2步骤，在此不再赘述。
第三数据处理模块630，用于根据所述多帧UWB-VIO数据帧构建融合窗口，并根据所述多帧UWB-VIO数据帧的预测结果及所述多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束来迭代优化所述融合窗口，得到优化后的所述多运动节点的运动状态，，该运动状态表征所述多帧UWB-VIO数据帧的最优定位估计信息。根据本公开的实施例，该第三数据处理模块630例如可以用于执行上文参考图1所描述的S3步骤，在此不再赘述。
根据本公开的实施例的模块、子模块、单元、子单元中的任意多个、或其中任意多个的至少部分功能可以在一个模块中实现。根据本公开实施例的模块、子模块、单元、子单元中的任意一个或多个可以被拆分成多个模块来实现。根据本公开实施例的模块、子模块、单元、子单元中的任意一个或多个可以至少被部分地实现为硬件电路，例如现场可编程门阵列(FPGA)、可编程逻辑阵列(PLA)、片上系统、基板上的系统、封装上的系统、专用集成电路(ASIC)，或可以通过对电路进行集成或封装的任何其他的合理方式的硬件或固件来实现，或以软件、硬件以及固件三种实现方式中任意一种或以其中任意几种的适当组合来实现。或者，根据本公开实施例的模块、子模块、单元、子单元中的一个或多个可以至少被部分地实现为计算机程序模块，当该计算机程序模块被运行时，可以执行相应的功能。
例如，第一数据处理模块610、第二数据处理模块620及第三数据处理模块630中的任意多个可以合并在一个模块中实现，或者其中的任意一个模块可以被拆分成多个模块。或者，这些模块中的一个或多个模块的至少部分功能可以与其他模块的至少部分功能相结合，并在一个模块中实现。根据本公开的实施例，第一数据处理模块610、第二数据处理模块620及第三数据处理模块630中的至少一个可以至少被部分地实现为硬件电路，例如现场可编程门阵列(FPGA)、可编程逻辑阵列(PLA)、片上系统、基板上的系统、封装上的系统、专用集成电路(ASIC)，或可以通过对电路进行集成或封装的任何其他的合理方式等硬件或固件来实现，或以软件、硬件以及固件三种实现方式中任意一种或以其中任意几种的适当组合来实现。或者，第一数据处理模块610、第二数据处理模块620及第三数据处理模块630中的至少一个可以至少被部分地实现为计算机程序模块，当该计算机程序模块被运行时，可以执行相应的功能。
图7示意性示出了根据本公开实施例的适于实现上文描述的方法的电子设备的方框图。图7示出的电子设备仅仅是一个示例，不应对本公开实施例的功能和使用范围带来任何限制。
如图7所示，本实施例中所描述的电子设备700，包括：处理器701，其可以根据存储在只读存储器(ROM)702中的程序或者从存储部分708加载到随机访问存储器(RAM)703中的程序而执行各种适当的动作和处理。处理器701例如可以包括通用微处理器(例如CPU)、指令集处理器和/或相关芯片组和/或专用微处理器(例如，专用集成电路(ASIC))，等等。处理器701还可以包括用于缓存用途的板载存储器。处理器701可以包括用于执行根据本公开实施例的方法流程的不同动作的单一处理单元或者是多个处理单元。
在RAM 703中，存储有系统700操作所需的各种程序和数据。处理器701、ROM 702以及RAM 703通过总线704彼此相连。处理器701通过执行ROM 702和/或RAM 703中的程序来执行根据本公开实施例的方法流程的各种操作。需要注意，所述程序也可以存储在除ROM 702和RAM 703以外的一个或多个存储器中。处理器701也可以通过执行存储在所述一个或多个存储器中的程序来执行根据本公开实施例的方法流程的各种操作。
根据本公开的实施例，电子设备700还可以包括输入/输出(I/O)接口705，输入/输出(I/O)接口705也连接至总线704。系统700还可以包括连接至I/O接口705的以下部件中的一项或多项：包括键盘、鼠标等的输入部分706；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分707；包括硬盘等的存储部分708；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分709。通信部分709经由诸如因特网的网络执行通信处理。驱动器710也根据需要连接至I/O接口705。可拆卸介质711，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器710上，以便于从其上读出的计算机程序根据需要被安装入存储部分708。
根据本公开的实施例，根据本公开实施例的方法流程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读存储介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分709从网络上被下载和安装，和/或从可拆卸介质711被安装。在该计算机程序被处理器701执行时，执行本公开实施例的系统中限定的上述功能。根据本公开的实施例，上文描述的系统、设备、装置、模块、单元等可以通过计算机程序模块来实现。
本发明实施例还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施例中描述的设备/装置/系统中所包含的；也可以是单独存在，而未装配入该设备/装置/系统中。上述计算机可读存储介质承载有一个或者多个程序，当上述一个或者多个程序被执行时，实现根据本公开实施例的基于UWB和VIO融合的多运动节点定位方法。
根据本公开的实施例，计算机可读存储介质可以是非易失性的计算机可读存储介质，例如可以包括但不限于：便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开的实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。例如，根据本公开的实施例，计算机可读存储介质可以包括上文描述的ROM 702和/或RAM703和/或ROM 702和RAM 703以外的一个或多个存储器。
本公开的实施例还包括一种计算机程序产品，其包括计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。当计算机程序产品在计算机系统中运行时，该程序代码用于使计算机系统实现本公开实施例所提供的基于UWB和VIO融合的多运动节点定位方法。
在该计算机程序被处理器701执行时执行本公开实施例的系统/装置中限定的上述功能。根据本公开的实施例，上文描述的系统、装置、模块、单元等可以通过计算机程序模块来实现。
在一种实施例中，该计算机程序可以依托于光存储器件、磁存储器件等有形存储介质。在另一种实施例中，该计算机程序也可以在网络介质上以信号的形式进行传输、分发，并通过通信部分709被下载和安装，和/或从可拆卸介质711被安装。该计算机程序包含的程序代码可以用任何适当的网络介质传输，包括但不限于：无线、有线等等，或者上述的任意合适的组合。
在这样的实施例中，该计算机程序可以通过通信部分709从网络上被下载和安装，和/或从可拆卸介质711被安装。在该计算机程序被处理器701执行时，执行本公开实施例的系统中限定的上述功能。根据本公开的实施例，上文描述的系统、设备、装置、模块、单元等可以通过计算机程序模块来实现。
根据本公开的实施例，可以以一种或多种程序设计语言的任意组合来编写用于执行本公开实施例提供的计算机程序的程序代码，具体地，可以利用高级过程和/或面向对象的编程语言、和/或汇编/机器语言来实施这些计算程序。程序设计语言包括但不限于诸如Java，C++，python，“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。
需要说明的是，在本公开各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来。
附图中的流程图和框图，图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
本领域技术人员可以理解，本公开的各个实施例和/或权利要求中记载的特征可以进行多种组合和/或结合，即使这样的组合或结合没有明确记载于本公开中。特别地，在不脱离本公开精神和教导的情况下，本公开的各个实施例和/或权利要求中记载的特征可以进行多种组合和/或结合。所有这些组合和/或结合均落入本公开的范围。
尽管已经参照本公开的特定示例性实施例示出并描述了本公开，但是本领域技术人员应该理解，在不背离所附权利要求及其等同物限定的本公开的精神和范围的情况下，可以对本公开进行形式和细节上的多种改变。因此，本公开的范围不应该限于上述实施例，而是应该不仅由所附权利要求来进行确定，还由所附权利要求的等同物来进行限定。

Claims (13)

1.一种基于UWB和VIO融合的多运动节点定位方法，其特征在于，包括：
S1，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将所述UWB-VIO数据帧进行初始化和状态实时预测处理，得到所述UWB-VIO数据帧的预测结果；
S2，重复S1步骤多次，得到多帧UWB-VIO数据帧及所述多帧UWB-VIO数据帧的预测结果；
S3，根据所述多帧UWB-VIO数据帧构建融合窗口，并根据所述多帧UWB-VIO数据帧的预测结果及所述多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束迭代优化所述融合窗口，得到优化后的所述多运动节点的运动状态，该运动状态表征所述多帧UWB-VIO数据帧的最优定位估计信息。
2.根据权利要求1所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述S3包括：
S31，根据所述多帧UWB-VIO数据帧构建融合窗口；
S32，将所述多帧UWB-VIO数据帧的预测结果作为所述融合窗口的初始值；
S33，根据所述多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束来迭代优化所述融合窗口，直至约束残差不再减小为止，得到优化后的所述多运动节点的运动状态。
3.根据权利要求2所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述S33包括：
将所述多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束输入至所述融合窗口的优化器，利用高斯牛顿法优化所述融合窗口，直至约束残差不再减小为止，得到优化后的所述多运动节点的运动状态。
4.根据权利要求1所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述S1包括：
S11，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，其中，所述多运动节点包括一主运动节点和多个从运动节点；
S12，以所述主运动节点的载体坐标系为全局坐标系原点，根据所述UWB测距数据得到所述多个从运动节点的全局状态，并将所述UWB-VIO数据帧作为当前参考数据帧；
S13，根据所述当前参考数据帧中的VIO里程计数据、所述全局状态及当前时刻的VIO里程计数据，实时预测所述多运动节点的节点状态，得到所述UWB-VIO数据帧的预测结果。
5.根据权利要求4所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，该方法还包括：
S4，将所述融合窗口内的所述多帧UWB-VIO数据帧进行边缘化处理，并将所述边缘化处理时移除的最后一帧UWB-VIO数据帧作为当前最新参考数据帧，该当前最新参考数据帧用于下一周期中多帧UWB-VIO数据帧的预测参考帧。
6.根据权利要求5所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，该方法还包括：
S5，将所述最后一帧UWB-VIO数据帧替换所述S12中的所述当前参考数据帧，重复S11、S13、S2～S4步骤多次，得到多个不同时间段的所述多运动节点的运动状态，该多个不同时间段的所述多运动节点的运动状态表征所述多运动节点的最优估计运动轨迹。
7.根据权利要求4所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述多运动节点的全局状态包括所述多运动节点的全局位置和方向。
8.根据权利要求7所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述多运动节点的测距约束euwb表征不同运动节点间每两个UWB模块的测距关系，其满足以下关系：
Figure FDA0003279022940000021
Figure FDA0003279022940000031
其中，|| ||2表示欧式距离，pi和pj分别表示第i个和第j个运动节点的全局位置，Ri和Rj分别表示第i个和第j个运动节点的全局方向，
Figure FDA0003279022940000032
和
Figure FDA0003279022940000033
分别表示第i个和第j个运动节点的载体坐标系下UWB模块的位置，dobs表示两个UWB模块之间观测到的距离数据，Euwb表示测距约束残差函数，ρ表示鲁棒核函数，∑uwb表示UWB模块的测距数据信息量，
Figure FDA0003279022940000034
表示测距约束euwb的转置矩阵。
9.根据权利要求8所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述多运动节点的里程计约束表征每个节点不同时刻状态之间的约束，其满足以下关系：
Figure FDA0003279022940000035
Figure FDA0003279022940000036
Figure FDA0003279022940000037
其中，evio_p表示某个运动节点不同时刻状态之间的位置约束，evio_R表示某个运动节点不同时刻状态之间的方向约束，Evio表示里程计约束残差函数，pt、Rt分别表示t时刻某个运动节点的全局位置和方向，
Figure FDA0003279022940000038
Figure FDA0003279022940000039
分别表示某个运动节点在参考时刻的VIO输出的局部位置和方向，pref、Rref分别表示参考时刻的全局位置和方向，
Figure FDA00032790229400000310
分别为某个运动节点在参考时刻的VIO输出的局部位置和方向，
Figure FDA00032790229400000311
为Rref的逆矩阵，
Figure FDA00032790229400000312
为
Figure FDA00032790229400000313
的逆矩阵，∑vio表示VIO里程计数据信息矩阵，
Figure FDA00032790229400000314
分别为evio_p、evio_R的转置矩阵。
10.根据权利要求9所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述UWB-VIO数据帧的预测结果满足以下关系：
Figure FDA00032790229400000315
Figure FDA00032790229400000316
其中，vt表示某个运动节点在t时刻的全局速度，
Figure FDA0003279022940000041
表示某个运动节点在参考时刻的VIO输出的局部速度，
Figure FDA0003279022940000042
为
Figure FDA0003279022940000043
逆矩阵。
11.一种基于UWB和VIO融合的多运动节点定位系统，其特征在于，包括：
第一数据处理模块，用于获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB-VIO数据帧，并将所述UWB-VIO数据帧进行初始化和状态实时预测处理，得到所述UWB-VIO数据帧的预测结果；
第二数据处理模块，用于重复所述第一数据处理模块多次，得到多帧UWB-VIO数据帧及所述多帧UWB-VIO数据帧的预测结果；
第三数据处理模块，用于根据所述多帧UWB-VIO数据帧构建融合窗口，并根据所述多帧UWB-VIO数据帧的预测结果及所述多帧UWB-VIO数据帧中所述多运动节点的测距约束和里程计约束来迭代优化所述融合窗口，得到优化后的所述多运动节点的运动状态，该运动状态表征所述多帧UWB-VIO数据帧的最优定位估计信息。
12.一种电子设备，包括：存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现如权利要求1至10中任一项所述的基于UWB和VIO融合的多运动节点定位方法。
13.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如权利要求1至10中任一项所述的基于UWB和VIO融合的多运动节点定位方法。
CN202111132327.0A 2021-09-26 2021-09-26 一种基于uwb和vio融合的多运动节点定位方法及系统 Active CN113721189B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111132327.0A CN113721189B (zh) 2021-09-26 2021-09-26 一种基于uwb和vio融合的多运动节点定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111132327.0A CN113721189B (zh) 2021-09-26 2021-09-26 一种基于uwb和vio融合的多运动节点定位方法及系统

Publications (2)

Publication Number Publication Date
CN113721189A true CN113721189A (zh) 2021-11-30
CN113721189B CN113721189B (zh) 2023-08-01

Family

ID=78685080

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111132327.0A Active CN113721189B (zh) 2021-09-26 2021-09-26 一种基于uwb和vio融合的多运动节点定位方法及系统

Country Status (1)

Country Link
CN (1) CN113721189B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222556A (zh) * 2023-01-13 2023-06-06 浙江大学 一种基于多源传感器融合的室内定位方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
US20190368879A1 (en) * 2018-05-29 2019-12-05 Regents Of The University Of Minnesota Vision-aided inertial navigation system for ground vehicle localization
CN111854733A (zh) * 2020-07-28 2020-10-30 广东博智林机器人有限公司 一种多源融合定位方法及系统
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
US20190368879A1 (en) * 2018-05-29 2019-12-05 Regents Of The University Of Minnesota Vision-aided inertial navigation system for ground vehicle localization
CN111854733A (zh) * 2020-07-28 2020-10-30 广东博智林机器人有限公司 一种多源融合定位方法及系统
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222556A (zh) * 2023-01-13 2023-06-06 浙江大学 一种基于多源传感器融合的室内定位方法及系统
CN116222556B (zh) * 2023-01-13 2024-03-26 浙江大学 一种基于多源传感器融合的室内定位方法及系统

Also Published As

Publication number Publication date
CN113721189B (zh) 2023-08-01

Similar Documents

Publication Publication Date Title
Murata et al. Smartphone-based localization for blind navigation in building-scale indoor environments
CN109885080B (zh) 自主控制系统及自主控制方法
Foote tf: The transform library
JP2021177168A (ja) 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム
CN107917707B (zh) 一种任意姿态下行人方向的确定方法、装置及电子设备
CN105229490A (zh) 使用卫星可见性数据来用于提升的位置准确性
CN115112121B (zh) 基于数据与模型结合的多源融合定位方法、系统及终端
US20230341228A1 (en) Global position and orientation correction method
JP7543745B2 (ja) 粒子フィルタを使用した位置特定方法、プログラム、及びシステム
CN113721189A (zh) 一种基于uwb和vio融合的多运动节点定位方法及系统
AU2016202042A1 (en) Backtracking indoor trajectories using mobile sensors
Tian et al. A heading estimation algorithm for wrist device assisted by sequential geomagnetic observations
CN110779533B (zh) 导航方法、系统和电子设备
MAILKA et al. An efficient end-to-end EKF-SLAM architecture based on LiDAR, GNSS, and IMU data sensor fusion for autonomous ground vehicles
Zhou et al. XDRNet: Deep Learning-based Pedestrian and Vehicle Dead Reckoning Using Smartphones
WO2019242251A1 (zh) 一种定位方法、装置及移动设备
Bianchi et al. Efficient visual sensor fusion for autonomous agents
Kamil et al. Low-cost object tracking with MEMS sensors, Kalman filtering and simplified two-filter-smoothing
Zhang et al. Improved grid mapping technology based on Rao-Blackwellized particle filters and the gradient descent algorithm
Elsergany et al. Novel Augmented Quaternion UKF for Enhanced Loosely Coupled GPS/INS Integration
Liu et al. Vision-inertial collaborative localization of multi-agents with remote interaction
CN111811500A (zh) 目标对象的位姿估计方法、装置、存储介质和电子设备
EP3533243A1 (en) Systems and methods of improving stability of a displayed user location using recent location history
Zhuang et al. A Real-Time Embedded Localization in Indoor Environment Using LiDAR Odometry
Liu Adaptive Edge Systems for Smart IoT Applications

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
GR01 Patent grant
GR01 Patent grant