CN114022519A - 坐标系转换方法、装置及包含该装置的多源融合slam系统 - Google Patents

坐标系转换方法、装置及包含该装置的多源融合slam系统 Download PDF

Info

Publication number
CN114022519A
CN114022519A CN202010735376.2A CN202010735376A CN114022519A CN 114022519 A CN114022519 A CN 114022519A CN 202010735376 A CN202010735376 A CN 202010735376A CN 114022519 A CN114022519 A CN 114022519A
Authority
CN
China
Prior art keywords
coordinate system
slam
matrix
slam system
positioning 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.)
Pending
Application number
CN202010735376.2A
Other languages
English (en)
Inventor
姚铮
周豪宇
陆明泉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua 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 Tsinghua University filed Critical Tsinghua University
Priority to CN202010735376.2A priority Critical patent/CN114022519A/zh
Publication of CN114022519A publication Critical patent/CN114022519A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本申请提出了用于包括第一SLAM系统和第二SLAM系统的多源融合SLAM系统的坐标系转换方法，其包括：通过第一SLAM系统获得运动载体在第一坐标系内的第一运动轨迹；通过第二SLAM系统获得运动载体在第二坐标系内的第二运动轨迹；基于第一运动轨迹和第二运动轨迹确定坐标系转换参数；以及根据坐标系转换参数进行坐标系转换。本申请还提出了坐标系转换装置以及包括该装置的多源融合SLAM系统。通过本申请的技术方案，能够不依赖先验信息，在SLAM系统的坐标系之间进行实时转换，满足实时性与鲁棒性的要求，还能提高系统的抗退化性能。

Description

