CN113310487B - 一种面向地面移动机器人的组合导航方法和装置 - Google Patents

一种面向地面移动机器人的组合导航方法和装置 Download PDF

Info

Publication number
CN113310487B
CN113310487B CN202110582342.9A CN202110582342A CN113310487B CN 113310487 B CN113310487 B CN 113310487B CN 202110582342 A CN202110582342 A CN 202110582342A CN 113310487 B CN113310487 B CN 113310487B
Authority
CN
China
Prior art keywords
sensor
error
ins
kalman filtering
rtk
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.)
Active
Application number
CN202110582342.9A
Other languages
English (en)
Other versions
CN113310487A (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.)
Electric Power Research Institute of Yunnan Power Grid Co Ltd
Original Assignee
Electric Power Research Institute of Yunnan Power Grid 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 Electric Power Research Institute of Yunnan Power Grid Co Ltd filed Critical Electric Power Research Institute of Yunnan Power Grid Co Ltd
Priority to CN202110582342.9A priority Critical patent/CN113310487B/zh
Publication of CN113310487A publication Critical patent/CN113310487A/zh
Application granted granted Critical
Publication of CN113310487B publication Critical patent/CN113310487B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system

Abstract

本申请公开了一种面向地面移动机器人的组合导航方法和装置，所述方法包括：在任一时刻，同时获取SLAM传感器、RTK‑GNSS传感器以及INS传感器输出信息；根据SLAM传感器和INS传感器输出信息，获取第一位置误差和第一姿态误差并构建第一卡尔曼滤波模型；将第一位置误差和第一姿态误差作为第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数；根据RTK‑GNSS传感器和INS传感器输出信息，获取第二位置误差和第一速度误差并构建第二卡尔曼滤波模型；同样获取第二卡尔曼滤波解算参数；根据预设融合权重，获取SLAM/INS/RTK‑GNSS传感器信息融合参数；根据融合参数，对机器人进行校正导航。本申请可以在对地面移动机器人进行导航时，提高移动的精确性。

Description

