当前位置: 首页 > news >正文

障碍感知 | 基于KD树的障碍物快速处理(附案例分析与ROS C++仿真)

目录

  • 1 障碍处理与KD树
  • 2 KD树核心原理
    • 2.1 KD树的构造
    • 2.2 KD树的查找
  • 3 仿真实现
    • 3.1 KD树基本算法
    • 3.2 ROS C++仿真

1 障碍处理与KD树

在机器人感知系统中,传感器(如激光雷达、摄像头等)会采集周围的环境数据,例如代价地图、八叉树地图等,都是环境数据的表现形式。机器人在移动过程中需要根据环境,确定当前路径上的潜在障碍物。然而,这些障碍物的数量通常非常庞大,直接遍历处理往往会产生非常高的延迟,在动态环境或高实时性任务中对下游算法非常不利。

KD树(K-Dimensional tree)是一种用于高维空间数据的高效数据结构,广泛应用于机器人障碍物感知与处理中。它的主要功能是提供快速的最近邻搜索、范围查询等操作,使机器人可以实时检测到路径上的障碍物,及时调整其运动轨迹,避免碰撞。对于复杂环境中的机器人,KD树的高效查询能力尤为重要。此外,KD树可以通过增量更新来适应这些变化,而不需要重新构建整个数据结构。这样,机器人可以持续感知并适应环境的变化,保持高效的障碍物检测和避障能力。

在这里插入图片描述

接下来通过一个案例分析KD树的原理,并展示基于ROS的实际应用效果

2 KD树核心原理

KD树是一棵空间二叉树,其中任意节点 x ∈ R k \boldsymbol{x}\in \mathbb{R} ^k xRk。所有非叶子节点包含一个把空间分成两个半空间的超平面。节点的左子树组织了位于其超平面左侧的点,右子树同理。KD树不仅应用于最近邻搜索,在多维键值搜索、范围搜寻等方面也有广泛应用。

2.1 KD树的构造

KD树构造的核心是超平面的选择与划分,经典方法是随着树深度加深,轮流选择特征维度作为超平面的法向量,二分空间时只考虑样本在该维度的取值。如图所示,KD树的每个节点是二维样本,则超平面轮流以 x x x y y y轴为法向量,划分时小于根节点法向特征值的样本被划分到左子树,反之划分到右子树。

在这里插入图片描述

2.2 KD树的查找

KD树最近邻查找的核心是递归搜索。在一次迭代中,根据测试点与KD树当前节点相应特征维度的大小关系选择要搜索的子树并执行搜索。但是最近邻点不一定位于这个子树中,需要进行判断:每次查询测试点与KD树节点关系时,会更新一次最小距离 d i s t min ⁡ \mathrm{dist}_{\min} distmin,最近邻点一定位于以测试点为圆心、 d i s t min ⁡ \mathrm{dist}_{\min} distmin为半径的圆内;计算相应特征维距离 d i s t a x i s \mathrm{dist}_{\mathrm{axis}} distaxis,若 d i s t a x i s < d i s t min ⁡ \mathrm{dist}_{\mathrm{axis}}<\mathrm{dist}_{\min} distaxis<distmin,说明当前KD树节点包含的超平面穿过了最近邻圆域,则该节点的另一子树也需要递归搜索

在这里插入图片描述

如图所示为KD树最近邻查找的实例。

如图(a)所示,第一次查找节点(30,40)并更新最近邻圆,由于测试点位于节点(30,40)右侧,因此需要继续查询右子树;同时最近邻圆被节点(30,40)包含的超平面 x = 30 x=30 x=30穿过,所以节点(30,40)的左侧也可能包含最近邻点,故左子树也被查询。

如图(b)所示是下一层递归时对节点(30,40)左子树根节点(5,25)的查询,由于测试点位于节点(5,25)下侧,因此需要继续查询左子树;此时最近邻圆没有更新,被节点(5,25)包含的超平面 y = 25 y=25 y=25穿过,所以节点(5,25)的上侧也可能包含最近邻点,但节点(5,25)不存在右子树,故其右子树查询退出递归。

如图(c)所示是下一层递归时对节点(5,25)左子树根节点(10,12)的查询,考虑到是叶节点,直接更新最近邻圆即可。

在节点(30,40)右子树的查询中,首先进入节点(35,45),类似地应查询其左子树,但节点(35,45)包含的超平面 y = 45 y=45 y=45没穿过最近邻圆,说明节点(35,45)右子树的所有点都在最近邻圆外部,不可能是最近邻点,可以进行剪枝,提高计算效率。

3 仿真实现

