Webots——PRM算法

PRM路径规划的实验报告

仓库地址：github

实验目标

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

实验环境

• 开发环境：macOS Monterey 12.0.1

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

实验内容

本实验内容主要是两个：

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

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

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

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

PRM路径规划

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

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

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

通过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)
            obs.insert(two2one(i, j));
    }

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

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

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

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

PRM算法一共三个步骤：

• 随即均匀撒点
• 相邻无障碍物点间连边
• 运用最短路算法求出一条最短路

随机均匀撒点

void randGenPoint(){
    for(int i = 0; i < rows; i += disX)
        for(int j = 0; j < cols; j += disY){
            int attempCnt = 25;
            while(attempCnt--){
                int x = rand() % disX;
                int y = rand() % disY;
                if (valid(x + i, j + y)){
                    point.push_back({x + i, y + j});
                    break;
                }
            }
        }
}

这里将图以disX,disY步长切分，在每块中尝试25次点的生成。其中valid函数如下。

inline bool valid(int x, int y){
    if (x < 0 && x >= rows)
        return false;
    if (y < 0 && y >= cols)
        return false;
    for(int i = -REC; i <= REC; ++ i)
        for(int j = -REC; j <= REC; ++ j)
            if (obs.find(two2one(x + i, y + j)) != obs.end())
                return false;
    return true;
}

该函数判断点是否合法，一是判断是否出界，而是判断以该点为中心的REC范围内是否有障碍物像素点存在。之所以这么处理也是因为上述提到的原因，即尽可能让规划出的路线不贴墙走，因为小车还有具体体积的缘故。

以下两张图分别展示了REC为0和15时随机生成点的情况。

相邻无障碍物点间连边

void connectNei(){
    edge.resize(rows * cols);
    for(auto [x, y] : point){
        for(auto [nx, ny] : point){
            if (nx == x && ny == y)
                continue;
            if (dis(x, y, nx, ny) < neiDis && checkLine(x, y, nx, ny)){
                edge[two2one(x, y)].push_back(two2one(nx, ny));
                edge[two2one(nx, ny)].push_back(two2one(x, y));
            }
        }
    }
}

思路比较简单，从生成出的点集里选出两个点，判断其连线是否存在障碍物像素以及距离是否小于预设值neiDis。并把合法连线保存到数组edge中。

其中判线是否有障碍物的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;
}

就运用直线的参数方程那样判断沿途的个个像素是否存在障碍物像素。

以下两张图分别展示了REC为0和15时连线的情况。

运用最短路算法求出一条最短路

得到了图的连边后，就是一个简单的最短路问题了。这里运用dijkstra算法进行求解。

vector<int> pre(cols * rows);
vector<bool> vis(cols * rows, false);
double oo = 1e9 + 7;
vector<double> distance(cols * rows, oo);
priority_queue<pair<double, int>> team;
team.push({0, two2one(sx, sy)});
distance[two2one(sx, sy)] = 0;
while(!team.empty()){
    auto [_, u] = team.top();
    team.pop();
    if (vis[u])
        continue;
    vis[u] = true;
    for(auto v : edge[u]){
        double w = dis(u, v);
        if (distance[v] > distance[u] + w){
            distance[v] = distance[u] + w;
            pre[v] = u;
            team.push({-distance[v], v});
        }
    }
}
if (distance[two2one(ex, ey)] == oo){
    printf("WARNING: CANNOT Find a path from start point to target point!\n");
    exit(0);
}
for(int u = two2one(ex, ey); u != two2one(sx, sy); u = pre[u])
    path.push_back(u);
path.push_back(two2one(sx, sy));
reverse(path.begin(), path.end());

dijkstra没什么特别的细节。用pre数组保存最短路中一个点的前继点。最后还判断是否存在一条最短路，存在的话保存一条最短路。因为保存的前继点，压入path数组是从终点到起点，因此最后还需要反转，reverse一下。

这里 auto [\_, u] = team.top() 是C++17标准的新特性，能够对
pair<int,int>进行拆包。

以下两张图展示了REC为0和15时连线的情况。

可以看出REC=0时找出来的最短路有好几处是贴着墙走的，这在实际小车寻路时极易与墙发生碰撞，而REC=15则于道路中间。

下图为一些分割和邻居阈值数据对应的结果。

小车巡线

