[发明专利]基于INS及GNSS的网格SLAM的导航系统及方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

GNSS将采集到的GNSS定位数据发送至卡尔曼滤波器EKF，INS模块将采集到的当前位姿发送至卡尔曼滤波器EKF，卡尔曼滤波器EKF对INS模块输出当前位姿进行修正，将修正后的位姿发送至定位模块，定位模块基于权利要求1所述基于INS及GNSS的网格SLAM的导航方法来构建地图；在GNSS定位数据读取失败时，定位模块基于构建的路标地图获取移动机器人在路标地图中的位置。