速度规划

优达无人车项目：ROS、检测红绿灯、加减速、轨迹跟踪
参考https://github.com/ericlavigne/CarND-Capstone-Wolf-Pack/tree/master/ros/src
检测红绿灯/路口通行

图片.png

轨迹规划：https://github.com/ericlavigne/CarND-Capstone-Wolf-Pack/blob/master/ros/src/waypoint_updater/waypoint_updater.py

图片.png

轨迹规划任务：接收基准路点、车辆信息、红绿灯检测信息，发布车将要行驶的200个路点（位置/速度）
1秒钟发布5次，局部路径规划是200ms
cruise_speed = 40/decel_limit = -5/accel_limit =1
定义路点个数、停止距离、停止余量、加速度因子、减速度因子

图片.png

1.计算安全刹车距离

self.safe_distance = (self.car_curr_vel ** 2)/(2 * self.decel_limit * SAFE_DECEL_FACTOR)
v*v=2as;

2.计算车离基准路径最近点索引
3.计算到红绿灯的距离

self.distance_to_tl = self.distance(self.waypoints,self.closestWaypoint, self.tl_idx)
路径点累计弧长

4.判断车的行为

图片.png 图片.png

停车情况：1.车到红绿灯的距离小于停车距离且灯为黄色/红色；2.最近点索引等于红绿灯点索引且车到红绿灯的距离小于停车距离且灯为黄色/红色; 3.红绿灯点索引+3 大于车最近点索引且灯为黄色/红色且车最近点索引大于红绿灯点索引。
减速情况：1.（车到红绿灯的距离大于停车距离）且（车到红绿灯的距离小于刹车距离）且（能检测到红绿灯）且还没过红绿灯
2.车到红绿灯的距离大于停车距离）且（车到红绿灯的距离小于2倍的停车距离）且（能检测到红绿灯）且还没过红绿灯且（车速大于3）
行车情况：1.红绿灯索引小于最近点索引；2绿灯且车到红绿灯的距离小于停车距离；3.车到红绿灯的距离大于刹车距离
5.根据行为规划

图片.png
停车情况
多个路点速度都为0
图片.png
行车情况
vv-v0v0=2as
循环规划所有路点速度
图片.png
减速情况
图片.png

———————————————————————————————————
判断轨迹最近点索引方法
（结合车辆航向，路点方向进行判断）

图片.png