【运动规划】C# 2D机器人运动规划仿真器-基于采样的运动规划算法-RRT、RRTConnect...-CSDN博客

本文链接：https://blog.csdn.net/cxyhjl/article/details/127456457

主界面

2D机器人运动规划仿真-视频演示

运动规划算法

RRT算法：

/*
Algorithm BuildRRT
  Input: Initial configuration qinit, number of vertices in RRT   K, incremental distance Δq)
  Output: RRT graph G
 
  G.init(qinit)❣
  for k = 1 to K❣
    qrand ← RAND_CONF()❣
    qnear ← NEAREST_VERTEX(qrand, G)❣
    qnew ← NEW_CONF(qnear, qrand, Δq)❣
    G.add_vertex(qnew)❣
    G.add_edge(qnear, qnew)❣
  return G❣
 * ❖❖❖❖❖❖❖❖❖❖❖❖❖❖❖❖
*/
        //求解
        public virtual bool solve()
        {
            DateTime timerStart = DateTime.Now;
            start_ = new Node();//当前开始节点
            start_.State_ = start;//开始节点状态
            start_.Parent = null;//开始节点的父节点null
            allNodes.AddFirst(start_);//起点添加到链表


            if (robotDot)//点机器人
            {
                RandomRobot = new RobotDot();
                robot = new RobotDot();
            }
            else//棍子机器人
            {
                RandomRobot = new RobotStick();
                robot = new RobotStick();
                robot.RobotHieght = robot_height;
                RandomRobot.RobotHieght = robot_height;
            }
            while (!solutionFound && maxIterations > allNodes.Count)//未找到解，且最大迭代次数大于节点数量
            {
                bias = rand2.Next(1, 100);//随机偏置


                if (bias >= biasmin)//大于等于2
                {
                    RandomRobot.robotBody = robot.GenerateRandomState(Widthofform,Heightofform);//生成随机机器人状态
                }
                else
                {
                    BiasNodesCounts++;
                    RandomRobot.robotBody = goal;//设置机器人状态为目标状态
                }


                //#找到随机位置的近邻节点, 从最近邻节点向随机位置节点步进k距离，到位置statenew。
                Node nearestneighbor = nn.nearest(allNodes, RandomRobot.robotBody);//找到最近邻节点
                Node node = new Node();
                float[] x0 = nearestneighbor.State_.Dimensions;
                float[] statenew = new float[RandomRobot.robotBody.Dimensions.Length];//statenew初始化
                float Length = 0;


                calculate_inpterpolation(ref statenew,ref Length, nearestneighbor, RandomRobot.robotBody, x0);//计算步进后的statenew
                robot.robotBody.Dimensions = statenew;//设置机器人新的步进k后的位置


                #region 校验步进的机器人位置是否合法
                if (ws.is_valid((int)statenew[0], (int)statenew[1]) != -1 || Length < k)  // 如果新节点与障碍物相交，则创建一个新的随机点。if the new node itersectcs with an obstacle, make a new random point.
                {
                    NumberOfRejections++;//拒绝次数
                    continue;
                }
                if (!robotDot)//棍子机器人
                {
                    State toppoint = robot.GetTopPoint();//头部点状态
                    State BotPoint =  robot.GetBotPoint();//底部点状态
                    if (ws.is_valid((int)toppoint.Dimensions[0], (int)toppoint.Dimensions[1]) != -1 )//头部与障碍物相交，拒绝
                    {
                         NumberOfRejections++;
                        continue;
                    }
                    if (ws.is_valid((int)BotPoint.Dimensions[0], (int)BotPoint.Dimensions[1]) != -1)//尾部与障碍物相交，拒绝
                    {
                        NumberOfRejections++;
                        continue;
                    }
                    
                }
                #endregion


                //经过合法检验后，构造步进后的位置节点，加入路径链表
                node = new Node(nearestneighbor, new State(robot.robotBody.Dimensions));//最近邻作为node的父节点，机器人位置作为节点位置
                allNodes.AddLast(node);//将该节点加入链表
 
                nn.addNode(node);//如果是线性近邻，没有操作。如果是KDTree近邻，插入节点。


                #region 校验是否找到解
                float distance_to_goal;


                distance_to_goal = calculate_distance(out distance_to_goal, node, goal);//计算节点到目标的距离
                //可能找到的解决方案将考虑目前已经找到。没有检查状态和目标之间的障碍。
                if (distance_to_goal <= k) // 
                {
                    Node finalnode = new Node(node, goal);//将该节点作为父节点，目标点位置作为 finalnode的位置。
                    allNodes.AddLast(finalnode);//终点加入链表
                    solutionFound = true;//找到解，结束求解
                }
                #endregion
            }
            
            NearestNeighborComputations = nn.NumberOfComputations;//近邻计算次数
            DateTime timerStop = DateTime.Now;
            timer = (timerStop - timerStart).TotalMilliseconds;//算法时间
            return solutionFound;


        }