运用PRM算法规划出一条路径出来后，便考虑如何进行移动。

一种简单，能够复用上次代码的实现方式是将路径规划到图上，即将规划的路线画在图上，然后通过摄像头巡线的方式行走。但考虑到这样子会有诸多问题，比如行走速度慢，一旦偏离路线就无法回来等。因此这里采用另一种通过GPS规划小车行走的方式。因为已经事先知晓全局路线，包括直线和转弯路径，因此可以动态调整不同速度。

Pure Pursuit

运动规划的算法很多，比较简单的是一个叫Pure Pursuit算法，也即纯跟踪算法。其思想比较简单，如下图所示。

我们将所有小车的运动都视为在圆上的运动，包括直线运动其实也是在半径无穷大的圆上运动。这样给定当前位置和目标位置，通过简单的数学计算可以算出圆的半径，进而知道该如何转弯。

但是这里我们使用的是麦克纳姆轮，一种可以全向运动的轮子。虽然麦克纳姆轮可以实现原地自旋实现转弯，但是经过实验发现没有一般小车转弯那么灵活性，在边行走边转弯非常不稳定。因此这里采用一个非常朴素的做法：以上下左右四个方向为基本方向，然后根据起始点和目标点的位置方向来决定速度方向。

代码的示例如下：

int f = nX < X ? 1 : -1;
int r = nY > Y ? 1 : -1;
for(int i = 0; i < 4; ++ i){
    speed1[i] = f * speed_forward[i] * fabs(nX - X);
    speed2[i] = r * speed_rightward[i] * fabs(nY - Y);
}

其中X，Y表示当前位置，
nX，nY表示目标点位置。这里速度还乘上了距离系数，即比如当目标点就在当前点左边，或左边稍微前方时，就只往左边走，或者放慢往前走的速度。

那么剩下的就是目标点该如何确定。事实上这里借鉴的就是Pure Pursuit的目标点确定的想法：Lookahead distance。

Lookahead distance

就犹如人开车一样，往前开车会选择一个参考点，但这个参考点会与当前位置保持一定距离。即前方有若干个参考点，一般会选择离自己比较远的。这个距离就是Lookahead distance。对于当前位置方圆Lookahead distance的点会被忽略，而去寻找在这之外的前方最近点的距离。

pair<int,int> findLookheadPoint(int x, int y){
    int id = findNearestPoint(x, y);
    if (id > curPoint)
        curPoint = id;
    int cur = two2one(x, y);
    while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){
        ++ curPoint;
    }
    return one2two(path[curPoint]);

首先找到距离自身最近的点，然后再往前寻找第一个距离大于Lookahead distance的点。其中 two2one, one2two函数是将坐标二维转一维或一维转二维的。

当前这里还有一点修改，正如GTA等著名游戏一样，当前速度很快的话，视线就会变得拉长，能看到的东西就很多，而速度变慢时，视线就会变窄，能看到的东西就比较少。这里参照了这个想法，动态调整Lookahead distance：

• 当速度快时，Lookahead就变大，参考点就距离自己比较远，这样当遇到转弯时就能提前知道，并做相应的动作。

• 当速度慢时，Lookahead就会变小，参考点就距离自己比较近，这样能有效进行转弯。

而前面提到，为了实现直线速度加快，弯道速度减缓，这里就需要思考如何判断当前是走直线还是走弯道。

为了实现转弯需要计算出当前圆的半径。我们就可以根据这个半径的大小来判断当前是走直线还是弯道。为方便起见，这里取了个阈值，当半径大于2m时我们就认为走的是直线，速度设定为原来的三倍，否则不变。

double l = maze.dis(Y, X, nX, nY);
double R = l * l / 2 / abs(X - nX);
double vv = (R > 2 ? 3 : 1);

...

for(int i = 0; i < 4; ++ i){
    speed1[i] = f * speed_forward[i] * fabs(nX - X) * vv;
    speed2[i] = r * speed_rightward[i] * fabs(nY - Y) * vv;
}

小车方向控制

上面解决了如何选取目标点以及一些改进和如何向目标点前进的问题，下面是一些额外的问题。

但在具体实验时发现一个重大问题：注意我们是根据当前位置和目标距离的差值来控制小车的前后速度和左右速度：这里的速度是相对于小车，而不是这个世界的。