3.1 KD树基本算法

KD树的查找主要分为以下三种模式

  • 最近邻查找

    int nnSearch(const PointT& query, double* min_dist = nullptr) const
    {int guess;double _min_dist = std::numeric_limits<double>::max();_nnSearchRecursive(query, root_, &guess, &_min_dist);if (min_dist)*min_dist = _min_dist;return guess;
    }void _nnSearchRecursive(const PointT& query, const KDNode* node, int* guess, double* min_dist) const
    {if (node == nullptr)return;const PointT& train = points_[node->idx];const double dist = _distance(query, train);if (dist < *min_dist){*min_dist = dist;*guess = node->idx;}const int axis = node->axis;const int dir = query[axis] < train[axis] ? 0 : 1;_nnSearchRecursive(query, node->next[dir], guess, min_dist);// if the min distance crosses the axis, the nearest neighbor maybe exist// in the other side of axis, therefore another direction should be searchedconst double diff = fabs(query[axis] - train[axis]);if (diff < *min_dist)_nnSearchRecursive(query, node->next[!dir], guess, min_dist);
    }
    
  • K近邻查找

    std::vector<int> knnSearch(const PointT& query, int k) const
    {KnnQueue queue(k);_knnSearchRecursive(query, root_, queue, k);std::vector<int> indices(queue.size());for (size_t i = 0; i < queue.size(); i++)indices[i] = queue[i].second;return indices;
    }void _knnSearchRecursive(const PointT& query, const KDNode* node, KnnQueue& queue, int k) const
    {if (node == nullptr)return;const PointT& train = points_[node->idx];const double dist = _distance(query, train);queue.push(std::make_pair(dist, node->idx));const int axis = node->axis;const int dir = query[axis] < train[axis] ? 0 : 1;_knnSearchRecursive(query, node->next[dir], queue, k);const double diff = fabs(query[axis] - train[axis]);if ((int)queue.size() < k || diff < queue.back().first)_knnSearchRecursive(query, node->next[!dir], queue, k);
    }
    
  • 圆形近邻查找

    std::vector<int> radiusSearch(const PointT& query, double radius) const
    {std::vector<int> indices;_radiusSearchRecursive(query, root_, indices, radius);return indices;
    }void _radiusSearchRecursive(const PointT& query, const KDNode* node, std::vector<int>& indices, double radius) const
    {if (node == nullptr)return;const PointT& train = points_[node->idx];const double dist = _distance(query, train);if (dist < radius)indices.push_back(node->idx);const int axis = node->axis;const int dir = query[axis] < train[axis] ? 0 : 1;_radiusSearchRecursive(query, node->next[dir], indices, radius);const double diff = fabs(query[axis] - train[axis]);if (diff < radius)_radiusSearchRecursive(query, node->next[!dir], indices, radius);
    }
    

3.2 ROS C++仿真

首先在主节点中构造KD树对象,并将代价地图的障碍信息传入KD树

rmp::common::structure::KDTree<rmp::common::geometry::Point3d> obs_kd_tree;
rmp::common::geometry::Points3d obstacles;
auto grid2Index = [&](int x, int y) { return x + costmap_ros_->getCostmap()->getSizeInCellsX() * y; };
for (int x = 0; x < costmap_ros_->getCostmap()->getSizeInCellsX(); x++)
{for (int y = 0; y < costmap_ros_->getCostmap()->getSizeInCellsY(); y++){if (costmap_ros_->getCostmap()->getCharMap()[grid2Index(x, y)] == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){double wx, wy;costmap_ros_->getCostmap()->mapToWorld(x, y, wx, wy);obstacles.emplace_back(wx, wy);}}
}obs_kd_tree.build(obstacles);
rmp::common::geometry::Point3d robot_pose(start.pose.position.x, start.pose.position.y, start.pose.position.z);

接着测试不同的KD树障碍物搜索算法

  • 最近邻查找

    auto idx = obs_kd_tree.nnSearch(robot_pose);
    Visualizer::Lines2d knn;
    knn.emplace_back(std::make_pair<Visualizer::Point2d, Visualizer::Point2d>({ start.pose.position.x, start.pose.position.y }, { obs_kd_tree[idx].x(), obs_kd_tree[idx].y() }));
    visualizer->publishLines2d(knn, particles_pub_, frame_id_, "knn", Visualizer::PURPLE, 0.1);
    

    在这里插入图片描述

  • K近邻查找

    auto obs_idx = obs_kd_tree.knnSearch(robot_pose, 10);
    Visualizer::Lines2d knn;
    for (const int idx : obs_idx)
    {knn.emplace_back(std::make_pair<Visualizer::Point2d, Visualizer::Point2d>({ start.pose.position.x, start.pose.position.y }, { obs_kd_tree[idx].x(), obs_kd_tree[idx].y() }));
    }
    visualizer->publishLines2d(knn, particles_pub_, frame_id_, "knn", Visualizer::PURPLE, 0.1);
    

