planning深度剖析
planning深度剖析
- 结合find命令过滤目录及文件名后缀: find /home/hadoop/nisj/automationDemand/ -type f -name '*.py'|xargs grep -n 'data_chushou_pay_info'
- 配置文件在每个模块的common文件的*_gflags.cc文件下定义.
- lattice文件夹下面的代码应该是路网格子相关的代码
planner文件夹下的内容
- EM_Planner -- EMPlanner是一个期望最大的planner
- 在EMPlanner::Plan函数中主要做的事情：
- 更新planning start point
- scenario_初始化
- scenario_处理planning_start_point和frame
- 在EMPlanner::Plan函数中主要做的事情：
- refernce_line -- 参考线相关的函数接口都在这里面,
- cos_theta问题
- qp_spline平滑
- spiral problem平滑
- deciders -- 决策者
- tuning -- 调整
Apollo Planning简介
- 整体架构:
- Apollo的定位，行为预测，感知，导航，高精定位都是通过Apollo适配器接入Planning的。
- planning主要根据上面的信息转换为参考线提供器，数据处理与管理。
- 针对参考线做二维参数化的Spline平滑器。
- 然后接入任务管理器进行任务管理。
- 然后进入路径、速度综合器。
- 再进入最终结果验证器。
- 整合传统的决策与规划两个模块。
- 统一、灵活、快捷。
- 参考线与主算法分离。
- 平滑的参考线是决策规划的基础、数据驱动的要求，采用二维Spline和异步更新，\[C(x,y)=C(f_{X}(t),f_{Y}(t)) \]
- Spline函数。
- 将三维问题转换为X-Y(路径规划(x,y))平面和S-T(速度规划(s,t))平面问题来处理。
- 数据设计:
- 模块配合复杂，需求主导:
- 行为预测(上游): proto中增加保护字段。
- 车辆控制(下游): 分离实际位置与规划位置。
- 障碍物属性复杂, 提取共性:
- S-L边界(boundary)。
- 阻塞S-T边界(boundary)。
- 横向、纵向决策。
- 自身结果复杂:
- 分而治之， 路径点、速度点、参考线点、trajectory点。
- 横向决策: 避让、绕行。
- 纵向决策: 停止、跟车、避让、超越。
- boundary有四个点构成((s0, l0), (s1, l1), (s2, l2), (s3, l3))。
- 模块配合复杂，需求主导:
- planning还需要考虑:
- 交通规则。
- 路径优化。
- 路径决策。
- 速度优化(DP)。
- 速度决策。
- 速度优化(QP)。
- 交通规则:
- 核心特点： 多。
- 容易适配、避免冲突、周期一致。
- 规则工厂，灵活配置不同规则。
- 规则库:
- 后方车辆；
- 人行横道；
- 探头右拐；
- 行驶终点；
- 借道绕行；
- Stop标志；
- 横向和纵向决策分离+决策融合；
- 有限状态机: drive --> wait --> side pass --> drive。
- 路径规划策略:
- RRT(rapidly exploring random tree)： 随机取点， 路径不稳定，常用于机器人走迷宫。
- State-lattice：格点预先设定，灵活度差，没有考虑道路特点。
- Quadratic Programming(二次规划): 成本函数受限， 受地图中心线影响大, 此问题的解决不受规划模块控制。
- 最终的策略: 基于采样，结合车辆与道路信息，比state-lattice灵活性更强。
- 离散化求解空间。
- 使用平滑曲线连接各层的采样点。
- 计算各条路径的成本。
- 动态规划选择合适的道路。
- 道路采样的优势:
- 避免陷入局部最优；
- 概率完备避免无解；
- 低速到高速全覆盖；
- 道路点采样 --> 路径生成 <--> Cost计算 <--> 路劲选择 --> 最终路径。
- 碰撞+物理边界+虚拟边界+数值(安全值+舒适值):
- 层层比较得到一个最终比较数值大小
- 速度规划器 - 动态规划
- 与主车有重叠的障碍物映射到ST图中：
- 停车(stop)；
- 避让(yield);
- 超越(overtake);
- 限速：ST路的限速区；
- 把问题转换为有限求解空间内的最优化问题。
- 构建网格;
- 构造成本函数;
- 安全性相关：与障碍区的间距；
- 体感相关:
- 主车速度、加速度；
- 主车速度变化率；
- 到达目的地相关:
- s轴相对位置。
- 动态规划选择成本最少的路径。
- 与主车有重叠的障碍物映射到ST图中：
解析EM-planning
- em_planner继承于PlannerWithReferenceLine， PlannerWithReferenceLine继承于Planner.
- Planner有三个成员变量: PlanningConfig, ScenarioManager, Scenario.
- PlanningConfig是定义在planning_config.proto中, 由planning_config.pb.txt文件配置。
- ScenarioManager是一个类，有Init, mutable_scenario, Update三个共有的成员函数和RegisterScenarios, DecideCurrentScenario两个私有的成员函数。
- common::util::Factory<ScenarioConfig::ScenarioType, Scenario>
scenario_factory_工厂成员变量。 - std::unique_ptr
scenario_; 一个Scenario独享指针。
- common::util::Factory<ScenarioConfig::ScenarioType, Scenario>
- ScenarioManager是一个单例模式, 可以获得scenario的分析结果, 有两个私有的成员变量FeatureExtractor和ScenarioAnalyzer。
- FeatureExtractor是一个为场景分析提供专有特征的类
- 里面有一个ADCTrajectoryContainer私有类，PoseContainer类和一个ScenarioFeature类。
- ADCTrajectoryContainer是车辆轨迹的容器, 继承于Container, 里面有planning::ADCTrajectory(车的轨迹点), common::math::Polygon2d(二维的多边形, 用于连接(junction)), std::shared_ptr
共享指针用于连接, 一个浮点的连接距离, std::unordered_setstd::string一个hashtable的集合用于车道线的indexes， 一个std::vectorstd::string用于保存车道线的序号， 还有一个std::mutex互斥量用于保存轨迹点。
- ADCTrajectoryContainer是车辆轨迹的容器, 继承于Container, 里面有planning::ADCTrajectory(车的轨迹点), common::math::Polygon2d(二维的多边形, 用于连接(junction)), std::shared_ptr
- 里面有一个ADCTrajectoryContainer私有类，PoseContainer类和一个ScenarioFeature类。
- FeatureExtractor是一个为场景分析提供专有特征的类
- Scenario类中主要有Init、Process和Transfer这三个接口函数。
- PlannerWithReferenceLine继承Planner的后抽象一个接口PlanOnReferenceLine， 主要有三个参数const common::TrajectoryPoint& planning_init_point, Frame* frame, ReferenceLineInfo* reference_line_info。
- Frame包含了做一次planning的一帧数据(Frame)。
- EM-planning中的实现代码其实很少， 就一个Init函数和一个Plan函数；
- 在Init函数中主要做的事情是获得PlanningConfig的配置参数， 和初始化场景管理器(scenario_ = scenario_factory_.CreateObject(ScenarioConfig::LANE_FOLLOW)😉 -- 其实就是创建一个LANE_FOLLOW的场景。
- Plan主要做的事情是scenario_manager更新Frame, 然后获得scenario, 再初始化这个scenario的一些参数， 最后从开始点处理scenario。
- ScenarioManager::Update中新建一个场景;
- DecideCurrentScenario函数中会根据TrajectoryPoint和Frame两个参数做Transfer(其实就是直接返回ScenarioConfig::LANE_FOLLOW)。
- mutable_scenario就是直接返回scenario的原始指针，因为是调用的scenario_.get();而scenario_是一个std::unique_ptr
指针, 到最后只有一个指针指向Scenario对象。 - Scenario::Init是一个纯虚函数，就是一个接口， 最终会调用到LaneFollowScenario::Init这个函数；
- 会注册一些task, 包括DP的路径优化器，路径决策器，DP的速度优化器，速度决策器， QP的速度优化器，多项式的速度优化器。
- scenario_->Process(planning_start_point, frame);情景的处理
- 最后其实还是调用LaneFollowScenario::Process， 在Process中调用PlanOnReferenceLine；然后调用各个优化器的Execute函数。
- 在Process中最后通过SetDrivable函数把reference_line_info设置为可执行的。
- ScenarioManager::Update中新建一个场景;
边看边记之杂谈
- ::google::protobuf::Message& message都是利用容器Container或其派生类PoseContainer、ADCTrajectoryContainer等。
- LaneInfo类封装了一些车道相关的信息， 在hdmap_common.cc和hdmap_common.h中被定义和声明一些接口。
- 各种类的继承关系图。
一些专属的数据结构
- 什么是ADCTrajectory?
- ADCTrajectory的字面意思地自动驾驶车的轨迹, 在planning.proto中定义;
- 主要有总路径的长度(total_path_length), 单位是米meters;
- 总路径的时间(total_path_time)， 单位是seconds;
- 重复的轨迹点TrajectoryPoint，TrajectoryPoint中主要包含的有(PathPoint，线性的速度v，线性的加速度a，相对时间relative_time(从轨迹的开始的相对时间))。
- TrajectoryPoint包含PathPoint和一些速度、加速度和时间相关的信息。
- PathPoint主要包含了一些坐标、曲率、path开始的距离、lane的id, x方向的导数和y方向的导数。
- 是否有紧急停车EStop;
- 路径点PathPoint(没有速度信息)。
- 是否重新规划is_replan；
- 具体的档位位置GearPosition;
- planning的决策结果DecisionResult，主要包含了主要决策(MainDecision)、目标决策(ObjectDecisions)和车辆信号(VehicleSignal);
- MainDecision主要包含： 当前的任务是那个(MainCruise, MainStop, MainEmergencyStop, MainChangeLane, MainMissionComplete, MainNotReady, MainParking)和重复的TargetLane;
- TargetLane主要包括了lane的id, 起点和终点限制的速度(speed_limit)。
- ObjectDecisions就是多个ObjectDecision, 每个ObjectDecision中包含了一个字符串的id, 感知目标的id和一个目标的决策类型ObjectDecisionType；
- ObjectDecisionType主要有几种类型: 忽略， 停止， 跟随， 避让， 超车， 绕行， 从旁边超过， avoid听让；
- MainDecision主要包含： 当前的任务是那个(MainCruise, MainStop, MainEmergencyStop, MainChangeLane, MainMissionComplete, MainNotReady, MainParking)和重复的TargetLane;
- LatencyStats(潜在的状态): 主要包括了总时间(total_time_ms), 任务的状态(task_stats)和初始frame的时间init_frame_time_ms;
- 其中task_stats主要包括了任务的名字和运行时间(time_ms);
- routing相关的航向角和debug的控制选项;
- ADCPathPoint(车辆路径的点) -- 主要包括x,y,z的三个坐标，曲率(curvature)和航向角(heading， 相对于绝对坐标系来说的)。
- ADCTrajectoryPoint(Deprecated: replaced by apollo.common.TrajectoryPoint), 里面主要是一个些坐标， 速度， 加速度， 曲率， 曲率的变化率， 相对时间， theta值， 从第一个点开始的距离， sl坐标系中的位置。
- 车辆的信号VehicleSignal， 主要包括(转向信号TurnSignal), 远光灯(high_beam), 近光灯(low_beam), 喇叭(horn)和应急灯(emergency_light)。
- 道路右边的状态；
- 沿车道中线参考线的lane ID;
- 根据当前的计划结果设置engage的建议; -- 这是一个遇到critical时才给予的运行时建议。
- 致命区域(CriticalRegion)。
- 轨迹的类型(TrajectoryType) -- 不知道是什么类型， 正常的类型， 路径反馈的类型， 速度反馈的类型;
- ADCTrajectory的字面意思地自动驾驶车的轨迹, 在planning.proto中定义;
- ScenarioFeature，是在scenario_feature.proto文件重定义的；
- 主要包括了速度，加速度，航向角， 当前lane的id, lane被走过的距离, 左边相邻的laneid和lanes, 右边相邻的laneid和lanes, 连接处的距离和id， 障碍物的id。
