【运动规划】C# 2D机器人运动规划仿真器-基于采样的运动规划算法-RRT、RRTConnect...
主界面2D机器人运动规划仿真-视频演示运动规划算法RRT算法：/*Algorithm BuildRRTInput: Initial configuration qinit, number of vertices in RRTK, incremental distance Δq)Output: RRT graph GG.init(qinit)❣for k = 1...
·
主界面
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
更多推荐
0
0- 0
已为社区贡献6条内容
所有评论(0)