碰撞检测 | 矩形增量膨胀安全走廊模型(附C++/Python仿真)

目录

  • 0 专栏介绍
  • 1 安全走廊建模的动机
  • 2 矩形增量膨胀算法
  • 3 算法仿真
    • 3.1 C++实现
    • 3.2 Python实现

0 专栏介绍

🔥课设、毕设、创新竞赛必备!🔥本专栏涉及更高阶的运动规划算法轨迹优化实战,包括:曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等),每个算法都包含代码实现加深理解

🚀详情:运动规划实战进阶:轨迹优化篇


1 安全走廊建模的动机

在轨迹优化算法中,通常约束机器人或自车不与障碍物发生碰撞,最朴素的办法是建立自身与所有障碍物的联系。然而,机器人并没有在每一刻都与每个障碍物发生碰撞的可能性

如图所示,考虑到从 A A A B B B的任务,如果自动驾驶车辆选择通过上面的路径行驶,则在底部可能很少与障碍物 O 1 O_1 O1 O 2 O_2 O2发生碰撞;如果选择下面的路径,则可能不会与 O 5 O_5 O5发生碰撞。因此,同时避免与 O 1 O_1 O1 O 2 O_2 O2 O 5 O_5 O5发生碰撞会导致冗余约束。如果预先给定了引导路径,可以减少碰撞回避约束的规模。

在这里插入图片描述

进一步考虑,如果沿着选定路径构建一条安全走廊来将机器人与周围障碍物隔离开来,则与环境相关的约束规模将完全独立于障碍物数量,与环境复杂度无关——这对于优化建模非常关键。

2 矩形增量膨胀算法

矩形增量膨胀算法的核心原理是:第 i i i个局部框最初由一个安全矩形构成——考虑机器人的几何尺寸以及安全边界等。接着依次在每条边上扩展直到检测到碰撞,扩展顺序和方向如图( b b b)所示。

在这里插入图片描述

具体地,每次扩展尝试都会通过恒定大小 将当前安全区域的相关边向外推,并增量地对扩展区域进行碰撞检测,如图( c c c)所示。若没有发生碰撞,则将扩展区域合并到安全区域中;否则它将被拒绝,同时在该方向上不会进行进一步的扩展。算法流程如下所示

在这里插入图片描述

3 算法仿真

3.1 C++实现