坐标系转换方法、装置及包含该装置的多源融合SLAM系统
技术领域
本申请涉及导航定位技术领域，具体涉及SLAM技术领域，更具体地涉及用于多源融合SLAM系统的坐标系转换方法、用于多源融合SLAM系统的坐标系转换装置、以及包含该坐标系转换装置的多源融合SLAM系统。
背景技术
同时定位与建图(Simultaneous Location and Mapping,简称为SLAM)技术得到了越来越多的关注和应用，但是复杂应用场景可能导致传感器性能退化，从而使得基于该传感器的SLAM系统性能下降甚至失效。基于多传感器的多源融合SLAM系统则能有效地解决复杂应用场景带来的问题，并且能提高整体系统的性能。下面以激光雷达SLAM系统和基于超宽带(Ultra Wideband，简称为UWB)的SLAM系统为例，对此进行进一步说明。
作为一种常用的SLAM技术，激光雷达SLAM系统具有较大的视场(通常为360度)、更直观的地图表示、以及低光环境下更高的定位制图精度，因而被广泛应用于无人驾驶、AR、VR等领域。然而，在长直隧道等纹理不丰富或几何退化的环境下，激光雷达SLAM系统的精度会下降；而且激光雷达SLAM系统的累积误差随着时间的推移会逐渐增大并且无法被消除。此外，激光雷达SLAM系统的重定位能力也相对较差。
此外，作为一种基于无线信标测距的SLAM技术，基于UWB的SLAM技术是另一种常用的SLAM技术，其通过部署多个UWB传感器(定位节点)以组成定位节点网络。与激光雷达SLAM相比，基于UWB的SLAM系统不易受环境影响，具有较强的重定位能力，且不存在累积误差。然而，基于UWB的SLAM系统的精度通常低于激光雷达SLAM系统，并且其建立的拓扑地图也不如激光雷达SLAM得到的点云地图直观。
为了克服激光雷达SLAM系统和基于UWB的SLAM系统各自的局限性，并且可以实现更高的定位和制图精度，这两种SLAM系统的融合近年来越来越受到关注。包括激光雷达SLAM系统和基于UWB的SLAM系统的多源融合SLAM系统的一个技术前提在于，需将来自激光雷达SLAM系统和基于UWB的SLAM系统的数据统一到同一坐标系中，也就是说，需要在激光雷达SLAM系统的坐标系与基于UWB的SLAM系统的坐标系之间进行转换。
目前，在本技术领域所采用坐标系转换方法中，需要在激光雷达SLAM的全局坐标系下，提前测量UWB定位节点的位置，根据获得先验信息来预先确定坐标系的转换参数。然而，在某些应用场景下，提前测量UWB定位节点的坐标是非常困难的，甚至是不可能的。例如，在利用UWB定位节点传感器来导航水下探测器时，水流的影响可导致UWB定位节点的位置改变；在紧急救援或者火星探测这样的任务场景下，须实时部署而无法提前部署UWB定位节点，此时无法获得定位节点位置的先验信息，因而无法确定坐标系的转换参数，多源融合SLAM系统无法工作。现有技术中尚不存在一种合适的技术方案，能够不依赖先验信息，在激光雷达SLAM系统的坐标系与基于UWB的SLAM系统的坐标系之间进行实时转换。
事实上，现有技术中也不存在这样的技术方案，能够不依赖先验信息，在多源融合SLAM系统的组成SLAM系统的各自坐标系之间进行实时转换。
发明内容
根据本申请的一个方面，提出了一种用于多源融合SLAM系统的坐标系转换方法，该多源融合SLAM系统至少包括第一SLAM系统和第二SLAM系统。该用于多源融合SLAM系统的坐标系转换方法可包括：可通过第一SLAM系统获得运动载体在第一坐标系内的第一运动轨迹；可通过第二SLAM系统获得运动载体在第二坐标系内的第二运动轨迹；可基于第一运动轨迹和第二运动轨迹确定第一坐标系与第二坐标系之间的坐标系转换参数；以及可根据坐标系转换参数在第一坐标系与第二坐标系之间进行坐标系转换。通过本申请提出的用于多源融合SLAM系统的坐标系转换方法，能够不依赖先验信息，在组成融合SLAM系统的各SLAM系统的坐标系之间进行实时转换。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，可对第一运动轨迹和第二运动轨迹同时进行采样，以获得来自第一运动轨迹的多个第一采样点以及对应的来自第二运动轨迹的多个第二采样点，并且多个第一采样点和多个第二采样点均不少于4个；可对多个第一采样点和多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及可对多个第一中心化采样点和多个第二中心化采样点进行正交普氏分析，以确定所述坐标系转换参数。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，坐标系转换参数可包括第一坐标系与第二坐标系之间的旋转转换矩阵。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，可通过以下确定旋转转换矩阵，
构造
Figure BDA0002604835160000031
通过奇异值分解，获得U、Σ和V；以及
基于U、Σ和V获得
Figure BDA0002604835160000032
其中，
Figure BDA0002604835160000033
其中，
Figure BDA0002604835160000034
表示所述多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000035
表示所述多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，以及
Figure BDA0002604835160000036
表示旋转转换矩阵。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，可通过将XL去除xL,means获得
Figure BDA0002604835160000037
以及通过将XU去除xU,means获得
Figure BDA0002604835160000038
其中，XL表示多个第一采样点对应的矩阵，
Figure BDA0002604835160000041
其中，
Figure BDA0002604835160000042
和
Figure BDA0002604835160000043
表示来自第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xL,means表示XL的均值列向量，
其中，XU表示多个第二采样点对应的矩阵，
Figure BDA0002604835160000044
其中，
Figure BDA0002604835160000045
和
Figure BDA0002604835160000046
表示来自第二运动轨迹的第i个采样点的坐标，
xU,means表示XU的均值列向量。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，坐标系转换参数可包括第一坐标系与第二坐标系之间的位姿转换矩阵。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，可通过以下确定位姿转换矩阵，
构造
Figure BDA0002604835160000047
通过奇异值分解，获得U、Σ和V；
基于U、Σ和V获得
Figure BDA0002604835160000048
其中，
Figure BDA0002604835160000049
以及
基于
Figure BDA00026048351600000410
获得
Figure BDA00026048351600000411
其中，
Figure BDA00026048351600000412
其中，
Figure BDA0002604835160000051
表示多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000052
表示多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，
Figure BDA0002604835160000053
表示旋转转换矩阵，
xL,means表示XL的均值列向量，
其中，XL表示多个第一采样点对应的矩阵，
Figure BDA0002604835160000054
其中，
Figure BDA0002604835160000055
和
Figure BDA0002604835160000056
表示来自第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xU,means表示XU的均值列向量，
其中，XU表示多个第二采样点对应的矩阵，
Figure BDA0002604835160000057
其中，
Figure BDA0002604835160000058
和
Figure BDA0002604835160000059
表示来自第二运动轨迹的第i个采样点的坐标，
Figure BDA00026048351600000510
表示位姿转换矩阵，
通过将XL去除xL,means获得
Figure BDA00026048351600000511
以及
将XU去除xU,means均值列向量获得
Figure BDA00026048351600000512
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，第一SLAM系统可以是激光雷达SLAM系统或视觉SLAM系统。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，第二SLAM系统为基于无线信标测距的SLAM系统，基于无线信标测距的SLAM系统包括运动定位节点和固定定位节点，该用于多源融合SLAM系统的坐标系转换方法还可包括：在第二SLAM系统中，对第二坐标系以及第二SLAM系统的运动定位节点和固定UWB定位节点进行位置初始化，以确定第二坐标系以及运动定位节点和固定定位节点的初始位置信息；以及基于初始位置信息，通过扩展卡尔曼滤波器对运动定位节点和固定定位节点的位置信息进行估计，从而获得第二运动轨迹。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，可通过MDS算法对第二坐标系以及第二SLAM系统的运动定位节点和固定定位节点进行位置初始化。
根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中还可包括：
确定第二SLAM系统的运动定位节点的退化方向；以及
在通过扩展卡尔曼滤波器对运动定位节点的位置信息进行估计时，将位置信息的状态值更新量在退化方向上的分量去除，从而降低第二SLAM系统的退化对运动定位节点的位置信息估计的影响。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，可根据第二SLAM系统中的运动定位节点的位置、对应固定定位节点的位置、以及运动定位节点与固定定位节点之间的距离构造费雪信息矩阵；可确定费雪信息矩阵的特征值以及对应的特征向量；以及可根据与小于预设阈值的特征值相对应的特征向量确定退化方向。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换方法中，基于无线信标测距的SLAM系统可以是基于UWB的SLAM系统、基于Wi-Fi的SLAM系统或基于蓝牙的SLAM系统。
根据本申请的另一方面，提出了一种用于多源融合SLAM系统的坐标系转换装置，其中，该多源融合SLAM系统至少包括第一系统和第二SLAM系统。该用于多源融合SLAM系统的坐标系转换装置可包括：第一运动轨迹确定模块，该第一运动轨迹可确定模块通过第一系统获得所述运动载体在第一坐标系内的第一运动轨迹；第二运动轨迹确定模块，该第二运动轨迹确定模块可通过第二SLAM系统获得运动载体在第二坐标系内的第二运动轨迹；坐标系转换参数确定模块，该坐标系转换参数确定模块可基于第一运动轨迹和第二运动轨迹确定第一坐标系与第二坐标系之间的坐标系转换参数；以及坐标系转换模块，该坐标系转换模块可根据坐标系转换参数在第一坐标系与第二坐标系之间进行坐标系转换。通过本申请提出的用于多源融合SLAM系统的坐标系转换装置，能够不依赖先验信息，在组成融合SLAM系统的各SLAM系统的坐标系之间进行实时转换。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，坐标系转换参数确定模块还可对第一运动轨迹和第二运动轨迹同时进行采样，以获得来自第一运动轨迹的多个第一采样点以及对应的来自第二运动轨迹的多个第二采样点，并且多个第一采样点和多个第二采样点均不少于4个；对多个第一采样点和多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及对多个第一中心化采样点和多个第二中心化采样点进行正交普氏分析，以确定坐标系转换参数。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，坐标系转换参数可包括第一坐标系与第二坐标系之间的旋转转换矩阵。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，可通过以下确定旋转转换矩阵，
构造
Figure BDA0002604835160000071
通过奇异值分解，获得U、Σ和V；以及
基于U、Σ和V获得
Figure BDA0002604835160000072
其中，
Figure BDA0002604835160000073
其中，
Figure BDA0002604835160000074
表示多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000075
表示多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，以及
Figure BDA0002604835160000081
表示旋转转换矩阵。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，可通过将XL去除xL,means获得
Figure BDA0002604835160000082
以及通过将XU去除xU,means获得
Figure BDA0002604835160000083
其中，XL表示多个第一采样点对应的矩阵，
Figure BDA0002604835160000084
其中，
Figure BDA0002604835160000085
和
Figure BDA0002604835160000086
表示来自第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xL,means表示XL的均值列向量，
其中，XU表示多个第二采样点对应的矩阵，
Figure BDA0002604835160000087
其中，
Figure BDA0002604835160000088
和
Figure BDA0002604835160000089
表示来自第二运动轨迹的第i个采样点的坐标，
xU,means表示XU的均值列向量。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，坐标系转换参数可包括第一坐标系与第二坐标系之间的位姿转换矩阵。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，可通过以下确定位姿转换矩阵，
构造
Figure BDA00026048351600000810
通过奇异值分解，获得U、Σ和V；
基于U、Σ和V获得
Figure BDA00026048351600000811
其中，
Figure BDA00026048351600000812
以及
基于
Figure BDA0002604835160000091
获得
Figure BDA0002604835160000092
其中，
Figure BDA0002604835160000093
其中，
Figure BDA0002604835160000094
表示多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000095
表示多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，
Figure BDA0002604835160000096
表示旋转转换矩阵，
xL,means表示XL的均值列向量，
其中，XL表示多个第一采样点对应的矩阵，
Figure BDA0002604835160000097
其中，
Figure BDA0002604835160000098
和
Figure BDA0002604835160000099
表示来自第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xU,means表示XU的均值列向量，
其中，XU表示多个第二采样点对应的矩阵，
Figure BDA00026048351600000910
其中，
Figure BDA00026048351600000911
和
Figure BDA00026048351600000912
表示来自第二运动轨迹的第i个采样点的坐标，
Figure BDA00026048351600000913
表示位姿转换矩阵，
通过将XL去除xL,means获得
Figure BDA00026048351600000914
以及
将XU去除xU,means均值列向量获得
Figure BDA00026048351600000915
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，第一SLAM系统可以是激光雷达SLAM系统或视觉SLAM系统。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，第二运动轨迹确定模块还可对第二坐标系以及第二系统的运动定位节点和固定定位节点进行位置初始化，以确定第二坐标系以及运动定位节点和固定定位节点的初始位置信息；以及基于初始状态信息，通过扩展卡尔曼滤波器对运动定位节点和固定定位节点的位置信息进行估计，从而获得第二运动轨迹。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，第二运动轨迹确定模块还可通过MDS算法对第二坐标系以及第二SLAM系统的运动定位节点和固定定位节点进行位置初始化。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，第二运动轨迹确定模块还可确定第二SLAM系统的运动定位节点的退化方向；以及在通过扩展卡尔曼滤波器对运动定位节点的位置信息进行估计时，将位置信息的状态值更新量在退化方向上的分量去除，从而降低第二SLAM系统的退化对运动定位节点的位置信息估计的影响。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，第二运动轨迹确定模块还可根据第二SLAM系统中的运动定位节点的位置、对应固定定位节点的位置、以及运动定位节点与固定定位节点之间的距离构造费雪信息矩阵；确定费雪信息矩阵的特征值以及对应的特征向量；以及根据与小于预设阈值的特征值相对应的特征向量确定所述退化方向。
在根据本申请的一个实施方式的用于多源融合SLAM系统的坐标系转换装置中，基于无线信标测距的SLAM系统可以是基于UWB的SLAM系统、基于Wi-Fi的SLAM系统或基于蓝牙的SLAM系统。
根据本申请的又一方面，还提出了一种多源融合SLAM系统，该多源融合SLAM系统包括第一系统和第二SLAM系统，并且还包括如上所述的用于多源融合SLAM系统的坐标系转换装置。该多源融合SLAM系统能够不依赖先验信息，在组成多源融合SLAM系统的SLAM系统的坐标系之间进行实时转换，能够满足实时性与鲁棒性的要求，而且还能够进一步提高系统的抗退化性能。
附图说明
附图示出了本申请的示例性实施方式，并与其说明一起用于解释本申请的原理，其中包括了这些附图以提出对本申请的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。
图1示出了根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法的流程图；
图2示出了根据本申请实施方式的用于多源融合SLAM系统的坐标系转换装置的框图；
图3示出了在对根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法进行试验所使用的场所的示意图；
图4示出了在对根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法进行试验所使用的手持设备的示意图；
图5示出了试验中未采用根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法时所获得的点云地图；
图6示出了试验中采用根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法时所获得的点云地图；以及
图7示出了采用根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法而进行的试验的定位平均误差曲线图。
具体实施方式
下面结合附图和实施方式对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本申请的限制。为简明起见，在本申请各实施方式的描述中，对于相同或者类似的装置/方法步骤，使用相同或者相似的附图标记。
需要说明的是，在不冲突的情况下，本申请中的实施方式及实施方式中的特征可以相互组合。
根据本申请的一方面，提出了一种用于多源融合SLAM系统的坐标系转换方法，下面参照附图并结合实施方式对该方法进行详细说明。
图1示出了根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法的流程图，其中该多源融合SLAM系统包括第一SLAM系统和第二SLAM系统。如图1所示，用于多源融合SLAM系统的坐标系转换方法M10包括：S100，通过第一SLAM系统获得运动载体在第一坐标系内的第一运动轨迹；S200，通过第二SLAM系统获得运动载体在第二坐标系内的第二运动轨迹；S300，基于第一运动轨迹和第二运动轨迹确定第一坐标系与第二坐标系之间的坐标系转换参数；以及S400，根据坐标系转换参数在第一坐标系与第二坐标系之间进行坐标系转换。
在根据本申请的实施方式中，第一SLAM系统为激光雷达SLAM系统或视觉SLAM系统，也就是说，组成多源融合SLAM系统的第二SLAM系统可以基于激光雷达或视觉传感器(例如双目摄像机)等传感器。
在根据本申请的实施方式中，第二SLAM系统可以是基于无线信标测距的SLAM系统。基于无线信标测距的SLAM系统是一种Range-only SLAM(即，测距SLAM)系统。也就是说，组成多源融合SLAM系统的第二SLAM系统基于无线信标测距技术。
在根据本申请的实施例中，基于无线信标测距的SLAM系统可以是基于UWB的SLAM系统、基于Wi-Fi的SLAM系统、或基于蓝牙的SLAM系统。
在下面的内容中，将以激光雷达SLAM系统作为第一SLAM系统的示例，以基于UWB的SLAM作为第二SLAM系统的示例，对根据本申请的用于多源融合SLAM系统的坐标系转换方法进行描述。显然，基于不同的传感器的具体SLAM系统对于根据本申请的用于多源融合SLAM系统的坐标系转换方法并不产生实质影响。
下面，结合根据申请的实施方式对根据本申请的用于多源融合SLAM系统的坐标系转换方法进行描述。
如上所述，在包括激光雷达SLAM系统和基于UWB的SLAM系统的多源融合SLAM系统中，激光雷达SLAM系统中的激光雷达与基于UWB的SLAM系统的运动UWB定位节点共同设置在运动载体上。具体地，可将激光雷达SLAM系统中的激光雷达(即激光雷达传感器)与基于UWB的SLAM系统的运动UWB定位节点(即UWB传感器)共同设置在运动载体上，通常可以将运动UWB定位节点放置于激光雷达的中心附近，二者随着运动载体共同运动；而固定UWB定位节点的位置是未知的。
如图1所示，在S100中，可以通过第一SLAM系统(激光雷达SLAM系统)获得运动载体在第一坐标系内的第一运动轨迹。
在激光雷达SLAM系统中，可以采用本技术领域中任何合适的方式(激光雷达SLAM算法)来确定激光雷达在激光雷达SLAM系统的全局坐标系内的位置，从而获得运动载体在第一坐标系内的第一运动轨迹。
激光雷达SLAM问题可以表示为一个最大似然估计问题，具体如下：
Figure BDA0002604835160000131
其中*表示估计值，
下标0和k分别表示初始时刻和当前时刻，
Figure BDA0002604835160000132
Figure BDA0002604835160000133
表示一序列激光雷达全局坐标系下的激光雷达传感器位姿包括三维平移和三维旋转，
Figure BDA0002604835160000134
表示当前构建的激光雷达SLAM系统的全局坐标系内的点云地图，
Figure BDA0002604835160000135
表示激光雷达体坐标系下从初始时刻到当前时刻扫描得到的点云序列，
P表示
Figure BDA0002604835160000136
和
Figure BDA0002604835160000137
关于
Figure BDA0002604835160000138
的似然函数。
根据本申请的一个实施方式，在激光雷达SLAM系统中，采用实时激光里程计与建图(Lidar Odometry and Mapping in Realtime，简称LOAM)算法来获得激光雷达在激光雷达SLAM系统的全局坐标系内的位置，从而获得运动载体在第一坐标系内的第一运动轨迹。
具体地，可以通过LOAM算法的激光里程计(Laser Odometry)进行高频测程，以估计激光雷达在两个连续帧之间的运动，并通过LOAM算法的激光建图(Laser Mapping)以较低的频率运行来匹配当前扫描得到的点云和已构建的点云地图，从而获得激光雷达在激光雷达SLAM系统的全局坐标系内的精确位姿估计值。
对于基于其他传感器的第一SLAM系统，同样地可以通过本技术领域中任何合适的方式(SLAM算法)来确定传感器在第一SLAM系统的全局坐标系内的位置，从而获得运动载体在第一坐标系内的第一运动轨迹。
现在继续参照附图1，对根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法进行描述。如图所示，在S200中，可以通过第二SLAM系统(基于UWB的SLAM系统)获得运动载体在第二坐标系内的第二运动轨迹。
在基于UWB的SLAM系统中，可以通过对每对UWB定位节点之间的距离进行测量，以构建欧几里得距离矩阵(EDM)，通过对EDM进行计算，获得包括运动UWB定位节点和固定UWB定位节点的所有UWB定位节点的位置。具体而言，可将基于UWB的SLAM问题描述为：
Figure BDA0002604835160000141
其中，
Figure BDA0002604835160000142
代表在基于UWB的SLAM系统的全局坐标系下所有UWB定位节点的坐标，其中*代表估计值，下标n代表UWB定位节点的数量，下标k代表当前时刻，
Figure BDA0002604835160000143
D代表由UWB定位节点提供的定位节点之间距离测量值所构成的EDM。
可以采用本技术领域中任何合适的方式(基于UWB的SLAM算法)来求解
Figure BDA0002604835160000144
从而获得运动载体在第二坐标系内的第二轨迹，即运动UWB定位节点在基于UWB的SLAM系统的全局坐标系内连续时刻对应的位置。
根据本申请的一个实施方式，在基于UWB的SLAM系统中，通过采用一种降维算法，即多维标度(Multiple Dimension Scaling，简称MDS)算法来计算
Figure BDA0002604835160000151
从而获得运动载体在第二坐标系内的第二轨迹。
根据本申请的另一实施方式，在基于UWB的SLAM系统中，通过采用半定规划(Semidefinite Programming，简称SDP)算法来计算
Figure BDA0002604835160000152
从而获得运动载体在第二坐标系内的第二轨迹。
根据本申请的一个优选实施方式，在基于UWB的SLAM系统中，对第二坐标系以及基于UWB的SLAM系统的运动UWB定位节点和固定UWB定位节点进行位置初始化，以确定第二坐标系以及运动UWB定位节点和固定UWB定位节点的初始位置信息；以及基于初始状态信息，通过扩展卡尔曼滤波器对运动UWB定位节点和固定UWB定位节点的位置信息进行估计，从而获得第二运动轨迹。
具体地，在该优选实施方式中，可以通过合适的方式，在初始时刻对基于UWB的SLAM系统的全局坐标系(即第二坐标系)以及包括运动UWB定位节点和固定UWB定位节点在内的所有UWB定位节点的位置进行位置初始化；在确定UWB定位节点在初始时刻的初始位置信息后，可以基于初始位置信息，通过扩展卡尔曼滤波器(Extended Kalman Filter，简称EKF)对运动UWB定位节点和固定UWB定位节点的位置信息进行估计，即，在后续的每个时刻通过EKF对UWB定位节点的位置进行更新，从而获得第二运动轨迹。
根据本申请的另一个优选实施方式，可以通过MDS算法对第二坐标系以及基于UWB的SLAM系统的运动UWB定位节点和固定UWB定位节点进行位置初始化。然后，如上述优选实施方式中那样，基于初始位置信息，通过EKF对运动UWB定位节点和固定UWB定位节点的位置信息进行估计。通过该优选实施方式，可以满足实时性和鲁棒性要求。
下面结合上述优选实施方式，对EKF的应用进行详细描述。考虑到UWB定位节点的控制输入是未知的，因此在EKF中只使用测量数据来更新定位节点的位置。为了方便起见，省略上标U，那么EKF的系统模型和观测模型分别表示如下:
Figure BDA0002604835160000161
Figure BDA0002604835160000162
其中，dij表示EDM的组成成分，下标k代表当前时刻，
Figure BDA0002604835160000163
Figure BDA0002604835160000164
表示第i个UWB定位节点与第j个UWB定位节点之间的欧几里得距离，该距离可以通过两个定位节点的坐标估计值计算得到，
w和v分别代表系统误差和观测误差，
hij表示如下所示的行向量，
Figure BDA0002604835160000171
其中^代表当前k时刻的估计值。
通过上述EKF，可以计算得到UWB定位节点在当前k时刻的位置估计值
Figure BDA0002604835160000172
从而获得第二运动轨迹。
类似地，对于基于其他传感器的第二SLAM系统，可以通过本技术领域中任何合适的方式(SLAM算法)来确定传感器在第二SLAM系统的全局坐标系内的位置，从而获得运动载体在第二坐标系内的第二运动轨迹。
如图1所示，在S300中，可以基于第一运动轨迹和第二运动轨迹确定第一坐标系与第二坐标系之间的坐标系转换参数。在根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法中，基于所获得的运动载体的运动轨迹来确定坐标系之间的坐标系转换参数，而不是通过预先测量固定UWB定位节点的位置并基于该先验信息获得坐标系之间的坐标系转换参数，以在激光雷达SLAM系统的坐标系与UWB定位节点SLAM系统的坐标系之间进行转换。这样，可以获得运动载体的运动轨迹的同时获得坐标系转换参数，在激光雷达SLAM系统的坐标系与基于UWB的SLAM系统的坐标系之间进行实时转换。
根据本申请的一个优选实施方式，可以对第一运动轨迹和第二运动轨迹同时进行采样，以获得来自第一运动轨迹的多个第一采样点以及对应的来自第二运动轨迹的多个第二采样点，并且多个第一采样点和多个第二采样点均不少于4个；对多个第一采样点和多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及对多个第一中心化采样点和多个第二中心化采样点进行正交普氏分析(OrthogonalProcrustes Analysis)，以确定坐标系转换参数。
在这里，对第一运动轨迹和第二运动轨迹同时进行采样，可以获得分别来自第一运动轨迹和第二运动轨迹的相匹配的点对，点对的数量不少于4个，就可以进行坐标系转换参数的确定。在根据本申请的优选实施方式中，在每个运动轨迹上采样的4个采样点应不共面。
此外，在这里，对多个采样点进行中心化处理，可以通过多个采样点在特定维度上取均值，并从这些采样点中去除该均值。在下面的内容中将对此进行详细描述。
接下来，结合根据本申请的优选实施方式，对用于多源融合SLAM系统的坐标系转换方法进行描述。
在根据本申请的一个优选实施方式中，坐标系转换参数包括激光雷达SLAM系统的全局坐标系(即，第一坐标系)与UWB SLAM系统的全局坐标系(即，第二坐标系)之间的旋转转换矩阵。在该实施方式中，在获得旋转转换矩阵后，就可以在第一坐标系与第二坐标系之间进行坐标系转换。
根据本申请的另一优选实施方式，在激光雷达SLAM系统获得的激光雷达在激光雷达SLAM系统的全局坐标系内的运动轨迹的估计值上采样m(m≥4)个点，并将这些点的坐标作为列向量存入XL；与此同时，在基于UWB的SLAM系统获得的运动UWB定位节点在基于UWB的SLAM系统的全局坐标系内的运动轨迹上也采样对应的m(m≥4)的点，并将这些点的坐标存入XU的列中。矩阵XL和XL如下所示：
Figure BDA0002604835160000191
Figure BDA0002604835160000192
其中xi，yi和zi表示第i个采样点的坐标。
在该优选实施方式中，在获得矩阵XL和XU之后，对矩阵XL和XU分别进行中心化处理，具体地，分别从矩阵XL和XU中去除其均值列向量xL,means和xU,means，也就是将矩阵的每一列减去对应的列向量，得到矩阵
Figure BDA0002604835160000193
和
Figure BDA0002604835160000194
即，对多个第一采样点和多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点。
接下来，对多个第一中心化采样点和多个第二中心化采样点进行正交普氏分析，以确定坐标系转换参数，在这里，即确定旋转转换矩阵。
具体地，旋转转换矩阵
Figure BDA0002604835160000195
可通过求解下式得到：
Figure BDA0002604835160000196
其中，
Figure BDA0002604835160000197
根据本申请的一个优选实施方式，通过以下确定旋转转换矩阵
Figure BDA0002604835160000198
构造
Figure BDA0002604835160000199
通过对上式进行奇异值(SVD)分解，获得U、Σ和V；以及
基于U、Σ和V获得
Figure BDA00026048351600001910
其中，
Figure BDA00026048351600001911
其中，
Figure BDA00026048351600001912
表示多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000201
表示多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵。
如此，根据本申请的用于多源融合SLAM系统的坐标系转换方法，就可以进行坐标系转换。如图1所示，在S400中，可以根据坐标系转换参数在第一坐标系与第二坐标系之间进行坐标系转换。在根据本申请的实施方式中，在获得了旋转转换矩阵
Figure BDA0002604835160000202
之后，就可以通过以下方式将运动载体的位置信息从第一坐标系(基于UWB的SLAM系统的全局坐标系)转换到第二坐标系(激光雷达SLAM系统的全局坐标系)下：
Figure BDA0002604835160000203
需要说明的是，在这里，出于简洁表达的需要，采用从第二SLAM系统(基于UWB的SLAM系统)的全局坐标系转换到第一SLAM系统(激光雷达SLAM系统)的全局坐标系的旋转转换矩阵
Figure BDA0002604835160000204
来表示第一SLAM系统与第二SLAM系统之间的旋转转换矩阵，这是因为作为第一SLAM系统与第二SLAM系统之间的旋转转换矩阵的另一方面，从第一SLAM系统(激光雷达SLAM系统)的全局坐标系转换到第二SLAM系统(基于UWB的SLAM系统)的全局坐标系的旋转转换矩阵
Figure BDA0002604835160000205
是
Figure BDA0002604835160000206
的逆矩阵。
根据本申请的另一优选实施方式，坐标系转换参数包括第一坐标系与第二坐标系之间的位姿转换矩阵。
在该实施方式中，可以以与上述实施方式中相同的方式获得旋转转换矩阵
Figure BDA0002604835160000207
然后通过以下过得位姿转换矩阵
Figure BDA0002604835160000208
Figure BDA0002604835160000209
在获得了旋转转换矩阵
Figure BDA00026048351600002010
之后，则可以通过以下方式将运动载体的位置信息从基于UWB的SLAM系统的全局坐标系转换到激光雷达SLAM系统的全局坐标系下：
Figure BDA0002604835160000211
同样地，出于简洁表达的需要，采用从第二SLAM系统(基于UWB的SLAM系统)的全局坐标系转换到第一SLAM系统(激光雷达SLAM系统)的全局坐标系的位姿转换矩阵
Figure BDA0002604835160000212
来表示第一SLAM系统与第二SLAM系统之间的位姿转换矩阵，这是因为作为第一SLAM系统与第二SLAM系统之间的位姿转换矩阵的另一方面，从第一SLAM系统(激光雷达SLAM系统)的全局坐标系转换到第二SLAM系统(基于UWB的SLAM系统)的全局坐标系的旋转转换矩阵
Figure BDA0002604835160000213
是
Figure BDA0002604835160000214
的逆矩阵。
如上所述，通过根据本申请的实施方式的用于多源融合SLAM系统的坐标系转换方法，能够实现在不依赖先验信息的情况下进行坐标系之间的实时转换，完善了多源融合SLAM系统的适用性；然而由于激光雷达SLAM系统和基于UWB的SLAM系统都存在某种程度的退化，尤其是基于UWB的SLAM系统在某些方向上出现退化，导致通过基于UWB的SLAM系统获得位置信息的可信度降低，影响多源融合SLAM系统的性能，降低了其适用性。
本申请的发明人对上述用于多源融合SLAM系统的坐标系转换方法进行了进一步优化，提出了抗退化技术方案，以应对基于UWB的SLAM系统的退化。
在根据本申请的一个优选实施方式的用于多源融合SLAM系统的坐标系转换方法中，确定基于UWB的SLAM系统的运动UWB定位节点的退化方向；以及在通过扩展卡尔曼滤波器对运动UWB定位节点的位置信息进行估计时，将位置信息的状态值更新量在退化方向上的分量去除，从而降低基于UWB的SLAM系统的退化对运动UWB定位节点的位置信息估计的影响，提高了多源融合SLAM系统的抗退化性能。
在根据本申请的另一优选实施方式的用于多源融合SLAM系统的坐标系转换方法中，根据基于UWB的SLAM系统中的运动UWB定位节点的位置、对应固定UWB定位节点的位置、以及运动UWB定位节点与固定UWB定位节点之间的距离构造费雪信息矩阵；确定费雪信息矩阵的特征值以及对应的特征向量；以及根据与小于预设阈值的特征值相对应的特征向量确定退化方向。
下面，对根据本申请优选实施方式的用于多源融合SLAM系统的坐标系转换方法进行详细描述。
由于固定UWB定位节点的位置估计值在EKF过程中的变化很小，因此可以仅考虑运动UWB定位节点的退化。
由于所有状态参量都在UWB全局坐标系下，为了方便起见，可以省略上标U。令ξ表示运动UWB定位节点的坐标估计值，ξ＝[xu,yu,zu]T，d表示由每个固定UWB定位节点与运动UWB定位节点之间的距离测量值组成的向量，那么运动UWB定位节点的观测方程可以表示为：
Figure BDA0002604835160000221
其中，di表示第i个固定UWB定位节点与运动UWB定位节点之间的距离测量值，
εi表示测量噪声，
Figure BDA0002604835160000222
表示通过二者坐标估计值计算得到的欧几里得距离，
其中，
Figure BDA0002604835160000223
其中，xui，yui和zui代表第i个固定UWB定位节点的坐标估计值。
假设εi是均值为0、方差为σ的独立高斯分布噪声，那么似然函数P(di,ξ)可以表示为：
Figure BDA0002604835160000224
那么对数似然函数logP(d,ξ)的导数为：
Figure BDA0002604835160000231
据此，可以获得费雪信息矩阵I(ξ)，
Figure BDA0002604835160000232
其中，E表示期望函数。
进一步，将
Figure BDA0002604835160000233
的表达式代入上式，可以得到：
Figure BDA0002604835160000234
这里，费雪信息矩阵I(ξ)表示在ξ的估计过程中观测数据d所能提供的平均信息量，I(ξ)的特征值越小，那么对应的特征向量方向估计值的不确定度越大。
可以用e表示I(ξ)的特征值，用v表示对应的特征向量。
对于第i个特征值ei，预设阈值ethr，如果ei<ethr，那么对应的特征向量vi则代表发生退化的方向，在此情况下，微小的测量噪声都会对vi方向上ξ的估计值造成巨大的误差。
因此，可以令Δξ代表通过EKF算法获得的UWB定位节点在每时刻估计得到的状态更新量，其中，Δξ＝ξk+1-ξk，ξk和ξk+1分别表示UWB定位节点在k和k+1时刻各UWB定位节点的位置估计值。
现在，可以从Δξ中将退化方向vi上的分量去除，保留在其他状态良好方向上的分量，从而降低基于UWB的SLAM系统的退化对运动UWB定位节点的位置信息估计的影响，具体的算法如下：
Δξ＝Δξ-(ΔξTvi)vi。
根据本申请的另一方面，还提出了一种用于多源融合SLAM系统的坐标系转换装置，下面参照附图并结合实施方式对该装置进行详细说明。
图2示出了根据本申请实施方式的用于多源融合SLAM系统的坐标系转换装置的框图，其中多源融合SLAM系统包括第一SLAM系统和第二SLAM系统。
如图2所示，用于多源融合SLAM系统的坐标系转换装置10包括：第一运动轨迹确定模块100，该第一运动轨迹确定模块100通过第一SLAM系统获得运动载体在第一坐标系内的第一运动轨迹；第二运动轨迹确定模块200，该第二运动轨迹确定模块200通过第二SLAM系统获得运动载体在第二坐标系内的第二运动轨迹；坐标系转换参数确定模块300，该坐标系转换参数确定模块300基于第一运动轨迹和第二运动轨迹确定第一坐标系与第二坐标系之间的坐标系转换参数；以及坐标系转换模块400，该坐标系转换模块400根据坐标系转换参数在第一坐标系与第二坐标系之间进行坐标系转换。
如上所述，在根据本申请的实施方式中，第一SLAM系统为激光雷达SLAM系统或视觉SLAM系统，也就是说，组成多源融合SLAM系统的第二SLAM系统可以基于激光雷达或视觉传感器(例如双目摄像机)等传感器。
在根据本申请的实施方式中，第二SLAM系统可以是基于无线信标测距的SLAM系统。基于无线信标测距的SLAM系统是一种Range-only SLAM(即，测距SLAM)系统。也就是说，组成多源融合SLAM系统的第二SLAM系统基于无线信标测距技术。
在根据本申请的实施例中，基于无线信标测距的SLAM系统可以是基于UWB的SLAM系统、基于Wi-Fi的SLAM系统、或基于蓝牙的SLAM系统。
在下面的内容中，将以激光雷达SLAM系统作为第一SLAM系统的示例，以基于UWB的SLAM作为第二SLAM系统的示例，对根据本申请的用于多源融合SLAM系统的坐标系转换装置进行描述。显然，基于不同的传感器的具体SLAM系统对于根据本申请的用于多源融合SLAM系统的坐标系转换装置并不产生实质影响。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，坐标系转换参数确定模块300还对第一运动轨迹和第二运动轨迹同时进行采样，以获得来自第一运动轨迹的多个第一采样点以及对应的来自第二运动轨迹的多个第二采样点，并且多个第一采样点和多个第二采样点均不少于4个；对多个第一采样点和多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及对多个第一中心化采样点和多个第二中心化采样点进行正交普氏分析，以确定坐标系转换参数。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，坐标系转换参数包括第一坐标系与第二坐标系之间的旋转转换矩阵。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，通过以下确定上述旋转转换矩阵，
构造
Figure BDA0002604835160000251
通过奇异值分解，获得U、Σ和V；以及
基于U、Σ和V获得
Figure BDA0002604835160000252
其中，
Figure BDA0002604835160000253
其中，
Figure BDA0002604835160000254
表示所述多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000255
表示所述多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，以及
Figure BDA0002604835160000256
表示所述旋转转换矩阵。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，通过将XL去除xL,means获得
Figure BDA0002604835160000257
以及通过将XU去除xU,means获得
Figure BDA0002604835160000261
其中，XL表示多个第一采样点对应的矩阵，
Figure BDA0002604835160000262
其中，
Figure BDA0002604835160000263
和
Figure BDA0002604835160000264
表示来自所述第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xL,means表示XL的均值列向量，
其中，XU表示所述多个第二采样点对应的矩阵，
Figure BDA0002604835160000265
其中，
Figure BDA0002604835160000266
和
Figure BDA0002604835160000267
表示来自所述第二运动轨迹的第i个采样点的坐标，
xU,means表示XU的均值列向量。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，坐标系转换参数包括第一坐标系与第二坐标系之间的位姿转换矩阵。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，通过以下确定上述位姿转换矩阵，
构造
Figure BDA0002604835160000268
通过奇异值分解，获得U、Σ和V；
基于U、Σ和V获得
Figure BDA0002604835160000269
其中，
Figure BDA00026048351600002610
以及
基于
Figure BDA00026048351600002611
获得
Figure BDA00026048351600002612
其中，
Figure BDA00026048351600002613
其中，
Figure BDA0002604835160000271
表示多个第一中心化采样点对应的矩阵，
Figure BDA0002604835160000272
表示多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，
Figure BDA0002604835160000273
表示旋转转换矩阵，
xL,means表示XL的均值列向量，
其中，XL表示多个第一采样点对应的矩阵，
Figure BDA0002604835160000274
其中，
Figure BDA0002604835160000275
和
Figure BDA0002604835160000276
表示来自第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xU,means表示XU的均值列向量，
其中，XU表示多个第二采样点对应的矩阵，
Figure BDA0002604835160000277
其中，
Figure BDA0002604835160000278
和
Figure BDA0002604835160000279
表示来自第二运动轨迹的第i个采样点的坐标，
Figure BDA00026048351600002710
表示所述位姿转换矩阵，
通过将XL去除xL,means获得
Figure BDA00026048351600002711
以及
将XU去除xU,means均值列向量获得
Figure BDA00026048351600002712
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，第二运动轨迹确定模块200还对第二坐标系以及基于UWB的SLAM系统的运动UWB定位节点和固定UWB定位节点进行位置初始化，以确定第二坐标系以及运动UWB定位节点和固定UWB定位节点的初始位置信息；以及基于初始位置信息，通过扩展卡尔曼滤波器对运动UWB定位节点和固定UWB定位节点的位置信息进行估计，从而获得第二运动轨迹。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，第二运动轨迹确定模块200还通过MDS算法对第二坐标系以及第二SLAM系统(基于UWB的SLAM系统)的运动UWB定位节点和固定UWB定位节点进行位置初始化。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，第二运动轨迹确定模块200还确定第二SLAM系统(基于UWB的SLAM系统)的运动定位节点的退化方向；以及在通过扩展卡尔曼滤波器对运动定位节点的位置信息进行估计时，将位置信息的状态值更新量在退化方向上的分量去除，从而降低第二SLAM系统的退化对运动定位节点的位置信息估计的影响。
根据本申请的一个实施方式，在用于多源融合SLAM系统的坐标系转换装置10中，第二运动轨迹确定模块200还根据第二SLAM系统(基于UWB的SLAM系统)中的运动定位节点的位置、对应固定定位节点的位置、以及运动定位节点与固定定位节点之间的距离构造费雪信息矩阵；确定费雪信息矩阵的特征值以及对应的特征向量；以及根据与小于预设阈值的特征值相对应的特征向量确定退化方向。
为了评估根据本申请的用于多源融合SLAM系统的坐标系转换方法/装置的性能，发明人还组织并进行了相关试验。下面结合附图3-图7对试验情况及其结果进行描述。
图3示出了试验中所使用的场所的示意图。如图3所示，在试验场所中安置了8个固定UWB定位节点，值得注意的是所有的固定UWB定位节点几乎都处于同一水平面。每个固定UWB定位节点都与一个无线通信模块连接，该模块将传感器数据上传到服务器。每个UWB定位节点的测量误差约为0.1m。
图4示出了试验中所使用的手持设备的示意图。如图4所示，手持设备由一台便携式电脑(Intel NUC8i7h)、一台激光雷达传感器(Velodyne VLP16)、一块无线通信模块和一块屏幕组成，此外，在激光雷达传感器的顶部设置有一个UWB传感器(即，运动UWB定位节点)。在试验中，还通过一台全站仪标定固定UWB定位节点在激光雷达SLAM系统的全局坐标系内的精确坐标。
在试验过程中，将手持设备从地上举起，然后绕着试验场所慢速运动一圈，从而可以对根据本申请的用于多源融合SLAM系统的坐标系转换方法/装置的性能进行测试和评估。
图5示出了试验中未采用根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法时所获得的点云地图，与此相对，图6示出了试验中采用根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法时所获得的点云地图，图中所示圆点表示UWB定位节点。对比图5和图6，可以清楚看到，图5所示的点云地图与实际情况相差过多，而图6所示的点云地图中的UWB定位节点的位置与实际位置非常接近，这说明采用根据本申请实施方式的用于多源融合SLAM系统的坐标系转换方法能够实时地进行坐标系的转换。
图7示出了试验的定位平均误差曲线图，其中示出了采用根据本申请的坐标系转换方法的抗退化技术方案与未采用抗退化技术方案的对比、以及平均误差与采样点数量之间的关系。从图7中可以看到，随着采样点对数量的增加，平均误差(垂直误差和水平误差)明显逐渐下降。
此外，从图7中还可以看到，在采样点对数量为50的情况下，当未采用抗退化技术方案时，定位的水平平均误差为0.33m，垂直平均误差为1.50m；而与此相对，当采用抗退化技术方案时，定位的水平平均误差和垂直平均误差分别为0.06m和0.54m，显然采用抗退化技术方案的平均误差低于未采用抗退化技术方案的平均误差，这说明通过根据本申请的坐标系转换方法，可以进一步提高多源融合SLAM系统的性能。
根据本申请的又一方面，还提出了一种多源融合SLAM系统，其至少包括第一SLAM系统和第二SLAM系统以及如上所述的用于多源融合SLAM系统的坐标系转换装置。
根据本申请的实施方式可以硬件、软件或其组合的形式来实现。此外，本申请的一个方面，提出了包括用于实现根据本申请实施方式的复合导航信号接收方法的可执行指令的计算机程序。此类计算机程序可使用例如光学或磁性可读介质、芯片、ROM、PROM或其他易失性或非易失性设备的任何形式的存储器来存储。根据本申请的一种实施例，还提出了存储此类计算机程序的机器可读存储器。
在本说明书的描述中，参考术语“一个实施例/方式”、“一些实施例/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例/方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例/方式或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例/方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例/方式或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例/方式或示例以及不同实施例/方式或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
本领域的技术人员应当理解，上述实施方式仅仅是为了清楚地说明本申请，而并非是对本申请的范围进行限定。对于所属领域的技术人员而言，在上述公开的基础上还可以做出其它变化或变型，并且这些变化或变型仍处于本申请的范围内。

