[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN113237485A - 一种基于多传感器融合的slam方法与系统 - Google Patents

一种基于多传感器融合的slam方法与系统 Download PDF

Info

Publication number
CN113237485A
CN113237485A CN202110505826.3A CN202110505826A CN113237485A CN 113237485 A CN113237485 A CN 113237485A CN 202110505826 A CN202110505826 A CN 202110505826A CN 113237485 A CN113237485 A CN 113237485A
Authority
CN
China
Prior art keywords
coordinate system
measurement information
geomagnetic field
carrier coordinate
field intensity
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.)
Withdrawn
Application number
CN202110505826.3A
Other languages
English (en)
Inventor
徐景焕
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Qinglong Technology Co ltd
Original Assignee
Beijing Qinglong 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 Beijing Qinglong Technology Co ltd filed Critical Beijing Qinglong Technology Co ltd
Priority to CN202110505826.3A priority Critical patent/CN113237485A/zh
Publication of CN113237485A publication Critical patent/CN113237485A/zh
Withdrawn legal-status Critical Current

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明提供了一种基于多传感器融合的SLAM方法与系统，属于路径规划技术领域，本发明提供的一种基于多传感器融合的SLAM方法，首先获取载体坐标系中的地磁场强度实际测量信息和加速度实际测量信息，并利用偏差补偿公式对陀螺仪测量的角速度进行补偿修正得到实际角速度测量值，并根据实际角速度测量值得到航姿参考系统位姿信息，然后计算单目相机位姿信息，最后利用卡尔曼状态空间模型对单目相机和航姿参考系统的位姿信息进行滤波融合得到多传感器融合数据。本发明通过利用卡尔曼状态空间模型对单目相机位姿信息和航姿参考位姿信息进行滤波融合，大大减少了环境光线对单目相机位姿信息的影响，同时也减轻了运动对单目相机的影响，提高了路径规划精度。

Description