一种面向地面移动机器人的组合导航方法和装置
技术领域
本申请主要涉及机器人导航技术领域，尤其是一种面向地面移动机器人的组合导航方法和装置。
背景技术
随着科技的发展，地面移动机器人常被用来代替人工执行任务，以提高工作的效率。机器人移动的精确性问题一直是技术研究的重点，因为只要机器人移动时存在误差，长期积累下去也会给工作带来很大障碍。现有技术中一般使用SLAM传感器、INS传感器或RTK-GNSS传感器来为移动机器人导航，通过这些传感器可以实时获取机器人当前的位置、速度和姿态信息，根据这些信息来得知机器人在移动时是否存在位置误差、速度误差和姿态误差。
但是目前使用的这三种传感器在导航时都有各自的缺点，不能对移动机器人进行准确的导航，如在使用SLAM传感器对机器人进行导航时，SLAM传感器输出的位置信息和姿态信息频率过低，当机器人行驶的距离增加时，产生的误差会逐渐累积；在使用RTK-GNSS传感器对机器人进行导航时，时常伴随着信号缺失的问题，不能准确为机器人导航；而在使用INS传感器对机器人进行导航时，INS传感器误差随着时间累积，所以不能长时间使用。
发明内容
为了解决目前地面移动机器人移动误差大的问题，本申请通过以下各个实施例公开了一种面向地面移动机器人的组合导航方法和装置。
本申请第一方面公开了一种面向地面移动机器人的组合导航方法，包括：
在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息；
根据所述SLAM传感器输出的位置信息和姿态信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第一卡尔曼滤波模型；
将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第一位置误差，并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比，获取第一姿态误差；
将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数，所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
根据所述RTK-GNSS传感器输出的位置信息和速度信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型；
将所述RTK-GNSS传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第二位置误差，并将所述SLAM传感器输出的速度信息与所述INS传感器输出的速度信息对比，获取第一速度误差；
将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数，所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数，所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差；所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取；
根据所述SLAM/INS/RTK-GNSS传感器信息融合参数，对机器人进行校正导航。
可选的，所述第一卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000021
式中，
Figure BDA0003083703030000022
为k时刻第一卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000023
Figure BDA0003083703030000024
为k时刻第一卡尔曼滤波解算出的速度误差；
Figure BDA0003083703030000025
为k时刻第一卡尔曼滤波解算出的姿态误差；其中E(k)表示东向，N(k)表示北向，U(k)表示天向。
可选的，第二卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000026
式中，
Figure BDA0003083703030000027
为k时刻第二卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000028
Figure BDA0003083703030000029
为k时刻第二卡尔曼滤波解算出的速度误差；
Figure BDA00030837030300000210
为k时刻第二卡尔曼滤波解算出的姿态误差。
可选的，所述根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数之前，包括：
获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果；
所述第一卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA00030837030300000211
表示；所述第二卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA00030837030300000212
表示。
可选的，所述预设的融合权重用如下公式表示：
Figure BDA00030837030300000213
式中，Yk(i)表示矩阵Yk的第i个元素，
Figure BDA00030837030300000214
表示矩阵
Figure BDA00030837030300000215
的第i个元素，
Figure BDA00030837030300000216
表示矩阵
Figure BDA00030837030300000217
的第i个元素，矩阵Yk表示SLAM/INS/RTK-GNSS传感器信息融合参数。
可选的，所述SLAM/INS/RTK-GNSS传感器信息融合参数用如下矩阵表示：
Yk＝[δpE(k) δpN(k) δpU(k) δvE(k) δvN(k) δvU(k) φE(k) φN(k) φU(k)]T；
式中，δpE(k)、δpN(k)、δpU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的位置误差；δvE(k)、δvN(k)、δvU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的速度误差；φE(k)、φN(k)、φU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的姿态误差。
可选的，所述获取RTK-GNSS传感器输出的位置信息和速度信息之前，还包括：检查RTK-GNSS传感器的信号可以实时接收。
本申请第二方面公开了一种面向地面移动机器人的组合导航装置，所述装置应用于所述一种基于信息融合的移动机器人导航方法，所述装置包括：
信息获取模块，用于在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息；
第一模型构建模块，用于根据所述SLAM传感器输出的位置信息和姿态信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第一卡尔曼滤波模型；
第一信息对比模块，用于将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第一位置误差，并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比，获取第一姿态误差；
第一误差获取模块，用于将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数，所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
第二模型构建模块，用于根据所述RTK-GNSS传感器输出的位置信息和速度信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型；
第二信息对比模块，用于将所述RTK-GNSS传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第二位置误差，并将所述SLAM传感器输出的速度信息与所述INS传感器输出的速度信息对比，获取第一速度误差；
第二误差获取模块，用于将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数，所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
误差融合模块，用于根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数，所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差；所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取；
导航模块，用于根据所述SLAM/INS/RTK-GNSS传感器信息融合参数，对机器人进行校正导航。
可选的，所述第一误差获取模块中的第一卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000031
式中，
Figure BDA0003083703030000032
为k时刻第一卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000033
Figure BDA0003083703030000034
为k时刻第一卡尔曼滤波解算出的速度误差；
Figure BDA0003083703030000035
为k时刻第一卡尔曼滤波解算出的姿态误差；其中E(k)表示东向，N(k)表示北向，U(k)表示天向。
可选的，所述第二误差获取模块中的第二卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000036
式中，
Figure BDA0003083703030000037
为k时刻第二卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000038
Figure BDA0003083703030000039
为k时刻第二卡尔曼滤波解算出的速度误差；
Figure BDA00030837030300000310
为k时刻第二卡尔曼滤波解算出的姿态误差。
可选的，所述误差融合模块之前，包括：
参数归一化模块，用于获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果；
所述第一卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA0003083703030000041
表示；所述第二卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA0003083703030000042
表示。
可选的，所述误差融合模块中的预设的融合权重用如下公式表示：
Figure BDA0003083703030000043
式中，Yk(i)表示矩阵Yk的第i个元素，
Figure BDA0003083703030000044
表示矩阵
Figure BDA0003083703030000045
的第i个元素，
Figure BDA0003083703030000046
表示矩阵
Figure BDA0003083703030000047
的第i个元素，矩阵Yk表示SLAM/INS/RTK-GNSS传感器信息融合参数。
可选的，所述误差融合模块中的SLAM/INS/RTK-GNSS传感器信息融合参数用如下矩阵表示：
Yk＝[δpE(k) δpN(k) δpU(k) δvE(k) δvN(k) δvU(k) φE(k) φN(k) φU(k)]T；
式中，δpE(k)、δpN(k)、δpU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的位置误差；δvE(k)、δvN(k)、δvU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的速度误差；φE(k)、φN(k)、φU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的姿态误差。
可选的，所述信息获取模块还包括信号检查模块，用于检查RTK-GNSS传感器的信号可以实时接收。
本申请公开了一种面向地面移动机器人的组合导航方法和装置，所述方法包括：在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息；根据SLAM传感器和INS传感器输出信息，构建第一卡尔曼滤波模型；将SLAM传感器与INS传感器输出信息对比，获取第一位置误差和第一姿态误差；将第一位置误差和第一姿态误差作为第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数；根据RTK-GNSS传感器和INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型；将RTK-GNSS传感器与INS传感器输出信息对比，获取第二位置误差和第一速度误差；将第二位置误差和第一速度误差作为第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数；根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数；根据融合参数，对机器人进行校正导航。
本申请可以在对地面移动机器人进行导航时，减少机器人移动时的位置误差、速度误差和姿态误差，提高地面移动机器人移动的精确性。
附图说明
为了更清楚地说明本申请的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例公开的一种面向地面移动机器人的组合导航方法的流程示意图；
图2为本申请实施例公开的一种面向地面移动机器人的组合导航装置的结构示意图；
图3为本申请实施例公开的地面移动机器人的路径轨迹图；
图4为本申请实施例公开的地面移动机器人的位置导航结果；
图5为本申请实施例公开的地面移动机器人的位置导航结果放大图；
图6为本申请实施例公开的地面移动机器人的速度导航结果；
图7为本申请实施例公开的地面移动机器人的速度导航结果放大图；
图8为本申请实施例公开的地面移动机器人的姿态导航结果；
图9为本申请实施例公开的地面移动机器人的姿态导航结果放大图。
具体实施方式
为了解决目前地面移动机器人移动误差大的问题，本申请通过以下实施例公开了一种面向地面移动机器人的组合导航方法和装置。
本申请第一实施例公开了一种面向地面移动机器人的组合导航方法，参见图1的流程图，包括：
在任一时刻，同时获取SLAM传感器输出的位置信息pE(k) SLAM、pN(k) SLAM和姿态信息ψ(k) SLAM，以及RTK-GNSS传感器输出的位置信息pE(k) RTK、pN(k) RTK和速度信息vE(k) RTK、vN(k) RTK，以及INS传感器输出的位置信息pE(k)、pN(k)、pU(k)、速度信息vE(k)、vN(k)、vU(k)和姿态信息
Figure BDA0003083703030000052
θ(k)、ψ(k)。
根据所述SLAM传感器输出的位置信息和姿态信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第一卡尔曼滤波模型。
将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第一位置误差，并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比，获取第一姿态误差。
将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数，所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差。
在k时刻第一卡尔曼滤波解算参数的融合权重设置如下：
Figure BDA0003083703030000051
式中，ai(i＝1,2,...,7)根据实际情况在0.95～0.99之间选取；bj(j＝1,2,...,7)选择第一卡尔曼滤波解算参数和第二卡尔曼滤波解算参数相近时的位移值；xk为k时刻机器人位移。
根据所述RTK-GNSS传感器输出的位置信息和速度信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型。
将所述RTK-GNSS传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第二位置误差，并将所述SLAM传感器输出的速度信息与所述INS传感器输出的速度信息对比，获取第一速度误差。
将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数，所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差。
在k时刻第二卡尔曼滤波解算参数的融合权重设置如下：
当机器人静止或近似匀速直线运动时：
Figure BDA0003083703030000061
当机器人存在机动时：
Figure BDA0003083703030000062
第一卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000063
式中，
Figure BDA0003083703030000064
为k时刻第一卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000065
Figure BDA0003083703030000066
为k时刻第一卡尔曼滤波解算出的速度误差；
Figure BDA0003083703030000067
为k时刻第一卡尔曼滤波解算出的姿态误差；其中E(k)表示东向，N(k)表示北向，U(k)表示天向。
第二卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000068
式中，
Figure BDA0003083703030000069
为k时刻第二卡尔曼滤波解算出的位置误差；
Figure BDA00030837030300000610
Figure BDA00030837030300000611
为k时刻第二卡尔曼滤波解算出的速度误差；
Figure BDA00030837030300000612
为k时刻第二卡尔曼滤波解算出的姿态误差。
进一步的，所述根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数之前，包括：
获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果。
所述第一卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA00030837030300000613
表示；所述第二卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA00030837030300000614
表示。
Figure BDA00030837030300000615
Figure BDA00030837030300000616
式中，
Figure BDA00030837030300000617
表示矩阵
Figure BDA00030837030300000618
的第i个元素；
Figure BDA00030837030300000619
表示矩阵
Figure BDA00030837030300000620
的第i个元素；
Figure BDA00030837030300000621
表示矩阵
Figure BDA00030837030300000622
的第i个元素；
Figure BDA00030837030300000623
表示矩阵
Figure BDA00030837030300000624
的第i个元素。
所述预设的融合权重用如下公式表示：
Figure BDA00030837030300000625
式中，Yk(i)表示矩阵Yk的第i个元素，
Figure BDA0003083703030000071
表示矩阵
Figure BDA0003083703030000072
的第i个元素，
Figure BDA0003083703030000073
表示矩阵
Figure BDA0003083703030000074
的第i个元素，矩阵Yk表示SLAM/INS/RTK-GNSS传感器信息融合参数。
获取融合结果，所述SLAM/INS/RTK-GNSS传感器信息融合参数用如下矩阵表示：
Yk＝[δpE(k) δpN(k) δpU(k) δvE(k) δvN(k) δvU(k) φE(k) φN(k) φU(k)]T；
式中，δpE(k)、δpN(k)、δpU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的位置误差；δvE(k)、δvN(k)、δvU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的速度误差；φE(k)、φN(k)、φU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的姿态误差。在进行融合时，为避免刚接入RTK-GNSS信号进行组合导航，导航结果不收敛的情况，第二卡尔曼滤波解算参数滤波几秒后与第一卡尔曼滤波解算参数进行融合。
根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数，所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差；所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取。
根据所述SLAM/INS/RTK-GNSS传感器信息融合参数，对机器人进行校正导航。
进一步的，所述获取RTK-GNSS传感器输出的位置信息和速度信息之前，还包括：检查RTK-GNSS传感器的信号可以实时接收。
本申请公开了一种面向地面移动机器人的组合导航方法和装置，所述方法包括：在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息；根据SLAM传感器和INS传感器输出信息，构建第一卡尔曼滤波模型；将SLAM传感器与INS传感器输出信息对比，获取第一位置误差和第一姿态误差；将第一位置误差和第一姿态误差作为第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数；根据RTK-GNSS传感器和INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型；将RTK-GNSS传感器与INS传感器输出信息对比，获取第二位置误差和第一速度误差；将第二位置误差和第一速度误差作为第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数；根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数；根据融合参数，对机器人进行校正导航。
本申请可以在对地面移动机器人进行导航时，减少机器人移动的位置误差、速度误差和姿态误差，提高地面移动机器人移动的精确性。
为验证本申请的技术效果，对本发明提出的一种基于信息融合的移动机器人导航方法进行MATLAB仿真验证，仿真时间为100s。
机器人路径具体如下：
次序1，机器人北向加速，加速度1m/s，加速时间3s；
次序2，机器人匀速运动，匀速运动时间27s；
次序3，机器人转向运动，航向角变化率-6°/s，转向运动时间5s；
次序4，机器人匀速运动，匀速运动时间20s；
次序5，机器人转向运动，航向角变化率6°/s，转向运动时间5s，此时接收到RTK-GNSS信号；
次序6，机器人匀速运动，匀速运动时间40s。
系统初始化参数如下：
初始位置：东向位置100m，北向位置0m，高度0m；
初始速度：东向速度0m/s，北向速度0m/s，天向速度0m/s；
初始姿态：俯仰角0°，横滚角0°，航向角30°；
初始纬度：30°；
初始经度：120°；°
采样时间：T＝0.01s；
卡尔曼滤波器初始参数：
初始状态量：X0＝[0 0 0 0 0 0 0 0 0]T；
系统噪声阵Q：
Figure BDA0003083703030000081
初始均方误差阵P0：
Figure BDA0003083703030000082
量测噪声方差阵：
Figure BDA0003083703030000091
Figure BDA0003083703030000092
激光SLAM/INS组合导航融合权重设置为：
Figure BDA0003083703030000093
其中，RTK-GNSS/INS组合导航滤波5s后与激光SLAM/INS组合导航进行融合。
利用本申请所述方法，得到机器人SLAM/INS/RTK-GNSS组合导航结果。其中图3为本实施例中路面移动机器人路径轨迹图；参见图4和图5可以看出只用激光SLAM/INS组合导航时，较好的抑制了INS的位置误差，第65s融合了RTK-GNSS/INS导航结果后，进一步降低了位置误差；参见图6和图7可以看出激光SLAM/INS组合导航后，较好的抑制了INS的速度误差，第65s融合了RTK-GNSS/INS导航结果后进一步降低了速度误差；参见图8和图9可以看出激光SLAM/INS组合导航较好的抑制了INS的姿态误差，第65s融合了RTK-GNSS/INS导航结果后，虽然因为激光SLAM/INS组合导航输出的姿态信息性能下降较慢，融合时激光SLAM/INS组合导航权重较大，也略微降低了姿态误差。
本申请第二实施例公开了一种面向地面移动机器人的组合导航装置，所述装置应用于所述一种基于信息融合的移动机器人导航方法，参见图2所示的流程图，所述装置包括：
信息获取模块，用于在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息。
第一模型构建模块，用于根据所述SLAM传感器输出的位置信息和姿态信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第一卡尔曼滤波模型。
第一信息对比模块，用于将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第一位置误差，并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比，获取第一姿态误差。
第一误差获取模块，用于将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数，所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差。
第二模型构建模块，用于根据所述RTK-GNSS传感器输出的位置信息和速度信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型。
第二信息对比模块，用于将所述RTK-GNSS传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第二位置误差，并将所述SLAM传感器输出的速度信息与所述INS传感器输出的速度信息对比，获取第一速度误差。
第二误差获取模块，用于将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数，所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差。
误差融合模块，用于根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数，所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差；所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取。
导航模块，用于根据所述SLAM/INS/RTK-GNSS传感器信息融合参数，对机器人进行校正导航。
进一步的，所述第一误差获取模块中的第一卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000101
式中，
Figure BDA0003083703030000102
为k时刻第一卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000103
Figure BDA0003083703030000104
为k时刻第一卡尔曼滤波解算出的速度误差；
Figure BDA0003083703030000105
为k时刻第一卡尔曼滤波解算出的姿态误差；其中E(k)表示东向，N(k)表示北向，U(k)表示天向。
进一步的，所述第二误差获取模块中的第二卡尔曼滤波解算参数用如下矩阵表示：
Figure BDA0003083703030000106
式中，
Figure BDA0003083703030000107
为k时刻第二卡尔曼滤波解算出的位置误差；
Figure BDA0003083703030000108
Figure BDA0003083703030000109
为k时刻第二卡尔曼滤波解算出的速度误差；
Figure BDA00030837030300001010
为k时刻第二卡尔曼滤波解算出的姿态误差。
进一步的，所述误差融合模块之前，包括：
参数归一化模块，用于获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果。
所述第一卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA00030837030300001011
表示；所述第二卡尔曼滤波解算参数的归一化结果用矩阵
Figure BDA00030837030300001012
表示。
进一步的，所述误差融合模块中的预设的融合权重用如下公式表示：
Figure BDA00030837030300001013
式中，Yk(i)表示矩阵Yk的第i个元素，
Figure BDA00030837030300001014
表示矩阵
Figure BDA00030837030300001015
的第i个元素，
Figure BDA00030837030300001016
表示矩阵
Figure BDA00030837030300001017
的第i个元素，矩阵Yk表示SLAM/INS/RTK-GNSS传感器信息融合参数。
进一步的，所述误差融合模块中的SLAM/INS/RTK-GNSS传感器信息融合参数用如下矩阵表示：
Yk＝[δpE(k) δpN(k) δpU(k) δvE(k) δvN(k) δvU(k) φE(k) φN(k) φU(k)]T；
式中，δpE(k)、δpN(k)、δpU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的位置误差；δvE(k)、δvN(k)、δvU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的速度误差；φE(k)、φN(k)、φU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的姿态误差。
进一步的，所述信息获取模块还包括信号检查模块，用于检查RTK-GNSS传感器的信号可以实时接收。
以上结合具体实施方式和实施例对本申请进行了详细说明，不过这些说明并不能理解为对本申请的限制。本领域技术人员理解，在不偏离本申请精神和范围的情况下，可以对本申请技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本申请的范围内。本申请的保护范围以所附权利要求为准。

