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

ubuntu20 noetic

功能简介

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

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

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

任务完成情况

标准地图

用时:15.80秒,未产生碰撞。

随机地图

用时:19.68秒,未产生碰撞

规划方法

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

1
2
3
4
5
6
7
8
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中,关键代码如下。

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

代码说明

环境要求

  • ubuntu-20.04
  • ros-noetic

使用测试方法

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

    1
    2
    sudo apt-get install ros-<distro>-map-server
    pip3 install scipy
  2. 创建新的工作空间,并添加src文件夹

    1
    git clone 
  3. 进行编译

    1
    catkin_make
  4. 设置环境变量

    1
    source devel/setup.bash
  5. 若要运行默认地图的小车仿真,则输入以下命令

    1
    roslaunch rrtpd_planner default.launch
  6. 若要运行随机地图的小车仿真,则输入以下命令

    1
    roslaunch rrtpd_planner random.launch
  7. 等待路径计算时间(平均约为20~30秒,少数情况会达到近1分钟,若超过一分钟,请中断程序并重新进行步骤5或6)

  8. 到达终点后,命令行终端将会显示所小车移动花费的时间

参数设置

在所附上的代码中,共有两个重要组成模块,分别是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。本人非常不建议这样做,因为这会大幅提高路径与障碍物碰撞的概率。

特殊情况处理

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

方法总结

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