碰撞检测 | 图解视线生成Bresenham算法(附ROS C++/Python/Matlab实现)

news2024/11/25 7:01:42

目录

  • 0 专栏介绍
  • 1 Bresenham算法介绍
  • 2 图解Bresenham算法
  • 3 算法流程
  • 4 仿真实现
    • 4.1 ROS C++实现
    • 4.2 Python实现
    • 4.3 Matlab实现

0 专栏介绍

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

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


1 Bresenham算法介绍

Bresenham视线生成算法是一种高效的算法,用于在二维网格上绘制直线。它是由Jack Bresenham在1962年提出的,广泛应用于计算机图形学和游戏开发中。该算法的主要优点是只使用整数运算,因此速度较快且易于实现。下面是该算法的动图案例

在这里插入图片描述

Bresenham算法可以巧妙地应用在栅格地图中进行一维碰撞检测:首先确定起点和终点,然后使用Bresenham算法绘制从起点到终点的线段,接着检查该路径上每个栅格点是否与障碍物重叠。

2 图解Bresenham算法

Bresenham碰撞测试在三种类型的移动中访问单元格:

  • x x x方向移动
  • y y y方向移动
  • 对角线移动

在栅格地图中,碰撞检测点连线经过若干离散栅格,因此每次移动都将产生非连续误差,Bresenham算法要求下一个移动偏差最小。通过迭代即可访问检测线经过的所有栅格,判断这些栅格的代价是否超过阈值即可完成碰撞检测。

具体地,设需要检测节点 v v v w w w间的连线是否经过障碍物,定义缩放误差分别为 δ x = ∣ w . x − v . x ∣ \delta _x=\left| w.x-v.x \right| δx=w.xv.x δ y = ∣ w . y − v . y ∣ \delta _y=\left| w.y-v.y \right| δy=w.yv.y;扩展误差分别为 e x e_x ex e y e_y ey;方向增量分别为 Δ x = s g n ( w . x − v . x ) \varDelta x=sgn\left( w.x-v.x \right) Δx=sgn(w.xv.x) Δ y = s g n ( w . y − v . y ) \varDelta y=sgn \left( w.y-v.y \right) Δy=sgn(w.yv.y)。下面考虑 δ x > δ y \delta _x>\delta _y δx>δy的情形, δ x ⩽ δ y \delta _x\leqslant \delta _y δxδy时可对称导出。

在这里插入图片描述

如图所示,根据三角形相似关系可得

