曲线平滑算法是Planning中一种基础算法，在路径优化、速度优化中都有广泛应用。本文主要研究下Apollo中基于优化方法的离散点平滑算法。
先上效果图。如下图所示，绿色线是待平滑的参考线(实际不会有这种参考线，只是为验证下效果)，通过优化平滑算法，可以得到青色的平滑曲线。
文章图片
红色线为车道中心线，黑色线为道路边界线 1. 离散点曲线平滑的数学原理 如下图所示，
文章图片
， 一共n+1个离散点组成原始参考线。
文章图片
开发者说丨离散点曲线平滑原理中介绍了一种通过对原始参考线上离散点的有限偏移对原始参考线进行平滑的方法，能够将原始参考线(黑色的离散点)转化为平滑的参考线(绿色曲线)。
文中使用的离散点平滑的Cost函数：
Cost分为三个部分：第一部分为平滑度Cost；第二部分为长度Cost；第三部分为相对原始点偏移Cost。
详细的原理可以参考原文，这里主要实现具体的代码实现。
2.离散点平滑转换为二次规划问题
2.1 平滑性Cost 
文章图片
展开上式的x部分：
文章图片
以6个点为例,展开上式:
文章图片
转化为二次型的形式：
文章图片
其中：
文章图片
2.2 长度Cost 
文章图片
以6个点为例,展开上式的x部分:
文章图片
转换为二次型形式：
文章图片
2.3 偏移Cost 
文章图片
以6个点为例,展开上式的x部分:
文章图片
【#|基于优化的离散点平滑算法】转换为二次型形式：
文章图片
2.4 计算P矩阵 
文章图片
因此P矩阵就是把P1，P2，P3三个矩阵加起来，同时增加曲率slack。
其中：
文章图片
2.5 计算Q矩阵 同P矩阵一样，Q矩阵也是把三部分加起来。
文章图片
2.6 计算待优化变量 
文章图片
2.7 约束条件
2.7.1 范围约束
优化时，现在坐标在一定范围内变化。
文章图片
文章图片
2.7.2 曲率约束
文章图片
文章图片
3. 代码实现
3.1. 待优化变量 待优化变量包括n个坐标和n-2个slack,因此共有2 * n + (n-2)个待优化变量。
num_of_points_ = static_cast(ref_points_.size());
num_of_pos_variables_ = num_of_points_ * 2;
num_of_slack_variables_ = num_of_points_ - 2;
num_of_variables_ = num_of_pos_variables_ + num_of_slack_variables_;
num_of_variable_constraints_ = num_of_variables_;
num_of_curvature_constraints_ = num_of_points_ - 2;
num_of_constraints_ =
num_of_variable_constraints_ + num_of_curvature_constraints_;
3.2. P矩阵(Kernel) 根据数值的规律，P矩阵有数值区域可以分为五个部分：第1、2列；第3、4列；倒数第1、2列；倒数第3、4列；中间所有列；
第1、2列：X + Y + Z；之所以有两列，是因为坐标有x和y两个变量。
std::vector>> columns;
columns.resize(num_of_variables_);
for (int col = 0;
col < 2;
++col) {
columns[col].emplace_back(col, weight_fem_pos_deviation_ +
weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
第3、4列：-2X-Y & 5X + 2Y + Z；
for (int col = 2;
col < 4;
++col) {
columns[col].emplace_back(
col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ +
weight_ref_deviation_);
++col_num;
}第5列~第num_of_points_-2列: X & -4X -Y & 6X + 2Y + Z；
int second_point_from_last_index = num_of_points_ - 2;
for (int point_index = 2;
point_index < second_point_from_last_index;
++point_index) {
int col_index = point_index * 2;
for (int col = 0;
col < 2;
++col) {
col_index += col;
columns[col_index].emplace_back(col_index - 4, weight_fem_pos_deviation_);
columns[col_index].emplace_back(
col_index - 2,
-4.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col_index].emplace_back(
col_index, 6.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ + weight_ref_deviation_);
++col_num;
}
}
倒数第3、4列：-2X-Y & 5X + 2Y + Z；
int second_point_col_from_last_col = num_of_pos_variables_ - 4;
int last_point_col_from_last_col = num_of_pos_variables_ - 2;
for (int col = second_point_col_from_last_col;
col < last_point_col_from_last_col;
++col) {
columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
columns[col].emplace_back(
col - 2, -4.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
倒数第1、2列：X + Y + Z；
for (int col = last_point_col_from_last_col;
col < num_of_pos_variables_;
++col) {
columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
columns[col].emplace_back(
col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col].emplace_back(col, weight_fem_pos_deviation_ +
weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
3.3. CSC矩阵处理 CSC是按列存储稀疏矩阵，稍微有点复杂，简单复习一下。
>>> indptr = np.array([0, 2, 3, 6])
>>> indices = np.array([0, 2, 2, 0, 1, 2])
>>> data = https://www.it610.com/article/np.array([1, 2, 3, 4, 5, 6])>>> csc_matrix((data, indices, indptr), shape=(3, 3)).toarray()
array([[1, 0, 4],
[0, 0, 5],
[2, 3, 6]])
其中indptr中的数据代表矩阵中每一列所存储数据在data中的开始和结束的索引。例如，这里indptr为[0, 2, 3, 6]，即表示在data中，索引[0,2)为第一列的数据，索引[2, 3)为第二列的数据，索引[3, 6)为第三列的数据；indices中数据代表对应的data中的数据在其所在列中的所在行数，例如，这里的indices为[0, 2, 2, 0, 1, 2]，表示在data中，数据1在第0行，数据2在第2行，数据3在第2行，数据4在第0行，数据5在1行，数据6在第6行。
int ind_p = 0;
for (int i = 0;
i < num_of_variables_;
++i) {
P_indptr->push_back(ind_p);
for (const auto& row_data_pair : columns[i]) {
// Rescale by 2.0 as the quadratic term in osqp default qp problem setup
// is set as (1/2) * x' * P * x
P_data->push_back(row_data_pair.second * 2.0);
P_indices->push_back(row_data_pair.first);
++ind_p;
}
}
P_indptr->push_back(ind_p);
3.4. Q矩阵
void FemPosDeviationOsqpInterface::CalculateOffset(std::vector* q) {
for (int i = 0;
i < num_of_points_;
++i) {
const auto& ref_point_xy = ref_points_[i];
q->push_back(-2.0 * weight_ref_deviation_ * ref_point_xy.first);
q->push_back(-2.0 * weight_ref_deviation_ * ref_point_xy.second);
}
for (int i = 0;
i < num_of_slack_variables_;
++i) {
(*q)[num_of_pos_variables_ + i] = weight_curvature_constraint_slack_var_;
}
}
3.5. 约束条件 3.5.1. 范围约束
构造A矩阵。
int ind_A = 0;
for (int i = 0;
i < num_of_variables_;
++i) {
A_data->push_back(1.0);
A_indices->push_back(i);
A_indptr->push_back(ind_A);
++ind_A;
}
A_indptr->push_back(ind_A);
构造LB和UB。
lower_bounds->resize(num_of_constraints_);
upper_bounds->resize(num_of_constraints_);
for (int i = 0;
i < num_of_points_;
++i) {
const auto& ref_point_xy = ref_points_[i];
(*upper_bounds)[i * 2] = ref_point_xy.first + bounds_around_refs_[i];
(*upper_bounds)[i * 2 + 1] = ref_point_xy.second + bounds_around_refs_[i];
(*lower_bounds)[i * 2] = ref_point_xy.first - bounds_around_refs_[i];
(*lower_bounds)[i * 2 + 1] = ref_point_xy.second - bounds_around_refs_[i];
}for (int i = 0;
i < num_of_slack_variables_;
++i) {
(*upper_bounds)[num_of_pos_variables_ + i] = 1e20;
(*lower_bounds)[num_of_pos_variables_ + i] = 0.0;
}
3.5.2. 曲率约束
计算A矩阵。
const double scale_factor = 1;
std::vector> lin_cache;
for (int i = 1;
i < num_of_points_ - 1;
++i) {
lin_cache.push_back(CalculateLinearizedFemPosParams(points, i));
}std::vector>> columns;
columns.resize(num_of_variables_);
for (int i = 0;
i < num_of_variables_;
++i) {
columns[i].emplace_back(i, 1.0);
}for (int i = num_of_pos_variables_;
i < num_of_variables_;
++i) {
columns[i].emplace_back(i + num_of_slack_variables_, -1.0 * scale_factor);
}for (int i = 2;
i < num_of_points_;
++i) {
int index = 2 * i;
columns[index].emplace_back(i - 2 + num_of_variables_,
lin_cache[i - 2][4] * scale_factor);
columns[index + 1].emplace_back(i - 2 + num_of_variables_,
lin_cache[i - 2][5] * scale_factor);
}for (int i = 1;
i < num_of_points_ - 1;
++i) {
int index = 2 * i;
columns[index].emplace_back(i - 1 + num_of_variables_,
lin_cache[i - 1][2] * scale_factor);
columns[index + 1].emplace_back(i - 1 + num_of_variables_,
lin_cache[i - 1][3] * scale_factor);
}for (int i = 0;
i < num_of_points_ - 2;
++i) {
int index = 2 * i;
columns[index].emplace_back(i + num_of_variables_,
lin_cache[i][0] * scale_factor);
columns[index + 1].emplace_back(i + num_of_variables_,
lin_cache[i][1] * scale_factor);
}int ind_a = 0;
for (int i = 0;
i < num_of_variables_;
++i) {
A_indptr->push_back(ind_a);
for (const auto& row_data_pair : columns[i]) {
A_data->push_back(row_data_pair.second);
A_indices->push_back(row_data_pair.first);
++ind_a;
}
}
A_indptr->push_back(ind_a);
计算曲率约束：
double interval_sqr = average_interval_length_ * average_interval_length_;
double curvature_constraint_sqr = (interval_sqr * curvature_constraint_) *
(interval_sqr * curvature_constraint_);
for (int i = 0;
i < num_of_curvature_constraints_;
++i) {
(*upper_bounds)[num_of_variable_constraints_ + i] =
(curvature_constraint_sqr - lin_cache[i][6]) * scale_factor;
(*lower_bounds)[num_of_variable_constraints_ + i] = -1e20;
}
3.6. OSQP优化求解 Kernel 和 约束都准备好之后，调用OSQP进行优化求解。
OSQPSettings* settings =
reinterpret_cast(c_malloc(sizeof(OSQPSettings)));
osqp_set_default_settings(settings);
settings->max_iter = max_iter_;
settings->time_limit = time_limit_;
settings->verbose = verbose_;
settings->scaled_termination = scaled_termination_;
settings->warm_start = warm_start_;
settings->polish = true;
settings->eps_abs = 1e-5;
settings->eps_rel = 1e-5;
settings->eps_prim_inf = 1e-5;
settings->eps_dual_inf = 1e-5;
// Define osqp workspace
OSQPWorkspace* work = nullptr;
osqp_setup(&work, data, settings);
// work = osqp_setup(data, settings);
// Initial solution
OptimizeWithOsqp(primal_warm_start, &work);
经过优化求解之后，就得到了文章开头的平滑曲线。
附一：OSQP安装 源码下载路径
https://github.com/osqp/osqp/releases/tag/v0.6.2，选择complete_sources.*下载。
文章图片
代码编译安装
mkdir buildcd buildcmake -D CMAKE_INSTALL_PREFIX=~/code/OSQP/osqp/output ..makemake install
推荐阅读
- #|Ceres计算Bundle Adjustment
- #|汽车中的ECU, VCU, MCU, HCU
- 数据库|谷歌、斯坦福联合发文（我们为什么一定要用大模型（））
- 机器学习|推荐系统，特征学习，协同过滤算法，均值归一化
- C语言经典100题|C语言经典100题——求一个二维数组的鞍点
- 机器学习|关于权重衰退和丢弃法
- 数学建模|预测——马尔可夫链
- Java|SpringMVC 的入门
- 机器学习|【Python】逻辑回归（softmax多分类）