本发明专利技术公开了一种基于关键帧地图的三维激光雷达定位方法，基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，并基于关键帧的搜索算法和定位算法，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，具体包括以下步骤：步骤一、加载地图；步骤二、解析地图；步骤三、粗位姿推算；步骤四、生成关键帧子图；步骤五、数据预处理；步骤六、当前帧和关键帧进行匹配；步骤七、精位姿再循环。本发明专利技术的有益效果是：避免了在建图完成后人工对地图进行修改和裁剪，在定位过程中在降低计算量的同时保证定位的精度，不过多的占用cpu的资源，在大地图场景下定位也不增长资源占用，减少了人为错误，提高了定位效率。
【技术实现步骤摘要】
本专利技术涉及机器人slam，尤其涉及一种基于关键帧地图的三维激光雷达定位方法。
技术介绍
1、当前机器人领域中的3d激光雷达定位技术主要依赖于所建立的全局地图，在定位的时候需要将当前帧和全局地图进行匹配，地图越大，所计算的时间也就越长，无法满足实时定位的需求。当前行业内使用比较多的方案主要有两种，第一种是对建立的全局地图进行切割，分成多个小地图，在定位使用的时候当前帧和小地图进行匹配；第二种是对全局地图进行降采样，将全局地图变得很稀疏，然后定位的时候当前帧和降采样后的全局地图进行匹配。前者需要人工介入，后者会影响定位的精度。
技术实现思路
1、本专利技术提出的一种基于关键帧地图的三维激光雷达定位方法，解决了现有的问题。
2、为了实现上述目的，本专利技术采用了如下技术方案：一种基于关键帧地图的三维激光雷达定位方法，基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，具体包括以下步骤：
3、步骤一、加载地图：地图中保存有关键帧的点云和关键帧的pose；
4、步骤二、解析地图：将地图中的关键帧点云以及关键帧的位姿提取出来并进行解析；
5、步骤三、粗位姿推算：输入imu和odometry数据，这两个传感器的数据为可选项，用作当前帧点云定位的粗位姿；
6、步骤四、生成关键帧子图：根据得出的粗位姿pt选取全局地图中的子图，子图由多个全局地图中的关键帧组成；
>7、步骤五、数据预处理：输入当前帧激光点云，该数据由激光雷达传感器获得，在接收到数据之后首先对数据进行预处理；8、步骤六、当前帧和关键帧进行匹配：对输入的激光点云降采样之后就可以和步骤四中选定局部子图进行匹配，采用《generalized-icp》论文中提出的gicp为匹配算法；
9、步骤七、精位姿再循环：根据步骤六得出的精位姿再循环到步骤3更新上一帧的位姿pt-1＝pt′，再对下一帧进行计算。
10、优选的，所述地图采用本公司自研的建图算法并保存为本公司自研的地图格式。
11、优选的，所述imu数据用于定位和导航，是一种用于测量和跟踪物体姿态的传感器，通过对imu输出的姿态信息进行处理，可以在空间中的位置、方向和速度等信息，从而实现自主运动和导航，也可用于姿态控制、运动控制和稳定控制等应用；所述odometry数据可以用于测速和测距，提供对机器人位姿和速度的局部准确估计。
12、优选的，首先判断是否输入odometry数据，若是，则将odometry数据传递至步骤三；同时判断是否输入imu数据，若是，则将imu数据传递至步骤三，将两项数据作为当前帧点云定位的粗位姿。
13、优选的，所述步骤三的具体方法为：上一时刻位姿pt-1＝(xt-1,yt-1,yawt-1)，如果使用imu和odometry的数据，则根据imu和odometry提供的角速度ω和线速度v可以得出pt＝(xt,yt,yawt)＝(xt-1+vx·δt,yt-1+vy·δt,yaw+ω·δt)，其中δt为t时刻与t-1时刻相邻两帧激光数据之间的时间间隔，得出的pt作为当前帧定位的粗值；
14、优选的，如果不使用imu和odometry数据，则将运动模型考虑为匀速运动模型，上一时刻t-1位姿为pt-1＝(xt-1,yt-1,yawt-1)，上上时刻t-2的位姿为pt-2＝(xt-2,yt-2,yawt-2)，根据两个时刻的位姿可以得出当前车辆的移动速度为可以大概计算出当前帧的粗位姿pt＝(xt,yt,yawt)＝(xt-1+vx·δt,yt-1+vy·δt,yawt-1+ω·δt)，其中δt为当前帧t时刻与上一帧t-1时刻之间的时间间隔。
15、优选的，所述步骤四的具体方法为：首先以当前位姿pt建立坐标轴，坐标系为右手坐标系，这样将全局地图中的关键帧位姿分为四个象限，每个象限各取一个位姿，且位姿都满足在该象限内距离pt最近邻，这样最少在地图中选取到一个关键帧，最多在地图中选取四个关键帧，将选取到的关键帧拼接为局部子图。
16、优选的，所述步骤五的具体方法为：将点云根据激光雷达的外参，即激光雷达在车体中心坐标系下的安装位置，转换到车体坐标系下，该参数由本公司研发的激光雷达标定算法给出，接着在当前激光点云中剔除掉车体的部分，然后根据激光点云的稠密程度对点云做简单的降采样处理，最后将激光点云根据求出的pt转换到全局地图坐标系下。
17、优选的，所述步骤六的具体方法为：根据当前帧和局部子图匹配出来的位姿p和粗位姿pt可以得出当前帧在全局地图坐标系下的精位姿pt′＝pt+p，也就是当前帧点云在全局地图下的坐标，即当前的定位位姿。
18、本专利技术的有益效果为：基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，并基于关键帧的定位算法，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，本专利技术有效地避免了在建图完成后人工进行地图分割，建完地图之后不需要人工对地图进行裁剪，在定位过程中在降低计算量的同时保证定位的精度，不过多的占用cpu的资源，在大地图场景下定位也不增长资源占用，减少了人为错误，提高了定位效率。
本文档来自技高网...
【技术保护点】
1.一种基于关键帧地图的三维激光雷达定位方法，其特征在于，基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，并基于关键帧的搜索算法和定位算法，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，具体包括以下步骤：
2.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述imu数据用于定位和导航，是一种用于测量和跟踪物体姿态的传感器，通过对imu输出的姿态信息进行处理，可以在空间中的位置、方向和速度信息，从而实现自主运动和导航，也可用于姿态控制、运动控制和稳定控制的应用；所述odometry数据可以用于测速和测距，提供对机器人位姿和速度的局部准确估计。
3.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，首先判断是否输入odometry数据，若是，则将odometry数据传递至步骤三；同时判断是否输入imu数据，若是，则将imu数据传递至步骤三，将两项数据作为当前帧点云定位的粗位姿。
4.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述步骤三的具体方法为：上一时刻位姿Pt-1＝(xt-1,yt-1,yawt-1)，如果使用imu和odometry的数据，则根据imu和odometry提供的角速度ω和线速度v可以得出Pt＝(xt,yt,yawt)＝(xt-1+vx·Δt,yt-1+vy·Δt,yaw+ω·Δt)，其中Δt为t时刻与t-1时刻相邻两帧激光数据之间的时间间隔，得出的Pt作为当前帧定位的粗值；
5.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述步骤四的具体方法为：首先以当前位姿Pt建立坐标轴，坐标系为右手坐标系，这样将全局地图中的关键帧位姿分为四个象限，每个象限各取一个位姿，且位姿都满足在该象限内距离Pt最近邻，这样最少在地图中选取到一个关键帧，最多在地图中选取四个关键帧，将选取到的关键帧拼接为局部子图。
6.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述步骤五的具体方法为：将点云根据激光雷达的外参，即激光雷达在车体中心坐标系下的安装位置，转换到车体坐标系下，该参数由本公司研发的激光雷达标定算法给出，接着在当前激光点云中剔除掉车体的部分，然后根据激光点云的稠密程度对点云做简单的降采样处理，最后将激光点云根据求出的Pt转换到全局地图坐标系下。
7.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述步骤六的具体方法为：根据当前帧和局部子图匹配出来的位姿P和粗位姿Pt可以得出当前帧在全局地图坐标系下的精位姿Pt′＝Pt+P，也就是当前帧点云在全局地图下的坐标，即当前的定位位姿。
...
【技术特征摘要】
1.一种基于关键帧地图的三维激光雷达定位方法，其特征在于，基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，并基于关键帧的搜索算法和定位算法，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，具体包括以下步骤：
2.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述imu数据用于定位和导航，是一种用于测量和跟踪物体姿态的传感器，通过对imu输出的姿态信息进行处理，可以在空间中的位置、方向和速度信息，从而实现自主运动和导航，也可用于姿态控制、运动控制和稳定控制的应用；所述odometry数据可以用于测速和测距，提供对机器人位姿和速度的局部准确估计。
3.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，首先判断是否输入odometry数据，若是，则将odometry数据传递至步骤三；同时判断是否输入imu数据，若是，则将imu数据传递至步骤三，将两项数据作为当前帧点云定位的粗位姿。
4.根据权利要求1所述的一种基于关键帧地图的三维激光雷达定位方法，其特征在于，所述步骤三的具体方法为：上一时刻位姿pt-1＝(xt-1,yt-1,yawt-1)，如果使用imu和odometry的数据，则根据imu和odometry提供的角速度ω和线速度v可以得出pt＝(xt,yt,yaw...
【专利技术属性】
技术研发人员：王冠，
申请(专利权)人：霞智科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。