bool TrajectoryOptimizer::GenerateBox(double time, double &x, double &y, double radius, AABox2d &result) const
{double ri = radius;AABox2d bound({-ri, -ri}, {ri, ri});if (CheckCollision(time, x, y, bound)){// initial condition not satisfied, involute to find feasible boxint inc = 4;double real_x, real_y;do{int iter = inc / 4;uint8_t edge = inc % 4;real_x = x;real_y = y;if (edge == 0){real_x = x - iter * 0.05;}else if (edge == 1){real_x = x + iter * 0.05;}else if (edge == 2){real_y = y - iter * 0.05;}else if (edge == 3){real_y = y + iter * 0.05;}inc++;} while (CheckCollision(time, real_x, real_y, bound) && inc < config_.corridor_max_iter);if (inc > config_.corridor_max_iter){return false;}x = real_x;y = real_y;}int inc = 4;std::bitset<4> blocked;double incremental[4] = {0.0};double step = radius * 0.2;do{int iter = inc / 4;uint8_t edge = inc % 4;inc++;if (blocked[edge])continue;incremental[edge] = iter * step;AABox2d test({-ri - incremental[0], -ri - incremental[2]},{ri + incremental[1], ri + incremental[3]});if (CheckCollision(time, x, y, test) || incremental[edge] >= config_.corridor_incremental_limit){incremental[edge] -= step;blocked[edge] = true;}} while (!blocked.all() && inc < config_.corridor_max_iter);if (inc > config_.corridor_max_iter){return false;}result = {{x - incremental[0], y - incremental[2]},{x + incremental[1], y + incremental[3]}};return true;
}

3.2 Python实现

def decompose(self, way_points: list) -> list:step = 1prune_waypoints, decomp_polygons = [], []nums = len(way_points)for i in range(nums - 1):x, y = way_points[i]if len(decomp_polygons) > 0 and decomp_polygons[-1].isInside(np.array([[x], [y]])):continue# adjustment for initial (x, y)if self.isRectCollision(x - self.radius, y - self.radius, x + self.radius, y + self.radius):inc = 4while inc < self.max_iter:it = inc / 4edge = inc % 4inc += 1real_x, real_y = x, yif edge == 0:real_x = int(x - it * step)elif edge == 1:real_y = int(y - it * step)elif edge == 2:real_x = int(x + it * step)else:real_y = int(y + it * step)# collision detectionif not self.isRectCollision(real_x - self.radius, real_y - self.radius,real_x + self.radius + 1, real_y + self.radius + 1):breakif inc > self.max_iter:return [], []else:x, y = real_x, real_y# main loop for construction in (rx, ry)inc = 4block, increment = np.zeros(4, dtype=np.bool_), np.zeros(4, dtype=np.int32)while inc < self.max_iter and not block.all():it = inc / 4edge = inc % 4inc += 1if block[edge]:continueincrement[edge] = int(it * step)if increment[edge] >= self.safe_range or self.isCollision(edge, x, y, increment):# increment[edge] -= stepblock[edge] = Trueif inc > self.max_iter:return [], []else:prune_waypoints.append((x, y))decomp_polygons.append(Polygon([Hyperplane(n=np.array([[-1], [0]]), d=np.array([[x - increment[0]], [y]])),Hyperplane(n=np.array([[1], [0]]), d=np.array([[x + increment[2]], [y]])),Hyperplane(n=np.array([[0], [-1]]), d=np.array([[x], [y - increment[1]]])),Hyperplane(n=np.array([[0], [1]]), d=np.array([[x], [y +  increment[3]]]))]))prune_waypoints.append(way_points[-1])return decomp_polygons, prune_waypoints

在这里插入图片描述

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


🔥 更多精彩专栏

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

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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://xiahunao.cn/news/3280753.html

如若内容造成侵权/违法违规/事实不符,请联系瞎胡闹网进行投诉反馈,一经查实,立即删除!

相关文章

哪个牌子的眼镜清洗机好?买超声波清洗机有必要吗

生活中&#xff0c;我们经常忽视眼镜的清洁。你知道吗&#xff1f;眼镜如果长时间不清洁的话&#xff0c;镜片上的污垢和油脂会让视线变得模糊不清&#xff0c;甚至油污滋生的细菌还可能伤害到我们的眼睛&#xff0c;比如引起眼睛疲劳或炎症。为了保持眼镜干净&#xff0c;现在…

生鲜 52 周 MD如何助力业绩提升

生鲜 52 周 MD &#xff0c;顾名思义&#xff0c;就是以一年 52 周为周期&#xff0c;对生鲜商品进行精细化、动态化的营销规划。它不再是传统的固定化、模式化的销售方式&#xff0c;而是根据每周的季节特点、节日氛围、消费趋势以及市场变化&#xff0c;精心策划生鲜商品的种…

11 优化器

目录 1. 随机梯度下降系优化器&#xff1a;SGD 1.1 算法种类 1.2 优缺点 2 SGDM 即为SGD with momentum 动量 2.1 公式 2.2 动量的优缺点 优点 缺点 2.3 使用场景 3 AdaGrad 3.1 公式 3.2 AdaGrad的优缺点 优点 缺点 3.3 使用场景 3.4 Adam 3.4.1 Adam优化器的…

计算机网络01

文章目录 浏览器输入URL后发生了什么&#xff1f;Linux 系统是如何收发网络包的&#xff1f;Linux 网络协议栈Linux 接收网络包的流程Linux 发送网络包的流程 浏览器输入URL后发生了什么&#xff1f; URL解析 当在浏览器中输入URL后&#xff0c;浏览器首先对拿到的URL进行识别…

2024最全RabbitMQ集群方案汇总

之前在网上找rabbitmq集群方案有哪几种时&#xff0c;基本上看到的答案都不太一样&#xff0c;所以此文的主要目的是梳理一下rabbitmq集群方案&#xff0c;对rabbitmq集群方案的笔记并不是搭建的笔记。 总结了一些文章&#xff0c;rabbitmq集群大概有五种方案&#xff1a;普通…

[Linux安全运维] iptables包过滤

前言 防火墙是网络安全中非常重要的设备&#xff0c;是一种将内部网络和外部网络隔离开的技术。简单来说&#xff0c;防火墙技术就是访问控制技术&#xff0c;由规则和动作组成。 1. Linux 包过滤防火墙 1 .1 概述 iptables&#xff1a; 指的是管理Linux防火墙的命令程序&a…

Windows 下后台启动 jar 包,UTF-8 启动 jar 包_windows启动jar

转自:https://blog.csdn.net/2401_83817971/article/details/137514739 本文介绍了如何使用javaw.exe后台启动Javajar包&#xff0c;如何在Windows中管理和设置cmd编码&#xff0c;以及与Python开发相关的学习资源。包括UTF-8编码启动jar包的方法和Windows下关闭后台服务的技…

superset 不显示mysql的选项问题

superset不显示mysql的选项 数据库驱动未安装&#xff1a;确保你已经安装了Python的MySQL数据库驱动&#xff0c;比如mysqlclient。 pip install mysqlclient

vue一些npm i 时报错问题解决【JAVA前后端分离】

前端vue npm i 安装时出现 作为一个懂些前端得 JAVA开发自然是要粗暴解决这个问题了 问题解决 使用命令 npx -p npm6 npm i 即可编译 原因&#xff1a; ERESOLVE与npm版本有关&#xff0c;因为npm版本高对某些事情比npm6.x更严格。通常&#xff0c;最简单的解决方法是将--…

【机器学习基础】机器学习概述与实践基础

【作者主页】Francek Chen 【专栏介绍】 ⌈ ⌈ ⌈Python机器学习 ⌋ ⌋ ⌋ 机器学习是一门人工智能的分支学科&#xff0c;通过算法和模型让计算机从数据中学习&#xff0c;进行模型训练和优化&#xff0c;做出预测、分类和决策支持。Python成为机器学习的首选语言&#xff0c;…

STC12C5A60S2单片机输出pwm的方法

所谓的pwm输出就是让单片机在某个管脚上按照时间输出特定频率和占空比的矩形方波。这里面有两个参数&#xff0c;一是频率&#xff0c;二是占空比。两者互不干涉。以STC12C5A60S2为例子&#xff0c;本51型单片机可以有两路pwm输出。以其中的一路为例说明&#xff1b; 首先说频…

Teltonika FMXXX系列定位器解析说明

1.产品外观 2.数据包说明 2.1.登录包 **模块第一次上线&#xff0c;先发送其对应的IMEI号&#xff0c;数据包如下&#xff1a;** 000F313233343536373839303132333435**数据包解析如下&#xff1a;** 000F --packet starts 313233343536373839303132333435 --IMEI 1234567890…

区块链软硬件协同,做产业数字化转型的“安全官” |《超话区块链》直播预告

今年的两会政府工作报告提出&#xff1a;“产业的数字化&#xff08;行业数字化转型&#xff09;是发展新质生产力的核心&#xff0c;是推动产业升级实现高质量发展的关键。”全面推进产业数字化&#xff0c;需要技术创新与产业应用深入协同&#xff1b;立足可持续发展的长远目…

泰迪智能科技入选广东省2024年第三批职业技能等级认定社会培训评价组织名单

7月17日&#xff0c;根据《关于公布广东省2023年社会培训评价组织新申请机构进入培育辅导期名单的通知》&#xff08;粤人社函〔2023〕269号&#xff09;、《关于公布广东省2023年社会培训评价组织&#xff08;“产教评”链主培育单位&#xff09;进入培育辅导期名单的通知》&a…

网络安全是什么?怎么入门网络安全?

一、网络安全的定义 网络安全&#xff0c;简单来说&#xff0c;就是保护网络系统中的硬件、软件以及其中的数据不因偶然或恶意的原因而遭到破坏、更改、泄露&#xff0c;保障系统连续可靠正常地运行&#xff0c;网络服务不中断。 随着信息技术的飞速发展&#xff0c;网络安全的…

docker compose 安装 kafka

一 前置准备 创建 /data/kafkadata /data/zookeeper-1用于保存kafka和zookeeper的配置文件 kafkadata中创建三个文件夹 /kafka1 /kafka2 /kafka3&#xff0c;用于存放三个kafka节点的配置文件 zookeeper-1文件夹中创建 /conf /data /logs /datalog四个文件夹&#xff0c;用于…

IoTDB 入门教程 实战篇④——C#示例(开源)

文章目录 一、前文二、新建C#项目三、NuGet安装四、示例源码五、查询数据六、参考 一、前文 IoTDB入门教程——导读 本文详细阐述了如何通过一个C#项目成功连接到IoTDB时序数据库&#xff0c;进而展示了如何向该数据库高效地写入数据以及执行精确的数据查询操作。 此示例旨在为…

Redis高可用之持久化,以及reids的性能管理

一、redis高可用&#xff1a; 在集群当中有一个非常重要的指标&#xff0c;提供正常服务的时间的百分比&#xff08;365天&#xff09;99.9% redis的高可用含义更加宽泛&#xff0c;正常服务是指标之一&#xff0c;数据容量的扩展&#xff0c;数据的安全性 在redis中实现高可…

多模态大模型(MM-LLMs)近期概述

《MM-LLMs: Recent Advances in MultiModal Large Language Models》 论文地址&#xff1a;https://arxiv.org/abs/2401.13601 GitHub地址&#xff1a;https://mm-llms.github.io/ 1.介绍 目的&#xff1a;MM-LLMs旨在减少计算费用并提高多模态预训练的效率, 因为从头开始训…

Data Augmentation数据增强

目录 数据增强是什么 为什么数据增强 数组增强分类 有监督数据增强 无监督数据增强 数据增强是什么 数据增强又称数据扩增&#xff0c;是一种通过应用合理且随机的变换&#xff08;例如图像位移、旋转&#xff09;来增加训练集多样性的技术。让有限的数据产生等价于更多数…