使用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样条的平滑方式,便可以得到一条无碰撞的规划路径。关键代码如下所示。
1 | path_list=rrt(start, goal, map) # 快速随机树 |
其中路径缩减(single_reduce)和点迹加密(dense)部分可以使用多次,以搜寻更佳的路线。经过这样的处理方法,所得到的路径在绝大多数的情况下不存在任何碰撞。
模型更新
借助于指导书所给的小车模型参数,仅需进行一个简单的模型更新即可。更新的时长dt为0.01秒,采用的更新方法为四阶龙格-库塔法(RK4)。其函数位于dynamic.py中,关键代码如下。
1 | k1 = slope(xC) |
在这里,slope函数即为指导书中所给的模型参数xC的微分函数,即求得指导书中所示的dx。
控制方法
纵向控制
在纵向控制方面,方法使用了PD控制器进行控制,以目标速度为跟踪量,油门为输出。目标速度拥有一个上vMax限和下限vMin,控制器将会检测最近路点前方一定数量的点,计算这段路径弯曲程度,路径越弯曲,则目标速度越接近vMin,路径越笔直,则目标速度越接近vMax。在本例中,P和D控制器的参数为5和2。
横向控制
横向控制方面,方案也使用了PD控制器进行控制,以最近路点与前方一定数量路点(如总路点数量的为前方路点)的连接线线段及方向为跟踪量,以这条连接线与当前位置的距离为误差,输出前轮转向角。在本例中P和D控制器的取值可以是多样的,根据多次实验,最佳的PD比例大约在1:10到1:20之间,在附上的代码中,采用了P和D控制器的参数为200和4000的方案。同时,横向控制需要显示前轮的最大转向角,在这里为。
代码说明
环境要求
- ubuntu-20.04
- ros-noetic
使用测试方法
-
安装map-server功能包以及python中的scipy
1
2sudo 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控制方法由于其广泛的适用性和较为简单的实现性而被选用。在给出的代码中,项目也进行了长时间的调参和增加微小改动以提升性能,代码中的默认参数,是尽可能的兼顾速度和碰撞的一组参数。