调试了很长时间的机器人自动导航,一直以为路径规划是个很高大上的东东。直到看完路径生成及选择代码,结果好无语。
可能这是最简单的路径规划算法,毕竟叫做simple trajectory generator。
路径规划分成两部分:1.样本路径生成。2.最优路径选择
第一部分 样本路径生成
1.获取当前的x轴速度vx,y轴速度vy,和z轴转向角速度th。这部分数据一般从odom获取,并且在2d地图上,y轴速度一般是0。
2.获取下发控制命令的频率H,算出每个控制命令的周期 1/H = sim_period_
3.以x轴速度vx为例,根据命令周期长度,加速度,减速度,最大速度,最小速度,算出下个周期时,vx的最大值max_vx和最小值min_vx。
max_vx = min(最大速度,vx + 加速度×命令周期);
min_vx = max(最小速度,vx - 减速度×命令周期)
4.获取每个速度的采样率,例如vx的最大速度为1,最小速度为0,采样率为6,那么针对vx的采样标本是0,0.2,0.4,0.6,0.8,1。
5.穷举法,针对vx,vy,th的所有标本进行全组合,一共就有(vx采样率×vy采样率×th采样率)个样本速度(vx,vy,th)。
6.针对每个样本速度,在 sim_period_时间内,可以生成一条路径(由一系列个点组成)。
需要注意是否使用dwa算法,dwa是非持续加速算法,即在sim_perio