大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

目录

1、Hybrid A*和平滑优化的效果对比

2、Optimization Smoothing的调用

3、添加障碍物

1、Hybrid A*和平滑优化的效果对比

如我在之前的博文所述，Baidu Apollo 先使用Hybrid A*算法规划了一条粗糙无碰的轨迹，然后使用优化方法来平滑这条轨迹。我搭建了一个非常简单的停车场景，使用Hybrid A*规划的轨迹和平滑后的轨迹分别如下所示，深绿色为粗糙轨迹，浅绿色为平滑轨迹。单纯看path的形状，有时“平滑”会不太明显，但是看平滑后的速度v、朝向phi、转向角steer、加速度a，会更明显些。（注：图1-2与图3-4并不是对应的，此处仅仅是对比优化前后的效果） 

从图2可以看到，Hybrid A*输出的路径是保障了没有碰撞的，因为在扩展路径的同时做了碰撞检测，而平滑后的路径却发生了碰撞，因为Apollo在优化的目标函数中并没有将“与障碍物的间距”考虑进去，仅仅考虑了曲线平滑。在接下来的优化介绍中，我会更详细的说明这一点。

2、Optimization Smoothing的调用

Open Space Planner的调用入口在 Apollo5.0/modules/planning/open_space/tools/distance_approach_problem_wrapper.cc 文件的DistancePlan()中。

bool DistancePlan(HybridAStar* hybridA_ptr, ObstacleContainer* obstacles_ptr,ResultContainer* result_ptr, double sx, double sy,double sphi, double ex, double ey, double ephi,double* XYbounds) {//载入Open Space Planner的配置文件，设置参数值            //在apollo/modules/planning/conf/planner_open_space_config.pb.txt中...//先调用Hybrid A*产生初步的轨迹作为warm startif (!hybridA_ptr->Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_,obstacles_ptr->GetObstacleVec(),&hybrid_astar_result)) {...}if (FLAGS_enable_parallel_trajectory_smoothing) {    ...} else {...//然后调用DistanceSmoothing()做平滑if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr, sx, sy,sphi, ex, ey, ephi, XYbounds_, &hybrid_astar_result,&state_result_ds, &control_result_ds,&time_result_ds, &dual_l_result_ds,&dual_n_result_ds)) {return false;}...}return true;
}

DistanceSmoothing()设置好状态量和控制量矩阵、初始状态和结束状态矩阵、车辆尺寸矩阵等优化需要的输入信息，然后调用DistanceApproachProblem::Solve()，在该函数内根据不同的方法设置，具体执行不同的优化算法。

bool DistanceSmoothing(...) {// load Warm Start result(horizon is the "N", not the size of step points)size_t horizon_ = hybrid_a_star_result->x.size() - 1;...//若初步的轨迹点较少，则插值if (horizon_ <= 10 && horizon_ > 2 &&planner_open_space_config.enable_linear_interpolation()) {...} else {//用Hybrid A*的输出来初始化状态分量和控制分量...}//状态量和控制量，注意不同的行所对应的不同的含义，顺序很重要Eigen::MatrixXd xWS = Eigen::MatrixXd::Zero(4, horizon_ + 1);Eigen::MatrixXd uWS = Eigen::MatrixXd::Zero(2, horizon_);xWS.row(0) = x;xWS.row(1) = y;xWS.row(2) = phi;xWS.row(3) = v;uWS.row(0) = steer;uWS.row(1) = a;//初始状态Eigen::MatrixXd x0(4, 1);x0 << sx, sy, sphi, 0.0;//结束状态Eigen::MatrixXd xF(4, 1);xF << ex, ey, ephi, 0.0;...//车辆尺寸ego_ << front_to_center, right_to_center, back_to_center, left_to_center;// result for distance approach problemEigen::MatrixXd l_warm_up;Eigen::MatrixXd n_warm_up;DualVariableWarmStartProblem* dual_variable_warm_start_ptr =new DualVariableWarmStartProblem(planner_open_space_config);if (FLAGS_use_dual_variable_warm_start) {bool dual_variable_warm_start_status = dual_variable_warm_start_ptr->Solve(...);} else {l_warm_up = 0.5 * Eigen::MatrixXd::Ones(obstacles.GetObstaclesEdgesNum().sum(), horizon_ + 1);n_warm_up = 0.5 * Eigen::MatrixXd::Ones(4 * obstacles.GetObstaclesNum(),horizon_ + 1);}DistanceApproachProblem* distance_approach_ptr =new DistanceApproachProblem(planner_open_space_config);bool status = distance_approach_ptr->Solve(...);...}return true;
}

DistanceApproachProblem类是个中间层，负责根据conf配置文件，为优化问题匹配不同的优化算法，该类只有一个成员函数Solve()。类似于《Effective C++》系列中提到的“指针与impl”的用法。