Claims (27)

1.用于多源融合SLAM系统的坐标系转换方法，其中，所述多源融合SLAM系统至少包括第一SLAM系统和第二SLAM系统，所述用于多源融合SLAM系统的坐标系转换方法包括：
通过所述第一SLAM系统获得运动载体在第一坐标系内的第一运动轨迹；
通过所述第二SLAM系统获得所述运动载体在第二坐标系内的第二运动轨迹；
基于所述第一运动轨迹和所述第二运动轨迹确定所述第一坐标系与所述第二坐标系之间的坐标系转换参数；以及
根据所述坐标系转换参数在所述第一坐标系与所述第二坐标系之间进行坐标系转换。
2.如权利要求1所述的用于多源融合SLAM系统的坐标系转换方法，其中，
对所述第一运动轨迹和所述第二运动轨迹同时进行采样，以获得来自所述第一运动轨迹的多个第一采样点以及对应的来自所述第二运动轨迹的多个第二采样点，并且所述多个第一采样点和所述多个第二采样点均不少于4个；
对所述多个第一采样点和所述多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及
对所述多个第一中心化采样点和所述多个第二中心化采样点进行正交普氏分析，以确定所述坐标系转换参数。
3.如权利要求2所述的用于多源融合SLAM系统的坐标系转换方法，其中，
所述坐标系转换参数包括所述第一坐标系与所述第二坐标系之间的旋转转换矩阵。
4.如权利要求3所述的用于多源融合SLAM系统的坐标系转换方法，其中，通过以下确定所述旋转转换矩阵，
构造
Figure FDA0002604835150000021
通过奇异值分解，获得U、Σ和V；以及
基于U、Σ和V获得
Figure FDA0002604835150000022
其中，
Figure FDA0002604835150000023
其中，
Figure FDA0002604835150000024
表示所述多个第一中心化采样点对应的矩阵，
Figure FDA0002604835150000025
表示所述多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，以及
Figure FDA0002604835150000026
表示所述旋转转换矩阵。
5.如权利要求4所述的用于多源融合SLAM系统的坐标系转换方法，其中，通过将XL去除xL，means获得
Figure FDA0002604835150000027
以及通过将XU去除xU,means获得
Figure FDA0002604835150000028
其中，XL表示所述多个第一采样点对应的矩阵，
Figure FDA0002604835150000029
其中，
Figure FDA00026048351500000210
和
Figure FDA00026048351500000211
表示来自所述第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xL,means表示XL的均值列向量，
其中，XU表示所述多个第二采样点对应的矩阵，
Figure FDA00026048351500000212
其中，
Figure FDA00026048351500000213
和
Figure FDA00026048351500000214
表示来自所述第二运动轨迹的第i个采样点的坐标，
xU,means表示XU的均值列向量。
6.如权利要求2所述的用于多源融合SLAM系统的坐标系转换方法，其中，
所述坐标系转换参数包括所述第一坐标系与所述第二坐标系之间的位姿转换矩阵。
7.如权利要求6所述的用于多源融合SLAM系统的坐标系转换方法，其中，通过以下确定所述位姿转换矩阵，
构造
Figure FDA0002604835150000031
通过奇异值分解，获得U、Σ和V；
基于U、Σ和V获得
Figure FDA0002604835150000032
其中，
Figure FDA0002604835150000033
以及
基于
Figure FDA0002604835150000034
获得
Figure FDA0002604835150000035
其中，
Figure FDA0002604835150000036
其中，
Figure FDA0002604835150000037
表示所述多个第一中心化采样点对应的矩阵，
Figure FDA0002604835150000038
表示所述多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，
Figure FDA0002604835150000039
表示旋转转换矩阵，
xL,means表示XL的均值列向量，
其中，XL表示所述多个第一采样点对应的矩阵，
Figure FDA00026048351500000310
其中，
Figure FDA00026048351500000311
和
Figure FDA00026048351500000312
表示来自所述第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xU,means表示XU的均值列向量，
其中，XU表示所述多个第二采样点对应的矩阵，
Figure FDA0002604835150000041
其中，
Figure FDA0002604835150000042
和
Figure FDA0002604835150000043
表示来自所述第二运动轨迹的第i个采样点的坐标，
Figure FDA0002604835150000044
表示所述位姿转换矩阵，
通过将XL去除xL，means获得
Figure FDA0002604835150000045
以及
将XU去除xU,means均值列向量获得
Figure FDA0002604835150000046
8.如前述权利要求任一项所述的用于多源融合SLAM系统的坐标系转换方法，其中，所述第一SLAM系统为激光雷达SLAM系统或视觉SLAM系统。
9.如前述权利要求任一项所述的用于多源融合SLAM系统的坐标系转换方法，其中，所述第二SLAM系统为基于无线信标测距的SLAM系统，所述基于无线信标测距的SLAM系统包括运动定位节点和固定定位节点，
所述用于多源融合SLAM系统的坐标系转换方法还包括：
在所述第二SLAM系统中，对所述第二坐标系以及所述第二SLAM系统的运动定位节点和固定定位节点进行位置初始化，以确定所述第二坐标系以及运动定位节点和固定定位节点的初始位置信息；以及
基于所述初始位置信息，通过扩展卡尔曼滤波器对运动定位节点和固定定位节点的位置信息进行估计，从而获得所述第二运动轨迹。
10.如权利要求9所述的用于多源融合SLAM系统的坐标系转换方法，其中，
通过MDS算法对所述第二坐标系以及所述第二SLAM系统的运动定位节点和固定定位节点进行位置初始化。
11.如权利要求9或10所述的用于多源融合SLAM系统的坐标系转换方法，还包括：
确定所述第二SLAM系统的运动定位节点的退化方向；以及
在通过扩展卡尔曼滤波器对运动定位节点的位置信息进行估计时，将所述位置信息的状态值更新量在退化方向上的分量去除，从而降低所述第二SLAM系统的退化对运动定位节点的位置信息估计的影响。
12.如权利要求11所述的用于多源融合SLAM系统的坐标系转换方法，其中，
根据所述第二SLAM系统中的运动定位节点的位置、对应固定定位节点的位置、以及所述运动定位节点与所述固定定位节点之间的距离构造费雪信息矩阵；
确定所述费雪信息矩阵的特征值以及对应的特征向量；以及
根据与小于预设阈值的特征值相对应的特征向量确定所述退化方向。
13.如权利要求8所述的用于多源融合SLAM系统的坐标系转换方法，其中，所述基于无线信标测距的SLAM系统为基于UWB的SLAM系统、基于Wi-Fi的SLAM系统或基于蓝牙的SLAM系统。
14.用于多源融合SLAM系统的坐标系转换装置，其中，所述多源融合SLAM系统至少包括第一SLAM系统和第二SLAM系统，所述用于多源融合SLAM系统的坐标系转换装置包括：
第一运动轨迹确定模块，所述第一运动轨迹确定模块通过所述第一SLAM系统获得所述运动载体在第一坐标系内的第一运动轨迹；
第二运动轨迹确定模块，所述第二运动轨迹确定模块通过所述第二SLAM系统获得所述运动载体在第二坐标系内的第二运动轨迹；
坐标系转换参数确定模块，所述坐标系转换参数确定模块基于所述第一运动轨迹和所述第二运动轨迹确定所述第一坐标系与所述第二坐标系之间的坐标系转换参数；以及
坐标系转换模块，所述坐标系转换模块根据所述坐标系转换参数在所述第一坐标系与所述第二坐标系之间进行坐标系转换。
15.如权利要求14所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述坐标系转换参数确定模块还
对所述第一运动轨迹和所述第二运动轨迹同时进行采样，以获得来自所述第一运动轨迹的多个第一采样点以及对应的来自所述第二运动轨迹的多个第二采样点，并且所述多个第一采样点和所述多个第二采样点均不少于4个；
对所述多个第一采样点和所述多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及
对所述多个第一中心化采样点和所述多个第二中心化采样点进行正交普氏分析，以确定所述坐标系转换参数。
16.如权利要求15所述的用于多源融合SLAM系统的坐标系转换装置，其中，
所述坐标系转换参数包括所述第一坐标系与所述第二坐标系之间的旋转转换矩阵。
17.如权利要求16所述的用于多源融合SLAM系统的坐标系转换装置，其中，通过以下确定所述旋转转换矩阵，
构造
Figure FDA0002604835150000061
通过奇异值分解，获得U、Σ和V；以及
基于U、Σ和V获得
Figure FDA0002604835150000062
其中，
Figure FDA0002604835150000063
其中，
Figure FDA0002604835150000071
表示所述多个第一中心化采样点对应的矩阵，
Figure FDA0002604835150000072
表示所述多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，以及
Figure FDA0002604835150000073
表示所述旋转转换矩阵。
18.如权利要求17所述的用于多源融合SLAM系统的坐标系转换装置，其中，通过将XL去除xL,means获得
Figure FDA0002604835150000074
以及通过将XU去除xU，means获得
Figure FDA0002604835150000075
其中，XL表示所述多个第一采样点对应的矩阵，
Figure FDA0002604835150000076
其中，
Figure FDA0002604835150000077
和
Figure FDA0002604835150000078
表示来自所述第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xL,means表示XL的均值列向量，
其中，XU表示所述多个第二采样点对应的矩阵，
Figure FDA0002604835150000079
其中，
Figure FDA00026048351500000710
和
Figure FDA00026048351500000711
表示来自所述第二运动轨迹的第i个采样点的坐标，
xU,means表示XU的均值列向量。
19.如权利要求15所述的用于多源融合SLAM系统的坐标系转换装置，其中，
所述坐标系转换参数包括所述第一坐标系与所述第二坐标系之间的位姿转换矩阵。
20.如权利要求19所述的用于多源融合SLAM系统的坐标系转换装置，其中，通过以下确定所述位姿转换矩阵，
构造
Figure FDA0002604835150000081
通过奇异值分解，获得U、Σ和V；
基于U、Σ和V获得
Figure FDA0002604835150000082
其中，
Figure FDA0002604835150000083
以及
基于
Figure FDA0002604835150000084
获得
Figure FDA0002604835150000085
其中，
Figure FDA0002604835150000086
其中，
Figure FDA0002604835150000087
表示所述多个第一中心化采样点对应的矩阵，
Figure FDA0002604835150000088
表示所述多个第二中心化采样点对应的矩阵，
U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，
Figure FDA0002604835150000089
表示旋转转换矩阵，
xL,means表示XL的均值列向量，
其中，XL表示所述多个第一采样点对应的矩阵，
Figure FDA00026048351500000810
其中，
Figure FDA00026048351500000811
和
Figure FDA00026048351500000812
表示来自所述第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，
xU,means表示XU的均值列向量，
其中，XU表示所述多个第二采样点对应的矩阵，
Figure FDA00026048351500000813
其中，
Figure FDA0002604835150000091
和
Figure FDA0002604835150000092
表示来自所述第二运动轨迹的第i个采样点的坐标，
Figure FDA0002604835150000093
表示所述位姿转换矩阵，
通过将XL去除xL,means获得
Figure FDA0002604835150000094
以及
将XU去除xU,means均值列向量获得
Figure FDA0002604835150000095
21.如权利要求14-20中任一项所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述第一SLAM系统为激光雷达SLAM系统或视觉SLAM系统。
22.如权利要求14-21中任一项所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述第二SLAM系统为基于无线信标测距的SLAM系统，所述基于无线信标测距的SLAM系统包括运动定位节点和固定定位节点，
所述第二运动轨迹确定模块还
对所述第二坐标系以及所述第二SLAM系统的运动定位节点和固定定位节点进行位置初始化，以确定所述第二坐标系以及运动定位节点和固定定位节点的初始位置信息；以及
基于所述初始位置信息，通过扩展卡尔曼滤波器对运动定位节点和固定定位节点的位置信息进行估计，从而获得所述第二运动轨迹。
23.如权利要求22所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述第二运动轨迹确定模块还
通过MDS算法对所述第二坐标系以及所述第二SLAM系统的运动定位节点和固定定位节点进行位置初始化。
24.如权利要求22或23所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述第二运动轨迹确定模块还
确定所述第二SLAM系统的运动定位节点的退化方向；以及
在通过扩展卡尔曼滤波器对运动定位节点的位置信息进行估计时，将所述位置信息的状态值更新量在退化方向上的分量去除，从而降低所述第二SLAM系统的退化对运动UWB定位节点的位置信息估计的影响。
25.如权利要求24所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述第二运动轨迹确定模块还
根据所述第二SLAM系统中的运动定位节点的位置、对应固定定位节点的位置、以及所述运动定位节点与所述固定定位节点之间的距离构造费雪信息矩阵；
确定所述费雪信息矩阵的特征值以及对应的特征向量；以及
根据与小于预设阈值的特征值相对应的特征向量确定所述退化方向。
26.如权利要求14-25中任一项所述的用于多源融合SLAM系统的坐标系转换装置，其中，所述基于无线信标测距的SLAM系统为基于UWB的SLAM系统、基于Wi-Fi的SLAM系统或基于蓝牙的SLAM系统。
27.多源融合SLAM系统，其至少包括第一SLAM系统和第二SLAM系统以及如权利要求14-26中任一项所述的用于多源融合SLAM系统的坐标系转换装置。
CN202010735376.2A 2020-07-28 2020-07-28 坐标系转换方法、装置及包含该装置的多源融合slam系统 Pending CN114022519A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010735376.2A CN114022519A (zh) 2020-07-28 2020-07-28 坐标系转换方法、装置及包含该装置的多源融合slam系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010735376.2A CN114022519A (zh) 2020-07-28 2020-07-28 坐标系转换方法、装置及包含该装置的多源融合slam系统

