障碍感知 | 基于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 缓存来优化查询性能。 © …...

Chapter03-Authentication vulnerabilities
文章目录 1. 身份验证简介1.1 What is authentication1.2 difference between authentication and authorization1.3 身份验证机制失效的原因1.4 身份验证机制失效的影响 2. 基于登录功能的漏洞2.1 密码爆破2.2 用户名枚举2.3 有缺陷的暴力破解防护2.3.1 如果用户登录尝试失败次…...

dedecms 织梦自定义表单留言增加ajax验证码功能
增加ajax功能模块,用户不点击提交按钮,只要输入框失去焦点,就会提前提示验证码是否正确。 一,模板上增加验证码 <input name"vdcode"id"vdcode" placeholder"请输入验证码" type"text&quo…...
C++ 基础特性深度解析
目录 引言 一、命名空间(namespace) C 中的命名空间 与 C 语言的对比 二、缺省参数 C 中的缺省参数 与 C 语言的对比 三、引用(reference) C 中的引用 与 C 语言的对比 四、inline(内联函数…...
python爬虫:Newspaper3k 的详细使用(好用的新闻网站文章抓取和解析的Python库)
更多内容请见: 爬虫和逆向教程-专栏介绍和目录 文章目录 一、Newspaper3k 概述1.1 Newspaper3k 介绍1.2 主要功能1.3 典型应用场景1.4 安装二、基本用法2.2 提取单篇文章的内容2.2 处理多篇文档三、高级选项3.1 自定义配置3.2 分析文章情感四、实战案例4.1 构建新闻摘要聚合器…...
JDK 17 新特性
#JDK 17 新特性 /**************** 文本块 *****************/ python/scala中早就支持,不稀奇 String json “”" { “name”: “Java”, “version”: 17 } “”"; /**************** Switch 语句 -> 表达式 *****************/ 挺好的ÿ…...

优选算法第十二讲:队列 + 宽搜 优先级队列
优选算法第十二讲:队列 宽搜 && 优先级队列 1.N叉树的层序遍历2.二叉树的锯齿型层序遍历3.二叉树最大宽度4.在每个树行中找最大值5.优先级队列 -- 最后一块石头的重量6.数据流中的第K大元素7.前K个高频单词8.数据流的中位数 1.N叉树的层序遍历 2.二叉树的锯…...
蓝桥杯 冶炼金属
原题目链接 🔧 冶炼金属转换率推测题解 📜 原题描述 小蓝有一个神奇的炉子用于将普通金属 O O O 冶炼成为一种特殊金属 X X X。这个炉子有一个属性叫转换率 V V V,是一个正整数,表示每 V V V 个普通金属 O O O 可以冶炼出 …...
《C++ 模板》
目录 函数模板 类模板 非类型模板参数 模板特化 函数模板特化 类模板的特化 模板,就像一个模具,里面可以将不同类型的材料做成一个形状,其分为函数模板和类模板。 函数模板 函数模板可以简化函数重载的代码。格式:templa…...

人机融合智能 | “人智交互”跨学科新领域
本文系统地提出基于“以人为中心AI(HCAI)”理念的人-人工智能交互(人智交互)这一跨学科新领域及框架,定义人智交互领域的理念、基本理论和关键问题、方法、开发流程和参与团队等,阐述提出人智交互新领域的意义。然后,提出人智交互研究的三种新范式取向以及它们的意义。最后,总结…...

【从零学习JVM|第三篇】类的生命周期(高频面试题)
前言: 在Java编程中,类的生命周期是指类从被加载到内存中开始,到被卸载出内存为止的整个过程。了解类的生命周期对于理解Java程序的运行机制以及性能优化非常重要。本文会深入探寻类的生命周期,让读者对此有深刻印象。 目录 …...