正常情况下，小车不会碰到障碍物，小车能完美地进行路线行走，没有任何问题。

但由于小车不是质点，有一定的体积，且带有一定的惯性，难免会一定程度偏离预定轨迹，而一旦碰到了障碍物，小车的受力情况就不再只有发动机的力，还有障碍物给予的力，这可能会导致小车的方向偏移，进而导致所谓的小车向前走和向右走的方向与世界参考方向不一致。

因此小车需要能够自动矫正当前方向的能力。

其实现起来挺简单，关键要获取小车当前的角度信息，根据小车的角度信息就能够得知是否偏离预定方向，偏离的话该以顺时针还是逆时针旋转能快速归位。

通过查阅官方文档，得知可以通过Supervisor获取小车的当前方向。于是通过麦克纳姆轮的自旋就可以实现小车的自动校正方向的能力。

    double orix = -1;
    double oriy = 0;
    double oriz = 0;

    ...

    const double curx = robot->getSelf()->getOrientation()[0];
    const double cury = robot->getSelf()->getOrientation()[1];
    const double curz = robot->getSelf()->getOrientation()[2];
    if (abs(curx - orix) > 0.1){
        if (cury < 0){
            printf("LEFT rec\n");
            for(int i = 0; i < 4; ++ i){
                speed1[i] *= 0.01;
                speed2[i] *= 0.01;
                speed3[i] = speed_leftCircle[i] * abs(curx - orix);
            }
        }else{
            printf("RIGHT rec\n");
            for(int i = 0; i < 4; ++ i){
                speed1[i] *= 0.01;
                speed2[i] *= 0.01;
                speed3[i] = speed_rightCircle[i] * abs(curx - orix);
            }
        }
    }else{
        for(int i = 0; i < 4; ++ i)
            speed3[i] = 0;
    }
```   

当小车进行方向矫正时，会降低原来行进的速度，优先进行矫正方向行为。

当然事后通过与同学交流，可以采用另一种行走的方法，即用两个`gps`，分别安置于车头和车身，通过目标点与车头和车身所形成的向量夹角（可以通过叉积求得）来判断当前是否转向且该往哪个方向转。个人觉得这样的方式非常挺好的，能够有效适应方向偏离情形。不过鉴于麦克纳姆轮为全向轮，这样实现也是可以的。

#### 障碍物检测 

矫正方向可以有效防止因障碍物碰撞带来的方向偏移问题，但是也存在一些情景会有小车撞在障碍物上。比如目标点在小车位置的左前方，但小车前方已经撞在障碍物上了，这导致小车虽然有前方和左方向的力，但还是会固定在墙上不动了。为了解决这样的情形我们应当减小小车前进的速度，因为前往已有障碍物，这样小车就能先往左边走，走出障碍物再往前走。

参考官方文档提供的[DistanceSensor](https://cyberbotics.com/doc/reference/distancesensor?tab-language=c++)，可以对障碍物距离进行检测，里面有个`Lookup Table`相当于一个距离与值的映射，如下图所示。

{% asset_img lookuptable.png Lookup Table %}

这里设置当距离小于`0.1m`时值为`0`，大于`0.2m`时值为`1`，中间值用线性函数表示。给小车分别安置前后左右四个方向的传感器，这样在代码中用`getValue`函数就能获取当前距离对应的值的结果。
```cpp
char distanceNames[4][8] = {"front", "back", "left", "right"};

for (int i = 0; i < 4; i++)
{
    ...
    dismotors[i] = robot->getDistanceSensor(distanceNames[i]);
    dismotors[i]->enable(1);
    ...
}

...

for(int i = 0; i < 4; ++ i){
    dismo[i] = dismotors[i]->getValue();
}

...

for(int i = 0; i < 4; ++ i){
    speed1[i] = f * speed_forward[i] * fabs(nX - X) * vv * min(1.0, (f == 1 ? dismo[0] : dismo[1]));
    speed2[i] = r * speed_rightward[i] * fabs(nY - Y) * vv * min(1.0, (r == 1 ? dismo[3] : dismo[2]));
}

这样当前小车的速度值就由四个参数决定：前后还是左右，距离目标点距离，当前行进的半径，与障碍物的距离。

坐标转换

上述解决了大部分运动规划的问题，还剩下一些小细节问题，其中一个是坐标转换。