在这里插入图片描述

  • 圆形近邻查找

    auto obs_idx = obs_kd_tree.radiusSearch(robot_pose, 2.0);
    Visualizer::Lines2d knn;
    for (const int idx : obs_idx)
    {knn.emplace_back(std::make_pair<Visualizer::Point2d, Visualizer::Point2d>({ start.pose.position.x, start.pose.position.y }, { obs_kd_tree[idx].x(), obs_kd_tree[idx].y() }));
    }
    visualizer->publishLines2d(knn, particles_pub_, frame_id_, "knn", Visualizer::PURPLE, 0.1);
    

    在这里插入图片描述

完整工程代码请联系下方博主名片获取


🔥 更多精彩专栏

  • 《ROS从入门到精通》
  • 《Pytorch深度学习实战》
  • 《机器学习强基计划》
  • 《运动规划实战精讲》

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇

相关文章:

障碍感知 | 基于KD树的障碍物快速处理(附案例分析与ROS C++仿真)

目录 1 障碍处理与KD树2 KD树核心原理2.1 KD树的构造2.2 KD树的查找 3 仿真实现3.1 KD树基本算法3.2 ROS C仿真 1 障碍处理与KD树 在机器人感知系统中&#xff0c;传感器&#xff08;如激光雷达、摄像头等&#xff09;会采集周围的环境数据&#xff0c;例如代价地图、八叉树地…...

Electron -- Electron Fiddle(一)

Electron Fiddle 是一个由 Electron 团队开发的开源工具&#xff0c;它允许开发者快速创建、运行和调试 Electron 应用。这个工具提供了一个简洁的界面&#xff0c;使用户无需配置复杂的开发环境&#xff0c;就能快速体验和学习 Electron。强烈建议将其安装为学习工具。 学习它…...

详解Redis的常用命令

目录 KEYS 语法 EXISTS 语法 DEL 语法 EXPIRE 语法 TTL 语法 TYPE 语法 Redis数据结构和内部编码 KEYS 返回所有满⾜样式&#xff08;pattern&#xff09;的 key。 返回值&#xff1a;匹配 pattern 的所有 key。 语法 ⽀持如下统配样式: h?llo matches hello, ha…...

elasticache备份

Elasticsearch 本地快照操作流程 配置快照存储路径 在 elasticsearch.yml 文件中配置以下字段以指定数据、日志和快照存储路径&#xff1a;path:data: /data/data # 数据存储路径logs: /data/log # 日志存储路径repo: /data/snapshot # 快照存储路径确保路径 /dat…...

Tomcat负载均衡全解析

一、Java项目概述 (一)Java语言特点 Java是一种计算机应用语言,在开发王者和管理系统等方面有着广泛的应用。它具有开源免费的特性,不过需要注意的是,虽然语言本身开源,但是后期开发工具可能会收取费用。 (二)、JDK和Tomcat 1,JDK:作为Java语言的开发工具,在Linu…...

[LeetCode-Python版] 定长滑动窗口8——2461. 长度为 K 子数组中的最大和

题目 给你一个整数数组 nums 和一个整数 k 。请你从 nums 中满足下述条件的全部子数组中找出最大子数组和&#xff1a; 子数组的长度是 k&#xff0c;且 子数组中的所有元素 各不相同 。 返回满足题面要求的最大子数组和。如果不存在子数组满足这些条件&#xff0c;返回 0 。…...

springboot476基于vue篮球联盟管理系统(论文+源码)_kaic

摘 要 如今社会上各行各业&#xff0c;都喜欢用自己行业的专属软件工作&#xff0c;互联网发展到这个时候&#xff0c;人们已经发现离不开了互联网。新技术的产生&#xff0c;往往能解决一些老技术的弊端问题。因为传统篮球联盟管理系统信息管理难度大&#xff0c;容错率低&am…...

预约参观华为基地,见证行业巅峰

✨ 大家好呀&#xff01;今天要跟大家分享一个超酷的体验&#xff0c;关于华为的参观学习之旅&#xff01;&#x1f680; 华为成立于1987年&#xff0c;位于深圳&#xff0c;是全球领先的信息与通信技术&#xff08;ICT&#xff09;解决方案供应商哦&#xff01;他们专注于科技…...

