CN111025366A - 基于ins及gnss的网格slam的导航系统及方法 - Google Patents

Abstract

本发明适用于定位技术领域，提供了一种基于INS及GNSS的网格SLAM的导航方法及系统，包括：S1、检测当前是否存在GNSS定位数据，GNSS定位数据包括：移动机器人的当前位置；S2、若检测结果为是，则将GNSS定位数据中的当前位置作为移动机器人的当前位置，并执行步骤S3，若检测结果为否，则执行步骤S4；S3、将GNSS定位数据及INS模块采集的位姿数据输入EKF滤波模型，EKF滤波模型基于GNSS定位数据对INS模块采集的位姿数据进行修正，同时基于修正后的位姿构建路标地图；S4、基于构建的路标地图获取移动机器人当前在路标地图坐标系中的位置。在GNSS能正常定位时，通过GNSS能实现移动机器人的快速及精准定位；在GNSS拒绝环境下时，通过构建的高精路标地图进行定位，弥补了GNSS的缺陷。

Description

基于INS及GNSS的网格SLAM的导航系统及方法

技术领域

本发明属于导航技术领域，提供了一种基于INS及GNSS的网格SLAM 的导航系统及方法。

背景技术

受限于GNSS技术本身的脆弱性，其在城市、室内等复杂环境下极易受到干扰，以致无法正常提供导航定位服务。为此，国内外学者围绕惯性导航技术、视觉导航技术、无线导航技术(Wi-Fi、UWB、蓝牙等)、激光、雷达、地图/ 地磁/重力匹配等技术展开了深入的研究。惯性导航属于航位推算技术，其基本原理是通过测量载体运动的角速率和比力，按照严格的数学物理关系积分，获得相对于初始状态的导航信息。随着人工智能、深度学习技术的发展以及图像处理器性能的提升，视觉导航在近十年发展迅速。以CCD相机传感器为代表，视觉导航通过序列影像特征点跟踪，可实现载体相对位置与姿态变化估算；另一方面，也可通过将相机获取的影像与先验高精度地图进行匹配，获取载体在地图坐标系下的绝对位置。除此以外，利用激光雷达、声纳等主动探测方式进行环境感知的导航技术也被称为主动视觉导航。其中激光雷达技术随着智能驾驶的兴起得到了学术界与产业界的广泛关注，激光雷达能够以厘米量级精度测定其与周边车辆或障碍物之间的距离，是实现自动驾驶环境感知的重要手段。

无论是GNSS，或是其他导航定位技术都有其局限性，任何单一技术都无法满足不同场景、不同环境导航需求。因此考虑不同传感器之间的互补性，基于多传感器信息融合，实现多源协同室内外无缝导航定位已成为当前研究热点。带有INS/GNSS的MMS在标准坐标系中提供地理空间信息，可以在各种应用中应用。通过使用严格的校准方法，可以得出每个系统之间的关系以获得更准确的结果；但要在困难的城市或GNSS敌对环境中实现准确性，需要先进行勘测和探索。

发明内容

本发明实施例提供一种基于INS及GNSS的网格SLAM的导航方法，基于INS、GNSS及网格SLAM多传感器组合导航方案，利用多传感器组合互补特性来弥补单一系统的不足，实现GNSS拒绝环境下的全局定位问题。

本发明是这样实现的，一种基于INS及GNSS的网格SLAM的导航方法，所述方法具体包括如下步骤：

S1、检测当前是否存在GNSS定位数据，GNSS定位数据包括：移动机器人的当前位置；

S2、若检测结果为是，则将GNSS定位数据中的当前位置作为移动机器人的当前位置，并执行步骤S3，若检测结果为否，则执行步骤S4；

S3、将GNSS定位数据及INS模块采集的位姿数据输入EKF滤波模型， EKF滤波模型基于GNSS定位数据对INS模块采集的位姿数据进行修正，同时基于修正后的位姿构建路标地图；

S4、基于构建的路标地图获取移动机器人当前在路标地图坐标系中的位置。

进一步的，路标地图的构建方法具体包括如下步骤：

S31、提取当前激光帧中的路标，获取各路标在激光雷坐标系中的坐标，称为路标的局部坐标；

S32、基于修正后的位姿将当前激光帧中所有路标映射到地图坐标系中，获取各路标的在地图坐标系中的坐标，即为路标的全局坐标；

S33、在路标地图中获取距各全局坐标最近的路标点，并计算各全局坐标与相应最近路标点间的距离，若距离大于允许的距离偏差，则该全局坐标对应的路标为未知路标，将未知路标对应的全局坐标纳入地图坐标系中，纳入地图的全局坐标即为地图坐标。

进一步的，基于路标地图的定位方法具体如下：

S41、获取上一帧的位姿，基于上一帧的位姿将当前帧中的路标映射到地图坐标系中，获取各路标在地图坐标系中的坐标，即为路标的全局坐标；

S42、在路标地图中获取距各全局坐标最近的路标点，并计算各全局坐标与相应最近路标点的距离，若距离小于允许的距离偏差，则该全局坐标对应的路标为已知路标，距全局坐标最近的路标点即为对应路标在地图坐标系中的位置；

