程序框架

视频演示

实时RRT-star算法介绍

实时RRT-star算法是一种基于采样的运动规划算法，它可以在有限的时间内找到一条渐进最优的路径。实时RRT-star算法是在RRT-star算法的基础上进行了改进，主要有两个方面：

- 实时更新起始点。实时RRT-star算法不是从固定的起始点开始搜索，而是每次迭代都将当前的位置作为起始点，这样可以适应动态的环境和目标。

- 实时调整采样区域。实时RRT-star算法不是在整个搜索空间中随机采样，而是根据当前的路径长度和目标位置，计算一个椭圆形的采样区域，只在该区域内进行采样，这样可以提高采样的效率和质量。

实时RRT-star算法的流程如下：

1. 初始化：将当前位置作为起始点，将目标位置作为终点，将起始点加入搜索树中，设置最大迭代次数和时间限制。

2. 迭代：重复以下步骤，直到达到最大迭代次数或时间限制。

    - 采样：根据当前的路径长度和目标位置，计算一个椭圆形的采样区域，从该区域内随机选择一个点作为采样点。

    - 扩展：在搜索树中找到离采样点最近的节点作为基准节点，从基准节点出发以一定步长朝着采样点进行延伸，延伸线的终点所在的位置被当做有效节点加入搜索树中。

    - 重布线：在搜索树中找到有效节点附近的一定范围内的所有节点，对每个节点检查是否可以通过有效节点改善其路径长度，如果可以，则将有效节点作为其父节点，并更新其路径长度。

    - 检查：检查是否存在从起始点到终点的可行路径，如果存在，则更新当前的路径长度和采样区域。

3. 输出：输出当前找到的最优路径或者提示无法找到可行路径。

实时RRT-star算法的优点是：

- 可以在有限的时间内找到一条渐进最优的路径，适用于时间敏感的任务。

- 可以实时更新起始点和采样区域，适应动态的环境和目标。

- 可以利用先验知识指导采样过程，提高采样的效率和质量。

实时RRT-star算法的缺点是：

- 需要计算椭圆形的采样区域，增加了计算量和复杂度。

- 需要存储搜索树中的所有节点和路径长度，增加了空间开销。

- 不能保证找到全局最优的路径，只能保证渐进最优。

实时RRT-star算法是一种有前途的运动规划算法，它结合了RRT-star算法和informed RRT-star算法的优点，同时进行了一些创新性的改进。实时RRT-star算法在多种场景下都有很好的表现，例如无人驾驶、机器人导航、游戏角色控制等。实时RRT-star算法还有很多可以改进和扩展的地方，例如采样区域的形状、采样点的分布、重布线的策略等，值得进一步的研究和探索。

ofApp主程序：

