传统RRT在三维导向或者复杂环境中会出现迭代终止仍然抵达不了目标点，可将随机导向改为概率偏向目标点，代码在VS2019中编写，集成了QT5.14.2和VTK8.2.0，不会QT和VTK的建议学一学再来看，么么哒，代码如下：
1.坐标系函数
void myRRT::CartesianCoordinateSystem()
{
// 创建坐标轴演员
vtkSmartPointer<vtkAxesActor> axesActor = vtkSmartPointer<vtkAxesActor>::New();
axesActor->SetPosition(0, 0, 0);
axesActor->SetTotalLength(1000, 1000, 1000);
axesActor->SetScale(0.1, 0.1, 0.1);
axesActor->SetXAxisLabelText(" ");
axesActor->SetYAxisLabelText(" ");
axesActor->SetZAxisLabelText(" ");
// 将坐标轴演员添加到渲染器中
vtkSmartPointer<vtkCamera> Camera = vtkSmartPointer<vtkCamera>::New();
Camera->SetPosition(1000,-600,1000);
Camera->SetFocalPoint(0, 200, 0);
Camera->SetViewUp(0, 0, 1);
renderer->AddActor(axesActor);
renderer->SetActiveCamera(Camera);
renderWindow->Render();
}2. 立方体障碍物函数
void myRRT::DrawCube(double CubePosition[3], double CubeSize[3])
{
vtkSmartPointer<vtkCubeSource> CubeSource = vtkSmartPointer<vtkCubeSource>::New();
CubeSource->SetXLength(CubeSize[0]);
CubeSource->SetYLength(CubeSize[1]);
CubeSource->SetZLength(CubeSize[2]);
CubeSource->Update();
vtkSmartPointer<vtkPolyDataMapper> mapper_cube = vtkSmartPointer<vtkPolyDataMapper>::New();
vtkSmartPointer<vtkActor> actor_cube = vtkSmartPointer<vtkActor>::New();
mapper_cube->SetInputConnection(CubeSource->GetOutputPort());
actor_cube->SetMapper(mapper_cube);
actor_cube->GetProperty()->SetColor(0.5, 0.6, 0.6);
actor_cube->SetPosition(CubePosition);
renderer->AddActor(actor_cube);
renderWindow->Render();
}3.球的绘制与立方体同理，使用的库为vtkSphereSource，一方面是生成障碍物，另一方面是使节点清晰可见
void myRRT::DrawSphere(double Center[3], double Randius)
{
vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New(); //创建球对象
sphereSource->SetCenter(Center); //设置球心位置
sphereSource->SetRadius(Randius); //设置球半径
sphereSource->Update();
vtkSmartPointer<vtkActor> actor_sphere = vtkSmartPointer<vtkActor>::New();
vtkSmartPointer<vtkPolyDataMapper> mapper_sphere = vtkSmartPointer<vtkPolyDataMapper>::New();
mapper_sphere->SetInputConnection(sphereSource->GetOutputPort()); //将球对象与映射器链接
actor_sphere->SetMapper(mapper_sphere); //映射器与演员绑定
actor_sphere->GetProperty()->SetColor(1, 1, 1); //设置球的颜色
renderer->AddActor(actor_sphere);
renderWindow->Render();
}4.绘制直线：直线的绘制是为了连接RRT算法在迭代时生成的节点
void myRRT::DrawLine(double StartPoint[3], double TargetPoint[3])
{
vtkSmartPointer<vtkLineSource> LineSource = vtkSmartPointer<vtkLineSource>::New();
vtkSmartPointer<vtkActor> actor_line = vtkSmartPointer<vtkActor>::New();
vtkSmartPointer<vtkPolyDataMapper> mapper_line = vtkSmartPointer<vtkPolyDataMapper>::New();
LineSource->SetPoint1(StartPoint);
LineSource->SetPoint2(TargetPoint);
LineSource->Update();
mapper_line->SetInputConnection(LineSource->GetOutputPort());
actor_line->SetMapper(mapper_line);
actor_line->GetProperty()->SetColor(1, 0, 0);
renderer->AddActor(actor_line);
renderWindow->Render();
}5.随机障碍物生成，其中ObstacleNum为障碍物数量，ObstacleInfomation为存储障碍物信息的数组，随机概率生成球或立方体障碍物，且位置随机，大小随机，数量取随机概率，大于0.5为球，小于0.5为立方体
void myRRT::RandomObstacle(int ObstacleNum)
{
double ObstaclePosition[3]; double ObstacleSize[3];
// 使用当前时间作为随机数生成器的种子
std::srand(std::time(0));
//随机树生成
std::uniform_real_distribution<double> selectCubeOrSphere(0.0, 1.0);
std::default_random_engine select1(std::rand());
std::uniform_real_distribution<double> selectObstaclePosition(100, 400);
std::default_random_engine select3(std::rand());
std::uniform_real_distribution<double> selectObstacleSize(30, 50);
std::default_random_engine select4(std::rand());
for (int i = 0; i < ObstacleNum; i++)
{
if (selectCubeOrSphere(select1) < 0.5)
{
//方形障碍物
ObstaclePosition[0] = selectObstaclePosition(select3); ObstaclePosition[1] = selectObstaclePosition(select3); ObstaclePosition[2] = selectObstaclePosition(select3);
ObstacleSize[0] = selectObstacleSize(select4); ObstacleSize[1] = selectObstacleSize(select4); ObstacleSize[2] = selectObstacleSize(select4);
DrawCube(ObstaclePosition, ObstacleSize);
ObstacleInformation[i][0] = ObstaclePosition[0]; ObstacleInformation[i][1] = ObstaclePosition[1]; ObstacleInformation[i][2] = ObstaclePosition[2];
ObstacleInformation[i][3] = ObstacleSize[0]; ObstacleInformation[i][4] = ObstacleSize[1]; ObstacleInformation[i][5] = ObstacleSize[2];
ObstacleInformation[i][6] = 1;
}
else
{
//球形障碍物
ObstaclePosition[0] = selectObstaclePosition(select3); ObstaclePosition[1] = selectObstaclePosition(select3); ObstaclePosition[2] = selectObstaclePosition(select3);
ObstacleSize[0] = 0; ObstacleSize[1] = selectObstacleSize(select4); ObstacleSize[2] = 0;
DrawSphere(ObstaclePosition, ObstacleSize[1]);
ObstacleInformation[i][0] = ObstaclePosition[0]; ObstacleInformation[i][1] = ObstaclePosition[1]; ObstacleInformation[i][2] = ObstaclePosition[2];
ObstacleInformation[i][3] = ObstacleSize[0]; ObstacleInformation[i][4] = ObstacleSize[1]; ObstacleInformation[i][5] = ObstacleSize[2];
ObstacleInformation[i][6] = 0;
}
}
}6.碰撞检测函数（注：这里的碰撞检测函数有点堆屎山代码的味道了），将RRT中的最近节点和新节点分为30段，每一段进行碰撞检测。立方体碰撞检测原理是点和不在立方体内部就行，遍历所有立方体，距离超过一定范围直接略过，球就简单了，直接判断点到球心的距离和半径进行比较就行了。（注：上一步随机障碍物生成时存储的障碍物信息的数组在这里就有用了）
bool myRRT::isCollisionChecking(double NearestNode[3], double NewNode[3])
{
int LineDiv = 30;//将两个点的间距分割为30份
double MiddleCheckNode[3];//中间点存储
for (int j = 0; j < ObstacleNumber; j++)
{
if (ObstacleInformation[j][6] == 1)
{
double CubeAllowDistance = sqrt((ObstacleInformation[j][3] / 2) * (ObstacleInformation[j][3] / 2) + (ObstacleInformation[j][4] / 2) * (ObstacleInformation[j][4] / 2) + (ObstacleInformation[j][5] / 2) * (ObstacleInformation[j][5] / 2));
double NodeToCube = sqrt((NearestNode[0] - ObstacleInformation[j][0]) * (NearestNode[0] - ObstacleInformation[j][0]) + (NearestNode[1] - ObstacleInformation[j][1]) * (NearestNode[1] - ObstacleInformation[j][1]) + (NearestNode[2] - ObstacleInformation[j][2]) * (NearestNode[2] - ObstacleInformation[j][2]));
if (NodeToCube > (2*CubeAllowDistance))
{
continue;
}
else
{
for (int i = 1; i <= LineDiv; i++)
{
MiddleCheckNode[0] = NearestNode[0] + (NewNode[0] - NearestNode[0]) * i / LineDiv;
MiddleCheckNode[1] = NearestNode[1] + (NewNode[1] - NearestNode[1]) * i / LineDiv;
MiddleCheckNode[2] = NearestNode[2] + (NewNode[2] - NearestNode[2]) * i / LineDiv;
//判断该点是否在障碍物内
if (MiddleCheckNode[0] < (double(ObstacleInformation[j][0]) - double(ObstacleInformation[j][3]) / 2) || MiddleCheckNode[0] > (double(ObstacleInformation[j][0]) + double(ObstacleInformation[j][3] / 2)) || MiddleCheckNode[1] < (double(ObstacleInformation[j][1]) - double(ObstacleInformation[j][4] / 2)) || MiddleCheckNode[1] > (double(ObstacleInformation[j][1]) + double(ObstacleInformation[j][4] / 2)) || MiddleCheckNode[2] < (double(ObstacleInformation[j][2]) - double(ObstacleInformation[j][5] / 2)) || MiddleCheckNode[2] > (double(ObstacleInformation[j][2]) + double(ObstacleInformation[j][5] / 2)));
else
{
return true;
}
}
}
}
else
{
double NodeToSphere = sqrt((NearestNode[0] - ObstacleInformation[j][0]) * (NearestNode[0] - ObstacleInformation[j][0]) + (NearestNode[1] - ObstacleInformation[j][1]) * (NearestNode[1] - ObstacleInformation[j][1]) + (NearestNode[2] - ObstacleInformation[j][2]) * (NearestNode[2] - ObstacleInformation[j][2]));
if (NodeToSphere >= (2 * double(ObstacleInformation[j][4])))
{
continue;
}
else
{
for (int k = 1; k <= LineDiv; k++)
{
MiddleCheckNode[0] = NearestNode[0] + (NewNode[0] - NearestNode[0]) * k / LineDiv;
MiddleCheckNode[1] = NearestNode[1] + (NewNode[1] - NearestNode[1]) * k / LineDiv;
MiddleCheckNode[2] = NearestNode[2] + (NewNode[2] - NearestNode[2]) * k / LineDiv;
double MiddleNodeToSphere = sqrt((MiddleCheckNode[0] - ObstacleInformation[j][0]) * (MiddleCheckNode[0] - ObstacleInformation[j][0]) + (MiddleCheckNode[1] - ObstacleInformation[j][1]) * (MiddleCheckNode[1] - ObstacleInformation[j][1]) + (MiddleCheckNode[2] - ObstacleInformation[j][2]) * (MiddleCheckNode[2] - ObstacleInformation[j][2]));
if (MiddleNodeToSphere > ObstacleInformation[j][4]);
else
{
return true;
}
}
}
}
}
return false;
}7.RRT运行迭代主体，按照传统RRT步骤来的（不懂的可以去找找其他博客，这里不过多讲解），但是因为传统的随机性太强，所以加入了概率导向，随机概率小于0.6就生成随机点，大于则就以目标点为随机点。参数比如起始点，目标点啥的根据自己的要求调整就行
void myRRT::RRTStartButton()
{
RRT_tree[0][0] = x_start; RRT_tree[0][1] = y_start; RRT_tree[0][2] = z_start; //子节点
RRT_tree[0][3] = x_start; RRT_tree[0][4] = y_start; RRT_tree[0][5] = z_start; //父节点
RRT_tree[0][6] = 0; //父节点到该节点的距离，取欧氏距离
int count = 1; //节点计数
double MapXLength = 500; double MapYLength = 500; double MapZLength = 500; //设置地图的大小
double RandNode[3]; //随机节点
double NearestNode[3]; //最近的节点
double MinDistance = 2000; //寻找随机节点到所有树中节点的最小距离值
int index = 0; //寻找最近节点在RRT_tree中的位置
double distance; //随机节点到最近节点的距离
double NewNode[3]; //创建存储新节点位置信息的数组
double NewtoGoalDistance; //新节点到目标点的距离
for (int iter = 0; iter < 2000; iter++)
{
//RRT树的初始化
//开始搜索并构建树
//Step1:在地图上随机采样一个点
// 使用当前时间作为随机数生成器的种子
std::srand(std::time(0));
//为了提高算法导向性，从0-1随机生成一个数，小于0.5则生成随机点，大于0.5则以目标点为随机点
std::uniform_real_distribution<double> select(0.0, 1.0);
std::default_random_engine selectnum(std::rand());
// 创建一个均匀分布的随机数生成器，范围是0到1000
std::uniform_real_distribution<double> distribution(100.0, MapXLength);
std::default_random_engine generator(std::rand());
if (select(selectnum) <= 0.6)
{
RandNode[0] = distribution(generator);//随机点生成
RandNode[1] = distribution(generator);
RandNode[2] = distribution(generator);
}
else
{
RandNode[0] = x_goal;
RandNode[1] = y_goal;
RandNode[2] = z_goal;
}
//Step2:遍历树，从树中找到最近的节点
for (int i = 0; i < count; i++)
{
//计算随机节点到树上节点的距离
distance = (RandNode[0] - RRT_tree[i][0]) * (RandNode[0] - RRT_tree[i][0]) + (RandNode[1] - RRT_tree[i][1]) * (RandNode[1] - RRT_tree[i][1]) + (RandNode[2] - RRT_tree[i][2]) * (RandNode[2] - RRT_tree[i][2]);
distance = sqrt(distance);
if (i == 0) MinDistance = distance;
if (distance < MinDistance)
{
MinDistance = distance;
index = i; //找到最近的节点后，使用index记录该节点在树中的位置
}
}
NearestNode[0] = RRT_tree[index][0]; //将最近的节点的xyz值传给NearestNode
NearestNode[1] = RRT_tree[index][1];
NearestNode[2] = RRT_tree[index][2];
//Step3:扩展得到NewNode新节点
NewNode[0] = NearestNode[0] + (RandNode[0] - NearestNode[0]) * ExpansionStep / MinDistance;//根据扩展步长和已知的最近节点距离算出扩展的新节点的位置
NewNode[1] = NearestNode[1] + (RandNode[1] - NearestNode[1]) * ExpansionStep / MinDistance;
NewNode[2] = NearestNode[2] + (RandNode[2] - NearestNode[2]) * ExpansionStep / MinDistance;
if (isCollisionChecking(NearestNode, NewNode) == true)
{
std::this_thread::sleep_for(std::chrono::milliseconds(50)); //目前已知在此处加入一段延迟可以解决问题（单次循环调试正常运行，直接运行则会跳出整个迭代循环）
continue;
}
count++;
//Step 4:将NewNode插入树中
RRT_tree[count-1][0] = NewNode[0]; RRT_tree[count-1][1] = NewNode[1]; RRT_tree[count-1][2] = NewNode[2];
RRT_tree[count-1][3] = NearestNode[0]; RRT_tree[count-1][4] = NearestNode[1]; RRT_tree[count-1][5] = NearestNode[2];
RRT_tree[count-1][6] = MinDistance;
//Step 5:检查新节点是否到达目标点附近
NewtoGoalDistance = sqrt((x_goal - RRT_tree[count-1][0]) * (x_goal - RRT_tree[count-1][0]) + (y_goal - RRT_tree[count-1][1]) * (y_goal - RRT_tree[count-1][1]) + (z_goal - RRT_tree[count-1][2]) * (z_goal - RRT_tree[count-1][2])); //设置目标点阈值
if (NewtoGoalDistance <= TargetPointThreshold)
{
double TargetPoint[3] = { x_goal, y_goal, z_goal };
DrawSphere(NewNode, 3.0);
DrawLine(NewNode, TargetPoint);
DrawLine(NearestNode, NewNode);
break;
}
//Step 6:将NearestNode和NewNode连接
DrawSphere(NewNode, 3.0);
DrawLine(NearestNode, NewNode);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}8.代码运行结果如下：
9.注：由于算法太过传统，仅供学习用，如果VS集成QT和VTK不会，CSDN上面一搜一大把。
5137
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