一种基于多传感器融合的SLAM方法与系统
技术领域
本发明属于路径规划技术领域，更具体地说，是涉及一种基于多传感器融合的SLAM方法与系统。
背景技术
SLAM(simultaneous localization and mapping，同步定位与建图)是指搭载特定传感器的主体，在完全未知环境中通过自身传感器感知周围环境和位姿信息进行自身定位，并构建环境的增量式地图，最终完成自身定位与导航。根据搭载的传感器不同SLAM又可分为基于激光雷达的SLAM、基于图像传感器的SLAM、基于惯性导航的SLAM和基于超声波的SLAM等，目前较为成熟的是基于图像传感器的SLAM。
图像传感器具有丰富的图像特征信息，空间特征的重复性低使得基于视觉的三维点云地图具有较好全局定位效果，但由于图像传感器易受环境光线影响且当环境中纹理特征较少时定位精度会下降。
发明内容
本发明的目的在于提供一种基于多传感器融合的SLAM方法与系统，旨在解决现有基于图像传感器的SLAM在地图构建时，易受环境光线影响从而导致机器人在进行导航定位时精度低的问题。
为实现上述目的，本发明采用的技术方案是：一种基于多传感器融合的SLAM方法，包括：
步骤1：获取航姿参考系统上三轴磁力计在载体坐标系中的载体坐标系地磁场强度实际测量信息；
步骤2：获取航姿参考系统上加速度计在载体坐标系中的载体坐标系加速度实际测量信息；
步骤3：根据所述载体坐标系地磁场强度实际测量信息和所述载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值；
步骤4：根据所述实际角速度测量值得到航姿参考系统位姿信息；
步骤5：获取单目相机采集的视觉数据；
步骤6：根据所述视觉数据得到所述单目相机位姿信息；
步骤7：对所述单目相机位姿信息和所述航姿参考系统位姿信息利用卡尔曼状态空间模型进行滤波融合，得到多传感器融合数据；其中，所述卡尔曼状态空间模型为：
Figure BDA0003058390050000021
其中，X(k)为航姿参考系统位姿信息，Z(k)为单目相机位姿信息，
Figure BDA0003058390050000022
为k时刻机器人在世界坐标系中的加速度，W(k)为输入白噪声，V(k)为观测噪声，Δt表示采样时间，
Figure BDA0003058390050000023
步骤8：根据所述多传感器融合数据完成机器人路径规划。
优选的，所述步骤3：根据所述载体坐标系地磁场强度实际测量信息和所述载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值，包括：
步骤3.1：将所述载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息；
步骤3.2：获取三轴磁力计在世界坐标系中的世界坐标系地磁场强度理论测量信息；
步骤3.3：根据所述世界坐标系地磁场强度实际测量信息将所述世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息；
步骤3.4：根据所述载体坐标系地磁场强度实际测量信息与所述载体坐标系地磁场强度理论测量信息之间的偏差，建立地磁场强度偏差模型；
步骤3.5：获取在载体坐标系中加速度计的载体坐标系加速度实际测量信息和载体坐标系加速度理论测量信息；
步骤3.6：根据所述载体坐标系加速度实际测量信息和载体坐标系加速度理论测量信息之间的偏差，建立加速度偏差模型；
步骤3.7：利用偏差补偿公式对所述加速度偏差模型和所述地磁场强度偏差模型对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值。
优选的，所述步骤3.1：将所述载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息，包括：
步骤3.1.1：设立四元数；其中，所述四元数为：
q＝q0+q1i+q2j+q3k
其中，q0为实数，q1为第一系数，q2为第二系数，q3为第三系数；
步骤3.1.2：根据所述四元数得到旋转矩阵；其中，所述旋转矩阵为：
Figure BDA0003058390050000031
步骤3.1.3：采用所述旋转矩阵将所述载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息。
优选的，所述步骤3.3：根据所述世界坐标系地磁场强度实际测量信息将所述世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息，包括：
步骤3.3.1：根据所述世界坐标系地磁场强度实际测量信息与所述世界坐标系地磁场强度理论测量信息向量的模相等，建立模关系公式；其中，所述模关系公式为：
Figure BDA0003058390050000041
其中，
Figure BDA0003058390050000042
表示在世界坐标系中x轴的理论地磁场强度，
Figure BDA0003058390050000043
表示在世界坐标系中z轴的理论地磁场强度，
Figure BDA0003058390050000044
表示在世界坐标系中x轴的实际地磁场强度，
Figure BDA0003058390050000045
表示在世界坐标系中y轴的实际地磁场强度，
Figure BDA0003058390050000046
表示在世界坐标系中z轴的实际地磁场强度；
步骤3.3.2：利用旋转矩阵转换公式将所述世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息；其中，所述旋转矩阵转换公式为：
Figure BDA0003058390050000047
其中，mb表示载体坐标系地磁场强度理论测量信息。
优选的，所述偏差补偿公式为：
Figure BDA0003058390050000048
其中，ei表示系统误差，
Figure BDA0003058390050000049
表示加速度偏差模型在第i时刻的值，
Figure BDA00030583900500000410
表示地磁场强度偏差模型在第i时刻的值，eci表示误差变化率，kp表示比例系数，
Figure BDA00030583900500000411
表示初始比例系数，ΔkP表示比例系数变化值，kI表示积分系数，
Figure BDA00030583900500000412
表示初始积分系数，ΔkI表示积分系数变化值，
Figure BDA00030583900500000413
表示在第i时刻陀螺仪在载体坐标系中的实际角速度测量值。
优选的，所述步骤6：根据所述视觉数据得到所述单目相机位姿信息，包括：
步骤6.1：利用运动轨迹积分公式得到航姿参考系统输出的机器人运动轨迹；其中，所述运动轨迹积分公式为：
Figure BDA0003058390050000051
其中，Pt表示t时刻机器人在世界坐标系中的位置，vt-1表示t-1时刻机器人在世界坐标系的速度，Δt表示采样时间，
Figure BDA0003058390050000052
表示t时刻机器人在世界坐标系中的加速度；
步骤6.2：根据所述视觉数据得到所述单目相机输出的运动轨迹；
步骤6.3：采用运动轨迹解算公式得到尺度因子；
步骤6.4：根据所述尺度因子对所述单目相机输出的运动轨迹进行转换得到所述单目相机位姿信息。
优选的，所述运动轨迹解算公式为：
bi＝sRBC·ci+b0+ei
其中，
Figure BDA0003058390050000053
为航姿参考系统输出的机器人的横坐标，
Figure BDA0003058390050000054
为航姿参考系统输出的机器人的纵坐标，
Figure BDA0003058390050000055
为航姿参考系统输出的机器人的竖坐标，
Figure BDA0003058390050000056
为单目相机输出的机器人的横坐标，
Figure BDA0003058390050000057
为单目相机输出的机器人的纵坐标，
Figure BDA0003058390050000058
为单目相机输出的机器人的竖坐标，s为尺度因子，RBC表示相机坐标系到载体坐标系的旋转矩阵，b0表示相机坐标系到载体坐标系的平移向量，ei为系统误差。
本发明还提供了一种基于多传感器融合的SLAM系统，包括：
地磁场强度实际测量信息获取模块，用于获取航姿参考系统上三轴磁力计在载体坐标系中的载体坐标系地磁场强度实际测量信息；
加速度实际测量信息获取模块，用于获取航姿参考系统上加速度计在载体坐标系中的载体坐标系加速度实际测量信息；
偏差补偿模块，用于根据所述载体坐标系地磁场强度实际测量信息和所述载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值；
航姿参考系统位姿信息计算模块，用于根据所述实际角速度测量值得到航姿参考系统位姿信息；
视觉数据获取模块，用于获取单目相机采集的视觉数据；
单目相机位姿信息计算模块，用于根据所述视觉数据得到所述单目相机位姿信息；
多传感器融合模块，用于对所述单目相机位姿信息和所述航姿参考系统位姿信息利用卡尔曼状态空间模型进行滤波融合，得到多传感器融合数据；其中，所述卡尔曼状态空间模型为：
Figure BDA0003058390050000061
其中，X(k)为航姿参考系统位姿信息，Z(k)为单目相机位姿信息，
Figure BDA0003058390050000062
为k时刻机器人在世界坐标系中的加速度，W(k)为输入白噪声，V(k)为观测噪声，Δt表示采样时间，
Figure BDA0003058390050000063
机器人路径规划模块，用于根据所述多传感器融合数据完成机器人路径规划。
本发明提供的一种基于多传感器融合的SLAM方法与系统的有益效果在于：与现有技术相比，本发明的一种基于多传感器融合的SLAM方法，首先获取载体坐标系中的地磁场强度实际测量信息和加速度实际测量信息，并利用偏差补偿公式对陀螺仪测量的角速度进行补偿修正得到实际角速度测量值，并根据实际角速度测量值得到航姿参考系统位姿信息，然后根据视觉数据得到单目相机位姿信息，最后利用卡尔曼状态空间模型对单目相机位姿信息和航姿参考位姿信息进行滤波融合得到多传感器融合数据。本发明通过利用卡尔曼状态空间模型对对单目相机位姿信息和航姿参考位姿信息进行滤波融合，大大减少了环境光线对单目相机位姿信息的影响，同时也减轻了运动对单目相机的影响，提高了单目相机在快速移动时的定位和跟踪能力。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种基于多传感器融合的SLAM方法流程图；
图2为本发明实施例提供的一种基于多传感器融合的SLAM方法原理图。
具体实施方式
为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明的目的在于提供一种基于多传感器融合的SLAM方法与系统，旨在解决现有基于图像传感器的SLAM在地图构建时，易受环境光线影响从而导致机器人在进行导航定位时精度低的问题。
请一并参阅图1-2，为实现上述目的，本发明采用的技术方案是：一种基于多传感器融合的SLAM方法，包括：
S1：获取航姿参考系统上三轴磁力计在载体坐标系中的载体坐标系地磁场强度实际测量信息；
航姿参考系统即AHRS传感器(Attitude and heading reference system，AHRS)，航姿参考系统通常由陀螺仪，加速度计和三轴磁力计组成。
S2：获取航姿参考系统上加速度计在载体坐标系中的载体坐标系加速度实际测量信息；
陀螺仪用于测量机器人的角速度，对其角速度进行积分可以得到机器人的姿态角等信息，但是在实际应用中，如果单单使用陀螺仪来解算姿态角，会使得到的姿态角产生较大的误差，因此本发明利用偏差补偿公式对陀螺仪测量的角速度进行补偿修正。
S3：根据载体坐标系地磁场强度实际测量信息和载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到陀螺仪在载体坐标系中的实际角速度测量值；
S3具体包括：
S3.1：将载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息；
S3.1具体包括：
S3.1.1：设立四元数；其中，四元数为：
q＝q0+q1i+q2j+q3k
其中，q0为实数，q1为第一系数，q2为第二系数，q3为第三系数；i、j和k均为虚数单位。
S3.1.2：根据四元数得到旋转矩阵；其中，旋转矩阵为：
Figure BDA0003058390050000081
在本发明中的旋转矩阵可根据四元数，通过利用罗德里格斯公式转换得到。
S3.1.3：采用旋转矩阵将载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息。
在实际应用中，将载体坐标系地磁场强度实际测量信息与旋转矩阵相乘即可得到世界坐标系地磁场强度实际测量信息，即
Figure BDA0003058390050000082
表示世界坐标系地磁场强度实际测量信息，
Figure BDA0003058390050000083
表示载体坐标系地磁场强度实际测量信息。
S3.2：获取三轴磁力计在世界坐标系中的世界坐标系地磁场强度理论测量信息；
S3.3：根据世界坐标系地磁场强度实际测量信息将世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息；
S3.3具体包括：
S3.3.1：根据世界坐标系地磁场强度实际测量信息与世界坐标系地磁场强度理论测量信息向量的模相等，建立模关系公式；其中，模关系公式为：
Figure BDA0003058390050000091
其中，
Figure BDA0003058390050000092
表示在世界坐标系中x轴的理论地磁场强度，
Figure BDA0003058390050000093
表示在世界坐标系中z轴的理论地磁场强度，
Figure BDA0003058390050000094
表示在世界坐标系中x轴的实际地磁场强度，
Figure BDA0003058390050000095
表示在世界坐标系中y轴的实际地磁场强度，
Figure BDA0003058390050000096
表示在世界坐标系中z轴的实际地磁场强度；
S3.3.2：利用旋转矩阵转换公式将世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息；其中，旋转矩阵转换公式为：
Figure BDA0003058390050000097
其中，mb表示载体坐标系地磁场强度理论测量信息。
S3.4：根据载体坐标系地磁场强度实际测量信息与载体坐标系地磁场强度理论测量信息之间的偏差，建立地磁场强度偏差模型；
具体的，设在载体坐标系中，磁力计的实际测量值与理论值的地磁场强度偏差模型可以使用向量积表示：
Figure BDA0003058390050000098
其中，
Figure BDA0003058390050000099
表示载体坐标系中磁力计的实际测量值，
Figure BDA00030583900500000910
表示在载体坐标系中x轴的实际地磁场强度，
Figure BDA0003058390050000101
表示在载体坐标系中y轴的实际地磁场强度，
Figure BDA0003058390050000102
表示在载体坐标系中z轴的实际地磁场强度。
S3.5：获取在载体坐标系中加速度计的载体坐标系加速度实际测量信息和载体坐标系加速度理论测量信息；
S3.6：根据载体坐标系加速度实际测量信息和载体坐标系加速度理论测量信息之间的偏差，建立加速度偏差模型；
在实际应用中，设定在世界坐标系中加速度计的理论测量值为aw＝[0 0 g]T，根据刚体运动学方程可得：
ab＝RBWaw，其中ab表示载体坐标系加速度理论测量信息；
在载体坐标系中加速度的实际测量值为
Figure BDA0003058390050000103
则加速度偏差模型可以用向量积表示为：
Figure BDA0003058390050000104
S3.7：利用偏差补偿公式对加速度偏差模型和地磁场强度偏差模型对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到陀螺仪在载体坐标系中的实际角速度测量值。其中，偏差补偿公式为：
Figure BDA0003058390050000105
其中，ei表示系统误差，
Figure BDA0003058390050000106
表示加速度偏差模型在第i时刻的值，
Figure BDA0003058390050000107
表示地磁场强度偏差模型在第i时刻的值，eci表示误差变化率，kp表示比例系数，
Figure BDA0003058390050000108
表示初始比例系数，ΔkP表示比例系数变化值，kI表示积分系数，
Figure BDA0003058390050000109
表示初始积分系数，ΔkI表示积分系数变化值，
Figure BDA00030583900500001010
表示在第i时刻陀螺仪在载体坐标系中的实际角速度测量值。
在实际应用中，
Figure BDA00030583900500001011
在本发明中取值为0，
Figure BDA00030583900500001012
在本发明中取值为1，且ΔkP和ΔkI由系统误差ei和误差变化率eci模糊化模糊整定得到。
S4：根据实际角速度测量值得到航姿参考系统位姿信息；
S5：获取单目相机采集的视觉数据；
S6：根据视觉数据得到单目相机位姿信息；
S6具体包括：
S6.1：利用运动轨迹积分公式得到航姿参考系统输出的机器人运动轨迹；其中，运动轨迹积分公式为：
Figure BDA0003058390050000111
其中，Pt表示t时刻机器人在世界坐标系中的位置，vt-1表示t-1时刻机器人在世界坐标系的速度，Δt表示采样时间，
Figure BDA0003058390050000112
表示t时刻机器人在世界坐标系中的加速度；
S6.2：根据视觉数据得到单目相机输出的运动轨迹；
在本发明中使用ORB-SLAM2系统来得到单目相机输出的运动轨迹。
S6.3：采用运动轨迹解算公式得到尺度因子；
运动轨迹解算公式为：
bi＝sRBC·ci+b0+ei
其中，
Figure BDA0003058390050000113
为航姿参考系统输出的机器人的横坐标，
Figure BDA0003058390050000114
为航姿参考系统输出的机器人的纵坐标，
Figure BDA0003058390050000115
为航姿参考系统输出的机器人的竖坐标，
Figure BDA0003058390050000116
为单目相机输出的机器人的横坐标，
Figure BDA0003058390050000117
为单目相机输出的机器人的纵坐标，
Figure BDA0003058390050000118
为单目相机输出的机器人的竖坐标，s为尺度因子，RBC表示相机坐标系到载体坐标系的旋转矩阵，b0表示相机坐标系到载体坐标系的平移向量，ei为系统误差。
在实际应用中可通过求s使得
Figure BDA0003058390050000119
最小来得到尺度因子，即
Figure BDA00030583900500001110
S6.4：根据尺度因子对单目相机输出的运动轨迹进行转换得到单目相机位姿信息。
S7：对单目相机位姿信息和航姿参考系统位姿信息利用卡尔曼状态空间模型进行滤波融合，得到多传感器融合数据；其中，卡尔曼状态空间模型为：
Figure BDA0003058390050000121
其中，X(k)为航姿参考系统位姿信息，Z(k)为单目相机位姿信息，
Figure BDA0003058390050000122
为k时刻机器人在世界坐标系中的加速度，W(k)为输入白噪声，V(k)为观测噪声，Δt表示采样时间，
Figure BDA0003058390050000123
S8：根据多传感器融合数据完成机器人路径规划。
本发明通过利用卡尔曼状态空间模型对对单目相机位姿信息和航姿参考位姿信息进行滤波融合，大大减少了环境光线对单目相机位姿信息的影响，同时也减轻了运动对单目相机的影响，提高了单目相机在快速移动时的定位和跟踪能力。
本发明还提供了一种基于多传感器融合的SLAM系统，包括：
地磁场强度实际测量信息获取模块，用于获取航姿参考系统上三轴磁力计在载体坐标系中的载体坐标系地磁场强度实际测量信息；
加速度实际测量信息获取模块，用于获取航姿参考系统上加速度计在载体坐标系中的载体坐标系加速度实际测量信息；
偏差补偿模块，用于根据载体坐标系地磁场强度实际测量信息和载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到陀螺仪在载体坐标系中的实际角速度测量值；
航姿参考系统位姿信息计算模块，用于根据实际角速度测量值得到航姿参考系统位姿信息；
视觉数据获取模块，用于获取单目相机采集的视觉数据；
单目相机位姿信息计算模块，用于根据视觉数据得到单目相机位姿信息；
多传感器融合模块，用于对单目相机位姿信息和航姿参考系统位姿信息利用卡尔曼状态空间模型进行滤波融合，得到多传感器融合数据；其中，卡尔曼状态空间模型为：
Figure BDA0003058390050000131
其中，X(k)为航姿参考系统位姿信息，Z(k)为单目相机位姿信息，
Figure BDA0003058390050000132
为k时刻机器人在世界坐标系中的加速度，W(k)为输入白噪声，V(k)为观测噪声，Δt表示采样时间，
Figure BDA0003058390050000133
机器人路径规划模块，用于根据多传感器融合数据完成机器人路径规划。
本发明提供的一种基于多传感器融合的SLAM方法与系统的有益效果在于：与现有技术相比，本发明的一种基于多传感器融合的SLAM方法，首先获取载体坐标系中的地磁场强度实际测量信息和加速度实际测量信息，并利用偏差补偿公式对陀螺仪测量的角速度进行补偿修正得到实际角速度测量值，并根据实际角速度测量值得到航姿参考系统位姿信息，然后根据视觉数据得到单目相机位姿信息，最后利用卡尔曼状态空间模型对单目相机位姿信息和航姿参考位姿信息进行滤波融合得到多传感器融合数据。本发明通过利用卡尔曼状态空间模型对对单目相机位姿信息和航姿参考位姿信息进行滤波融合，大大减少了环境光线对单目相机位姿信息的影响，同时也减轻了运动对单目相机的影响，提高了单目相机在快速移动时的定位和跟踪能力。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于多传感器融合的SLAM方法，其特征在于，包括：
步骤1：获取航姿参考系统上三轴磁力计在载体坐标系中的载体坐标系地磁场强度实际测量信息；
步骤2：获取航姿参考系统上加速度计在载体坐标系中的载体坐标系加速度实际测量信息；
步骤3：根据所述载体坐标系地磁场强度实际测量信息和所述载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值；
步骤4：根据所述实际角速度测量值得到航姿参考系统位姿信息；
步骤5：获取单目相机采集的视觉数据；
步骤6：根据所述视觉数据得到所述单目相机位姿信息；
步骤7：对所述单目相机位姿信息和所述航姿参考系统位姿信息利用卡尔曼状态空间模型进行滤波融合，得到多传感器融合数据；其中，所述卡尔曼状态空间模型为：
Figure FDA0003058390040000011
其中，X(k)为航姿参考系统位姿信息，Z(k)为单目相机位姿信息，
Figure FDA0003058390040000012
为k时刻机器人在世界坐标系中的加速度，W(k)为输入白噪声，V(k)为观测噪声，Δt表示采样时间，
Figure FDA0003058390040000013
步骤8：根据所述多传感器融合数据完成机器人路径规划。
2.如权利要求1所述的一种基于多传感器融合的SLAM方法，其特征在于，所述步骤3：根据所述载体坐标系地磁场强度实际测量信息和所述载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值，包括：
步骤3.1：将所述载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息；
步骤3.2：获取三轴磁力计在世界坐标系中的世界坐标系地磁场强度理论测量信息；
步骤3.3：根据所述世界坐标系地磁场强度实际测量信息将所述世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息；
步骤3.4：根据所述载体坐标系地磁场强度实际测量信息与所述载体坐标系地磁场强度理论测量信息之间的偏差，建立地磁场强度偏差模型；
步骤3.5：获取在载体坐标系中加速度计的载体坐标系加速度实际测量信息和载体坐标系加速度理论测量信息；
步骤3.6：根据所述载体坐标系加速度实际测量信息和载体坐标系加速度理论测量信息之间的偏差，建立加速度偏差模型；
步骤3.7：利用偏差补偿公式对所述加速度偏差模型和所述地磁场强度偏差模型对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值。
3.如权利要求2所述的一种基于多传感器融合的SLAM方法，其特征在于，所述步骤3.1：将所述载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息，包括：
步骤3.1.1：设立四元数；其中，所述四元数为：
q＝q0+q1i+q2j+q3k
其中，q0为实数，q1为第一系数，q2为第二系数，q3为第三系数；
步骤3.1.2：根据所述四元数得到旋转矩阵；其中，所述旋转矩阵为：
Figure FDA0003058390040000031
步骤3.1.3：采用所述旋转矩阵将所述载体坐标系地磁场强度实际测量信息转换到世界坐标系中，得到世界坐标系地磁场强度实际测量信息。
4.如权利要求3所述的一种基于多传感器融合的SLAM方法，其特征在于，所述步骤3.3：根据所述世界坐标系地磁场强度实际测量信息将所述世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息，包括：
步骤3.3.1：根据所述世界坐标系地磁场强度实际测量信息与所述世界坐标系地磁场强度理论测量信息向量的模相等，建立模关系公式；其中，所述模关系公式为：
Figure FDA0003058390040000032
其中，
Figure FDA0003058390040000033
表示在世界坐标系中x轴的理论地磁场强度，
Figure FDA0003058390040000034
表示在世界坐标系中z轴的理论地磁场强度，
Figure FDA0003058390040000035
表示在世界坐标系中x轴的实际地磁场强度，
Figure FDA0003058390040000036
表示在世界坐标系中y轴的实际地磁场强度，
Figure FDA0003058390040000037
表示在世界坐标系中z轴的实际地磁场强度；
步骤3.3.2：利用旋转矩阵转换公式将所述世界坐标系地磁场强度理论测量信息转换到载体坐标系中，得到载体坐标系地磁场强度理论测量信息；其中，所述旋转矩阵转换公式为：
Figure FDA0003058390040000038
其中，mb表示载体坐标系地磁场强度理论测量信息。
5.如权利要求4所述的一种基于多传感器融合的SLAM方法，其特征在于，所述偏差补偿公式为：
Figure FDA0003058390040000041
其中，ei表示系统误差，
Figure FDA0003058390040000042
表示加速度偏差模型在第i时刻的值，
Figure FDA0003058390040000043
表示地磁场强度偏差模型在第i时刻的值，eci表示误差变化率，kp表示比例系数，
Figure FDA0003058390040000044
表示初始比例系数，ΔkP表示比例系数变化值，kI表示积分系数，
Figure FDA0003058390040000045
表示初始积分系数，ΔkI表示积分系数变化值，
Figure FDA0003058390040000046
表示在第i时刻陀螺仪在载体坐标系中的实际角速度测量值。
6.如权利要求5所述的一种基于多传感器融合的SLAM方法，其特征在于，所述步骤6：根据所述视觉数据得到所述单目相机位姿信息，包括：
步骤6.1：利用运动轨迹积分公式得到航姿参考系统输出的机器人运动轨迹；其中，所述运动轨迹积分公式为：
Figure FDA0003058390040000047
其中，Pt表示t时刻机器人在世界坐标系中的位置，vt-1表示t-1时刻机器人在世界坐标系的速度，Δt表示采样时间，
Figure FDA0003058390040000048
表示t时刻机器人在世界坐标系中的加速度；
步骤6.2：根据所述视觉数据得到所述单目相机输出的运动轨迹；
步骤6.3：采用运动轨迹解算公式得到尺度因子；
步骤6.4：根据所述尺度因子对所述单目相机输出的运动轨迹进行转换得到所述单目相机位姿信息。
7.如权利要求6所述的一种基于多传感器融合的SLAM方法，其特征在于，所述运动轨迹解算公式为：
bi＝sRBC·ci+b0+ei
其中，
Figure FDA0003058390040000051
Figure FDA0003058390040000052
为航姿参考系统输出的机器人的横坐标，
Figure FDA0003058390040000053
为航姿参考系统输出的机器人的纵坐标，
Figure FDA0003058390040000054
为航姿参考系统输出的机器人的竖坐标，
Figure FDA0003058390040000055
Figure FDA0003058390040000056
为单目相机输出的机器人的横坐标，
Figure FDA0003058390040000057
为单目相机输出的机器人的纵坐标，
Figure FDA0003058390040000058
为单目相机输出的机器人的竖坐标，s为尺度因子，RBC表示相机坐标系到载体坐标系的旋转矩阵，b0表示相机坐标系到载体坐标系的平移向量，ei为系统误差。
8.一种基于多传感器融合的SLAM系统，其特征在于，包括：
地磁场强度实际测量信息获取模块，用于获取航姿参考系统上三轴磁力计在载体坐标系中的载体坐标系地磁场强度实际测量信息；
加速度实际测量信息获取模块，用于获取航姿参考系统上加速度计在载体坐标系中的载体坐标系加速度实际测量信息；
偏差补偿模块，用于根据所述载体坐标系地磁场强度实际测量信息和所述载体坐标系加速度实际测量信息，利用偏差补偿公式对航姿参考系统上陀螺仪测量的角速度进行补偿修正得到所述陀螺仪在载体坐标系中的实际角速度测量值；
航姿参考系统位姿信息计算模块，用于根据所述实际角速度测量值得到航姿参考系统位姿信息；
视觉数据获取模块，用于获取单目相机采集的视觉数据；
单目相机位姿信息计算模块，用于根据所述视觉数据得到所述单目相机位姿信息；
多传感器融合模块，用于对所述单目相机位姿信息和所述航姿参考系统位姿信息利用卡尔曼状态空间模型进行滤波融合，得到多传感器融合数据；其中，所述卡尔曼状态空间模型为：
Figure FDA0003058390040000059
其中，X(k)为航姿参考系统位姿信息，Z(k)为单目相机位姿信息，
Figure FDA0003058390040000061
为k时刻机器人在世界坐标系中的加速度，W(k)为输入白噪声，V(k)为观测噪声，Δt表示采样时间，
Figure FDA0003058390040000062
机器人路径规划模块，用于根据所述多传感器融合数据完成机器人路径规划。
CN202110505826.3A 2021-05-10 2021-05-10 一种基于多传感器融合的slam方法与系统 Withdrawn CN113237485A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110505826.3A CN113237485A (zh) 2021-05-10 2021-05-10 一种基于多传感器融合的slam方法与系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110505826.3A CN113237485A (zh) 2021-05-10 2021-05-10 一种基于多传感器融合的slam方法与系统

