任务概要

  • 在给定的迷宫场景中,找到唯一的通路,走出迷宫。
  • 路径规划算法要求使用人工势场或 RRT 路径规划算法中的一种或多种算法。
  • 作业提示:
    • 使用 Vision Sensor 获取迷宫的全局地图。
    • 构建二维全局地图与迷宫的映射关系。
    • 使用RRT或人工势场规划出一条通路。
    • 将通路映射到迷宫之中。
    • 机器人巡线,走出迷宫。

完成情况

  • 已学习并能较为熟悉使用 V-REP Python Remote API 接口。
  • 已实现基于 RRT 路径规划算法的迷宫通路计算功能。
  • 已实现 RRT 通路的剪枝功能。
  • 已实现机器人按通路巡线的功能。

V-REP Python Remote API

由于在前面实验中,我们已进行了 V-REP Remote API 接口的使用,积累了部分经验,并且考虑到本次实验任务的复杂性,因此在本次实验中,我们使用 V-REP Python Remote API 实现机器人在路径规划算法下走出迷宫的目标。

首先,我们为机器人模型添加 Non-threaded Script ,其具体内容如下。在 sysCall_init 函数中,我们在端口 19999 开启了 Remote API 服务。

为了成功使用 Python 与 V-REP 交互,我们需要导入 remoteApiBindings 至项目文件夹,具体目录为 V-REP 安装目录下的 programming\remoteApiBindings ,我们只需导入 vrep.py、vrepConst.py 和 remoteApi.dll 文件。我们编写简单的 Python 代码,测试 Remote API 是否调用成功,具体代码如下。

点击 V-REP 仿真运行按钮,随后执行上述脚本,若无发生异常,说明 Remote API 建立成功。

我们可通过 vrep.simxGetObjectHandle 获取 V-REP 仿真环境下的物体句柄。

获取全局地图

按照作业提示,我们需要通过 Vision Sensor 获取全局地图,为此我们实现了 get_image 工具函数,其具体代码如下。

get_image 函数中,我们对获取的图像进行了二值化边缘提取的处理,目的是减少障碍点的数量,提高后续算法运行性能。

在调用 get_image 函数获取全局地图之前,我们需要对 Vision Sensor 进行初始化,否则,我们可能无法获取图像,初始化函数 init 的具体代码如下。

以下是我们初始化后,调用 get_image 获取的迷宫全局地图,其中白色像素表示障碍物的边缘,黑色像素表示无障碍物的通路。

RRT 路径规划

快速扩展随机树(RRT)算法,通过对状态空间中的采样点(随机采样)进行碰撞检测,将搜索导向空白区域,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题,适合解决多自由度机器人在复杂环境下和动态环境中的路径规划,该算法是概率完备且不最优的。

算法具体流程大致如下:在状态空间中随机选择一个采样点,然后从随机树中选择与其最近的点进行扩展新的树节点,并进行障碍物检测,若与障碍物发生碰撞,则放弃生长,否则将新节点(以及扩展路径)添加至随机树中。

在进行 RRT 路径规划前,我们需要在二值化图像中,提取障碍物的坐标信息。在 get_image 获取的二值化图像中,障碍物点为白色像素,其像素值为 255 。利用此信息,我们可提取图像中所有的障碍物边界点,具体代码参照以下 get_obstacles 代码(障碍物边界点半径为15)。

我们在 get_random_node 函数中,实现状态空间下的随机采样点生成,其中为了加快通路的搜索速度,我们根据随机概率来决定采样点是随机点还是目标点:对于范围为 0 到 100 的随机值 rnd ,若 rnd 大于 goal_sample_rate ,则采样点为随机点,否则为目标点。

在生成随机采样点后,我们需要在随机树中查询离其最近的节点,该部分功能由 get_nearest_node_index 函数实现。

我们在需要在“最近点”和采样点之间进行随机树扩展,其具体代码如下。

在得到扩展后的新节点后,我们需要对其进行障碍物检测,若通过障碍物检测,则添加此节点至随机树中,否则抛弃此节点。

综合上述各个子模块,我们即可实现完整的 RRT 路径规划算法,具体代码如下。

对迷宫的二值化地图执行 RRT 路径规划后,可得以下的路径通路(白色像素点轨迹)。

RRT 通路剪枝

通过 RRT 算法,我们能获得一条不错的路径,但实际上我们通过肉眼会发现,由于路径是随机生成的,会有一些点是不需要经过的,我们可以同时跨越几个点,这样我们的小车的拐点就不会太多,小车行驶路线也会更加流畅。

通路剪枝算法大致流程如下:从当前点出发,枚举下一个可行点,两点确定一条直线,判断直线是否与障碍点距离过近,如果过近则不合法,否则即合法。合法后用最远的可行点更新当前点。该剪枝算法属于贪心算法,具体代码如下。

