CN113310487A - 一种面向地面移动机器人的组合导航方法和装置 - Google Patents
一种面向地面移动机器人的组合导航方法和装置 Download PDFInfo
- Publication number
- CN113310487A CN113310487A CN202110582342.9A CN202110582342A CN113310487A CN 113310487 A CN113310487 A CN 113310487A CN 202110582342 A CN202110582342 A CN 202110582342A CN 113310487 A CN113310487 A CN 113310487A
- Authority
- CN
- China
- Prior art keywords
- sensor
- error
- ins
- kalman filtering
- slam
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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传感器信息融合参数,对机器人进行校正导航。
可选的,所述第一卡尔曼滤波解算参数用如下矩阵表示:
可选的,第二卡尔曼滤波解算参数用如下矩阵表示:
可选的,所述根据预设融合权重,获取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传感器信息融合参数,对机器人进行校正导航。
可选的,所述第一误差获取模块中的第一卡尔曼滤波解算参数用如下矩阵表示:
可选的,所述第二误差获取模块中的第二卡尔曼滤波解算参数用如下矩阵表示:
可选的,所述误差融合模块之前,包括:
参数归一化模块,用于获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果;
可选的,所述误差融合模块中的预设的融合权重用如下公式表示:
可选的,所述误差融合模块中的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)和姿态信息θ(k)、ψ(k)。
根据所述SLAM传感器输出的位置信息和姿态信息,以及所述INS传感器输出的位置信息、速度信息和姿态信息,构建第一卡尔曼滤波模型。
将所述SLAM传感器输出的位置信息与所述INS传感器输出的位置信息对比,获取第一位置误差,并将所述SLAM传感器输出的姿态信息与所述INS传感器输出的姿态信息对比,获取第一姿态误差。
将所述第一位置误差和所述第一姿态误差作为所述第一卡尔曼滤波模型的观测量,获取第一卡尔曼滤波解算参数,所述第一卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差。
在k时刻第一卡尔曼滤波解算参数的融合权重设置如下:
式中,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时刻第二卡尔曼滤波解算参数的融合权重设置如下:
当机器人静止或近似匀速直线运动时:
当机器人存在机动时:
第一卡尔曼滤波解算参数用如下矩阵表示:
第二卡尔曼滤波解算参数用如下矩阵表示:
进一步的,所述根据预设融合权重,获取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:
初始均方误差阵P0:
量测噪声方差阵:
激光SLAM/INS组合导航融合权重设置为:
其中,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传感器信息融合参数,对机器人进行校正导航。
进一步的,所述第一误差获取模块中的第一卡尔曼滤波解算参数用如下矩阵表示:
进一步的,所述第二误差获取模块中的第二卡尔曼滤波解算参数用如下矩阵表示:
进一步的,所述误差融合模块之前,包括:
参数归一化模块,用于获取所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数的归一化结果。
进一步的,所述误差融合模块中的预设的融合权重用如下公式表示:
进一步的,所述误差融合模块中的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传感器输出的位置信息对比,获取第二位置误差,并将所述SLAM传感器输出的速度信息与所述INS传感器输出的速度信息对比,获取第一速度误差;
将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量,获取第二卡尔曼滤波解算参数,所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差;
根据预设融合权重,获取SLAM/INS/RTK-GNSS传感器信息融合参数,所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差;所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取;
根据所述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传感器输出的位置信息对比,获取第二位置误差,并将所述SLAM传感器输出的速度信息与所述INS传感器输出的速度信息对比,获取第一速度误差;
第二误差获取模块,用于将所述第二位置误差和所述第一速度误差作为所述第二卡尔曼滤波模型的观测量,获取第二卡尔曼滤波解算参数,所述第二卡尔曼滤波解算参数包括位置误差、速度误差和姿态误差;
误差融合模块,用于根据预设融合权重,获取SLAM/INS/RTK-GNSS传感器信息融合参数,所述SLAM/INS/RTK-GNSS传感器信息融合参数包括位置误差、速度误差和姿态误差;所述SLAM/INS/RTK-GNSS传感器信息融合参数通过对所述第一卡尔曼滤波解算参数和所述第二卡尔曼滤波解算参数中的位置误差、速度误差和姿态误差进行对应融合获取;
导航模块,用于根据所述SLAM/INS/RTK-GNSS传感器信息融合参数,对机器人进行校正导航。
9.根据权利要求8所述的一种面向地面移动机器人的组合导航装置,其特征在于,所述信息获取模块还包括信号检查模块,用于检查RTK-GNSS传感器的信号可以实时接收。
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 true CN113310487A (zh) | 2021-08-27 |
CN113310487B 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) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114167858A (zh) * | 2021-11-12 | 2022-03-11 | 广州文远知行科技有限公司 | 车辆的导航控制方法、装置、导航控制器和车辆控制*** |
Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102445200A (zh) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | 微小型个人组合导航***及其导航定位方法 |
CN103528587A (zh) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | 自主组合导航*** |
CN103630136A (zh) * | 2013-12-05 | 2014-03-12 | 中国航空无线电电子研究所 | 冗余传感器配置下基于三级滤波的导航参数最优融合方法 |
US20150369923A1 (en) * | 2014-06-19 | 2015-12-24 | Novatel Inc. | Method for using partially occluded images for navigation and positioning |
CN106780699A (zh) * | 2017-01-09 | 2017-05-31 | 东南大学 | 一种基于sins/gps和里程计辅助的视觉slam方法 |
CN107024216A (zh) * | 2017-03-14 | 2017-08-08 | 重庆邮电大学 | 引入全景地图的智能车辆融合定位***及方法 |
CN107235013A (zh) * | 2017-07-28 | 2017-10-10 | 深圳普思英察科技有限公司 | 车载导航定位全景云台 |
US20180164124A1 (en) * | 2016-09-15 | 2018-06-14 | Syracuse University | Robust and stable autonomous vision-inertial navigation system for unmanned vehicles |
CN110260885A (zh) * | 2019-04-15 | 2019-09-20 | 南京航空航天大学 | 一种卫星/惯性/视觉组合导航***完好性评估方法 |
CN110632633A (zh) * | 2019-09-29 | 2019-12-31 | 成都云科新能汽车技术有限公司 | 一种车载集成式高精度定位设备及定位方法 |
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 |
CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
CN112362051A (zh) * | 2020-10-16 | 2021-02-12 | 无锡卡尔曼导航技术有限公司 | 一种基于gnss/ins/lidar-slam信息融合的移动机器人导航定位*** |
CN112556719A (zh) * | 2020-11-27 | 2021-03-26 | 广东电网有限责任公司肇庆供电局 | 一种基于cnn-ekf的视觉惯性里程计实现方法 |
CN114074693A (zh) * | 2020-08-13 | 2022-02-22 | 比亚迪股份有限公司 | 多传感器融合的列车定位方法、装置、定位***及列车 |
-
2021
- 2021-05-25 CN CN202110582342.9A patent/CN113310487B/zh active Active
Patent Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102445200A (zh) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | 微小型个人组合导航***及其导航定位方法 |
CN103528587A (zh) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | 自主组合导航*** |
CN103630136A (zh) * | 2013-12-05 | 2014-03-12 | 中国航空无线电电子研究所 | 冗余传感器配置下基于三级滤波的导航参数最优融合方法 |
US20150369923A1 (en) * | 2014-06-19 | 2015-12-24 | Novatel Inc. | Method for using partially occluded images for navigation and positioning |
US20180164124A1 (en) * | 2016-09-15 | 2018-06-14 | Syracuse University | Robust and stable autonomous vision-inertial navigation system for unmanned vehicles |
CN106780699A (zh) * | 2017-01-09 | 2017-05-31 | 东南大学 | 一种基于sins/gps和里程计辅助的视觉slam方法 |
CN107024216A (zh) * | 2017-03-14 | 2017-08-08 | 重庆邮电大学 | 引入全景地图的智能车辆融合定位***及方法 |
CN107235013A (zh) * | 2017-07-28 | 2017-10-10 | 深圳普思英察科技有限公司 | 车载导航定位全景云台 |
CN110260885A (zh) * | 2019-04-15 | 2019-09-20 | 南京航空航天大学 | 一种卫星/惯性/视觉组合导航***完好性评估方法 |
CN110632633A (zh) * | 2019-09-29 | 2019-12-31 | 成都云科新能汽车技术有限公司 | 一种车载集成式高精度定位设备及定位方法 |
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 | 比亚迪股份有限公司 | 多传感器融合的列车定位方法、装置、定位***及列车 |
CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
CN112362051A (zh) * | 2020-10-16 | 2021-02-12 | 无锡卡尔曼导航技术有限公司 | 一种基于gnss/ins/lidar-slam信息融合的移动机器人导航定位*** |
CN112556719A (zh) * | 2020-11-27 | 2021-03-26 | 广东电网有限责任公司肇庆供电局 | 一种基于cnn-ekf的视觉惯性里程计实现方法 |
Non-Patent Citations (2)
Title |
---|
ZHE YUE 等: "A novel adaptive federated filter for GNSS/INS/VO integrated navigation system", 《MEASUREMENT SCIENCE AND TECHNOLOGY》 * |
万晓帆 等: "基于联邦滤波算法的组合导航信息融合方法设计", 《长春理工大学学报(自然科学版)》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114167858A (zh) * | 2021-11-12 | 2022-03-11 | 广州文远知行科技有限公司 | 车辆的导航控制方法、装置、导航控制器和车辆控制*** |
Also Published As
Publication number | Publication date |
---|---|
CN113310487B (zh) | 2022-11-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11002859B1 (en) | Intelligent vehicle positioning method based on feature point calibration | |
CN103314274B (zh) | 活动元件或物体的轨迹的估算方法及*** | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
Roumeliotis et al. | Extended Kalman filter for frequent local and infrequent global sensor data fusion | |
CN108873038A (zh) | 自主泊车定位方法及定位*** | |
CN106052691B (zh) | 激光测距移动制图中闭合环误差纠正方法 | |
CN111982106A (zh) | 导航方法、装置、存储介质及电子装置 | |
CN102692225A (zh) | 一种用于低成本小型无人机的姿态航向参考*** | |
CN102589552A (zh) | 低成本组合导航***的数据融合方法和装置 | |
WO2014042710A2 (en) | Pose estimation | |
CN103363991A (zh) | 一种适应月面崎岖地形的imu与测距敏感器融合方法 | |
US8521428B1 (en) | Heading determination using sensors mounted on rotatable assembly | |
CN103196445A (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
CN110398251B (zh) | 一种基于多传感器融合的无轨导航agv定位***及其定位方法 | |
US20050137750A1 (en) | Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program | |
Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
CN111552757B (zh) | 生成电子地图的方法、装置、设备及存储介质 | |
CN113310487B (zh) | 一种面向地面移动机器人的组合导航方法和装置 | |
North et al. | Enhanced mobile robot outdoor localization using INS/GPS integration | |
CN113063415A (zh) | 车辆及其导航方法、惯性导航***的修正方法、存储介质 | |
CN102393204A (zh) | 一种基于sins/cns的组合导航信息融合方法 | |
CN102506875A (zh) | 无人机的导航方法及装置 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN112325878A (zh) | 基于ukf与空中无人机节点辅助的地面载体组合导航方法 |
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 |