使用RRT规划及PD控制的ROS仿真小车竞速

使用RRT规划及PD控制的ROS仿真小车竞速

功能简介

本作业通过ros进行完成，编写了功能包planner进行实现，功能包中由一系列python代码负责ros话题通信。主要文件为rrt_planner.py和dynamic.py。

本项目是根据SYSU-HI-LAB/Fundamentals-of-autopilot-project_v2（指导书）的要求而实现的。

本项目的所有代码均发布在了lankoestee/rrtpd-planner。

任务完成情况

标准地图

用时：15.80秒，未产生碰撞。

随机地图

用时：19.68秒，未产生碰撞

规划方法

在规划方法实施前，编写了yaml文件以便使用map_server将png文件发布为地图话题。收到地图话题之后，对障碍物进行了膨胀处理，即对每个障碍物膨胀0.15米（或其他数值），以便为后续规划和仿真给予冗余。在任务规划方面，本项目使用了RRT快速随机树的方法进行路径规划，并使用路径缩短方法（即找到上一个路点往后范围内可达的最后一个路点，并舍弃中间所有的路点）得到较佳的、仅有关键点的折线路径。随后，通过对点迹的平均加密和B样条的平滑方式，便可以得到一条无碰撞的规划路径。关键代码如下所示。

path_list=rrt(start, goal, map)          # 快速随机树
path_list=single_reduce(map, path_list)  # 路径缩减
path_list=denser(path_list, 3)           # 点迹加密（2^3倍）
path_list=single_reduce(map, path_list)
path_list=denser(path_list, 3)
path_list=single_reduce(map, path_list)
path_list=denser(path_list, 2)           # 点迹加密（2^2倍）
path_list=bspline_fit(path_list)         # B样条平滑

其中路径缩减（single_reduce）和点迹加密（dense）部分可以使用多次，以搜寻更佳的路线。经过这样的处理方法，所得到的路径在绝大多数的情况下不存在任何碰撞。

模型更新

借助于指导书所给的小车模型参数，仅需进行一个简单的模型更新即可。更新的时长dt为0.01秒，采用的更新方法为四阶龙格-库塔法（RK4）。其函数位于dynamic.py中，关键代码如下。

k1 = slope(xC)
k2 = slope(xC + dt/2*k1)
k3 = slope(xC + dt/2*k2)
k4 = slope(xC + dt*k3)
xC = xC + dt/6*(k1 + 2*k2 + 2*k3 + k4)

在这里，slope函数即为指导书中所给的模型参数xC的微分函数，即求得指导书中所示的dx。

控制方法

纵向控制

在纵向控制方面，方法使用了PD控制器进行控制，以目标速度为跟踪量，油门为输出。目标速度拥有一个上vMax限和下限vMin，控制器将会检测最近路点前方一定数量的点，计算这段路径弯曲程度，路径越弯曲，则目标速度越接近vMin，路径越笔直，则目标速度越接近vMax。在本例中，P和D控制器的参数为5和2。

横向控制

横向控制方面，方案也使用了PD控制器进行控制，以最近路点与前方一定数量路点（如总路点数量的 $\frac{1}{100}$ 为前方路点）的连接线线段及方向为跟踪量，以这条连接线与当前位置的距离为误差，输出前轮转向角。在本例中P和D控制器的取值可以是多样的，根据多次实验，最佳的PD比例大约在1:10到1:20之间，在附上的代码中，采用了P和D控制器的参数为200和4000的方案。同时，横向控制需要显示前轮的最大转向角，在这里为 $\frac{6}{\pi}$ 。

代码说明

环境要求

ubuntu-20.04
ros-noetic

使用测试方法

安装map-server功能包以及python中的scipy

1 2	sudo apt-get install ros-<distro>-map-server pip3 install scipy

创建新的工作空间，并添加src文件夹
1
git clone
进行编译
1
catkin_make
设置环境变量
1
source devel/setup.bash
若要运行默认地图的小车仿真，则输入以下命令
1
roslaunch rrtpd_planner default.launch
若要运行随机地图的小车仿真，则输入以下命令
1
roslaunch rrtpd_planner random.launch
等待路径计算时间（平均约为20~30秒，少数情况会达到近1分钟，若超过一分钟，请中断程序并重新进行步骤5或6）
到达终点后，命令行终端将会显示所小车移动花费的时间

参数设置

在所附上的代码中，共有两个重要组成模块，分别是rrt_planner.py和dynamic.py。

规划部分的代码放在了rrt_planner.py中，它不存在参数设置的问题。

仿真和控制的代码放在了dynamic.py中，它拥有几个可以控制以给出不同小车性能的参数，一是vMax和vMin，在默认值中它们被设置为了1.8和0.8；二是角度控制器的PD参数，在默认值中它们被设置为了100和1800。在这个参数配置下，在大部分随机地图的情况中，它都能够较为快速且无碰撞的进行仿真，完成时间都均为20秒左右。若采取更加激进的配置，及拉高vMax至2.0或是更高，会提高任务完成的速度，并提高路径与障碍物碰撞的可能性。

在第一张动图，也就是默认地图跑圈的情况中，方案采用了一种更为激进的参数设置，即降低了最后一步点迹加密的密集程度，以使路径更加平滑，但是增加了碰撞的概率。具体而言，即是将rrt_planner.py文件中第250行从path_list=denser(path_list, 2)改为path_list=denser(path_list)，并将vMax改为2.5，vMin改为1.0。本人非常不建议这样做，因为这会大幅提高路径与障碍物碰撞的概率。

特殊情况处理

rrt运算具有一定的随机性，若在命令行窗口出现了Path planning finished且未在rviz中出现代表小车运动轨迹的红线，则说明rrt运行失败，运行失败，需要中断并重新运行roslaunch。
由于在规划过程中采取了路径膨胀手法，故若出现两个障碍物贴近且留空路径不足的情况，可能会出现没有通路的情况，需要中断并重新运行roslaunch。
随机障碍物可能会出现在起点和终点的位置，若在命令行的中出现了红色的Start point is not accessible或Goal point is not accessible，则说明起点或终点不可达，需要中断并重新运行roslaunch。

方法总结

在本任务的实现中，方案曾将采用了许多许多种的方法进行规划与控制，包括A*、hybrid A*以及其他规划，和LQR、MPC等其他控制。规划上，RRT算法较之与A*算法而言，其拥有更快的有点，故在本例中被选用；控制上，PID控制方法由于其广泛的适用性和较为简单的实现性而被选用。在给出的代码中，项目也进行了长时间的调参和增加微小改动以提升性能，代码中的默认参数，是尽可能的兼顾速度和碰撞的一组参数。