IT数码 购物 网址 头条 软件 日历 阅读 图书馆
TxT小说阅读器
↓语音阅读,小说下载,古典文学↓
图片批量下载器
↓批量下载图片,美女图库↓
图片自动播放器
↓图片自动播放器↓
一键清除垃圾
↓轻轻一点,清除系统垃圾↓
开发: C++知识库 Java知识库 JavaScript Python PHP知识库 人工智能 区块链 大数据 移动开发 嵌入式 开发工具 数据结构与算法 开发测试 游戏开发 网络协议 系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑 笔记本 显卡 显示器 固态硬盘 硬盘 耳机 手机 iphone vivo oppo 小米 华为 单反 装机 图拉丁
 
   -> 数据结构与算法 -> 路径规划——RRT算法实现 -> 正文阅读

[数据结构与算法]路径规划——RRT算法实现

RRT——rapidly exploring random tree,即快速扩展随机树,是基于随机采样的路径规划算法,以起点为根节点,在搜索空间中随机产生节点,通过进行步长计算、碰撞检测等操作来产生新的子节点,直到采样至终点为止。伪代码如图所示:
RRT伪代码

图源: https://www.cnblogs.com/long5683/p/11795987.html

matlab代码实现:
Near函数产生新的随机采样点,同时还要计算树上与该点距离最近的节点,并返回其在树上的索引值。

function [index,new_point]=Near(T,x_range,y_range,StepSize)%产生新的节点,并返回树上距该点最近点的索引
rand_point=[x_range(2)*rand, y_range(2)*rand];%产生随机点
min_dist=sqrt((rand_point(1)-T.vertex(1).x)^2+(rand_point(2)-T.vertex(1).y)^2);
index=1;
for i=1:size(T.vertex,2)
    dist=sqrt((rand_point(1)-T.vertex(i).x)^2+(rand_point(2)-T.vertex(i).y)^2);
    if dist<min_dist
        min_dist=dist;
        index=i;
    end
end
if min_dist<=StepSize%在步长范围内直接使用该点
    new_point(1)=rand_point(1);
    new_point(2)=rand_point(2);
else
    theta = atan2(rand_point(1)-T.vertex(index).x,rand_point(2)-T.vertex(index).y);
    new_point(1)=T.vertex(index).x+StepSize*sin(theta);
    new_point(2)=T.vertex(index).y+StepSize*cos(theta);
end

check_Collision函数用于对新生成的节点进行碰撞检测,方法是在新生成的节点与树上最近点之间进行定步长采样,计算采样点是否与障碍物发生碰撞,因此采样步长需要足够小,防止误判。

function feasible=check_Collision(obs,new_point,x,y)
min_dist=sqrt((new_point(1)-x)^2+(new_point(2)-y)^2);
theta = atan2(new_point(1)-x,new_point(2)-y);
feasible=false;
for k=1:size(obs,2)
    for step=0:0.001:min_dist
        check_point.x=x+step*sin(theta);
        check_point.y=y+step*cos(theta);
        feasible=isinPolygon(check_point,obs(k));
        if feasible
            return;
        end
    end
end

判断指定点与凸多边形的关系可见以下文章,此处不再赘述:

https://blog.csdn.net/qq_36250209/article/details/123763849

function in_out=isinPolygon(Q,obs)%判断一个点是否在凸多边形之外——利用目标点与多边形各顶点构成的面积与多边形面积进行计算判断
%如果面积之和大于多边形面积,则在外部,否则在内部
%将多边形面积计算转化为各个三角形的面积计算,可以简化计算过程
%输入:目标点的坐标,凸多边形的顶点坐标  最大边数为五边形   输入的多边形顶点按逆时针或顺时针顺序输入
%输出:判断结果
S1=calc_triangle(Q.x,Q.y,obs.x(1),obs.y(1),obs.x(2),obs.y(2));
S2=calc_triangle(Q.x,Q.y,obs.x(2),obs.y(2),obs.x(3),obs.y(3));
S3=calc_triangle(Q.x,Q.y,obs.x(3),obs.y(3),obs.x(4),obs.y(4));
S4=calc_triangle(Q.x,Q.y,obs.x(4),obs.y(4),obs.x(5),obs.y(5));
S = calc_triangle(obs.x(1),obs.y(1),obs.x(2),obs.y(2),obs.x(3),obs.y(3))+ ...
    calc_triangle(obs.x(1),obs.y(1),obs.x(3),obs.y(3),obs.x(4),obs.y(4));
if S==S1+S2+S3+S4
    in_out=true;
else
    in_out=false;
end
end
function S=calc_triangle(x0,y0,x1,y1,x2,y2)%计算三角形面积——利用三角形各点与横坐标垂直连线构成的梯形面积相加减计算三角形面积
    S=0.5*abs(x0*y1+x1*y2+x2*y0-x0*y2-x1*y0-x2*y1);
end

运行结果如下图所示:
运行结果
终点处的圆表示的是终止搜索范围,当采样点进入该范围内,即可直接连接至终点,不过,程序中并未对此步骤进行碰撞检测,可以根据自己的需要进行修改完善。

  数据结构与算法 最新文章
【力扣106】 从中序与后续遍历序列构造二叉
leetcode 322 零钱兑换
哈希的应用:海量数据处理
动态规划|最短Hamilton路径
华为机试_HJ41 称砝码【中等】【menset】【
【C与数据结构】——寒假提高每日练习Day1
基础算法——堆排序
2023王道数据结构线性表--单链表课后习题部
LeetCode 之 反转链表的一部分
【题解】lintcode必刷50题<有效的括号序列
上一篇文章      下一篇文章      查看所有文章
加:2022-04-01 00:19:45  更:2022-04-01 00:23:31 
 
开发: C++知识库 Java知识库 JavaScript Python PHP知识库 人工智能 区块链 大数据 移动开发 嵌入式 开发工具 数据结构与算法 开发测试 游戏开发 网络协议 系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑 笔记本 显卡 显示器 固态硬盘 硬盘 耳机 手机 iphone vivo oppo 小米 华为 单反 装机 图拉丁

360图书馆 购物 三丰科技 阅读网 日历 万年历 2024年11日历 -2024/11/26 10:04:58-

图片自动播放器
↓图片自动播放器↓
TxT小说阅读器
↓语音阅读,小说下载,古典文学↓
一键清除垃圾
↓轻轻一点,清除系统垃圾↓
图片批量下载器
↓批量下载图片,美女图库↓
  网站联系: qq:121756557 email:121756557@qq.com  IT数码