Claims (9)

1.一种面向地面移动机器人的组合导航方法，其特征在于，包括：
在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息；
根据所述SLAM传感器输出的位置信息和姿态信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第一卡尔曼滤波模型；
将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第一位置误差，并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比，获取第一姿态误差；
将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数，所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
根据所述RTK-GNSS传感器输出的位置信息和速度信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型；
将所述RTK-GNSS传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第二位置误差，并将所述RTK-GNSS传感器输出的速度信息与所述INS传感器输出的速度信息对比，获取第一速度误差；
将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数，所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数，所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差；所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取；
根据所述SLAM/INS/RTK-GNSS传感器信息融合参数，对机器人进行校正导航。
2.根据权利要求1所述的一种面向地面移动机器人的组合导航方法，其特征在于，所述第一卡尔曼滤波解算参数用如下矩阵表示：
Figure FDA0003817258550000011
式中，
Figure FDA0003817258550000012
为k时刻第一卡尔曼滤波解算出的位置误差；
Figure FDA0003817258550000013
Figure FDA0003817258550000014
为k时刻第一卡尔曼滤波解算出的速度误差；
Figure FDA0003817258550000015
为k时刻第一卡尔曼滤波解算出的姿态误差；其中E(k)表示东向，N(k)表示北向，U(k)表示天向。
3.根据权利要求2所述的一种面向地面移动机器人的组合导航方法，其特征在于，第二卡尔曼滤波解算参数用如下矩阵表示：
Figure FDA0003817258550000016
式中，
Figure FDA0003817258550000017
为k时刻第二卡尔曼滤波解算出的位置误差；
Figure FDA0003817258550000018
Figure FDA0003817258550000021
为k时刻第二卡尔曼滤波解算出的速度误差；
Figure FDA0003817258550000022
为k时刻第二卡尔曼滤波解算出的姿态误差。
4.根据权利要求3所述的一种面向地面移动机器人的组合导航方法，其特征在于，所述根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数之前，包括：
获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果；
所述第一卡尔曼滤波解算参数的归一化结果用矩阵
Figure FDA0003817258550000023
表示；所述第二卡尔曼滤波解算参数的归一化结果用矩阵
Figure FDA0003817258550000024
表示。
5.根据权利要求4所述的一种面向地面移动机器人的组合导航方法，其特征在于，所述预设的融合权重用如下公式表示：
Figure FDA0003817258550000025
式中，Yk(i)表示矩阵Yk的第i个元素，
Figure FDA0003817258550000026
表示矩阵
Figure FDA0003817258550000027
的第i个元素，
Figure FDA0003817258550000028
表示矩阵
Figure FDA0003817258550000029
的第i个元素，矩阵Yk表示SLAM/INS/RTK-GNSS传感器信息融合参数。
6.根据权利要求5所述的一种面向地面移动机器人的组合导航方法，其特征在于，所述SLAM/INS/RTK-GNSS传感器信息融合参数用如下矩阵表示：
Yk＝[δpE(k) δpN(k) δpU(k) δvE(k) δvN(k) δvU(k) φE(k) φN(k) φU(k)]T；
式中，δpE(k)、δpN(k)、δpU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的位置误差；δvE(k)、δvN(k)、δvU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的速度误差；φE(k)、φN(k)、φU(k)为k时刻SLAM/INS/RTK-GNSS传感器融合参数中的姿态误差。
7.根据权利要求1所述的一种面向地面移动机器人的组合导航方法，其特征在于，所述获取RTK-GNSS传感器输出的位置信息和速度信息之前，还包括：检查RTK-GNSS传感器的信号能够实时接收。
8.一种面向地面移动机器人的组合导航装置，所述装置应用于权利要求1-7任一项所述一种面向地面移动机器人的组合导航方法，所述装置包括：
信息获取模块，用于在任一时刻，同时获取SLAM传感器输出的位置信息和姿态信息，以及RTK-GNSS传感器输出的位置信息和速度信息，以及INS传感器输出的位置信息、速度信息和姿态信息；
第一模型构建模块，用于根据所述SLAM传感器输出的位置信息和姿态信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第一卡尔曼滤波模型；
第一信息对比模块，用于将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第一位置误差，并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比，获取第一姿态误差；
第一误差获取模块，用于将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量，获取第一卡尔曼滤波解算参数，所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
第二模型构建模块，用于根据所述RTK-GNSS传感器输出的位置信息和速度信息，以及所述INS传感器输出的位置信息、速度信息和姿态信息，构建第二卡尔曼滤波模型；
第二信息对比模块，用于将所述RTK-GNSS传感器输出的位置信息与所述INS传感器输出的位置信息对比，获取第二位置误差，并将所述RTK-GNSS传感器输出的速度信息与所述INS传感器输出的速度信息对比，获取第一速度误差；
第二误差获取模块，用于将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量，获取第二卡尔曼滤波解算参数，所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差；
误差融合模块，用于根据预设融合权重，获取SLAM/INS/RTK-GNSS传感器信息融合参数，所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差；所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取；
导航模块，用于根据所述SLAM/INS/RTK-GNSS传感器信息融合参数，对机器人进行校正导航。
9.根据权利要求8所述的一种面向地面移动机器人的组合导航装置，其特征在于，所述信息获取模块还包括信号检查模块，用于检查RTK-GNSS传感器的信号能够实时接收。
CN202110582342.9A 2021-05-25 2021-05-25 一种面向地面移动机器人的组合导航方法和装置 Active CN113310487B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110582342.9A CN113310487B (zh) 2021-05-25 2021-05-25 一种面向地面移动机器人的组合导航方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110582342.9A CN113310487B (zh) 2021-05-25 2021-05-25 一种面向地面移动机器人的组合导航方法和装置