Publications (1)

Publication Number Publication Date
CN113237485A true CN113237485A (zh) 2021-08-10

Family

ID=77132938

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110505826.3A Withdrawn CN113237485A (zh) 2021-05-10 2021-05-10 一种基于多传感器融合的slam方法与系统

Country Status (1)

Country Link
CN (1) CN113237485A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114440873A (zh) * 2021-12-30 2022-05-06 南京航空航天大学 封闭环境下磁场叠加的惯性行人slam方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114440873A (zh) * 2021-12-30 2022-05-06 南京航空航天大学 封闭环境下磁场叠加的惯性行人slam方法

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN109991636B (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN111426318B (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN108375370B (zh) 一种面向智能巡防无人机的复合导航系统
CN105203098B (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN110007354B (zh) 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN102809377A (zh) 飞行器惯性/气动模型组合导航方法
CN112630813A (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN109916395A (zh) 一种姿态自主冗余组合导航算法
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN116448145A (zh) 一种基于偏振矢量空间差分的航姿确定方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN114018242A (zh) 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法
CN111025366A (zh) 基于ins及gnss的网格slam的导航系统及方法
CN113237485A (zh) 一种基于多传感器融合的slam方法与系统
CN211012986U (zh) 一种基于惯导技术的无人自主巡航车导航系统
CN116222560B (zh) 基于偏振时间差分的陀螺漂移与姿态失准角解耦估计方法
CN115344033B (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN114509071A (zh) 一种风洞试验模型姿态测量方法
Xin et al. Study on Dynamic and Accurate 6-DOF Measurement System and Approach

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
WW01 Invention patent application withdrawn after publication

Application publication date: 20210810

WW01 Invention patent application withdrawn after publication