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