大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
目录
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 start
if (!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 problem
Eigen::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<Ipopt::TNLP> problem = ptop;
// Create an instance of the IpoptApplication
Ipopt::SmartPtr<Ipopt::IpoptApplication> 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 <class T>
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, 2
if (!(VPresentationObstacle(ROI_distance_approach_parking_boundary) &&
HPresentationObstacle())) {
AINFO << "obstacle presentation fails";
}
}AddObstacle()调用VPresentationObstacle()将传入的double数组转换成线段-点的形式，即std::vector<std::vector<Vec2d>> obstacles_vertices_vec_，Vec2d表示顶点， std::vector<Vec2d>表示连续的一组线段，即一个障碍物。这里硬编码为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<Vec2d> 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()负责将障碍物表示成矩阵的形式，这个函数我没有看懂。
6055
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
