lego-loam 同步构建2d栅格导航地图

2023-10-30 20:30

本文主要是介绍lego-loam 同步构建2d栅格导航地图,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

lego-loam 同步构建2d栅格导航地图

  • 3d点云预处理
  • keypose保存
  • 根据闭环条件更新2d map
  • 构建和2d map
  • 总结

基于目前移动机器人的应用可知,目前3d slam存储的主要为点云地图,由于其特征点比2D激光器数据更加丰富,因此用于后期的定位具有更好的抗干扰性和鲁棒性。但是用于导航的基本路径规划功能,目前仍主要依赖于2d栅格地图。
其中16年开源的cartographer的3dslam则同步发布了2d map格式,而存储的点云也是基于stream自定义格式,而不是传统的点云地图。因此定位时可直接使用3d定位结果,2d地图结果进行导航。

本文参考cartographer中2d 栅格概率更新的功能,在lego-loam开源代码中实现其2d栅格地图的同步创建,同时2d地图可自动剔除slam过程中的移动物体(注:lego-loam 创建的3d点云地图,没有剔除);其效果图如下:

在这里插入图片描述
操作步骤如下:

3d点云预处理

imageProjection.cpp3d点云分割代码中,根据分割后的结果,将地面上的点以及高过机器人高度的所有点云进行剔除。并计算同一个水平扫描ID下的距离值,并保存。如此可获取投影水平面的2D scan message格式。

  for (size_t j = 0; j < _horizontal_scans; ++j) {float min_range = 1000;size_t id_min = 0;for (size_t i = 0; i < _vertical_scans; ++i) {size_t Ind = j + (i)*_horizontal_scans;float Z = _full_cloud->points[Ind].z;if ((_ground_mat(i, j) != 1) &&(Z > 0.4) && (Z<1.2) &&(_range_mat(i, j)<40)) {                     // 地面上点云忽略, 过高过矮的点忽略, 过远的点忽略if(_range_mat(i, j) < min_range) {           // 计算最小距离min_range = _range_mat(i, j);id_min = Ind;}}}if (min_range<1000) {_scan_msg->push_back(_full_cloud->points[id_min]);}}

keypose保存

类似于基本图优化结构和lego-loam存储keypose轨迹序列,并同步记录每个2d点云的笛卡尔坐标。

  // 将极坐标转换为直角坐标系for(int i = 0; i < _scan_msg->points.size(); ++i) {scan_points.emplace_back(_scan_msg->points[i].x, _scan_msg->points[i].y);}// 定义新的scan格式,每一束光采用直角坐标std::shared_ptr<slam::LaserScan> laser_scan(new slam::LaserScan(scan_points));laser_scan->setId(_scans.size());       // 第一帧激光不做处理,仅记录并放入优化器顶点中// laser_scan->setPose(Eigen::Vector3f(0, 0, 0));laser_scan->setPose(pose);              //记录初始激光帧位置,用于slam建图初始坐标(即创建地图坐标系)laser_scan->transformPointCloud();      //根据激光位置,计算每个点的在map的位置_scans.push_back(laser_scan);           //收集每帧激光
}

根据闭环条件更新2d map

闭环条件由lego-loam 3d slam 触发和后端优化,根据3d位置更新 2d投影位置。

// 若存在闭环处理,则需要对位姿进行修正,将历史的的位姿用优化后的数据进行更新
void MapOptimization::correctPoses() {if (aLoopIsClosed == true) {recentCornerCloudKeyFrames.clear();recentSurfCloudKeyFrames.clear();recentOutlierCloudKeyFrames.clear();// update key posesint numPoses = isamCurrentEstimate.size();for (int i = 0; i < numPoses; ++i) {cloudKeyPoses3D->points[i].x =isamCurrentEstimate.at<Pose3>(i).translation().y();cloudKeyPoses3D->points[i].y =isamCurrentEstimate.at<Pose3>(i).translation().z();cloudKeyPoses3D->points[i].z =isamCurrentEstimate.at<Pose3>(i).translation().x();cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;cloudKeyPoses6D->points[i].roll =isamCurrentEstimate.at<Pose3>(i).rotation().pitch();cloudKeyPoses6D->points[i].pitch =isamCurrentEstimate.at<Pose3>(i).rotation().yaw();cloudKeyPoses6D->points[i].yaw =isamCurrentEstimate.at<Pose3>(i).rotation().roll();// 更新 2d 投影位置_scans[i]->setPose(Eigen::Vector3f(cloudKeyPoses6D->points[i].z,cloudKeyPoses6D->points[i].x,cloudKeyPoses6D->points[i].pitch));_scans[i]->transformPointCloud();}aLoopIsClosed = false;                  // 修正完成}
}

构建和2d map

已知每个时刻的2d绝对位置和对应的range_scan的中所有point笛卡尔坐标,基于cartographer中概率地图的生成和更新,从而构建2d栅格地图。 其中bresenham为经典的画线法,用于更新无障碍栅格,而range_scan的端点用来更新障碍栅格。其具体理论可详看:
cartographer 代码思想解读(4)- probability grid地图更新1
cartographer 代码思想解读(5)- probability grid地图更新2

    for(const std::shared_ptr<LaserScan>& scan : scans) {Eigen::Vector2f start = getMapCoords(scan->getPose());const PointCloud& point_cloud = scan->getTransformedPointCloud();for(const Eigen::Vector2f& point : point_cloud) {Eigen::Vector2f end = getMapCoords(point);std::vector<Eigen::Vector2i> points;bresenham(start[0],  start[1], end[0], end[1], points);int n = points.size();if(n == 0) {continue;}for(int j = 0; j < n - 1; ++j) {int index = getIndex(points[j][0], points[j][1]);if(value_[index] + log_odds_free_ >= log_odds_min_) {value_[index] += log_odds_free_;}}int index = getIndex(points[n - 1][0], points[n - 1][1]);if(value_[index] + log_odds_occupied_ <= log_odds_max_) {value_[index] += log_odds_occupied_;}}}

总结

目前3d slam主要目的是用于移动机器人的后期定位使用,而SLAM主要用于一个新环境的第一次配置。因此,3d定位对应的2d栅格地图十分必要,本文简单理解就是已知定位建图的功能。

这篇关于lego-loam 同步构建2d栅格导航地图的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!


原文地址:https://blog.csdn.net/jiajiading/article/details/111867010
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.chinasem.cn/article/310395

相关文章

Spring Boot Maven 插件如何构建可执行 JAR 的核心配置

《SpringBootMaven插件如何构建可执行JAR的核心配置》SpringBoot核心Maven插件,用于生成可执行JAR/WAR,内置服务器简化部署,支持热部署、多环境配置及依赖管理... 目录前言一、插件的核心功能与目标1.1 插件的定位1.2 插件的 Goals(目标)1.3 插件定位1.4 核

使用Python构建一个高效的日志处理系统

《使用Python构建一个高效的日志处理系统》这篇文章主要为大家详细讲解了如何使用Python开发一个专业的日志分析工具,能够自动化处理、分析和可视化各类日志文件,大幅提升运维效率,需要的可以了解下... 目录环境准备工具功能概述完整代码实现代码深度解析1. 类设计与初始化2. 日志解析核心逻辑3. 文件处

使用Docker构建Python Flask程序的详细教程

《使用Docker构建PythonFlask程序的详细教程》在当今的软件开发领域,容器化技术正变得越来越流行,而Docker无疑是其中的佼佼者,本文我们就来聊聊如何使用Docker构建一个简单的Py... 目录引言一、准备工作二、创建 Flask 应用程序三、创建 dockerfile四、构建 Docker

springboot项目中整合高德地图的实践

《springboot项目中整合高德地图的实践》:本文主要介绍springboot项目中整合高德地图的实践,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一:高德开放平台的使用二:创建数据库(我是用的是mysql)三:Springboot所需的依赖(根据你的需求再

canal实现mysql数据同步的详细过程

《canal实现mysql数据同步的详细过程》:本文主要介绍canal实现mysql数据同步的详细过程,本文通过实例图文相结合给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的... 目录1、canal下载2、mysql同步用户创建和授权3、canal admin安装和启动4、canal

基于Python构建一个高效词汇表

《基于Python构建一个高效词汇表》在自然语言处理(NLP)领域,构建高效的词汇表是文本预处理的关键步骤,本文将解析一个使用Python实现的n-gram词频统计工具,感兴趣的可以了解下... 目录一、项目背景与目标1.1 技术需求1.2 核心技术栈二、核心代码解析2.1 数据处理函数2.2 数据处理流程

Linux实现线程同步的多种方式汇总

《Linux实现线程同步的多种方式汇总》本文详细介绍了Linux下线程同步的多种方法,包括互斥锁、自旋锁、信号量以及它们的使用示例,通过这些同步机制,可以解决线程安全问题,防止资源竞争导致的错误,示例... 目录什么是线程同步?一、互斥锁(单人洗手间规则)适用场景:特点:二、条件变量(咖啡厅取餐系统)工作流

Python FastMCP构建MCP服务端与客户端的详细步骤

《PythonFastMCP构建MCP服务端与客户端的详细步骤》MCP(Multi-ClientProtocol)是一种用于构建可扩展服务的通信协议框架,本文将使用FastMCP搭建一个支持St... 目录简介环境准备服务端实现(server.py)客户端实现(client.py)运行效果扩展方向常见问题结

Mysql的主从同步/复制的原理分析

《Mysql的主从同步/复制的原理分析》:本文主要介绍Mysql的主从同步/复制的原理分析,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录为什么要主从同步?mysql主从同步架构有哪些?Mysql主从复制的原理/整体流程级联复制架构为什么好?Mysql主从复制注意

详解如何使用Python构建从数据到文档的自动化工作流

《详解如何使用Python构建从数据到文档的自动化工作流》这篇文章将通过真实工作场景拆解,为大家展示如何用Python构建自动化工作流,让工具代替人力完成这些数字苦力活,感兴趣的小伙伴可以跟随小编一起... 目录一、Excel处理:从数据搬运工到智能分析师二、PDF处理:文档工厂的智能生产线三、邮件自动化: