障碍感知 | 基于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 x∈Rk。所有非叶子节点包含一个把空间分成两个半空间的超平面。节点的左子树组织了位于其超平面左侧的点,右子树同理。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树 在机器人感知系统中,传感器(如激光雷达、摄像头等)会采集周围的环境数据,例如代价地图、八叉树地…...
Electron -- Electron Fiddle(一)
Electron Fiddle 是一个由 Electron 团队开发的开源工具,它允许开发者快速创建、运行和调试 Electron 应用。这个工具提供了一个简洁的界面,使用户无需配置复杂的开发环境,就能快速体验和学习 Electron。强烈建议将其安装为学习工具。 学习它…...

详解Redis的常用命令
目录 KEYS 语法 EXISTS 语法 DEL 语法 EXPIRE 语法 TTL 语法 TYPE 语法 Redis数据结构和内部编码 KEYS 返回所有满⾜样式(pattern)的 key。 返回值:匹配 pattern 的所有 key。 语法 ⽀持如下统配样式: h?llo matches hello, ha…...
elasticache备份
Elasticsearch 本地快照操作流程 配置快照存储路径 在 elasticsearch.yml 文件中配置以下字段以指定数据、日志和快照存储路径: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 中满足下述条件的全部子数组中找出最大子数组和: 子数组的长度是 k,且 子数组中的所有元素 各不相同 。 返回满足题面要求的最大子数组和。如果不存在子数组满足这些条件,返回 0 。…...

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

预约参观华为基地,见证行业巅峰
✨ 大家好呀!今天要跟大家分享一个超酷的体验,关于华为的参观学习之旅!🚀 华为成立于1987年,位于深圳,是全球领先的信息与通信技术(ICT)解决方案供应商哦!他们专注于科技…...

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

Odrive源码分析(四) 位置爬坡算法
Odrive中自带一个简单的梯形速度爬坡算法,本文分析下这部分代码。 代码如下: #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 创建一个动态蜈蚣。蜈蚣由多个节段组成,每个节段看起来像一个小圆形,并且每个节段上都附带有“脚”。蜈蚣的头部会在画布上随机移动。 完整代码在底部!&…...

计算机视觉目标检测——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. 摘要(Abstract)2. 引言(Introduction&#…...

uniapp .gitignore
打开HBuilderX,在项目根目录下新建文件 .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的一个案例。但是存在很多问题,现在我们要做优化,优化的步骤如下: 每个Fruit请求都需…...
备忘一个FDBatchMove数据转存的问题
使用FDBatchMove的SQL导入excel表到sql表,设置条件时一头雾水,函数不遵守sql的规则。 比如替换字段的TAB键值为空,replace(字段名,char(9),)竟然提示错误,百思不得其解。 试遍了几乎所有的函数,竟然是chr(9)。 这个…...

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

【更新】LLM Interview
课程链接:BV1o217YeELo 文章目录 LLM基础相关1. LLMs概述2. 大语言模型尺寸3. LLMs的优势与劣势4. 常见的大模型分类5. 目前主流的LLMs开源模型体系有哪些(Prefix Decoder,Causal Decoder,Encoder-Decoder的区别是什么)…...
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 …...
工程地质软件市场:发展现状、趋势与策略建议
一、引言 在工程建设领域,准确把握地质条件是确保项目顺利推进和安全运营的关键。工程地质软件作为处理、分析、模拟和展示工程地质数据的重要工具,正发挥着日益重要的作用。它凭借强大的数据处理能力、三维建模功能、空间分析工具和可视化展示手段&…...
WEB3全栈开发——面试专业技能点P2智能合约开发(Solidity)
一、Solidity合约开发 下面是 Solidity 合约开发 的概念、代码示例及讲解,适合用作学习或写简历项目背景说明。 🧠 一、概念简介:Solidity 合约开发 Solidity 是一种专门为 以太坊(Ethereum)平台编写智能合约的高级编…...

零基础设计模式——行为型模式 - 责任链模式
第四部分:行为型模式 - 责任链模式 (Chain of Responsibility Pattern) 欢迎来到行为型模式的学习!行为型模式关注对象之间的职责分配、算法封装和对象间的交互。我们将学习的第一个行为型模式是责任链模式。 核心思想:使多个对象都有机会处…...
汇编常见指令
汇编常见指令 一、数据传送指令 指令功能示例说明MOV数据传送MOV EAX, 10将立即数 10 送入 EAXMOV [EBX], EAX将 EAX 值存入 EBX 指向的内存LEA加载有效地址LEA EAX, [EBX4]将 EBX4 的地址存入 EAX(不访问内存)XCHG交换数据XCHG EAX, EBX交换 EAX 和 EB…...

图表类系列各种样式PPT模版分享
图标图表系列PPT模版,柱状图PPT模版,线状图PPT模版,折线图PPT模版,饼状图PPT模版,雷达图PPT模版,树状图PPT模版 图表类系列各种样式PPT模版分享:图表系列PPT模板https://pan.quark.cn/s/20d40aa…...
Go 语言并发编程基础:无缓冲与有缓冲通道
在上一章节中,我们了解了 Channel 的基本用法。本章将重点分析 Go 中通道的两种类型 —— 无缓冲通道与有缓冲通道,它们在并发编程中各具特点和应用场景。 一、通道的基本分类 类型定义形式特点无缓冲通道make(chan T)发送和接收都必须准备好࿰…...
Web中间件--tomcat学习
Web中间件–tomcat Java虚拟机详解 什么是JAVA虚拟机 Java虚拟机是一个抽象的计算机,它可以执行Java字节码。Java虚拟机是Java平台的一部分,Java平台由Java语言、Java API和Java虚拟机组成。Java虚拟机的主要作用是将Java字节码转换为机器代码&#x…...
鸿蒙(HarmonyOS5)实现跳一跳小游戏
下面我将介绍如何使用鸿蒙的ArkUI框架,实现一个简单的跳一跳小游戏。 1. 项目结构 src/main/ets/ ├── MainAbility │ ├── pages │ │ ├── Index.ets // 主页面 │ │ └── GamePage.ets // 游戏页面 │ └── model │ …...

PH热榜 | 2025-06-08
1. Thiings 标语:一套超过1900个免费AI生成的3D图标集合 介绍:Thiings是一个不断扩展的免费AI生成3D图标库,目前已有超过1900个图标。你可以按照主题浏览,生成自己的图标,或者下载整个图标集。所有图标都可以在个人或…...