S43、基于当前帧中的已知路标来计算移动机器人当前的位姿，并基于移动机器人当前位姿来确定移动机器人在路标地图中的位置。

本发明是这样实现的，一种基于INS、GNSS及网格SLAM的导航系统，所述系统包括：

设于定位区域的路标，设于移动机器人上的INS模块，GNSS及激光雷达，激光雷达与定位模块连接，定位模块、INS模块及GNSS与卡尔曼滤波器EKF 连接，INS模块由加速度和陀螺仪组成；

GNSS将采集到的GNSS定位数据发送至卡尔曼滤波器EKF，INS模块将采集到的当前位姿发送至卡尔曼滤波器EKF，卡尔曼滤波器EKF对INS模块输出当前位姿进行修正，将修正后的位姿发送至定位模块，定位模块基于修正后的位姿来构建地图；在GNSS定位数据读取失败时，定位模块基于构建的路标地图获取移动机器人在路标地图中的位置。

本发明提供的基于INS、GNSS及网格SLAM的导航系统具有如下有益技术效果：

1)在GNSS能正常定位时，通过GNSS能实现移动机器人的快速及精准定位；此外，通过GNSS的准确定位来对INS输出的位姿进行修正，构建精准的路标地图；2)在GNSS拒绝环境下时，通过构建的高精路标地图进行定位，弥补了GNSS的缺陷。

附图说明

图1为本发明实施例提供的基于INS及GNSS的网格SLAM的导航方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1为本发明实施例提供的基于INS及GNSS的网格SLAM的导航方法流程图，该方法具体包括如下步骤：

S1、检测当前是否存在GNSS定位数据，GNSS定位数据包括：移动机器人的当前位置；

在本发明实施例中，GNSS定位数据是通过设置于移动机器人上的GNSS 获取的，例如进行室内进行定位时，会存在GNSS无法提供正常定位的情形。

S2、若检测结果为是，则将GNSS定位数据中的当前位置作为移动机器人的当前位置，并执行步骤S3，若检测结果为否，则执行步骤S4；

S3、将GNSS定位数据及INS模块采集的位姿数据输入EKF滤波模型， EKF滤波模型基于GNSS定位数据对INS模块采集的位姿数据进行修正，同时基于修正后的位姿构建路标地图；

在本发明实施例中，位姿数据的修正具体包括如下步骤：

S31、建立的状态方程及观测方程，状态方程及观测方程即构成EKF滤波模型，状态方程表示如下：

其中，δx_k代表k时刻状态向量x_k，δx_k+1代表k+1时刻状态向量x_k+1，

δr代表GNSS输出的位置，δv,δψ代表INS 模块输出的速度及角度矢量，δb_a,δb_g分别表示加速度计和陀螺仪，δS_a,δS_g分别表示加速度计和陀螺仪的尺度因子，

代表状态转移矩阵，W_k代表系统噪音(高斯白噪声)。

S32、将GNSS定位数据及INS模块输出的位姿数据输入状态方程，预估下一时刻的状态，将GNSS定位数据输入观测方程，基于下一时刻的状态及当前时刻的观测数据获取下一时刻的观测值，包括位置观测值、速度观测值及角度观测值，所述位置观测值及角度观测值即构成修正后的位姿。

INS载体位置为(x_I,y_I,z_I)^T，即为GNSS的位置，第i颗卫星位置为(x_si,y_si,z_si)^T，从INS载体位置到第i号卫星与接收机之间的伪距：

它们相对于载体位置真值(x,y,z)^T(即为位置标定值，为绝对位置值)处展开泰勒级数，取前两项得：

其中，

r_i代表载体位置真值到第i号卫星的距离；

INS给出的位置和速度所对应于卫星i的计算伪距率为：

用ρ_G，

分别表示GNSS接收机测得的伪距和伪距率，则系统的观测方程可表示为：

其中，H(t)为观测矩阵，V(t)为高斯白噪声，即观测噪声，X(t)为状态向量；

离散化得到：

δZ_k＝H_kδX_k+V_k

其中，V_k为k时刻的伪距观测噪声。

S33、将状态向量的偏差及观测值输入EKF滤波模型，EKF滤波模型输出的位姿即为修正后的位姿。

在本发明实施例中，路标地图的构建方法具体包括如下步骤：

S31、提取当前激光帧中的路标，获取各路标在激光雷坐标系中的坐标，称为路标的局部坐标；

S32、基于修正后的位姿将当前激光帧中所有路标映射到地图坐标系中，获取各路标的在地图坐标系中的坐标，即为路标的全局坐标；

S33、在路标地图中获取距各全局坐标最近的路标点，并计算各全局坐标与相应最近路标点间的距离，若距离大于允许的距离偏差，则该全局坐标对应的路标为未知路标，将未知路标对应的全局坐标纳入地图坐标系中，纳入地图的全局坐标即为地图坐标，若检测距离小于或等于允许的距离偏差，则该全局坐标对应的路标为已知路标，该路标对应的地图坐标已经存在于地图中，将无需将路标对应的全局坐标加入地图中。