先前我们通过OpenCV读取迷宫图像并规划出路线，其坐标是像素坐标：800x600的一个坐标系。而在模拟世界是以m为单位计算的。我们需要将像素位置转换成模拟世界的位置。

通过研究模拟世界的坐标，其原点在正中间，不同于图片的左上角。量取真实世界的长宽为6米和4.5米，通过简单的数学换算就可以进行转换了。

inline pair<int,int> real2pic(double X, double Y){
    return {int((3 - X) / 6 * 800), int((Y + 2.25) / 4.5 * 600)};
}

inline pair<double, double> pic2real(int x, int y){
    return {3 - 6 * x * 1.0 / 800, -2.25 + 4.5 * y * 1.0 / 600};
}

可视化路线

为了能够更好地看到小车正在追踪的点的位置，我们可以将其标记出来。

参考官网文档提供的display元件，按照文档的说明通过对其设置一个shape，就可以让原本显示在一个小窗口的图像显示在shape上，实现标记点的效果。

参考官方给出的控制器示例和世界文件示例。这里需要另一个robot充当display的载体，里面的一个children为display充当图像的绘制，再给display的children来一个shape来显示绘制的图像。

具体实现这个效果遇到的最大的问题就是坐标的定位。注意到display也有width和height属性，因此要绘制的坐标要依靠这个属性来确定，原点同样在左上角，这里就需要一个坐标转换公式。之前一直以为是模拟世界的坐标就一直弄不出效果。

还有一个小车圈圈的动态移动的实现，这里运用的是图像的保存和重绘制。即在绘制小车圈圈前保存当前图像，然后再绘制下一次小车圈圈时先还原图像，再绘制下一个圈圈，否则前后圈圈会相互重叠。

还有一个大问题就是通信。由于display在另一个robot上，这个robot与前面实现的robot不是同一个robot，需要用不同的controller程序。那么寻路小车当前追随的目标点信息只保存在那个程序里，在display的robot程序不得而知。由于网上关于webots的display教程几乎没有，官方提供的示例仅仅是小车的位置，而这个可以通过先前提到的supervisor获得。因此，由于不太会进程间的通信，这里实现的方式是在display的robot也模拟同样的行为：根据小车的位置获取最近的目标点。虽然会消耗一定的不必要计算资源但也是个可行的解决方案。

实验结果

演示视频：bilibili

实验总结

通过本次实验，我了解到了PRM算法实现的具体流程，明白了PRM算法的重要意义：连续变离散化。由于世界是连续的，PRM算法通过对世界采样将其离散化，然后在离散化的点之间建立连边，就变成了算法课上学到的图论中的图，然后应用最短路算法就可以解决了。除此之外还了解到了如何对规划的路径进行行走，在网上搜索资料的时候，除了Pure Pursuit算法之外，貌似还有另一种叫MPC算法，一种精度更高但需要更大计算量的算法。除此之外还有对环境实时感知，就犹如老师上课演示的一个视频，未知区域默认无障碍物下的路径规划行走情景，但探查到有障碍物后再进一步修正路线，所采取的SLAM算法，起初在思考这类情形的时候，觉得最棘手的就是如何通过小车的视觉传感器（？或者可能用其他的）去构建平面图，这里精度误差是关键，但未能想到好的方案，即如何从画面中的障碍物转换到平面地图，包括位置和障碍物的长度。虽然感觉用相关的视觉数学知识可以转换但稍感到复杂。

尽管如此，结合麦克纳姆轮的特性，这里就只借鉴了Pure Pursuit的Lookahead distance的思想，以及结合自己曾经玩过的游戏的一些视觉感受，增加了动态调整Lookahead distance以及可视化路径跟踪过程。期间遇到的最大的问题就是进程间的通信，限于Webots的实现，暂时没能想到好的向display的robot传输行走的robot的相关信息，只能让display也模拟其行为。为了实现相关功能，参考最多的就是Webots官方文档，尤其是display相关的知识，只有官方文档的英文说明，其余的资料几乎没有。但最后还是根据说明和官方给出的C语言的示例代码，编写出了相应的C++的控制器，还是颇有成就感的。只是期间些许问题，尤其是坐标定位，只能琢磨官方给的文档和代码，网上也没其他资料了。

总之，本次实验收获颇丰，进一步熟悉了Webots的软件，以及知晓了机器人的路径规划和行路的流程。