RRTConnect算法：

public override bool solve()
        {
            DateTime timerStart = DateTime.Now;
            start_ = new Node();
            start_.State_ = start;
            start_.Parent = null;
            allNodes.AddFirst(start_);//起点加入链表
            if (robotDot)
            {
                RandomRobot = new RobotDot();
                robot = new RobotDot();
            }
            else
            {
                robot = new RobotStick();
                RandomRobot = new RobotStick();
            }
            while (!solutionFound)
            {
                bias = rand2.Next(1, 100);


                if (bias >= biasmin)
                    RandomRobot.robotBody = robot.GenerateRandomState(Widthofform,Heightofform);//随机节点状态
                else
                {
                    BiasNodesCounts++;
                    RandomRobot.robotBody = goal;//随机点为目标
                }
                Node nearestneighbor = nn.nearest(allNodes, RandomRobot.robotBody);//从链表中找到近邻
                while (true)
                {
                    Node node = new Node();
                    float[] x0 = nearestneighbor.State_.Dimensions;
                    float[] statenew = new float[RandomRobot.robotBody.Dimensions.Length];
                    float Length = 0;
                    //机器人向随机状态点步进k距离，抵达statenew坐标。
                    calculate_inpterpolation(ref statenew, ref Length, nearestneighbor, RandomRobot.robotBody, x0);
                    robot.robotBody.Dimensions = statenew;
                    //校验步进后位置的合法性
                    if (ws.is_valid((int)statenew[0], (int)statenew[1]) != -1 || Length < k)  // 如果新节点与障碍物相交，则创建一个新的随机点。
                    {
                        NumberOfRejections++;
                        break;
                    }
                    if (!robotDot)//棍子型机器人，校验合法性
                    {
                        State toppoint = robot.GetTopPoint();
                        State BotPoint = robot.GetBotPoint();
                        if (ws.is_valid((int)toppoint.Dimensions[0], (int)toppoint.Dimensions[1]) != -1)
                        {
                            NumberOfRejections++;
                            break;
                        }
                        if (ws.is_valid((int)BotPoint.Dimensions[0], (int)BotPoint.Dimensions[1]) != -1)
                        {
                            NumberOfRejections++;
                            break;
                        }


                    }
                    node = new Node(nearestneighbor,new State(robot.robotBody.Dimensions));//构建步进后节点：近邻作为父节点，步进后位置作为坐标


                    float distance_to_randomPoint;
                    distance_to_randomPoint = calculate_distance(out distance_to_randomPoint, node, RandomRobot.robotBody);//计算步进后节点与随机位置距离
                    
                    allNodes.AddLast(node);//步进后节点加入链表
                    nn.addNode(node);//步进后节点加入近邻节点


                    #region //校验是否找到解：步进后位置节点是否与目标节点距离小于k。
                    float distance_to_goal;


                    distance_to_goal = calculate_distance(out distance_to_goal, node, goal);//计算步进后节点与目标节点距离
                    //校验是否找到解：步进后位置节点是否与目标节点距离小于k。
                    if (distance_to_goal <= k) // 
                    {
                        Node finalnode = new Node(node, goal);
                        allNodes.AddLast(finalnode);
                        solutionFound = true;//找到解
                    }
                    #endregion


                    if (distance_to_randomPoint <= k)//计算步进后节点与随机位置距离小于k，结束搜索。
                        break;
                    nearestneighbor = node;//更新近邻为步进后的节点
                }
            }
            DateTime timerStop = DateTime.Now;
            timer = (timerStop - timerStart).TotalMilliseconds;
            NearestNeighborComputations = nn.NumberofComputaions;


            return solutionFound;
        }

参考：

https://en.wikipedia.org/wiki/Rapidly-exploring_random_tree

https://wenku.baidu.com/view/8de40fafbdeb19e8b8f67c1cfad6195f312be80a.html 基于RRT的运动规划算法综述

https://blog.csdn.net/weixin_43795921/article/details/88557317

运动规划RRT*算法图解

https://blog.csdn.net/gophae/article/details/103231053

全局路径规划：图搜索算法介绍4(RRT/RRT*)

The End