RRT路径规划算法详细C#

RRT是通过随机节点进行路径规划的（寻找的路径不是最优路径）

我们先设置障碍物（黑色），起点（绿色点），终点（红色点）

因为是通过节点进行寻找路径，先设置下节点的属性

父节点pre，节点位置data，两个构造函数

class Node{public Node pre;public Point data;public Node() { }public Node(Node pre, Point data){this.pre = pre;this.data = data;}}

在图中通过随机数产生一个节点P（randomX，randomY），我们假设随机节点为P（图中的蓝色点）

然后寻找距离节点P点最近的节点nearest，图中P的nerest就是起点绿点

寻找最近点判断的代码：

double shortLength = Math.Sqrt((nearest.data.X - randomNode.X) * (nearest.data.X - randomNode.X) + (nearest.data.Y - randomNode.Y) * (nearest.data.Y - randomNode.Y));double tempLenght;for (int i = 1; i < allNode.Count; i++){tempLenght = Math.Sqrt((allNode[i].data.X - randomNode.X) * (allNode[i].data.X - randomNode.X) + (allNode[i].data.Y - randomNode.Y) * (allNode[i].data.Y - randomNode.Y));if (shortLength > tempLenght){nearest = allNode[i];shortLength = tempLenght;}}

从nearest点向随机点P方向进行直线延伸，延伸距离lenght设为30，距离nearest长度为30处的点为newNode，接下来把nearest和newNode进行连接（具体length的大小，依照最小障碍物的大小与地图的大小决定，lenght太小了，可能导致寻路时间过长）

在进行P和newNode相互连接前，要判断两点之间是否有障碍物。怎么进行相连时障碍物的判断？我这里是每相隔距离5进行判断下，30的长度，所以一共判断6下。如果两点间有障碍物，放弃相连，否则则进行连接。如图四是放大的黄线，对线进行6点的均匀取样

如何知道是不是障碍物呢？我这里有个比较简单的思路。因为我的障碍物是黑色，除障碍物外其他物体不是黑色，所以没次进行取点判断时，如果发现改点为黑色，就判定为障碍物，放弃连接

判断障碍物代码：

 public Boolean opConnet(Point front, Point back)//以一个阀值的长度连接radomNode和nearest{int dtx = back.X - front.X, dty = back.Y - front.Y;double opLength;opLength = Math.Sqrt(dtx * dtx + dty * dty);int check = (int)(opLength / 5);for (int i = 1; i <= check; i++){Color color = map.GetPixel((front.X + (dtx * i) / check), (front.Y + (dty * i) / check));int colorNum = color.R + color.G + color.B;if (colorNum<15/*map.GetPixel((front.X + (dtx * i) / check), (front.Y + (dty * i) / check)).Name == "ff000000"*/){return false;}}return true;}

如果可以连接，这把newNode保存到一个list中

然后判断终点是否在newNode附近，如果终点在newNode附近（附近在这里的定义也是距离小于30，具体的情况要有自己的情况而定），则把newNode和终点连接，否则继续生成随机点，重复上述步骤，直到终点在新的newNode附近。

RRT路径规划算法详细C#相关推荐

RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现
在上一节中,介绍了 RRT 算法的原理,这一节将一步步实现 RRT 路径规划算法在二维环境中的路径规划,来进一步加深对 RRT 算法的理解. 二维环境的搭建我们将搭建下图所示的二维环境,绿色点为起点 ...
RRT路径规划算法（matlab实现）
基于快速扩展随机树(RRT / rapidly exploring random tree)的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的 ...
自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
python无人机路径规划算法_快速拓展随机树（RRT)路径规划，python
1 """2 version1.1,2018-05-09 3 <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文4 <基于改进人工势场法的路径 ...
RRT、RRT-connect、RRT*等算法、A*等等路径规划算法
1 原始RRT算法运行结果:python,这里以2D_rrt及其衍生相关算法为例(边做边更新) CV搬运工们,先上github连接:(点个赞呗)(不想要拿github包的后面有现成代码)GitHub ...
A星融合DWA的路径规划算法，可实现静态避障碍及动态避障,代码注释详细,matlab源码
A星融合DWA的路径规划算法,可实现静态避障碍及动态避障,代码注释详细,matlab源码 ID:4525679980340317云的歌儿
【机器人路径规划算法RRT和RRG】
路径规划算法 RRT路径规划 RRG路径规划 RRT路径规划 RRT算法:Rapid-exploration Random Tree 快速搜索随机数算法,是一种在完全已知的环境中通过随机采样扩展搜索的 ...
从RRT到RRT*，再到Informed RRT*，路径规划算法怎么写
从RRT到RRT*,再到Informed RRT*,路径规划算法怎么写 1.RRT算法 1.1 假设 1.2 RRT算法步骤与实现 1.3 伪代码 2.RRT*算法 3.Informed RRT*算法 ...
Dijkstra、RRT两类路径规划算法原理的直观理解
在路径规划的算法里,有两大类算法是很常用的,一类是基于搜索和图的Dijkstra算法,还有一类是基于采样的RRT算法.本文对其算法原理进行简单的理解,力图生动的展示枯燥的数学公式背后精彩的思想. Di ...

RRT路径规划算法详细C#

RRT路径规划算法详细C#相关推荐

最新文章

热门文章