【Flink-scala】DataSet编程模型介绍及数据源

DataStream 学习 1.DataStream编程模型总结 文章目录 DataStream 学习介绍一、DataSet编程模型二、数据源1.文件类数据源2.集合类数据源3.通用类数据源4第三方文件系统 介绍 Flink把批处理看成是一个流处理的特例&#xff0c;因此可以在底层统一的流处理引擎上&#xff0c;同…...

Odrive源码分析(四) 位置爬坡算法

Odrive中自带一个简单的梯形速度爬坡算法&#xff0c;本文分析下这部分代码。 代码如下&#xff1a; #include <cmath> #include "odrive_main.h" #include "utils.hpp"// A sign function where input 0 has positive sign (not 0) float sign_ha…...

[Unity Shader][图形渲染] Shader数学基础11 - 复合变换详解

在图形学与Shader编程中,复合变换是将平移、旋转和缩放等基本几何变换组合在一起,从而实现更复杂的物体变换效果。复合变换的本质是通过矩阵的串联操作,依次应用多个变换。 本文将介绍复合变换的数学原理、矩阵计算方法及注意事项,并结合实际编程中的实现细节帮助你掌握其…...

使用Python实现智能家居控制系统:开启智慧生活的钥匙

友友们好! 我的新专栏《Python进阶》正式启动啦!这是一个专为那些渴望提升Python技能的朋友们量身打造的专栏,无论你是已经有一定基础的开发者,还是希望深入挖掘Python潜力的爱好者,这里都将是你不可错过的宝藏。 在这个专栏中,你将会找到: ● 深入解析:每一篇文章都将…...

使用 HTML5 Canvas 实现动态蜈蚣动画

使用 HTML5 Canvas 实现动态蜈蚣动画 1. 项目概述 我们将通过 HTML 和 JavaScript 创建一个动态蜈蚣。蜈蚣由多个节段组成&#xff0c;每个节段看起来像一个小圆形&#xff0c;并且每个节段上都附带有“脚”。蜈蚣的头部会在画布上随机移动。 完整代码在底部&#xff01;&…...

计算机视觉目标检测——DETR(End-to-End Object Detection with Transformers)

计算机视觉目标检测——DETR(End-to-End Object Detection with Transformers) 文章目录 计算机视觉目标检测——DETR(End-to-End Object Detection with Transformers)摘要Abstract一、DETR算法1. 摘要&#xff08;Abstract&#xff09;2. 引言&#xff08;Introduction&#…...

uniapp .gitignore

打开HBuilderX&#xff0c;在项目根目录下新建文件 .gitignore复制下面内容 #忽略unpackge目录下除了res目录的所有目录 unpackage/* !unpackage/res/#忽略.hbuilderx目录 .hbuilderx# 忽略node_modules目录下的所有文件 node_modules/# 忽略锁文件 package-lock.json yarn.l…...

JavaWeb Servlet的反射优化、Dispatcher优化、视图(重定向)优化、方法参数值获取优化

目录 1. 背景2. 实现2.1 pom.xml2.2 FruitController.java2.3 DispatcherServlet.java2.4 applicationContext.xml 3. 测试 1. 背景 前面我们做了Servlet的一个案例。但是存在很多问题&#xff0c;现在我们要做优化&#xff0c;优化的步骤如下&#xff1a; 每个Fruit请求都需…...

备忘一个FDBatchMove数据转存的问题

使用FDBatchMove的SQL导入excel表到sql表&#xff0c;设置条件时一头雾水&#xff0c;函数不遵守sql的规则。 比如替换字段的TAB键值为空&#xff0c;replace(字段名,char(9),)竟然提示错误&#xff0c;百思不得其解。 试遍了几乎所有的函数&#xff0c;竟然是chr(9)。 这个…...

CEF127 编译指南 MacOS 篇 - 编译 CEF(六)

1. 引言 经过前面的准备工作&#xff0c;我们已经完成了所有必要的环境配置。本文将详细介绍如何在 macOS 系统上编译 CEF127。通过正确的编译命令和参数配置&#xff0c;我们将完成 CEF 的构建工作&#xff0c;最终生成可用的二进制文件。 2. 编译前准备 2.1 确认环境变量 …...

【更新】LLM Interview

课程链接&#xff1a;BV1o217YeELo 文章目录 LLM基础相关1. LLMs概述2. 大语言模型尺寸3. LLMs的优势与劣势4. 常见的大模型分类5. 目前主流的LLMs开源模型体系有哪些&#xff08;Prefix Decoder&#xff0c;Causal Decoder&#xff0c;Encoder-Decoder的区别是什么&#xff09…...