bool DistanceApproachProblem::Solve(...) {//DistanceApproachInterface类是所有distance approach的基类DistanceApproachInterface* ptop = nullptr;if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT) {ptop = new DistanceApproachIPOPTInterface(...);} else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_TS) {ptop = new DistanceApproachIPOPTFixedTsInterface(...);} else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_CUDA) {ptop = new DistanceApproachIPOPTCUDAInterface(...);} else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_DUAL) {ptop = new DistanceApproachIPOPTFixedDualInterface(...);} else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_RELAX_END) {ptop = new DistanceApproachIPOPTRelaxEndInterface(...);}//构造IPOPT求解器要求解的目标问题Ipopt::SmartPtr problem = ptop;// Create an instance of the IpoptApplicationIpopt::SmartPtr app = IpoptApplicationFactory();//按照IPOPT的使用方法设定参数...//初始化IPOPT求解器Ipopt::ApplicationReturnStatus status = app->Initialize();//status = app->OptimizeTNLP(problem);...ptop->get_optimization_results(state_result, control_result, time_result,dual_l_result, dual_n_result);...
}

在DistanceApproachProblem::Solve()中定义了一个DistanceApproachInterface类型的指针。DistanceApproachInterface类继承自Ipopt::TNLP虚基类，实现其中的几个虚函数（图5），便可以利用IPOPT来求解优化问题。关于IPOPT的使用，可以参考官方文档，我还没有搞懂，后续可能会补上一篇。

继承自DistanceApproachInterface类的几个distance approach，便对上述函数进行了重写实现。如图6、7所示，虽然这个文件夹内的文件和内容很多，但是不用吓到，其实逻辑结构非常简单和清晰。 

我们以DistanceApproachIPOPTInterface类为例，看一下其目标函数的设置。我们的目标是在Hybrid A*的轨迹基础上，计算一条平滑、无碰的轨迹。很自然的，目标函数应该至少涵盖这2方面。根据图5，eval_f()负责目标函数的设置，而其中，并没有考虑无碰，那么就出现了图2的情况，说明Apollo并没有开源停车入库轨迹规划的全部细节，这里可以参考Stanford的论文，利用Voronoi Diagram计算到障碍物的距离。

bool DistanceApproachIPOPTInterface::eval_f(int n, const double* x, bool new_x,double& obj_value) {eval_obj(n, x, &obj_value);return true;
}

template 
void DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) {// Objective is :// min control inputs// min input rate// min time (if the time step is not fixed)// regularization wrt warm start trajectory//此处注释已表明，没有考虑远离障碍物，即无碰...//obj_value即总的cost值，目标就是 min(obj_value)*obj_value = 0.0;// 1. objective to minimize state diff to warm up...// 2. objective to minimize u square...// 3. objective to minimize input change rate for first horizon...// 4. objective to minimize input change rates, [0- horizon_ -2]...// 5. objective to minimize total time [0, horizon_]...
}

3、添加障碍物

Open Space Planner的实际调用发生在 Apollo/modules/tools/open_space_visualization/distance_approach_visualizer.py 文件中，该文件定义了停车入库的起点和终点，并设置了几个“线条”障碍物，形状和图1非常相似。这里添加障碍物的代码非常离奇，难以复用，难以理解，估计是偷懒了吧。。。

# 以double数组的形式定义障碍物的顶点，对应Python list
# 这里只能看出来有10个点，看不出有几条线段
ROI_distance_approach_parking_boundary = (c_double * 20)(*[-13.6407054776, ... 5.61797800844])
# 在Python文件中使用编译好的Open Space Planner C++库
OpenSpacePlanner.AddObstacle(ROI_distance_approach_parking_boundary)

ObstacleContainer::AddObstacle()在 apollo/modules/planning/open_space/tools/distance_approach_problem_wrapper.cc中，输入参数是所有的障碍物顶点的坐标，单个障碍物的顶点按照顺时针排列，先x后y。

  void AddObstacle(const double* ROI_distance_approach_parking_boundary) {// the obstacles are hard coded into vertice sets of 3, 2, 3, 2if (!(VPresentationObstacle(ROI_distance_approach_parking_boundary) &&HPresentationObstacle())) {AINFO << "obstacle presentation fails";}}

AddObstacle()调用VPresentationObstacle()将传入的double数组转换成线段-点的形式，即std::vector> obstacles_vertices_vec_，Vec2d表示顶点， std::vector表示连续的一组线段，即一个障碍物。这里硬编码为4个障碍物，分别具有2、1、2、1条边，如图8（就是图1的线条拆解）。

bool VPresentationObstacle(const double* ROI_distance_approach_parking_boundary) {//定义了4个障碍物obstacles_num_ = 4;obstacles_edges_num_.resize(4, 1);//第1个障碍物有2条边，第2个障碍物有1条边，以此类推obstacles_edges_num_ << 2, 1, 2, 1;size_t index = 0;for (size_t i = 0; i < obstacles_num_; i++) {std::vector vertices_cw;//获取单个障碍物的所有顶点for (int j = 0; j < obstacles_edges_num_(i, 0) + 1; j++) {//获取单个顶点的x y坐标Vec2d vertice =Vec2d(ROI_distance_approach_parking_boundary[index],ROI_distance_approach_parking_boundary[index + 1]);index += 2;vertices_cw.emplace_back(vertice);}obstacles_vertices_vec_.emplace_back(vertices_cw);}return true;}

HPresentationObstacle()负责将障碍物表示成矩阵的形式，这个函数我没有看懂。

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！