Webots——RRT算法

RRT路径规划的实验报告

仓库地址：github

实验目标

• 实现RRT路径规划算法，并让小车依照规划的路线进行行走，从起点到终点。

实验环境

• 开发环境：macOS Monterey 12.0.1

• 开发工具：Webots 2021b, OpenCV 4, clang-1300.0.29.3

实验内容

本实验内容主要是两个：

• 运用RRT算法规划出一条从起点到终点的最短路线

• 让小车依照此路线从起点走到终点

限于系统原因，OpenCV安装时所编译的动态链接库为Arm架构，而Webots暂未对MacOS的M1类型适配，其动态链接库均为x86_64架构。这将导致Webots在生成项目时，在链接动态链接库阶段出现找不到OpenCV相关成员变量的x86_64的动态库，而在手动用命令行编译时，亦出现找不到Webots相关成员变量的Arm的动态库。因此这里将这两个代码分开，即路径规划和巡线代码为不同文件，其中路径规划代码引用了OpenCV库，在命令行编译下能够正常编译成功运行，巡线代码在Webots下亦能正常编译成功运行。而理论上它们在Linux或Windows应能融洽在同一个代码中。

本次实验提供了一个已经制作好的迷宫的世界。

RRT路径规划

首先是进行RRT路径规划的实现。

本次实验的平面图如图下图所示。

右上角绿色圈圈为起点，左下角红色圈圈为终点。

通过OpenCV的cv::imread(s.c_str(), cv::IMREAD_COLOR);函数，以彩色方式读取该图像，输出其长宽发现是一张800\times 600的图像，将其输入的三通道像素数输出到文件qwq进行分析，可以得到起点的绿色圈圈的色素值为$\left( 76,76,76 \right)$，终点的红色圈圈的色素值为$\left( 36,36,36 \right)$，对全图遍历找到可以找到起点和终点像素位置并保存。

maze = cv::imread(s.c_str(), cv::IMREAD_COLOR);
cols = maze.cols;
rows = maze.rows;
for(int i = 0; i < rows; ++ i)
    for(int j = 0; j < cols; ++ j){
        int b = maze.at<cv::Vec3b>(i, j)[0];
        int g = maze.at<cv::Vec3b>(i, j)[0];
        int r = maze.at<cv::Vec3b>(i, j)[0];
        if (b == ST && sx == -1){
            sx = i;
            sy = j;
        }
        if (b == ED){
            ex = i;
            ey = j;
        }
        if (b != ST && b != ED && b != 255){ // 障碍物
            for(int x = -REC; x <= REC; ++ x)
                for(int y = -REC; y <= REC; ++ y)
                    obs.insert(two2one(i + x, j + y));
        }
    }

同时将一切像素值不是255的点都视为障碍物。虽然这不太精确，但是考虑到小车是有实际体积而非质点，而规划路径时力求最短路，会很大可能造成规划出来的路径贴墙，因此将非255的视为障碍物能够一定程度地将墙边缘的像素视为障碍物，让规划出来的路径尽可能不贴着墙。

这里相比上次实验代码加上了优化，在判障碍物时将障碍物方圆REC的切比雪夫距离的像素均视为障碍物，这虽然增加了些许图像处理时间，但对下面运用RRT算法求解路径时带来了$REC^2$倍的速度提升，意义重大。

这里将障碍物的二维坐标转换成一维坐标，并保存在set中。

inline int two2one(int x, int y){
    return x * cols + y;
}

OpenCV的运行结果如下图所示。其输出了起点和终点坐标。

RRT算法一共三个步骤：

• 图上随机采样点a

• 找到与该点距离最近的点b，让点b往点a方向步长距离step，得到新点c，建立有向边$b\to c$

• 重复以上操作，直到走到终点

值得注意的是，RRT算法生成出来的是一棵树。众所周知，树上两点路径唯一。

由于朴素的RRT算法存在诸多问题，比如收敛速度慢，因此加上了概率选择目标点作为采样点的方案；但也有难以求解最优的可行路的问题，因此这里实现的是RRT*算法。

RRT*算法相比朴素的RRT算法，多了两个步骤：重新选择父节点过程和重布线随机树过程，总的过程如下：

• 图上随机采样点a

• 找到与该点距离最近的点b，让点b往点a方向步长距离step，得到新点c

• 在点c方圆dis1距离找到点d，使得从$st\to d\to c$的距离最小（重新选择父节点），建立有向边$d\to c$（重新选择父节点，即不一定选点b作为父节点），其中st表示起点

• 在点c方圆dis2距离找到符合条件的点e，使得$st\to c\to e$距离小于原$st\to e$的距离，更改点e的父亲为点
  c$即从点c走到点e），删去原先的有向边$fa[e]\to e$，新建有向边$c\to e$，同时更新点e的子树的所有点的距离。其中fa[i]表示点i的父亲。（重布线随机数）

