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
• 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轴相对位置。
    • 动态规划选择成本最少的路径。

解析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独享指针。
  • ScenarioManager是一个单例模式, 可以获得scenario的分析结果, 有两个私有的成员变量FeatureExtractor和ScenarioAnalyzer。
    • FeatureExtractor是一个为场景分析提供专有特征的类
      • 里面有一个ADCTrajectoryContainer私有类，PoseContainer类和一个ScenarioFeature类。
        • ADCTrajectoryContainer是车辆轨迹的容器, 继承于Container, 里面有planning::ADCTrajectory(车的轨迹点), common::math::Polygon2d(二维的多边形, 用于连接(junction)), std::shared_ptr共享指针用于连接, 一个浮点的连接距离, std::unordered_set一个hashtable的集合用于车道线的indexes， 一个std::vector用于保存车道线的序号， 还有一个std::mutex互斥量用于保存轨迹点。
  • 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设置为可执行的。

边看边记之杂谈

• ::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听让；
    • 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) -- 不知道是什么类型， 正常的类型， 路径反馈的类型， 速度反馈的类型;
• ScenarioFeature，是在scenario_feature.proto文件重定义的；
  • 主要包括了速度，加速度，航向角， 当前lane的id, lane被走过的距离, 左边相邻的laneid和lanes, 右边相邻的laneid和lanes, 连接处的距离和id， 障碍物的id。