{ e y e x = ∣ Δ y ∣ t t δ y = ∣ Δ x ∣ δ x ⇒ e y e x = ∣ Δ y ∣ ∣ Δ x ∣ ⋅ δ x δ y = δ x δ y \begin{cases} \frac{e_y}{e_x}=\frac{\left| \varDelta y \right|}{t}\\ \frac{t}{\delta _y}=\frac{\left| \varDelta x \right|}{\delta _x}\\\end{cases}\Rightarrow \frac{e_y}{e_x}=\frac{\left| \varDelta y \right|}{\left| \varDelta x \right|}\cdot \frac{\delta _x}{\delta _y}=\frac{\delta _x}{\delta _y} {exey=tΔyδyt=δxΔxexey=ΔxΔyδyδx=δyδx

不妨令 e y = δ x e_y=\delta _x ey=δx e x = δ y e_x=\delta _y ex=δy,则沿 x x x方向移动将产生负向偏差 δ y \delta _y δy,沿 y y y方向移动将产生正向偏差 δ x \delta _x δx,根据最小化偏差原则选择移动方向

( x , y ) ← { ( x + Δ x , y ) , i f ∣ e + δ x ∣ > ∣ e − δ y ∣ ( x , y + Δ y ) , i f ∣ e + δ x ∣ < ∣ e − δ y ∣ ( x + Δ x , y + Δ y ) , i f ∣ e + δ x ∣ = ∣ e − δ y ∣ \left( x,y \right) \gets \begin{cases} \left( x+\varDelta x,y \right) , \mathrm{if} \left| e+\delta _x \right|>\left| e-\delta _y \right|\\ \left( x,y+\varDelta y \right) , \mathrm{if} \left| e+\delta _x \right|<\left| e-\delta _y \right|\\ \left( x+\varDelta x,y+\varDelta y \right) , \mathrm{if} \left| e+\delta _x \right|=\left| e-\delta _y \right|\\\end{cases} (x,y)(x+Δx,y),ife+δx>eδy(x,y+Δy),ife+δx<eδy(x+Δx,y+Δy),ife+δx=eδy

下图为Bresenham扩展节点的实例

在这里插入图片描述

3 算法流程

算法流程如下所示

在这里插入图片描述

4 仿真实现

4.1 ROS C++实现

核心代码如下所示

template <typename Point, typename F_is_obs>
static bool BresenhamCollisionDetection(const Point& pt1, const Point& pt2, F_is_obs func_is_obs)
{
  int s_x = (pt1.x() - pt2.x() == 0) ? 0 : (pt1.x() - pt2.x()) / std::abs(pt1.x() - pt2.x());
  int s_y = (pt1.y() - pt2.y() == 0) ? 0 : (pt1.y() - pt2.y()) / std::abs(pt1.y() - pt2.y());
  int d_x = std::abs(pt1.x() - pt2.x());
  int d_y = std::abs(pt1.y() - pt2.y());

  // check if any obstacle exists between pt1 and pt2
  if (d_x > d_y)
  {
    int tau = d_y - d_x;
    int x = pt2.x(), y = pt2.y();
    int e = 0;
    while (x != pt1.x())
    {
      if (e * 2 > tau)
      {
        x += s_x;
        e -= d_y;
      }
      else if (e * 2 < tau)
      {
        y += s_y;
        e += d_x;
      }
      else
      {
        x += s_x;
        y += s_y;
        e += d_x - d_y;
      }
      if (func_is_obs(Point(x, y)))
        // obstacle detected
        return true;
    }
  }
  else
  {
    // similar. swap x and y
    int tau = d_x - d_y;
    int x = pt2.x(), y = pt2.y();
    int e = 0;
    while (y != pt1.y())
    {
      if (e * 2 > tau)
      {
        y += s_y;
        e -= d_x;
      }
      else if (e * 2 < tau)
      {
        x += s_x;
        e += d_y;
      }
      else
      {
        x += s_x;
        y += s_y;
        e += d_y - d_x;
      }
      if (func_is_obs(Point(x, y)))
        // obstacle detected
        return true;
    }
  }
  return false;
}

4.2 Python实现

核心代码如下所示

def BresenhamCollisionDetection(self, node1: Node, node2: Node) -> bool:
	if node1.current in self.obstacles or node2.current in self.obstacles:
	    return False
	
	x1, y1 = node1.current
	x2, y2 = node2.current
	
	if x1 < 0 or x1 >= self.env.x_range or y1 < 0 or y1 >= self.env.y_range:
	    return False
	if x2 < 0 or x2 >= self.env.x_range or y2 < 0 or y2 >= self.env.y_range:
	    return False
	
	d_x = abs(x2 - x1)
	d_y = abs(y2 - y1)
	s_x = 0 if (x2 - x1) == 0 else (x2 - x1) / d_x
	s_y = 0 if (y2 - y1) == 0 else (y2 - y1) / d_y
	x, y, e = x1, y1, 0
	
	# check if any obstacle exists between node1 and node2
	if d_x > d_y:
	    tau = (d_y - d_x) / 2
	    while not x == x2:
	        if e > tau:
	            x = x + s_x
	            e = e - d_y
	        elif e < tau:
	            y = y + s_y
	            e = e + d_x
	        else:
	            x = x + s_x
	            y = y + s_y
	            e = e + d_x - d_y
	        if (x, y) in self.obstacles:
	            return False
	# swap x and y
	else:
	    tau = (d_x - d_y) / 2
	    while not y == y2:
	        if e > tau:
	            y = y + s_y
	            e = e - d_x
	        elif e < tau:
	            x = x + s_x
	            e = e + d_y
	        else:
	            x = x + s_x
	            y = y + s_y
	            e = e + d_y - d_x
	        if (x, y) in self.obstacles:
	            return False
	
	return True

4.3 Matlab实现

核心代码如下所示

function flag = BresenhamCollisionDetection(map, node1, node2)
% @breif: Judge collision when moving from node1 to node2 using Bresenham.
    if (map(node1(1), node1(2)) == 2) || (map(node2(1), node2(2)) == 2)
        flag = true;
        return
    end
    x1 = node1(1); y1 = node1(2);
    x2 = node2(1); y2 = node2(2);
    
    d_x = abs(x2 - x1);
    d_y = abs(y2 - y1);
    if  (x2 - x1) == 0
        s_x = 0;
    else
        s_x = (x2 - x1) / d_x;
    end
    if  (y2 - y1) == 0
        s_y = 0;
    else
        s_y = (y2 - y1) / d_y;
    end
    x = x1; y = y1; e = 0;
    
    % check if any obstacle exists between node1 and node2
    if d_x > d_y
        tao = (d_y - d_x) / 2;
        while x ~= x2
            if e > tao
                x = x + s_x;
                e = e - d_y;
            elseif e < tao
                y = y + s_y;
                e = e + d_x;
            else
                x = x + s_x;
                y = y + s_y;
                e = e + d_x - d_y;
            end
            if map(x, y) == 2
                flag = true;
                return;
            end
        end
    % swap x and y
    else
        tao = (d_x - d_y) / 2;
        while y ~= y2
            if e > tao
                y = y + s_y;
                e = e - d_x;
            elseif e < tao
                x = x + s_x;
                e = e + d_y;
            else
                x = x + s_x;
                y = y + s_y;
                e = e + d_y - d_x;
            end
            if map(x, y) == 2
                flag = true;
                return;
            end
        end
    end
    flag = false;
end

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


🔥 更多精彩专栏

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

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

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

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

相关文章

HarmonyOS Next元服务开发快速入门案例

项目代码gitee地址&#xff1a; (HarmonyOS Next 元服务开发快速入门: HarmonyOS Next 元服务开发快速入门 - Gitee.com) 开源协议使用&#xff1a;Apache License 2.0 &#xff0c;代码包支持免费使用&#xff0c;可进行二次开发后选择开源或闭源。 一、创建项目 1.创建项目&…

大学生社团活动系统小程序的设计

管理员账户功能包括&#xff1a;系统首页&#xff0c;个人中心&#xff0c;学生管理&#xff0c;社长管理&#xff0c;社团分类管理&#xff0c;社团信息管理&#xff0c;社团加入管理&#xff0c;社团活动管理&#xff0c;轮播图信息 微信端账号功能包括&#xff1a;系统首页…

基于SpringBoot vue3 的山西文旅网java网页设计与实现

博主介绍&#xff1a;专注于Java&#xff08;springboot ssm springcloud等开发框架&#xff09; vue .net php phython node.js uniapp小程序 等诸多技术领域和毕业项目实战、企业信息化系统建设&#xff0c;从业十五余年开发设计教学工作☆☆☆ 精彩专栏推荐订阅☆☆☆☆…

项目完整开发的流程

流程 1.设计产品 2.写需求文档 2.1需求分析&#xff0c;后端设计数据库&#xff0c;建表&#xff0c;客户沟通&#xff0c;说完签字&#xff0c;留证据&#xff0c;防止后面扯皮&#xff0c;和防止后续变需求重新写业务 3.画原型图&#xff0c;也就是草图&#xff0c;初始的…

LeetCode讲解篇之2466. 统计构造好字符串的方案数

文章目录 题目描述题解思路题解代码题目链接 题目描述 题解思路 组成长度为i的字符串个数 组成长度为i - zero的字符串个数 组成长度为i - one的字符串个数 设数组f中i号元素的值为组成长度为i的字符串个数 则状态转移方程为f[i] f[i - zero] f[i - one]&#xff0c;其中需…

【unity进阶知识10】从零手搓一个UI管理器/UI框架,自带一个提示界面,还有自带DOTween动画效果

最终效果 文章目录 最终效果前言UI管理器1、新增UI面板层枚举2、初始化2.1、用代码创建画布2.2、用代码创建UI面板的父物体层2.3、代码添加EventSystem物体 3、ShowPanel显示面板方法4、HidePanel隐藏面板的方法5、CloseUI关闭界面的方法6、UI界面基类 测试调用优化绑定按钮事件…

C语言初步介绍(初学者,大学生)【上】

1.C语⾔是什么&#xff1f; ⼈和⼈交流使⽤的是⾃然语⾔&#xff0c;如&#xff1a;汉语、英语、⽇语 那⼈和计算机是怎么交流的呢&#xff1f;使⽤ 计算机语⾔ 。 ⽬前已知已经有上千种计算机语⾔&#xff0c;⼈们是通过计算机语⾔写的程序&#xff0c;给计算机下达指令&am…

【AIGC产品经理】面试7家,拿到2个offer,薪资中上水平

Hello&#xff0c;大家好&#xff0c;我是一名不知名的5年B端金融产品经验的产品经理&#xff0c;成功转行AI产品经理&#xff0c;前期面试了北京百度、阿里、理想汽车、百川智能、华为、OPPO等多家大厂面试&#xff0c;但是由于已定居成都&#xff0c;主动终止了后续需要线下的…

ubuntu 系统安装

使用VMware虚拟机上进行实现 官网下载地址&#xff1a; https://cn.ubuntu.com/download https://releases.ubuntu.com 操作系统手册&#xff1a; https://ubuntu.com/server/docs/ &#xff08;里面包含安装文档&#xff09; 安装指南&#xff08;详细&#xff09;&#xff1a…

一手信息:用ai怎么做短视频赚钱。

AI制作短视频赚钱的具体数据与分析如下&#xff1a; 数据展示 ​ 更多实操教程和AI绘画工具,可以扫描下方,免费获取 \1. 收入情况&#xff1a; - 有案例显示&#xff0c;通过AI生成历史解说视频&#xff0c;半年内可以赚取64万人民币。 - 另一个案例则是通过AI生成电影解说…

鸿蒙 Next 实战: 环境搭建

前言 作为独立开发者&#xff0c;如果我们错过了传统移动 App&#xff0c;和后起小程序的红利&#xff0c;那万物互联 AI 的应用开发就得抓住了。 虽然个人上架应用平台难易都差不多&#xff0c;但是鸿蒙生态当前正需要广大开发者参与&#xff0c;一旦上架&#xff0c;相比其…

AI绘画ComfyUI 完全入门:基本功能完全掌握!

前言 大家好&#xff0c;我是每天分享AI应用的萤火君&#xff01; 在AI绘画领域&#xff0c;Stable Diffusion 因其开源特性而受到广泛的关注和支持&#xff0c;背后聚拢了一大批的应用开发者和艺术创作者&#xff0c;是AI绘画领域当之无愧的王者。 目前使用 Stable Diffusi…

外国钞票面值检测系统源码分享

外国钞票面值检测系统源码分享 [一条龙教学YOLOV8标注好的数据集一键训练_70全套改进创新点发刊_Web前端展示] 1.研究背景与意义 项目参考AAAI Association for the Advancement of Artificial Intelligence 项目来源AACV Association for the Advancement of Computer Vis…

新版AndroidStudio log使用

从Android Studio Dophin开始&#xff0c;Android Studio中的默认展示了新版的logcat。新版的logcat色彩上是更加的好看了&#xff0c;不同的tag会有不同的颜色&#xff0c;不同level等级的log默认也有不同的颜色。log过滤修改的更简洁了&#xff0c;当然使用起来也更加复杂了。…

系统开发基础错题解析一【软考】

目录 前言1.开发模型1.1快速原型模型优点1.2敏捷统一模型1.3增量模型的优缺点1.4极限编程1.5螺旋模型 2.软件开发方法3.数据流图与数据字典3.1判定表3.2数据流图绘制3.3决策树 4.概要设计和详细设计5.内聚性6.耦合性 前言 本文专门用来记录本人在做软考中有关系统开发基础的错…

基于SpringBoot+Vue的宠物店管理系统

作者&#xff1a;计算机学姐 开发技术&#xff1a;SpringBoot、SSM、Vue、MySQL、JSP、ElementUI、Python、小程序等&#xff0c;“文末源码”。 专栏推荐&#xff1a;前后端分离项目源码、SpringBoot项目源码、Vue项目源码、SSM项目源码、微信小程序源码 精品专栏&#xff1a;…

0基础跟德姆(dom)一起学AI 机器学习03-线性回归

线性回归 线性回归介绍 【理解】举个栗子 假若有了身高和体重数据&#xff0c;来了播仔的身高&#xff0c;你能预测播仔体重吗? 这是一个回归问题&#xff0c;该如何求解呢? **思路**:先从已知身高X和体重Y中找规律&#xff0c;再预测 •数学问题&#xff1a;用一条线来拟…

【LeetCode】每日一题 2024_10_7 最低加油次数(堆、贪心)

前言 每天和你一起刷 LeetCode 每日一题~ 大家国庆节快乐呀~ LeetCode 启动&#xff01; 国庆最后一天&#xff0c;力扣还在加油站&#xff0c;怕不是国庆回家路上堵车了 题目&#xff1a;最低加油次数 代码与解题思路 func minRefuelStops(target int, startFuel int, st…

失业的程序员除了找工作,还有哪些赚钱的路子?零基础入门到精通,收藏这篇就够了_网络开发怎么赚钱

看到一个平台上的博主&#xff0c;目前在做独立开发者&#xff0c;开发了20多个网站&#xff0c;网站的类型主要是工具型和信息整理型&#xff0c;谷歌广告的收益一个月1万多。 目前他除了依靠谷歌广告的收入外&#xff0c;也在做自媒体&#xff0c;拓展这一块的收入&#xff…

41亿收购百年零部件巨头,「果链一哥」欲再造千亿规模新版图?

、 为了进一步拓展汽车业务版图&#xff0c;果链一哥立讯精密再次开启“买买买”模式。 日前&#xff0c;立讯精密发布公告称&#xff0c;计划以5.25亿欧元&#xff08;约41.3亿元人民币&#xff09;的价格收购Leoni AG&#xff08;以下简称“莱尼公司”&#xff09;50.1%股权…