• 重复以上操作，直到走到终点

新增的两个步骤都是优化最短路。以下是一个简单的例子。

在上图里，这里通过采样后从右边的点拓展了一个点，此时在方圆距离寻找一个距离起点最近的点。

如蓝色线所示，事实上左边的点距离起点最近，因此重新选择父节点为左边的点。如上图所示。

重新选择父节点后，进行重新布线操作。同样以采样点方圆距离寻找是否有点通过该点能以更近的距离到达起点，如上图所示。

发现蓝色线回到起点跟红色线回到起点近，于是删除原先父边，建立新边，如上图所示。这就是重布线操作。当然如果其点还有后继点，需要沿路更新最短距离到叶子。

以下是代码讲解。

void findPath(){ 
    fa.resize(rows * cols);
    fill(fa.begin(), fa.end(), 0);
    // 距离起点的数据
    vector<double> distance(rows * cols, 1e9 + 7);
    // 记录从父亲走向儿子的有向边
    edge.resize(rows * cols);
    int st = two2one(sx, sy);
    distance[st] = 0;
    int ed = 0;
    // 已扩展点集
    pointSet.insert(st);

核心函数findPath，前面均为初始化。然后开始扩展。

while(true){
    // 随机采样点
    int samplePoint = randPoint();
    // 在点集中找到距离采样点最近的点
    int nearstPoint = getNearestPoint(samplePoint, pointSet);
    // 以采样点方向以stepDistance步长得到新点，valid表示该点是否合法，即该点是否位于障碍物或超出界，或已在扩展点集里，或沿途有障碍物
    auto [valid, newPoint] = goStep(nearstPoint, samplePoint);
    if (!valid)
        continue;
    // 重新选择父亲点，选一条从st -> fatherPoint -> newPoint最近的fatherPoint
    int fatherPoint = getBestFatherPoint(newPoint, pointSet, distance);
    // 指定父亲
    fa[newPoint] = fatherPoint;
    // 更新距离
    distance[newPoint] = distance[fatherPoint] + dis(newPoint, fatherPoint);
    // 建立新有向边
    edge[fatherPoint].insert(newPoint);
    // 扩展点集合
    pointSet.insert(newPoint);
    // 重布线操作
    rewrite(newPoint, pointSet, distance);
    // 判断是否在终点附近
    if (nearTargetPoint(newPoint)){
        ed = newPoint;
        break;
    }
}

步骤也如上述所说的，先采样点，然后从点集中找到距离最近的点，往采样点方向进行扩展，然后重新选择父节点和进行重布线操作。

随机采样点函数很简单。

// 随机生成点
int randPoint(){
    int x = rand() % rows;
    int y = rand() % cols;
    return two2one(x, y);
}

其中two2one函数是将点二维坐标转换成一维。

寻找距离最近的点的函数也很简单。

// 寻找pointSet中距离point最近的点
int getNearestPoint(int point, set<int> &pointSet){
    int target = -1;
    double targetDis = 1e9 + 7;
    for(auto p : pointSet){
        double distance = dis(point, p);
        if (distance < targetDis){
            targetDis = distance;
            target = p;
        }
    }
    return target;
}

步长扩展点函数稍有点复杂。

pair<bool, int> goStep(int curPoint, int dirPoint){
    double distance = dis(curPoint, dirPoint);
    auto [curX, curY] = one2two(curPoint);
    auto [dirX, dirY] = one2two(dirPoint);
    // 比例关系
    int nxtX = curX + 1.0 * (dirX - curX) * stepDistance / distance;
    int nxtY = curY + 1.0 * (dirY - curY) * stepDistance / distance;
    int nxtPoint = two2one(nxtX, nxtY);
    // 扩展点不在障碍物，未出界，连线无障碍，非点集点
    return {valid(nxtX, nxtY) && checkLine(nxtX, nxtY, curX, curY) && pointSet.find(nxtPoint) == pointSet.end(), nxtPoint};
}

主要是判断扩展的点是否合法，除了判断是否在障碍物，出界，以及连线无障碍之外，还要判断是否在点集里，在的话也视为非法，因为已经扩展的点就没必要在此扩展了，在早期版本中，pointSet还是vector类型，就发生了栈溢出的现象，主要是重复点加进来太多爆内存了。

valid函数如下。

inline bool valid(int x, int y){ // 判断当前点是否合法，即是否越界或者是否在障碍物边缘
    if (x < 0 || x >= rows)
        return false;
    if (y < 0 || y >= cols)
        return false;
    return obs.find(two2one(x, y)) == obs.end();
    // 大大的优化，速度提升了REC*REC倍
}

上个实验代码里对点判越界写错了，原先写的
是与关系，这里已修正为或关系。以及正如前文所说，原先这里需要$O\left( REC^2 \right)$
次判断来判断是否在障碍物附近，由于在处理图像时已经进行了预处理，故此处优化成只需$1$次了。实测运行速度大幅提升。

checkline函数如上次一致。

bool checkLine(int x1, int y1, int x2, int y2){ // 判断一条直线上是否有障碍物
    double k = 0.01;
    for(; k <= 1; k += 0.01)
        if (!valid(x1 + (x2 - x1) * k, y1 + (y2 - y1) * k))
            return false;
    return true;
}

重新选择父亲的函数如下。

// 重新选择父亲
int getBestFatherPoint(int point, set<int>& pointSet, vector<double>& pointDistance){
    int target = 0;
    double targetDis = 1e9 + 7;
    for(auto p : pointSet){ // 枚举点集的点
        double distance = dis(point, p);
        // 距离在方圆内，且从该点到起点距离更小
        if (distance < fatherDistance && checkLine(point, p) && pointDistance[p] + distance < targetDis){
            targetDis = pointDistance[p] + distance;
            target = p;
        }
    }
    return target;
}

这跟寻找最近点的距离很类似，但判断的条件不同。距离最近的就单纯地看距离最近，而最好的父亲节点是到起点最近的节点。

重布线函数如下。

void rewrite(int point, set<int>& pointSet, vector<double>& pointDistance){
    for(auto p : pointSet){ // 枚举点集的点
        double distance = dis(point, p);
        // 判断距离是否在方圆内，且之间连边无障碍物
        if (distance > rewriteDistance || !checkLine(point, p))
            continue;
        // 如果距离更优
        if (pointDistance[p] > pointDistance[point] + distance){
            // 更新距离
            pointDistance[p] = pointDistance[point] + distance;
            // 删除原先边
            edge[fa[p]].erase(p);
            fa[p] = point;
            // 建立新的边
            edge[point].insert(p);
            // 更新点p之后的点的最短距离
            update(p, pointDistance);
        }
    }
}
```   

从新扩展的点的方圆点中寻找可以更新距离的点，一旦可以更新就删去原有边，并新增边，且通过`update`函数更新沿途下去的所有点的最短距离。
```cpp
// 更新距离
void update(int point, vector<double>& pointDistance){
    for(auto nxtPoint : edge[point]){
        pointDistance[nxtPoint] = dis(point, nxtPoint) + pointDistance[point];
        update(nxtPoint, pointDistance);
    }
}

距离更新就通过edge储存的父亲到儿子到有向边进行递归更新。

// 计算到终点距离，判断是否在终点附近
bool nearTargetPoint(int point){
    return dis(point, two2one(ex, ey)) < targetDistance;
}

判断是否到达终点距离的函数就比较简单了。一旦到达终点附近，求解路径就算结束了。

for(int point = ed; point != st; point = fa[point])
    path.push_back(point);
path.push_back(st);
reverse(path.begin(), path.end());

最后根据fa数组关系记录最短路径即可。

以下是一些跑出来的效果图。

如第一次结果所示，尽管有众多优化，RRT算法能有可能会找到较远路径，尽管概率很小，此为稀有路径了。

这三张图是无重布线操作RRT。

这三张图是有重布线操作RRT。

从图中可以很好的看出，不带重布线操作的连边情况犹如一棵正常的树一样，且得到的路径稍有摆动。而带有重布线操作的连边情况像是放射性一般，且得到的路径非常平整。

小车巡线

此部分与上一次PRM实验报告相一致，本次实验未改动，直接沿用上次代码。在此就不重复赘述，具体详情参见PRM算法

实验结果

演示视频：bilibili

实验总结

通过本次实验，我了解到了RRT算法实现的具体流程，明白了RRT算法的重要意义：连续变离散化。由于世界是连续的，RRT算法通过对世界采样将其离散化，往采样点方向前进。这非常类似于人对现实环境的探索。而且通过对世界随机采样点，再以采样点的方向行走，这在一定程度上能保证探索到的地方尽可能平均尽可能多。因为比如当前位置在世界的最左边，通过采样点的方向前进，则有非常大的概率会往右边走，因为会往右边走的采样点远多于会往左边走的点，避免一头撞到墙然后不动了。但这其实也有点不太好的地方就是，如果终点在某个角落，则可能会难以走到终点，因为每次接近终点时，采样点方向远离终点的概率就很大，从而不往终点那边走。因此才有会以一定概率选择终点为采样点，加快收敛速度的方法。为了解决RRT探索出来的路径一般不会是最优路径以及无意义的路径摆动，又增加了重新选择父节点和重新布线的方法，而从实验结果可以看出重布线的方案能有效较少路径无意义地摆动的情况，让路径尽可能平坦。

本次实验未改动过控制器部分代码，仅仅修改路径规划的核心代码，控制器可以不加修改地直接复用，这也体现了人类抽象能力的伟大。