Publications (2)

Publication Number Publication Date
CN113310487A CN113310487A (zh) 2021-08-27
CN113310487B true CN113310487B (zh) 2022-11-04

Family

ID=77375369

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110582342.9A Active CN113310487B (zh) 2021-05-25 2021-05-25 一种面向地面移动机器人的组合导航方法和装置

Country Status (1)

Country Link
CN (1) CN113310487B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114167858B (zh) * 2021-11-12 2022-11-11 广州文远知行科技有限公司 车辆的导航控制方法、装置、导航控制器和车辆控制系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN110260885A (zh) * 2019-04-15 2019-09-20 南京航空航天大学 一种卫星/惯性/视觉组合导航系统完好性评估方法
AU2020103576A4 (en) * 2019-12-27 2021-02-04 Wuhan University Autonomous orbit and attitude determination method of low-orbit satellite based on non-navigation satellite signal
CN114074693A (zh) * 2020-08-13 2022-02-22 比亚迪股份有限公司 多传感器融合的列车定位方法、装置、定位系统及列车

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103528587B (zh) * 2013-10-15 2016-09-28 西北工业大学 自主组合导航系统
CN103630136A (zh) * 2013-12-05 2014-03-12 中国航空无线电电子研究所 冗余传感器配置下基于三级滤波的导航参数最优融合方法
US9411053B2 (en) * 2014-06-19 2016-08-09 Novatel Inc. Method for using partially occluded images for navigation and positioning
US10578457B2 (en) * 2016-09-15 2020-03-03 Syracuse University Robust and stable autonomous vision-inertial navigation system for unmanned vehicles
CN106780699B (zh) * 2017-01-09 2020-10-16 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法
CN107024216B (zh) * 2017-03-14 2020-08-04 重庆邮电大学 引入全景地图的智能车辆融合定位系统及方法
CN107235013A (zh) * 2017-07-28 2017-10-10 深圳普思英察科技有限公司 车载导航定位全景云台
CN110632633A (zh) * 2019-09-29 2019-12-31 成都云科新能汽车技术有限公司 一种车载集成式高精度定位设备及定位方法
CN112347840B (zh) * 2020-08-25 2022-12-02 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN112362051B (zh) * 2020-10-16 2021-11-02 无锡卡尔曼导航技术有限公司 一种基于信息融合的移动机器人导航定位系统
CN112556719B (zh) * 2020-11-27 2022-01-21 广东电网有限责任公司肇庆供电局 一种基于cnn-ekf的视觉惯性里程计实现方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN110260885A (zh) * 2019-04-15 2019-09-20 南京航空航天大学 一种卫星/惯性/视觉组合导航系统完好性评估方法
AU2020103576A4 (en) * 2019-12-27 2021-02-04 Wuhan University Autonomous orbit and attitude determination method of low-orbit satellite based on non-navigation satellite signal
CN114074693A (zh) * 2020-08-13 2022-02-22 比亚迪股份有限公司 多传感器融合的列车定位方法、装置、定位系统及列车