#include "ofApp.h"//--------------------------------------------------------------
void ofApp::setup() {
#ifdef randomSeedofSeedRandom(randomSeed);//如果定义了 randomSeed，则使用它来设置随机数种子，以便在每次运行时获得可重复的随机结果。
#endif // randomSeed
#ifdef CLKauto start = std::chrono::steady_clock::now();
#endif // DEBUGofSetVerticalSync(true);//启用垂直同步，以匹配显示器的刷新率。ofSetFrameRate(30);//设置帧率为30帧/秒。ofSetWindowTitle("Dynamic-obstacles");//设置窗口标题为 "Dynamic-obstacles"。ofBackground(200,200,200,200);//设置窗口背景颜色为浅灰色。myfont.loadFont("Roboto-Regular.ttf", 10);//加载字体文件 "Roboto-Regular.ttf" 并设置字体大小为10//map = new Enviroment();//car.setup();//接下来的几段代码是创建迷宫墙壁的示例ofVec2f w;w.set(ofGetWidth() / 2, 0);wall = new maze(w);//通过创建 maze 类的对象并将其添加到 obst 向量中，来创建障碍物。obstacles *ob = wall;obst.push_back(ob);w.set(ofGetWidth() / 2, 0.6*ofGetHeight());wall = new maze(w);ob = wall;obst.push_back(ob);w.set(ofGetWidth() / 4, 0.4*ofGetHeight());wall = new maze(w, 60, 0.2*ofGetHeight());ob = wall;obst.push_back(ob);w.set(0.75*ofGetWidth(), 0.4*ofGetHeight());wall = new maze(w, 60, 0.2*ofGetHeight());ob = wall;obst.push_back(ob);//使用一个循环创建了一些障碍物对象，并将它们添加到 obst 向量中for (unsigned int i = 0; i < numberOfobst; i++){//obstacles *ob = new obstacles(); //固定障碍物OBST = new movingObst();//移动障碍物obstacles *ob = OBST;//移动障碍物obst.push_back(ob);}//创建了一个 movingObst 对象，并将其添加到 obst 向量中OBST = new movingObst();//移动的对象ob = OBST;obst.push_back(ob);cout << "Obst size: " << obst.size() << endl; //输出 obst 的大小#ifdef randomSeed //根据是否定义了 randomSeed 输出随机种子的值std::cout << "RandomSeed:" << randomSeed << endl;
#endif#ifdef CLKauto end = std::chrono::steady_clock::now();std::cout << std::endl << "Setup:" << std::chrono::duration<double, std::milli>(end - start).count() << " ms" << std::endl;
#endif // DEBUG
}//一些更新操作--------------------------------------------------------------
void ofApp::update(){if (!updateFlag) return;//如果 updateFlag 为假，则函数直接返回
#ifdef CLKauto start = std::chrono::steady_clock::now();
#endif // DEBUG#ifdef automatic //如果定义了 automatic，则遍历 obst 向量中的每个障碍物对象，并调用其 move() 函数，将 obst 向量作为参数传递。for (auto i : obst) {i->move(obst);//cout << "location: " << i->loc() << "Radius: " << i->rad() << endl;//cout << i.getX() << "  " << i.getY() << endl;}
#endif // automatic//如果 map 不为空指针，则调用 map 对象的 update() 函数，并将 car 和 obst 作为参数传递。if (map!= NULL) map->update(car,obst);
#ifdef CLKauto end = std::chrono::steady_clock::now();/*std::cout << std::endl << "Update:" << std::chrono::duration<double, std::milli>(end - start).count() << " ms" << std::endl;*/updateTime = std::chrono::duration<double, std::milli>(end - start).count();//计算更新时间并存储在 updateTime 变量中
#endif // DEBUG
}//绘制操作--------------------------------------------------------------
void ofApp::draw(){
#ifdef CLKauto start = std::chrono::steady_clock::now();
#endif // DEBUG//遍历 obst 向量中的每个障碍物对象，并调用其 render() 函数进行绘制for (auto i : obst) {i->render();}if (map != NULL) map->render();//如果 map 不为空指针，则调用 map 对象的 render() 函数进行绘制。if (car!= NULL) car->render();//如果 car 不为空指针，则调用 car 对象的 render() 函数进行绘制。//创建一个用于存储帧率字符串的字符数组 fpsStr，并将帧率信息存储在其中。然后使用 myfont 对象绘制该字符串在窗口的指定位置。char fpsStr[255]; // an array of charsofSetColor({ 255,0,0 });sprintf(fpsStr, "Frame rate: %d", int(ofGetFrameRate()));myfont.drawString(fpsStr, ofGetWindowWidth() - 100, ofGetWindowHeight() - 25);//如果 map 不为空指针，则创建一个用于存储节点数字符串的字符数组 numNode，并将节点数信息存储在其中。然后 字符串在窗口的指定位置使用 myfont 对象绘制该。if (map != NULL) {char numNode[255];sprintf(numNode, "Number of nodes: %d", int(map->numofnode()));myfont.drawString(numNode, ofGetWindowWidth() - 140, ofGetWindowHeight() - 10);}#ifdef CLK //如果定义了 CLK，则计算绘制时间并存储在 drawTime 变量中，然后绘制更新时间和绘制时间的字符串。auto end = std::chrono::steady_clock::now();/*std::cout << std::endl << "Draw:" << std::chrono::duration<double, std::milli>(end - start).count() << " ms" << std::endl;*/drawTime = std::chrono::duration<double, std::milli>(end - start).count();char time[255];sprintf(time, "Update rate: %f", updateTime);myfont.drawString(time, ofGetWindowWidth() - 140, ofGetWindowHeight() - 755);sprintf(time, "Draw rate: %f", drawTime);myfont.drawString(time, ofGetWindowWidth() - 140, ofGetWindowHeight() - 740);#endif // DEBUG
}//根据按键的值执行相应的操作--------------------------------------------------------------
void ofApp::keyPressed(int key){if (key == 'p'){//如果按下的键是 'p'，则切换 updateFlag 变量的值updateFlag = !updateFlag;}else if(key=='g'){//如果按下的键是 'g'，则切换 map 对象的 grid 变量的值。map->grid = !map->grid;}else if (key == 'x') { //如果按下的键是 'x'，则截取屏幕的图像并保存为 "screenshot.png"。ofImage img;img.grabScreen(0, 0, ofGetWidth(), ofGetHeight());img.save("screenshot.png");}
#ifdef manual //如果定义了 manual，则调用 OBST 对象的 move() 函数，并将按下的键作为参数传递。OBST->move(key);
#endif // manual}//鼠标按下的位置和按钮执行相应的操作--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){ofVec2f loc;//创建一个 ofVec2f 对象 loc，loc.set(x, y);//并将鼠标按下的位置设置为其坐标。if (button == 0) {if (car != NULL) {//如果按下的按钮是鼠标左键（按钮值为0），并且 car 不为空指针，//则调用 map 对象的 targetSet() 函数，将 loc 作为参数传递。map->targetSet(loc);//设定目标位置}}else if (button == 2) {//如果按下的按钮是鼠标右键（按钮值为2），则创建一个 Robot 对象和一个 Enviroment 对象，并将 loc 作为参数传递给它们。car = new Robot(loc);map = new Enviroment(car->getLocation());//重建环境}else{}
}

The End