自动驾驶---打造自动驾驶系统之参考线平滑（四）
各位读者朋友，大家好。本次打造的自动驾驶系统仿真系统，涉及感知，预测，规控等多个模块（以规控算法为主，包括Polynomial预测，MCTS决策算法，通行走廊Corridor构建，QP/CILQR轨迹生成求解器，LQR+PID的控制器等），同时也支持其它相关规控/感知算法的扩展（部署&开发自身感兴趣的算法），非常便捷。笔者在该系列中开发的规控算法主要依据专栏《自动驾驶Planning决策规划》中的章节逐步搭建，后续实践系列涉及的博客包括但不局限于以下内容：
《自动驾驶---打造自动驾驶系统之环境准备（一）》---已更新
《自动驾驶---打造自动驾驶系统之定位模块开发（二）》---已更新
《自动驾驶---打造自动驾驶系统之导航模块开发（三）》---已更新
《自动驾驶---打造自动驾驶系统之参考线平滑开发（四）》
《自动驾驶---打造自动驾驶系统之感知环境开发（五）》
《自动驾驶---打造自动驾驶系统之预测模块开发（六）》
《自动驾驶---打造自动驾驶系统之决策模块开发（七）》
《自动驾驶---打造自动驾驶系统之轨迹生成模块开发（八）
《自动驾驶---打造自动驾驶系统之控制模块开发（九）》
最终呈现的静态效果（无法直接贴视频）如下：
1 系统环境
笔者的环境是：Ubuntu 18.04 上安装 ROS Melodic，当然18.04以上的环境也支持。
其它依赖库的安装参考之前的博客《自动驾驶---打造自动驾驶系统之环境准备（一）》，后续如果有遗漏，继续补充。
当然，如果各位读者朋友感兴趣或者在此过程中遇到相关问题，欢迎私信咨询！
2 planner框架
到目前为止，前面两节中已经分别介绍了定位和导航，已经具备开发planner的基础了，因此在开发参考线平滑之前，搭一下planner的框架。
主要包括以下两个部分：
- planner：planner主进程，收发上下游的ros msg，数据预处理等，并且还用于调度不同的planner（后续会扩展多种planner算法）；
- 接受感知、定位、导航以及预测等模块的信息；
- 参考线平滑；
- 决策；
- 通行走廊；
- 轨迹生成；
- 轨迹消息发布；
- task：planner任务管理模块，用于重规划等任务。
3 参考线平滑
关于参考线平滑的知识（二次规划建模，公式推导，约束问题处理等），笔者在之前的博客《自动驾驶---Motion Planning之参考线Path平滑》中作过详细介绍：
总结下来，就是在构造二次规问题的过程中考虑三个代价：（1）轨迹平滑度（2）接近参考点（3）轨迹长度；最后，再考虑约束问题，使用OSQP求解二次规划问题即可。
3.1 模块结构
主体结构主要包括以下两个文件：
其中，使用的是FEM（有限元）平滑方法，求解器使用OSQP。
熟悉Apollo的朋友应该已经看出来了，没错，主要依赖的文件主要来源于Apollo。
3.2 主要代码
#include "reference_line_smooth.h"
#include "../common/log.h"
namespace planning {
ReferenceLineSmooth::ReferenceLineSmooth(const PlannerOpenSpaceConfig& planner_open_space_config){
planner_open_space_config_ = planner_open_space_config;
}
bool ReferenceLineSmooth::SetPathProfile(
const std::vector<std::pair<double, double>>& point2d,
DiscretizedPath* raw_path_points) {
CHECK_NOTNULL(raw_path_points);
raw_path_points->clear();
// Compute path profile
std::vector<double> headings;
std::vector<double> kappas;
std::vector<double> dkappas;
std::vector<double> accumulated_s;
if (!DiscretePointsMath::ComputePathProfile(
point2d, &headings, &accumulated_s, &kappas, &dkappas)) {
return false;
}
CHECK_EQ(point2d.size(), headings.size());
CHECK_EQ(point2d.size(), kappas.size());
CHECK_EQ(point2d.size(), dkappas.size());
CHECK_EQ(point2d.size(), accumulated_s.size());
// Load into path point
size_t points_size = point2d.size();
for (size_t i = 0; i < points_size; ++i) {
common::PathPoint path_point;
path_point.set_x(point2d[i].first);
path_point.set_y(point2d[i].second);
path_point.set_theta(headings[i]);
path_point.set_s(accumulated_s[i]);
path_point.set_kappa(kappas[i]);
path_point.set_dkappa(dkappas[i]);
raw_path_points->push_back(std::move(path_point));
}
return true;
}
size_t ReferenceLineSmooth::FindNearestPoint(const LocalView& local_view){
size_t index = 0;
double pos_x = local_view.localization.odom_x;
double pos_y = local_view.localization.odom_y;
size_t points_size = local_view.route.poses.size();
if(points_size > 1){
double min_distance = std::hypot(pos_x - local_view.route.poses[index].pose.position.x,
pos_y - local_view.route.poses[index].pose.position.y);
for(size_t i = 1; i < points_size; i++){
double distance = std::hypot(pos_x - local_view.route.poses[i].pose.position.x,
pos_y - local_view.route.poses[i].pose.position.y);
if(distance < min_distance ){
index = i;
min_distance = distance;
}
}
}
return index;
}
bool ReferenceLineSmooth::ReferLineSmoothProc(const LocalView& local_view){
size_t nearest_index = FindNearestPoint(local_view);
size_t min_index = 0;
size_t max_index = 0;
if(nearest_index > 10){
min_index = nearest_index - 10;
}
max_index = min_index + 100;
size_t route_size = local_view.route.poses.size();
max_index = std::min(route_size, max_index);
DiscretizedPath warm_start_path;
double accumulated_s = 0.0;
common::math::Vec2d last_path_point(local_view.route.poses[0].pose.position.x,
local_view.route.poses[0].pose.position.y);
for (size_t i = min_index; i < max_index; ++i) {
double x = local_view.route.poses[i].pose.position.x;
double y = local_view.route.poses[i].pose.position.y;
common::math::Vec2d cur_path_point(x, y);
accumulated_s += cur_path_point.DistanceTo(last_path_point);
common::PathPoint path_point;
path_point.set_x(x);
path_point.set_y(y);
path_point.set_theta(0.0);
path_point.set_s(accumulated_s);
warm_start_path.push_back(std::move(path_point));
last_path_point = cur_path_point;
}
std::vector<double> default_bounds(warm_start_path.size(), 2.0);
if(Smooth(warm_start_path, default_bounds)){
return true;
} else{
return false;
}
}
bool ReferenceLineSmooth::Smooth(const DiscretizedPath& raw_path_points,
const std::vector<double>& bounds){
FemPosDeviationSmoother fem_pos_smoother(
planner_open_space_config_.iterative_anchoring_smoother_config()
.fem_pos_deviation_smoother_config());
std::vector<std::pair<double, double>> raw_point2d;
std::vector<double> flexible_bounds;
for (const auto& path_point : raw_path_points) {
raw_point2d.emplace_back(path_point.x(), path_point.y());
}
flexible_bounds = bounds;
std::vector<std::pair<double, double>> smoothed_point2d;
std::vector<double> opt_x;
std::vector<double> opt_y;
if (!fem_pos_smoother.Solve(raw_point2d, flexible_bounds, &opt_x, &opt_y)) {
AINFO << "Smoothing path fails";
return false;
}
if (opt_x.size() < 2 || opt_y.size() < 2) {
AINFO << "Return by fem_pos_smoother is wrong. Size smaller than 2 ";
return false;
}
CHECK_EQ(opt_x.size(), opt_y.size());
size_t point_size = opt_x.size();
smoothed_point2d.clear();
for (size_t i = 0; i < point_size; ++i) {
smoothed_point2d.emplace_back(opt_x[i], opt_y[i]);
}
DiscretizedPath smoothed_path_points;
if (!SetPathProfile(smoothed_point2d, &smoothed_path_points)) {
AINFO << "Set path profile fails";
return false;
}
smoothed_path_points_ = smoothed_path_points;
return true;
}
} // namespace planning3.3 呈现效果
通过参考线平滑后，最终呈现的效果如下图所示：
4 量产
平滑之后的参考线，只有trajectory point的信息，比如 x ，y，s，theta，kappa等信息，对于正常的仿真已经足够，但如果在量产中是不够的，其中主要缺少的是左右边界的宽度及属性，为后续corridor的生成做一点点准备，因此在量产中，每一个参考线上的点还会包括左右边界信息（通常这些边界信息来源于地图和感知）：
- left_bound_type（车道线实线，车道线虚线，路沿等），left_bound_width（车道宽度）；
- right_bound_type，right_bound_width；
