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附近。