【openframework】实时路径规划（RTRRTstar算法）

程序框架

视频演示

实时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 randomSeed
  ofSeedRandom(randomSeed);//如果定义了 randomSeed，则使用它来设置随机数种子，以便在每次运行时获得可重复的随机结果。
#endif // randomSeed
#ifdef CLK
  auto start = std::chrono::steady_clock::now();
#endif // DEBUG
  ofSetVerticalSync(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 CLK
  auto 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 CLK
  auto 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 CLK
  auto 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 CLK
  auto 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 chars
  ofSetColor({ 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