autoware.auto泊车规划模块介绍(二)

autoware.auto泊车规划模块介绍(二)

1.简介：

本节介绍autoware.auto泊车规划模块的基本流程，可以与前一节内容【autoware.auto泊车规划模块介绍(一)】进行联合学习。autoware.auto的泊车规划包含两个文件夹【parking_planner和parking_planner_node】。其中parking_planner_node负责节点通讯、可视化以及parking_planner的ros封装等，parking_planner为主要的泊车规划代码。

2. 泊车规划流程

parking_planner_node.hpp
一、init函数：
1、规划器的初始化【车辆模型 非线性优化中权重系数 状态量的低约束 状态量的上界约束 输入的下界约束 输入的上界约束】 [parking_planner.hpp]
二、plan_trajectory函数：
convert_drivable_area_to_obstacles提取可行驶区域障碍物【Polytope2D向量格式】
1、m_planner-》plan进行轨迹规划【起始点，目标点、障碍物】
[parking_planner.hpp]
【【其余功能：包括launch文件数据加载、高精地图请求、障碍物及车辆始末状态发布、轨迹发布】】

一．1 parking_planner.hpp
规划器的初始化
1、m_astar_planner构建a*默认构造函数，无输入数据[astar_path_planner.hpp]
2、m_nlp_planner输入代价权重、车辆状态上下边界、车辆控制上下边界[nlp_path_planner.hpp]
3、m_model_parameters：自行车模型参数类的初始化,输入自行车模型参数类[bicycle_model.hpp]
二．1 parking_planner.hpp
ParkingPlanner::plan：
简介：轨迹规划，输入当前车辆状态 目标点车辆状态 静态障碍物列表，输出PlanningResult类[parking_planner.hpp]【轨迹、nlp迭代次数、nlp处理时间、规划结果状态】
流程：
1、将目标点的朝向进行重写，以减少当前点与目标点的差值，减少车辆最终规划轨迹旋转情况。
2、m_astar_planner.plan_astar：输入：当前状态、修正后的目标点状态、车辆包围框、障碍物；输出：规划成功则返回路径
[astar_path_planner.hpp]
3、create_trajectory_from_states函数：输出：全轨迹【车辆状态、车辆控制】，作为nlp求解器的初值
4、m_nlp_planner.plan_nlp：输入：车辆起始状态、车辆目标状态、轨迹初值【可以非动力学可行】、障碍物、车辆的自行车模型。输出：最终轨迹[nlp_path_planner.hpp]
5、m_nlp_planner.check_trajectory：输入：将要被检查的轨迹、轨迹初始状态、轨迹终止状态、障碍物列表、车辆模型、轨迹检测的容忍度。输出：轨迹达到要求返回ok.[nlp_path_planner.hpp]
6、轨迹返回

二．1.2
astar_path_planner.hpp
plan_astar函数：
输入：车辆起始状态、车辆的终止状态、用于碰撞检测的车轮廓框图、障碍物的包围盒。
输出：规划成功则返回路径，失败则返回仅包含当前状态的路径，路径长度为1.
流程：与传统的A*搜索过程类似，区别在于后继节点的扩展【父节点共有六个子节点，沿着车头朝向前进后退步长分辨率：在前后位置车辆朝向为目前朝向、左移一个角度分辨率、右移一个角度分辨率】

二．1.4
nlp_path_planner.hpp
plan_nlp函数；
输入：车辆起始状态、车辆目标状态、轨迹初值【可以非动力学可行】、障碍物、车辆的自行车模型。
输出：最终轨迹。m_solve_info用于确定求解器误差。但是最终的轨迹会被再次进行约束条件的检查。
流程：
1、assemble_parameter_vector函数将nlp求解器的输入转换为一个向量【输入：当前状态、目标状态、自行车模型、障碍物物理参数、权重】
2、create_obstacles_from_polyhedra函数得到nlp求解器中NLPObstacle向量【10个，不足10个则建立虚拟障碍物】.包括每个障碍物的状态变量和物理参数变量。
3、assemble_variable_vector_and_bounds函数根据输入进行目标函数待优化变量整合：车辆状态、车辆控制、障碍物状态变量并设定待优化变量的上界和下界
4、casadi::nlpsol()构建nlp求解器
5、nlp求解器求解，返回最终轨迹规划结果【NLPResults
类】。

二．1.5
nlp_path_planner.hpp
check_trajectory函数；
输入：将要被检查的轨迹、轨迹初始状态、轨迹终止状态、障碍物列表、车辆模型、轨迹检测的容忍度
输出：轨迹达到要求返回ok
流程：
针对轨迹的每一个轨迹片，通过check_dynamics检查两轨迹片的距离不满足动力学，通过check_bounds判断车辆状态和车辆控制是否在给定的边界范围内部，通过check_absence_of_collisions车辆给定状态是否与障碍物发生碰撞，通过are_vectors_close判断轨迹的始末状态是否与给定始末状态比较接近

缺陷：
1、generate_nlp_planner_solver.cpp和nlp_path_planner.cpp共享库过程以及ros2中两者编译过程
2、casadi的使用
备注：规划流程按照序号进行对应。仅供自看，水平有限