Also Published As

Publication number Publication date
CN113310487A (zh) 2021-08-27

Similar Documents

Publication Publication Date Title
US11002859B1 (en) Intelligent vehicle positioning method based on feature point calibration
KR101988786B1 (ko) 관성 항법 장치의 초기 정렬 방법
US20050125108A1 (en) Motion estimation method and system for mobile body
CN102692225B (zh) 一种用于低成本小型无人机的姿态航向参考系统
US9482536B2 (en) Pose estimation
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN103363991B (zh) 一种适应月面崎岖地形的imu与测距敏感器融合方法
CN110017850B (zh) 一种陀螺仪漂移估计方法、装置及定位系统
CN102589552B (zh) 低成本组合导航系统的数据融合方法和装置
CN101393025A (zh) Auv组合导航系统无迹切换方法
CN107607113A (zh) 一种两轴姿态倾角测量方法
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN102393204B (zh) 一种基于sins/cns的组合导航信息融合方法
CN113310487B (zh) 一种面向地面移动机器人的组合导航方法和装置
CN108007477A (zh) 一种基于正反向滤波的惯性行人定位系统误差抑制方法
CN111832690B (zh) 基于粒子群优化算法的惯导系统的陀螺测量值计算方法
CN104075711A (zh) 一种基于CKF的IMU/Wi-Fi信号紧组合室内导航方法
North et al. Enhanced mobile robot outdoor localization using INS/GPS integration
Abdelfatah et al. UAV tracking system using integrated sensor fusion with RTK-GPS
CN112325878A (zh) 基于ukf与空中无人机节点辅助的地面载体组合导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN115290090A (zh) 基于多传感器信息融合的slam地图构建方法
CN110779551A (zh) 一种基于加性四元数的两阶段线性对准在线切换方法
CN113108782B (zh) 一种海空旋转调制惯导/天文组合导航方法
Ilyas et al. Improving unmanned ground vehicle navigation by exploiting virtual sensors and multisensor fusion

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