在实验过程中,剪枝算法并未考虑车体大小信息,降低了实验成功率。所以,在实现中,我们保留了原始通路和剪枝后的通路,根据实际情况选取合适的通路进行仿真。

在算法实现中,我们使用了一个 Trick 点:我们不需要计算线段与障碍物点的距离,我们只用取线段上的 n 等分点来计算即可。当 n 取一定大小(约 10 )基本上就会有相同的剪枝效果,剪枝效果如下图所示。

从上图可以看到,剪枝效果还是很不错的。不过实际实验后发现,由于车身大小限制,车旋转时占用空间有点大,导致车体与墙体碰撞。因此,我们要看情况选取原有路经或者剪枝后的路径。

机器人搭建

考虑到迷宫路径的复杂性,我们使用差速转向机器人完成实验,静态演示如下图所示。

由于机器人的运动学模型是差速转向,因此我们需要计算其反运动学公式:根据速度和角度推导左右两电机的转速,运动学控制代码如下图所示。

RRT 通路巡线

在获取到通路路径后,我们需要让机器人按照路径点进行巡线。我们的巡线策略很简单:获取当前机器人的角度,计算路径点相对机器人的角度,进行原地转向使得路径点位于机器人正前方,随后直线运动至路径点,重复此过程直至抵达目的地,具体代码如下图所示。

在代码中,我们调用 simxGetObjectPosition 获取机器人当前位置,从而计算机器人和路径点间的相对角度,随后我们调用 get_beta_angle 函数获取机器人对世界坐标系的角度,该函数通过调用 simxGetObjectOrientation 方法获取机器人当前的欧拉角参数,获取 Beta 角度(即为相对于世界坐标的角度),具体代码如下。

在执行巡线算法之前,我们需要对获取的通路路径点进行坐标转换,这是因为我们在执行巡线算法之前,我们需要对获取的通路路径点进行坐标转换:从图像坐标(像素行列号)转换到世界坐标,坐标转换的代码如下图所示。

效果展示

演示效果的详细细节可参考视频:https://www.bilibili.com/video/av74889189

RRT 路径规划

使用 python 运行 path_planning.py 即可对地图进行 RRT 路径规划,其具体结果如下。

RRT 路径剪枝

使用 python 运行 path_pruning.py 即可对 RRT 规划路径进行剪枝操作,其具体结果如下。

RRT 路径巡线

执行 path_following.py 文件,即可进行 RRT 路径巡线,巡线效果如下图所示。

存在问题

本次实验,内容是使用路径规划算法求取迷宫通路、并按此通路巡线,具备很强的挑战性。在实验过程中,我们遇到了几个难题,最终大多数都被成功解决。

我们将本次实验划分了三个部分:路径规划求取通路通路路径剪枝通路巡线。在路径规划求取通路部分中,我们一开始使用的是人工势场规划算法APF),但在算法测试环节中,我们发现在 APF 算法规划下,路径容易出现**“局部最小”**的情况:由于各个障碍物对机器人的斥力相互叠加,出现合力为零的情况,且由于目标物的引力不足,导致机器人停止了运动,无法到达目的地。解决此问题的方法是为现有的 APF 算法添加随机行走策略,但出于进度考虑,我们更换了路径规划算法,使用鲁棒性更为优异的RRT快速扩张随机树算法。

我们在测试 RRT 规划算法时,出现了求取通路时间过长的问题,我们尝试更改扩展步长以及障碍物半径大小,发现并不能很好解决此问题。最终,经过我们的努力,我们将算法运行时间控制在 5 分钟内。或许,我们可以使用更为高效的数据结构(K-D Tree)来求取离随机抽样点最近的随机树节点,进而提升算法速度。由于时间的原因,我们并未在程序代码中使用 K-D Tree 数据结构,这是本次实验的一个遗憾。

在求取出通路路径后,我们发现该路径存在优化空间,如可利用“两点之间线段最短”来移除不必要的路径点。但在剪枝算法实现中,我们遇到了运行时间过长的问题:由于障碍物边界点数量过多(2万多个),导致两点连线的障碍物检测运行时间过长,从而增长了算法总体运行时间。针对该问题,我们选择了使用等分点判断碰撞的方法,一定程度减少了算法运行时间。

在实现机器人按通路巡路部分时,我们遇到了欧拉角相关的问题。在实验中,欧拉角的 Beta 即为我们需要的转向角度,但我们一开始发现 Beta 存在二义性(其范围为 -90 度到 +90 度),并不能反映当前机器人相对于世界的转向角度。在耽误了好长时间后,我们发现可通过考虑 Alpha 以及 Gamma 的正负情况,转换 Beta 角度至 -180 度至 +180 度。

总体来说,本次实验是成功的,我们成功完成了各项实验任务,收获颇丰。