Publications (1)

Publication Number Publication Date
CN114022519A true CN114022519A (zh) 2022-02-08

Family

ID=80054095

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010735376.2A Pending CN114022519A (zh) 2020-07-28 2020-07-28 坐标系转换方法、装置及包含该装置的多源融合slam系统

Country Status (1)

Country Link
CN (1) CN114022519A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114964212A (zh) * 2022-06-02 2022-08-30 广东工业大学 面向未知空间探索的多机协同融合定位与建图方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108337915A (zh) * 2017-12-29 2018-07-27 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品
CN109752725A (zh) * 2019-01-14 2019-05-14 天合光能股份有限公司 一种低速商用机器人、定位导航方法及定位导航系统
CN110146089A (zh) * 2019-06-21 2019-08-20 合肥小步智能科技有限公司 一种基于嵌入式ai计算平台的巡检机器人定位方法
WO2020053215A1 (de) * 2018-09-14 2020-03-19 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. System und verfahren zur funkwellenbasierten ortung und koordinatentransformation

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108337915A (zh) * 2017-12-29 2018-07-27 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品
WO2020053215A1 (de) * 2018-09-14 2020-03-19 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. System und verfahren zur funkwellenbasierten ortung und koordinatentransformation
CN109752725A (zh) * 2019-01-14 2019-05-14 天合光能股份有限公司 一种低速商用机器人、定位导航方法及定位导航系统
CN110146089A (zh) * 2019-06-21 2019-08-20 合肥小步智能科技有限公司 一种基于嵌入式ai计算平台的巡检机器人定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
HAOYU ZHOU ET AL.: "UWB/Lidar Coordinate Matching Method With Anti-Degeneration Capability", 《IEEE SENSORS COUNCIL》, vol. 21, no. 3, 28 February 2021 (2021-02-28) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114964212A (zh) * 2022-06-02 2022-08-30 广东工业大学 面向未知空间探索的多机协同融合定位与建图方法