Django 视图中使用 Redis 缓存优化查询性能

在 Web 应用程序开发中,查询数据库是一个常见的操作,但如果查询过于频繁或耗时,就会影响应用程序的性能。为了解决这个问题,我们可以使用缓存技术,将查询结果暂时存储在内存中,从而减少对数据库的访问。本文将介绍如何在 Django 视图中使用 Redis 缓存来优化查询性能。 © …...

css实现圆环展示百分比,根据值动态展示所占比例

代码如下 <view class""><view class"circle-chart"><view v-if"!!num" class"pie-item" :style"{background: conic-gradient(var(--one-color) 0%,#E9E6F1 ${num}%),}"></view><view v-else …...

工程地质软件市场:发展现状、趋势与策略建议

一、引言 在工程建设领域&#xff0c;准确把握地质条件是确保项目顺利推进和安全运营的关键。工程地质软件作为处理、分析、模拟和展示工程地质数据的重要工具&#xff0c;正发挥着日益重要的作用。它凭借强大的数据处理能力、三维建模功能、空间分析工具和可视化展示手段&…...

WEB3全栈开发——面试专业技能点P2智能合约开发(Solidity)

一、Solidity合约开发 下面是 Solidity 合约开发 的概念、代码示例及讲解&#xff0c;适合用作学习或写简历项目背景说明。 &#x1f9e0; 一、概念简介&#xff1a;Solidity 合约开发 Solidity 是一种专门为 以太坊&#xff08;Ethereum&#xff09;平台编写智能合约的高级编…...

零基础设计模式——行为型模式 - 责任链模式

第四部分&#xff1a;行为型模式 - 责任链模式 (Chain of Responsibility Pattern) 欢迎来到行为型模式的学习&#xff01;行为型模式关注对象之间的职责分配、算法封装和对象间的交互。我们将学习的第一个行为型模式是责任链模式。 核心思想&#xff1a;使多个对象都有机会处…...

汇编常见指令

汇编常见指令 一、数据传送指令 指令功能示例说明MOV数据传送MOV EAX, 10将立即数 10 送入 EAXMOV [EBX], EAX将 EAX 值存入 EBX 指向的内存LEA加载有效地址LEA EAX, [EBX4]将 EBX4 的地址存入 EAX&#xff08;不访问内存&#xff09;XCHG交换数据XCHG EAX, EBX交换 EAX 和 EB…...

图表类系列各种样式PPT模版分享

图标图表系列PPT模版&#xff0c;柱状图PPT模版&#xff0c;线状图PPT模版&#xff0c;折线图PPT模版&#xff0c;饼状图PPT模版&#xff0c;雷达图PPT模版&#xff0c;树状图PPT模版 图表类系列各种样式PPT模版分享&#xff1a;图表系列PPT模板https://pan.quark.cn/s/20d40aa…...

Go 语言并发编程基础:无缓冲与有缓冲通道

在上一章节中&#xff0c;我们了解了 Channel 的基本用法。本章将重点分析 Go 中通道的两种类型 —— 无缓冲通道与有缓冲通道&#xff0c;它们在并发编程中各具特点和应用场景。 一、通道的基本分类 类型定义形式特点无缓冲通道make(chan T)发送和接收都必须准备好&#xff0…...

Web中间件--tomcat学习

Web中间件–tomcat Java虚拟机详解 什么是JAVA虚拟机 Java虚拟机是一个抽象的计算机&#xff0c;它可以执行Java字节码。Java虚拟机是Java平台的一部分&#xff0c;Java平台由Java语言、Java API和Java虚拟机组成。Java虚拟机的主要作用是将Java字节码转换为机器代码&#x…...

鸿蒙(HarmonyOS5)实现跳一跳小游戏

下面我将介绍如何使用鸿蒙的ArkUI框架&#xff0c;实现一个简单的跳一跳小游戏。 1. 项目结构 src/main/ets/ ├── MainAbility │ ├── pages │ │ ├── Index.ets // 主页面 │ │ └── GamePage.ets // 游戏页面 │ └── model │ …...

PH热榜 | 2025-06-08

1. Thiings 标语&#xff1a;一套超过1900个免费AI生成的3D图标集合 介绍&#xff1a;Thiings是一个不断扩展的免费AI生成3D图标库&#xff0c;目前已有超过1900个图标。你可以按照主题浏览&#xff0c;生成自己的图标&#xff0c;或者下载整个图标集。所有图标都可以在个人或…...