S4、基于构建的地图进行定位，基于路标地图的定位方法具体如下：

S41、获取上一帧的位姿，基于上一帧的位姿将当前帧中的路标映射到地图坐标系中，获取各路标在地图坐标系中的坐标，即为路标的全局坐标；

S42、在路标地图中获取距各全局坐标最近的路标点，并计算各全局坐标与相应最近路标点的距离，若距离小于允许的距离偏差，则该全局坐标对应的路标为已知路标，距全局坐标最近的路标点即为对应路标在地图坐标系中的位置；

S43、基于当前帧中的已知路标来计算移动机器人当前的位姿，并基于移动机器人当前位姿来计算移动机器人在路标坐标系中的位置，即完成移动机器人的定位。

在本发明实施例中，基于3个已知路标即可计算出当前帧的位姿，即基于 3个已知路标在激光雷达坐标系中的位置及3个已知路标在地图中的位置即可计算出移动机器人当前的位姿，基于移动机器人当前的位姿即可获取移动机器人当前在地图坐标系中的位置，地图与GNSS的采用同一坐标系。

本发明还提供一种基于INS及GNSS的网格SLAM的导航系统，该系统包括：

设于定位区域的路标，设于移动机器人上的INS模块，GNSS及激光雷达，激光雷达与定位模块连接，定位模块、INS模块及GNSS与卡尔曼滤波器连接， INS模块由加速度和陀螺仪组成；

GNSS将采集到的GNSS定位数据发送至卡尔曼滤波器，INS模块将采集到的当前位姿发送至卡尔曼滤波器，卡尔曼滤波器对INS模块输出当前位姿进行修正，将修正后的位姿发送至定位模块，定位模块基于修正后的位姿来构建地图；在GNSS定位数据读取失败时，定位模块基于构建的地图进行移动机器人的定位。

本发明提供的基于INS及GNSS的网格SLAM的导航系统具有如下有益技术效果：

1)在GNSS能正常定位时，通过GNSS能实现移动机器人的快速及精准定位；此外，通过GNSS的准确定位来对INS输出的位姿进行修正，构建精准的路标地图；2)在GNSS不能正常定位时，通过构建的高精路标地图进行定位，弥补了GNSS的缺陷。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于INS及GNSS的网格SLAM的导航方法，其特征在于，所述方法具体包括如下步骤：

S1、检测当前是否存在GNSS定位数据，GNSS定位数据包括：移动机器人的当前位置；

S2、若检测结果为是，则将GNSS定位数据中的当前位置作为移动机器人的当前位置，并执行步骤S3，若检测结果为否，则执行步骤S4；

S3、将GNSS定位数据及INS模块采集的位姿数据输入EKF滤波模型，EKF滤波模型基于GNSS定位数据对INS模块采集的位姿数据进行修正，同时基于修正后的位姿构建路标地图；

S4、基于构建的路标地图获取移动机器人当前在路标地图坐标系中的位置。

2.如权利要求1所述基于INS及GNSS的网格SLAM的导航方法，其特征在于，路标地图的构建方法具体包括如下步骤：

S31、提取当前激光帧中的路标，获取各路标在激光雷坐标系中的坐标，称为路标的局部坐标；

S32、基于修正后的位姿将当前激光帧中所有路标映射到地图坐标系中，获取各路标的在地图坐标系中的坐标，即为路标的全局坐标；

S33、在路标地图中获取距各全局坐标最近的路标点，并计算全局坐标与相应最近路标点间的距离，若距离大于允许的距离偏差，则该全局坐标对应的路标为未知路标，将未知路标对应的全局坐标纳入地图坐标系中。

3.如权利要求1所述基于INS及GNSS的网格SLAM的导航方法，其特征在于，基于路标地图的定位方法具体如下：

S41、获取上一帧的位姿，基于上一帧的位姿将当前帧中的路标映射到地图坐标系中，获取各路标在地图坐标系中的坐标，即为路标的全局坐标；

S42、在路标地图中获取距各路标的全局坐标最近的路标点，并计算各全局坐标与相应最近路标点的距离，若距离小于允许的距离偏差，则该全局坐标对应的路标为已知路标，距全局坐标最近的路标点即为对应路标在地图坐标系中的位置；

S43、基于当前帧中的已知路标来计算移动机器人当前的位姿，并基于移动机器人当前位姿来确定移动机器人在路标地图中的位置。

4.一种基于INS及GNSS的网格SLAM的导航系统，其特征在于，所述系统包括：

设于定位区域的路标，设于移动机器人上的INS模块，GNSS及激光雷达，激光雷达与定位模块连接，定位模块、INS模块及GNSS与卡尔曼滤波器EKF连接，INS模块由加速度和陀螺仪组成；

GNSS将采集到的GNSS定位数据发送至卡尔曼滤波器EKF，INS模块将采集到的当前位姿发送至卡尔曼滤波器EKF，卡尔曼滤波器EKF对INS模块输出当前位姿进行修正，将修正后的位姿发送至定位模块，定位模块基于修正后的位姿来构建地图；在GNSS定位数据读取失败时，定位模块基于构建的路标地图获取移动机器人在路标地图中的位置。

Images