最小生成树不唯一但路径中的边权不完全相同_V-REP 路径规划算法与迷宫

最新推荐文章于 2024-09-16 12:58:29 发布

weixin_39712455

最新推荐文章于 2024-09-16 12:58:29 发布

阅读量217

点赞数

文章标签：最小生成树不唯一但路径中的边权不完全相同蚁群算法路径规划路径规划算法matlab

项目地址：github.com/Jiahonzheng/Robotics

任务概要

在给定的迷宫场景中，找到唯一的通路，走出迷宫。
路径规划算法要求使用人工势场或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 安装目录下的 programmingremoteApiBindings ，我们只需导入 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 度。

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