Similar Documents

Publication Publication Date Title
CN109916410B (zh) 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN105588566B (zh) 一种基于蓝牙与mems融合的室内定位系统及方法
CN105589064B (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN108521627B (zh) 基于HMM的wifi和地磁融合的室内定位系统及方法
CN111288989B (zh) 一种小型无人机视觉定位方法
Xu et al. An improved indoor localization method for mobile robot based on WiFi fingerprint and AMCL
Chai et al. 2D/3D indoor navigation based on multi-sensor assisted pedestrian navigation in Wi-Fi environments
Fernández-Moral et al. Extrinsic calibration of a set of 2D laser rangefinders
Kong et al. Hybrid indoor positioning method of BLE and PDR based on adaptive feedback EKF with low BLE deployment density
US20220136832A1 (en) Method and system for magnetic-based collaborative positioning
CN115031723A (zh) 一种uwb定位精度提升办法
CN114022519A (zh) 坐标系转换方法、装置及包含该装置的多源融合slam系统
Alves et al. Cost-effective indoor localization for autonomous robots using kinect and wifi sensors
CN112762929B (zh) 一种智能导航方法、装置和设备
Hu et al. 1d-lrf aided visual-inertial odometry for high-altitude mav flight
CN112180396A (zh) 一种激光雷达定位及地图创建方法
Ebner et al. On prior navigation knowledge in multi sensor indoor localisation
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Duan et al. A low‐drift and real‐time localisation and mapping method for handheld hemispherical view LiDAR‐IMU integration system
CN115951369A (zh) 面向复杂港口环境的多传感器融合定位方法
Liu et al. A calibrated-RSSI/PDR/Map intergrated system based on a novel particle filter for indoor navigation
Wang et al. WiFi-PDR indoor fusion positioning based on EKF
Sotelo et al. Low level controller for a POMDP based on WiFi observations
Weyers et al. Improving occupancy grid FastSLAM by integrating navigation sensors
CN114089746B (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