【规划】Apollo QSQP接口详解
Apollo OSQP接口详解
Created by Lemon
Date: 17/6/2021
参考资料：
https://zhuanlan.zhihu.com/p/325645742
1. OSQP简介
OSQP是一个凸二次规划问题的求解器，其解决的问题形如：
m i n i m i z e 1 2 x T P x + q T x s u b j e c t t o l ≤ A x ≤ u \begin{aligned} &minimize \qquad \frac{1}{2}x^TPx+q^Tx \\ &subject \ to \qquad l \le Ax \le u \end{aligned} minimize21xTPx+qTxsubject tol≤Ax≤u
其中 x x x是需要优化的变量，且 P ∈ S + n P \in \mathrm{S}^n_+ P∈S+n为半正定矩阵。
一般的，半正定矩阵指有矩阵P，使得对于非零向量x，有 x T P x ≥ 0 x^TPx \ge 0 xTPx≥0。或者P为Hermitian矩阵且所有特征值均非负。
在二次规划问题中，可通过系数矩阵是否半正定判断该问题是否是凸问题。当系数矩阵为半正定时该二次规划问题为凸二次规划，可求出全局最优解；当系数矩阵不为半正定矩阵时，该问题非凸，不一定可求出全局最优解（求解非凸问题的全局最优解为np-hard），需要通过限制条件获得局部最优解（在apollo内使用最大循环次数max_iter，其默认值为4000）。
关于凸二次规划和系数矩阵的关系:
In formal terms, the question of whether a quadratic objective function is convex or concave is equivalent to whether the matrix Q is positive semi-definite or negative semi-definite.
在Apollo中，使用OSQP进行速度规划和路径规划时系数矩阵P不是半正定矩阵，也就解释了速度、路径规划有时得不出最优解的现象。
2. 接口结构
Apollo为OSQP的使用提供了一个接口，并存放在modules/planning/math/piecewise_jerk/目录下。接口用于将速度规划、路径规划的问题转化成OSQP可以求解的形式。
PiecewiseJerkProblem是该接口的基类，PiecewiseJerkSpeedProblem和PiecewiseJerkPathProblem是其子类，并分别用来处理速度规划、路径规划问题。
在PiecewiseJerkProblem中实现了：
PiecewiseJerkProblem的构造函数，包括点的个数（size_t num_of_knots）、时间/距离间隔（double delta_s）和初始状态（arrayFormulateProblem()：将速度规划、路径规划问题转换成OSQP可求解形式的接口Optimize()：利用OSQP求解的主要逻辑CalculateAffineConstraint()：将速度规划、路径规划约束转化为约束矩阵A的压缩形式的接口SolverDefaultSettings()：一些默认参数的设置接口，包括：- polish----polish ADMM solution
- verbose----boolean, write out progress
- scaled_termination----boolean, use scaled termination criteria
set_x_ref()：x参考值x_ref的设置接口- x、dx、ddx上下限设置的接口以及末状态约束条件的设置接口
在PiecewiseJerkSpeedProblem和PiecewiseJerkPathProblem中均实现了：
- 构造函数的override
CalculateKernel()：目标函数二次项系数转化成系数矩阵P的压缩形式的接口CalculateOffset()：目标函数一次项系数转化成偏移量系数矩阵q的压缩形式的接口SolverDefaultSettings()：一些默认配置的参数接口
在PiecewiseJerkSpeedProblem额外实现的接口：
set_dx_ref()：速度参考值dx_ref的设置接口set_penalty_dx()：速度二次项的惩罚系数设置接口
3. OSQP在速度规划中的应用
在此对调用OSQP进行速度规划的流程、目标函数、约束条件转化为二次规划可求解形式的细节进行介绍。
3.1 速度规划流程
以piecewise_jerk_speed_nonlinear_optimizer为例：
其中FormulateProblem()内部：
3.2 代码解析
主要对三个矩阵计算接口
CalculateKernel(),CalculateAffineConstraint(),CalculateOffset()以及稀疏矩阵的转换方法csc_matrix()做详细介绍。
3.2.1 CalculateKernel()
3.2.1.1 源码
void PiecewiseJerkSpeedProblem::CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int n = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * n;
const int kNumValue = 4 * n - 1;
// columes为？*？*2矩阵
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
// 将columes矩阵resize为kNumParam*?*2大小
columns.resize(kNumParam);
int value_index = 0;
// x(i)^2 * w_x_ref
// 将columes的[0,0,:]到[0,n-2,:]填入i和weight_x_ref_ / (scale_factor_[0] * scale_factor_[0])
// 在1.2.3情况中weight_x_ref_ / (scale_factor_[0] * scale_factor_[0]) = 1/1.0*1.0 = 1
for (int i = 0; i < n - 1; ++i) {
columns[i].emplace_back(
i, weight_x_ref_ / (scale_factor_[0] * scale_factor_[0]));
++value_index;
}
// x(n-1)^2 * (w_x_ref + w_end_x)
// 将columes的[0,n-1,:]填入n-1和(weight_x_ref_ + weight_end_state_[0])/(scale_factor_[0] * scale_factor_[0])
// 在1.2.3情况中weight_end_state_ = {{0.0, 0.0, 0.0}}，为默认值，则
// (weight_x_ref_ + weight_end_state_[0])/(scale_factor_[0] * scale_factor_[0]) = 1
columns[n - 1].emplace_back(n - 1, (weight_x_ref_ + weight_end_state_[0]) /
(scale_factor_[0] * scale_factor_[0]));
++value_index;
// x(i)'^2 * (w_dx_ref + penalty_dx)
// 将columes的[0,n,:]到[0,2*n-2,:]填入n+i和
// (weight_dx_ref_ + penalty_dx_[i]) /(scale_factor_[1] * scale_factor_[1])
// 在1.2.3情况中weight_dx_ref未设置，默认值为0.0，penalty_dx_为大小为num_of_knots_值为0.0的vector（构造函数中penalty_dx_resize(num_of_knots_, 0.0))
// 即(weight_dx_ref_ + penalty_dx_[i]) /(scale_factor_[1] * scale_factor_[1]) = 0
for (int i = 0; i < n - 1; ++i) {
columns[n + i].emplace_back(n + i,
(weight_dx_ref_ + penalty_dx_[i]) /
(scale_factor_[1] * scale_factor_[1]));
++value_index;
}
// x(n-1)'^2 * (w_dx_ref + penalty_dx + w_end_dx)
// 将columes的[0,2*n-1,:]填入2*n-1和
// (weight_dx_ref_ + penalty_dx_[n - 1] + weight_end_state_[1])/(scale_factor_[1] * scale_factor_[1])
// 在1.2.3情况中
// (weight_dx_ref_ + penalty_dx_[n - 1] + weight_end_state_[1])/(scale_factor_[1] * scale_factor_[1]) = 0
columns[2 * n - 1].emplace_back(
2 * n - 1, (weight_dx_ref_ + penalty_dx_[n - 1] + weight_end_state_[1]) /
(scale_factor_[1] * scale_factor_[1]));
++value_index;
// 在1.2.3情况中delta_s_square = 0.1*0.1 = 0.01
auto delta_s_square = delta_s_ * delta_s_;
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
// 将columes的[0,2*n,:]填入2*n和
// (weight_ddx_ + weight_dddx_ / delta_s_square) / (scale_factor_[2] * scale_factor_[2])
// 在1.2.3情况中
// weight_ddx_和weight_ddx_未设置，默认值为0，则
// (weight_ddx_ + weight_dddx_ / delta_s_square) / (scale_factor_[2] * scale_factor_[2]) = 0
columns[2 * n].emplace_back(2 * n,
(weight_ddx_ + weight_dddx_ / delta_s_square) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
// 将columes的[0,2*n+1,:]至[0,3*n-2,:]填入2*n+i和
// (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) / (scale_factor_[2] * scale_factor_[2])
// 在1.2.3情况中，第二项为0
for (int i = 1; i < n - 1; ++i) {
columns[2 * n + i].emplace_back(
2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
}
// 将columes的[0,3*n-1,:]填入3*n-1和
// (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) / (scale_factor_[2] * scale_factor_[2])
// 在1.2.3情况中
// 第二项为0
columns[3 * n - 1].emplace_back(
3 * n - 1,
(weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
// 将columes的[1,2*n,:]到[1,3*n-2,:]填入2*n+i+1和
// -2.0 * weight_dddx_ / delta_s_square / (scale_factor_[2] * scale_factor_[2])
// 在1.2.3情况下第二项为0
for (int i = 0; i < n - 1; ++i) {
columns[2 * n + i].emplace_back(2 * n + i + 1,
-2.0 * weight_dddx_ / delta_s_square /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
}
CHECK_EQ(value_index, kNumValue);
// 将计算得到的结果存入P_indptr、P_data、P_indices
int ind_p = 0;
for (int i = 0; i < kNumParam; ++i) {
P_indptr->push_back(ind_p);
for (const auto& row_data_pair : columns[i]) {
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.2.1.2 压缩格式矩阵columes
由于二次规划问题中P、A矩阵是稀疏的，为方便储存这里采用的是稀疏矩阵存储格式CSC（Compressed Sparse Column Format）。
由于矩阵过于庞大，为方便显示将scale_factor_[n]改写成 s n s_n sn。
c o l u m e s = ( ( 0 , w x − r e f / s 0 2 ) ( 1 , w x − r e f / s 0 2 ) ⋮ ( n − 2 , w x − r e f / s 0 2 ) ( n − 1 , ( w x − r e f + w e n d − s t a t e [ 0 ] ) / s 0 2 ) ( n , ( w d x − r e f + p d x [ 0 ] ) / s 1 2 ) ( n + 1 , ( w d x − r e f + p d x [ 1 ] ) / s 1 2 ) ⋮ ( 2 n − 2 , ( w d x − r e f + p d x [ n − 2 ] ) / s 1 2 ) ( 2 n − 1 , ( w d x − r e f + p d x [ n − 1 ] + w e n d − s t a t e [ 1 ] ) / s 1 2 ) ( 2 n , ( w d d x + w d d d x / ( Δ s ) 2 ) / s 2 2 ) ( 2 n + 1 , ( − 2 w d d d x / ( Δ s ) 2 ) / ( s 2 2 ) ) ( 2 n + 1 , ( w d d x + 2 w d d d x / ( Δ s ) 2 ) / s 2 2 ) ( 2 n + 2 , ( − 2 w d d d x / ( Δ s ) 2 ) / ( s 2 2 ) ) ⋮ ⋮ ( 3 n − 2 , ( w d d x + 2 w d d d x / ( Δ s ) 2 ) / s 2 2 ) ( 3 n − 1 , ( − 2 w d d d x / ( Δ s ) 2 ) / ( s 2 2 ) ) ( 3 n − 1 , ( w d d x + w d d d x / ( Δ s ) 2 + w e n d − s t a t e [ 2 ] ) / s 2 2 ) ) columes= \begin{pmatrix} (0,w_{x-ref}/s_0^2) & \\ (1,w_{x-ref}/s_0^2) & \\ \vdots & \\ (n-2,w_{x-ref}/s_0^2) & \\ (n-1,(w_{x-ref}+w_{end-state}[0])/s_0^2) & \\ (n,(w_{dx-ref}+p_{dx}[0])/s_1^2) & \\ (n+1,(w_{dx-ref}+p_{dx}[1])/s_1^2) & \\ \vdots & \\ (2n-2,(w_{dx-ref}+p_{dx}[n-2])/s_1^2) & \\ (2n-1,(w_{dx-ref}+p_{dx}[n-1]+w_{end-state}[1])/s_1^2) & \\ (2n,(w_{ddx}+w_{dddx}/(\Delta{s})^2)/s_2^2) & (2n+1,(-2w_{dddx}/(\Delta{s})^2)/(s_2^2))\\ (2n+1,(w_{ddx}+2w_{dddx}/(\Delta{s})^2)/s_2^2) & (2n+2,(-2w_{dddx}/(\Delta{s})^2)/(s_2^2))\\ \vdots & \vdots \\ (3n-2,(w_{ddx}+2w_{dddx}/(\Delta{s})^2)/s_2^2) & (3n-1,(-2w_{dddx}/(\Delta{s})^2)/(s_2^2))\\ (3n-1,(w_{ddx}+w_{dddx}/(\Delta{s})^2+w_{end-state}[2])/s_2^2) & \\ \end{pmatrix} columes=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛(0,wx−ref/s02)(1,wx−ref/s02)⋮(n−2,wx−ref/s02)(n−1,(wx−ref+wend−state[0])/s02)(n,(wdx−ref+pdx[0])/s12)(n+1,(wdx−ref+pdx[1])/s12)⋮(2n−2,(wdx−ref+pdx[n−2])/s12)(2n−1,(wdx−ref+pdx[n−1]+wend−state[1])/s12)(2n,(wddx+wdddx/(Δs)2)/s22)(2n+1,(wddx+2wdddx/(Δs)2)/s22)⋮(3n−2,(wddx+2wdddx/(Δs)2)/s22)(3n−1,(wddx+wdddx/(Δs)2+wend−state[2])/s22)(2n+1,(−2wdddx/(Δs)2)/(s22))(2n+2,(−2wdddx/(Δs)2)/(s22))⋮(3n−1,(−2wdddx/(Δs)2)/(s22))⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
columes中每个元素的第一个值标记该值处于稀疏矩阵的第几行，第二个值则为稀疏矩阵中该元素的值。
3.2.1.3 压缩格式矩阵P_indptr
属于
cs.c提供的稀疏矩阵方法csc_matrix()要求提供的一个参数。
P i n d p t r P_{indptr} Pindptr大小为 3 n 3n 3n， P i n d p t r [ n ] P_{indptr}[n] Pindptr[n]元素值为 c o l u m e s columes columes中第n行第一个元素在 c o l u m e s columes columes中的序号（从左到右，从上到下）。位置计算方式，第1行第1列元素位置为0，第1行第2列元素为1，第1行第3列元素为2，第2行第1列元素为3，以此类推。
P i n d p t r [ n ] P_{indptr}[n] Pindptr[n]表示稀疏矩阵的第n列包含从 P d a t a [ P i n d p t r [ n ] ] P_{data}[P_{indptr}[n]] Pdata[Pindptr[n]]开始的元素。
P i n d p t r = ( 0 1 ⋮ 2 n − 1 2 n 2 n + 2 ⋮ 4 n − 2 4 n − 1 ) P_{indptr}= \begin{pmatrix} 0 \\ 1 \\ \vdots \\ 2n-1 \\ \hdashline 2n \\ 2n+2 \\ \vdots \\ 4n-2 \\ \hdashline 4n-1 \\ \end{pmatrix} Pindptr=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛01⋮2n−12n2n+2⋮4n−24n−1⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
其实从columes中已经可以获取足够的行、列位置信息，并可以将columes转换成稀疏矩阵（即columes中第k行的元素处于稀疏矩阵中的第k列，columes元素的第一个值表明对应值在稀疏矩阵中所处的行数）。但是由于cs.c提供的稀疏矩阵方法csc_matrix()要求提供一个参数p（Vector of column pointers），所以这里必须计算 P i n d p t r P_{indptr} Pindptr，这一点在Apollo源码中也进行了解释：
// We indeed need this line because of
// https://github.com/oxfordcontrol/osqp/blob/master/src/cs.c#L255
A_indptr->push_back(ind_p);
3.2.1.4 压缩格式矩阵P_data
也属于
cs.c提供的稀疏矩阵方法csc_matrix()要求提供的一个参数。
将columes中的元素的第二个值从左到右、从上到下压入std::vector中：
p d a t a = 2 ( w x − r e f / s 0 2 w x − r e f / s 0 2 ⋮ w x − r e f / s 0 2 ( w x − r e f + w e n d − s t a t e [ 0 ] ) / s 0 2 ( w d x − r e f + p d x [ 0 ] ) / s 1 2 ( w d x − r e f + p d x [ 1 ] ) / s 1 2 ⋮ ( w d x − r e f + p d x [ n − 2 ] ) / s 1 2 ( w d x − r e f + p d x [ n − 1 ] + w e n d − s t a t e [ 1 ] ) / s 1 2 ( w d d x + w d d d x / ( Δ s ) 2 ) / s 2 2 ( − 2 w d d d x / ( Δ s ) 2 ) / ( s 2 2 ) ( w d d x + 2 w d d d x / ( Δ s ) 2 ) / s 2 2 ( − 2 w d d d x / ( Δ s ) 2 ) / ( s 2 2 ) ⋮ ( w d d x + 2 w d d d x / ( Δ s ) 2 ) / s 2 2 ( − 2 w d d d x / ( Δ s ) 2 ) / ( s 2 2 ) ( w d d x + w d d d x / ( Δ s ) 2 + w e n d − s t a t e [ 2 ] ) / s 2 2 ) p_{data}=2 \begin{pmatrix} w_{x-ref}/s_0^2 \\ w_{x-ref}/s_0^2 \\ \vdots \\ w_{x-ref}/s_0^2 \\ (w_{x-ref}+w_{end-state}[0])/s_0^2 \\ (w_{dx-ref}+p_{dx}[0])/s_1^2 \\ (w_{dx-ref}+p_{dx}[1])/s_1^2 \\ \vdots & \\ (w_{dx-ref}+p_{dx}[n-2])/s_1^2 \\ (w_{dx-ref}+p_{dx}[n-1]+w_{end-state}[1])/s_1^2 \\ (w_{ddx}+w_{dddx}/(\Delta{s})^2)/s_2^2 \\ (-2w_{dddx}/(\Delta{s})^2)/(s_2^2) \\ (w_{ddx}+2w_{dddx}/(\Delta{s})^2)/s_2^2 \\ (-2w_{dddx}/(\Delta{s})^2)/(s_2^2) \\ \vdots \\ (w_{ddx}+2w_{dddx}/(\Delta{s})^2)/s_2^2 \\ (-2w_{dddx}/(\Delta{s})^2)/(s_2^2) \\ (w_{ddx}+w_{dddx}/(\Delta{s})^2+w_{end-state}[2])/s_2^2 \\ \end{pmatrix} pdata=2⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛wx−ref/s02wx−ref/s02⋮wx−ref/s02(wx−ref+wend−state[0])/s02(wdx−ref+pdx[0])/s12(wdx−ref+pdx[1])/s12⋮(wdx−ref+pdx[n−2])/s12(wdx−ref+pdx[n−1]+wend−state[1])/s12(wddx+wdddx/(Δs)2)/s22(−2wdddx/(Δs)2)/(s22)(wddx+2wdddx/(Δs)2)/s22(−2wdddx/(Δs)2)/(s22)⋮(wddx+2wdddx/(Δs)2)/s22(−2wdddx/(Δs)2)/(s22)(wddx+wdddx/(Δs)2+wend−state[2])/s22⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
3.2.1.5 压缩格式矩阵P_indices
也属于
cs.c提供的稀疏矩阵方法csc_matrix()要求提供的一个参数。
将columes中的元素的第一个值从左到右、从上到下压入std::vector中：
p i n d i c e s = ( 0 1 ⋮ 2 n − 1 2 n 2 n + 1 2 n + 1 2 n + 2 ⋮ 3 n − 2 3 n − 1 3 n − 1 ) p_{indices}= \begin{pmatrix} 0 \\ 1 \\ \vdots \\ 2n-1 \\ \hdashline 2n \\ 2n+1 \\ 2n+1 \\ 2n+2 \\ \vdots \\ 3n-2 \\ 3n-1 \\ \hdashline 3n-1 \\ \end{pmatrix} pindices=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛01⋮2n−12n2n+12n+12n+2⋮3n−23n−13n−1⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
3.2.2 CalculateAffineConstraint()
3.2.2.1 源码
void PiecewiseJerkProblem::CalculateAffineConstraint(
std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) {
// 总共有6*n个限制条件，其中：
// 3N params bounds on x, x', x''
// 3(N-1) constraints on x, x', x''
// 3 constraints on x_init_
const int n = static_cast<int>(num_of_knots_);
const int num_of_variables = 3 * n;
const int num_of_constraints = num_of_variables + 3 * (n - 1) + 3;
lower_bounds->resize(num_of_constraints);
upper_bounds->resize(num_of_constraints);
std::vector<std::vector<std::pair<c_int, c_float>>> variables(
num_of_variables);
int constraint_index = 0;
// set x, x', x'' bounds
// 0至n-1为x的约束
// n至2*n-1为x'的约束
// 2*n至3*n-1为x''的约束
// x_bounds_、dx_bounds_、ddx_bounds_、scale_factor_在对应的函数中设置
// 在1.2.3情况中，x对应s，x'为ds/dt，x''为d^2s/dt^2
// x约束的上下限为0 * 1.0 = 0和100.0 * 1.0 = 100.0
// x'约束的上下限为0 * 10.0 = 0和max(init_v,31.3m/s) * 10.0
// x''约束的上下限为-6 * 100.0 = -600和2 * 100.0 = 200
for (int i = 0; i < num_of_variables; ++i) {
if (i < n) {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
x_bounds_[i].first * scale_factor_[0];
upper_bounds->at(constraint_index) =
x_bounds_[i].second * scale_factor_[0];
} else if (i < 2 * n) {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
dx_bounds_[i - n].first * scale_factor_[1];
upper_bounds->at(constraint_index) =
dx_bounds_[i - n].second * scale_factor_[1];
} else {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
ddx_bounds_[i - 2 * n].first * scale_factor_[2];
upper_bounds->at(constraint_index) =
ddx_bounds_[i - 2 * n].second * scale_factor_[2];
}
++constraint_index;
}
CHECK_EQ(constraint_index, num_of_variables);
// x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s
for (int i = 0; i + 1 < n; ++i) {
variables[2 * n + i].emplace_back(constraint_index, -1.0);
variables[2 * n + i + 1].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
dddx_bound_.first * delta_s_ * scale_factor_[2];
upper_bounds->at(constraint_index) =
dddx_bound_.second * delta_s_ * scale_factor_[2];
++constraint_index;
}
// x(i+1)' - x(i)' - 0.5 * delta_s * x(i)'' - 0.5 * delta_s * x(i+1)'' = 0
for (int i = 0; i + 1 < n; ++i) {
variables[n + i].emplace_back(constraint_index, -1.0 * scale_factor_[2]);
variables[n + i + 1].emplace_back(constraint_index, 1.0 * scale_factor_[2]);
variables[2 * n + i].emplace_back(constraint_index,
-0.5 * delta_s_ * scale_factor_[1]);
variables[2 * n + i + 1].emplace_back(constraint_index,
-0.5 * delta_s_ * scale_factor_[1]);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// x(i+1) - x(i) - delta_s * x(i)'
// - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)''
auto delta_s_sq_ = delta_s_ * delta_s_;
for (int i = 0; i + 1 < n; ++i) {
variables[i].emplace_back(constraint_index,
-1.0 * scale_factor_[1] * scale_factor_[2]);
variables[i + 1].emplace_back(constraint_index,
1.0 * scale_factor_[1] * scale_factor_[2]);
variables[n + i].emplace_back(
constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]);
variables[2 * n + i].emplace_back(
constraint_index,
-delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]);
variables[2 * n + i + 1].emplace_back(
constraint_index,
-delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// constrain on x_init
variables[0].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
++constraint_index;
variables[n].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
++constraint_index;
variables[2 * n].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
++constraint_index;
CHECK_EQ(constraint_index, num_of_constraints);
int ind_p = 0;
for (int i = 0; i < num_of_variables; ++i) {
A_indptr->push_back(ind_p);
for (const auto& variable_nz : variables[i]) {
// coefficient
A_data->push_back(variable_nz.second);
// constraint index
A_indices->push_back(variable_nz.first);
++ind_p;
}
}
// We indeed need this line because of
// https://github.com/oxfordcontrol/osqp/blob/master/src/cs.c#L255
A_indptr->push_back(ind_p);
}
3.2.2.2 压缩格式矩阵variables
v a r i a b l e s = ( ( 0 , 1 ) ( 5 n − 2 , − s 1 s 2 ) ( 6 n − 3 , 1 ) ( 1 , 1 ) ( 5 n − 2 , s 1 s 2 ) ( 5 n − 1 , − s 1 s 2 ) ( 2 , 1 ) ( 5 n − 1 , s 1 s 2 ) ( 5 n , − s 1 s 2 ) ⋮ ⋮ ⋮ ( n − 2 , 1 ) ( 6 n − 5 , s 1 s 2 ) ( 6 n − 4 , − s 1 s 2 ) ( n − 1 , 1 ) ( 6 n − 4 , s 1 s 2 ) ( n , 1 ) ( 4 n − 1 , − s 2 ) ( 5 n − 2 , − Δ s s 0 s 2 ) ( 6 n − 2 , 1 ) ( n + 1 , 1 ) ( 4 n − 1 , s 2 ) ( 4 n , − s 2 ) ( 5 n − 1 , − Δ s s 0 s 2 ) ( n + 2 , 1 ) ( 4 n , s 2 ) ( 4 n + 1 , − s 2 ) ( 5 n , − Δ s s 0 s 2 ) ⋮ ⋮ ⋮ ⋮ ( 2 n − 2 , 1 ) ( 5 n − 4 , s 2 ) ( 5 n − 3 , − s 2 ) ( 6 n − 4 , − Δ s s 0 s 2 ) ( 2 n − 1 , 1 ) ( 5 n − 3 , s 2 ) ( 2 n , 1 ) ( 3 n , − 1 ) ( 4 n − 1 , − 0.5 Δ s s 1 ) ( 5 n − 2 , − ( Δ s ) 2 3 s 0 s 1 ) ( 6 n − 1 , 1 ) ( 2 n + 1 , 1 ) ( 3 n , 1 ) ( 3 n + 1 , − 1 ) ( 4 n − 1 , − 0.5 Δ s s 1 ) ( 4 n , − 0.5 Δ s s 1 ) ( 5 n − 2 , − ( Δ s ) 2 6 s 0 s 1 ) ( 5 n − 1 , − ( Δ s ) 2 3 s 0 s 1 ) ( 2 n + 2 , 1 ) ( 3 n + 1 , 1 ) ( 3 n + 2 , − 1 ) ( 4 n , − 0.5 Δ s s 1 ) ( 4 n + 1 , − 0.5 Δ s s 1 ) ( 5 n − 1 , − ( Δ s ) 2 6 s 0 s 1 ) ( 5 n , − ( Δ s ) 2 3 s 0 s 1 ) ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ( 3 n − 2 , 1 ) ( 4 n − 3 , 1 ) ( 4 n − 2 , − 1 ) ( 5 n − 4 , − 0.5 Δ s s 1 ) ( 5 n − 3 , − 0.5 Δ s s 1 ) ( 6 n − 5 , − ( Δ s ) 2 6 s 0 s 1 ) ( 6 n − 4 , − ( Δ s ) 2 3 s 0 s 1 ) ( 3 n − 1 , 1 ) ( 4 n − 2 , 1 ) ( 5 n − 3 , − 0.5 Δ s s 1 ) ( 6 n − 4 , − ( Δ s ) 2 6 s 0 s 1 ) ) variables= \begin{pmatrix} (0,1) & (5n-2,-s_1s_2) & (6n-3,1) & & & & \\ (1,1) & (5n-2,s_1s_2) & (5n-1,-s_1s_2) & & & & \\ (2,1) & (5n-1,s_1s_2) & (5n,-s_1s_2) & & & & \\ \vdots & \vdots & \vdots & & & & \\ (n-2,1) & (6n-5,s_1s_2) & (6n-4,-s_1s_2) & & & & \\ (n-1,1) & (6n-4,s_1s_2) & & & & & \\ (n,1) & (4n-1,-s_2) & (5n-2,-\Delta{s}s_0s_2) & (6n-2,1) & & & \\ (n+1,1) & (4n-1,s_2) & (4n,-s_2) & (5n-1,-\Delta{s}s_0s_2) & & & \\ (n+2,1) & (4n,s_2) & (4n+1,-s_2) & (5n,-\Delta{s}s_0s_2) & & & \\ \vdots & \vdots & \vdots & \vdots & & & \\ (2n-2,1) & (5n-4,s_2) & (5n-3,-s_2) & (6n-4,-\Delta{s}s_0s_2) & & & \\ (2n-1,1) & (5n-3,s_2) & & & & & \\ (2n,1) & (3n,-1) & (4n-1,-0.5\Delta{s}s_1) & (5n-2,\frac{-(\Delta{s})^2}{3}s_0s_1) & (6n-1,1) & & \\ (2n+1,1) & (3n,1) & (3n+1,-1) & (4n-1,-0.5\Delta{s}s_1) & (4n,-0.5\Delta{s}s_1) & (5n-2,\frac{-(\Delta{s})^2}{6}s_0s_1) & (5n-1,\frac{-(\Delta{s})^2}{3}s_0s_1) \\ (2n+2,1) & (3n+1,1) & (3n+2,-1) & (4n,-0.5\Delta{s}s_1) & (4n+1,-0.5\Delta{s}s_1) & (5n-1,\frac{-(\Delta{s})^2}{6}s_0s_1) & (5n,\frac{-(\Delta{s})^2}{3}s_0s_1) \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ (3n-2,1) & (4n-3,1) & (4n-2,-1) & (5n-4,-0.5\Delta{s}s_1) & (5n-3,-0.5\Delta{s}s_1) & (6n-5,\frac{-(\Delta{s})^2}{6}s_0s_1) & (6n-4,\frac{-(\Delta{s})^2}{3}s_0s_1) \\ (3n-1,1) & (4n-2,1) & (5n-3,-0.5\Delta{s}s_1) & (6n-4,\frac{-(\Delta{s})^2}{6}s_0s_1) & & & \\ \end{pmatrix} variables=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛(0,1)(1,1)(2,1)⋮(n−2,1)(n−1,1)(n,1)(n+1,1)(n+2,1)⋮(2n−2,1)(2n−1,1)(2n,1)(2n+1,1)(2n+2,1)⋮(3n−2,1)(3n−1,1)(5n−2,−s1s2)(5n−2,s1s2)(5n−1,s1s2)⋮(6n−5,s1s2)(6n−4,s1s2)(4n−1,−s2)(4n−1,s2)(4n,s2)⋮(5n−4,s2)(5n−3,s2)(3n,−1)(3n,1)(3n+1,1)⋮(4n−3,1)(4n−2,1)(6n−3,1)(5n−1,−s1s2)(5n,−s1s2)⋮(6n−4,−s1s2)(5n−2,−Δss0s2)(4n,−s2)(4n+1,−s2)⋮(5n−3,−s2)(4n−1,−0.5Δss1)(3n+1,−1)(3n+2,−1)⋮(4n−2,−1)(5n−3,−0.5Δss1)(6n−2,1)(5n−1,−Δss0s2)(5n,−Δss0s2)⋮(6n−4,−Δss0s2)(5n−2,3−(Δs)2s0s1)(4n−1,−0.5Δss1)(4n,−0.5Δss1)⋮(5n−4,−0.5Δss1)(6n−4,6−(Δs)2s0s1)(6n−1,1)(4n,−0.5Δss1)(4n+1,−0.5Δss1)⋮(5n−3,−0.5Δss1)(5n−2,6−(Δs)2s0s1)(5n−1,6−(Δs)2s0s1)⋮(6n−5,6−(Δs)2s0s1)(5n−1,3−(Δs)2s0s1)(5n,3−(Δs)2s0s1)⋮(6n−4,3−(Δs)2s0s1)⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
3.2.2.3 压缩格式A_data、A_indices
A d a t a A_{data} Adata、 A i n d i c e s A_{indices} Aindices的读取方式与 P d a t a P_{data} Pdata、 P i n d i c e s P_{indices} Pindices的读取方式相同。
3.2.2.4 压缩格式upperbound、lowerbound
限制条件中的上下限upperbound和lowerbound各有6n个元素：
- 0 至n-1号元素限制x上下限
- n至2n-1元素限制dx上下限
- 2n至3n-1元素限制ddx上下限
- 3n至4n-2元素限制dddx
- 4n-1至5n-3上下限为0
- 5n-2至6n-4上下限为0
- 6n-3、6n-2、6n-1元素限制初始状态
l o w e r b o u n d = ( x b o u n d s [ 0 ] [ 0 ] s 0 ⋮ x b o u n d s [ n − 1 ] [ 0 ] s 0 d x b o u n d s [ 0 ] [ 0 ] s 1 ⋮ d x b o u n d s [ n − 1 ] [ 0 ] s 1 d d x b o u n d s [ 0 ] [ 0 ] s 2 ⋮ d d x b o u n d s [ n − 1 ] [ 0 ] s 2 d d d x b o u n d s [ 0 ] Δ s s 2 ⋮ d d d x b o u n d s [ 0 ] Δ s s 2 0 ⋮ 0 0 ⋮ 0 x i n i t [ 0 ] s 0 x i n i t [ 1 ] s 1 x i n i t [ 2 ] s 2 ) lowerbound= \begin{pmatrix} x_{bounds}[0][0]s_0 \\ \vdots \\ x_{bounds}[n-1][0]s_0 \\ \hdashline dx_{bounds}[0][0]s_1 \\ \vdots \\ dx_{bounds}[n-1][0]s_1 \\ \hdashline ddx_{bounds}[0][0]s_2 \\ \vdots \\ ddx_{bounds}[n-1][0]s_2 \\ \hdashline dddx_{bounds}[0]\Delta{s}s_2 \\ \vdots \\ dddx_{bounds}[0]\Delta{s}s_2 \\ \hdashline 0 \\ \vdots \\ 0 \\ \hdashline 0 \\ \vdots \\ 0 \\ \hdashline x_{init}[0]s_0 \\ x_{init}[1]s_1 \\ x_{init}[2]s_2 \\ \end{pmatrix} lowerbound=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛xbounds[0][0]s0⋮xbounds[n−1][0]s0dxbounds[0][0]s1⋮dxbounds[n−1][0]s1ddxbounds[0][0]s2⋮ddxbounds[n−1][0]s2dddxbounds[0]Δss2⋮dddxbounds[0]Δss20⋮00⋮0xinit[0]s0xinit[1]s1xinit[2]s2⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
u p p e r b o u n d = ( x b o u n d s [ 0 ] [ 1 ] s 0 ⋮ x b o u n d s [ n − 1 ] [ 1 ] s 0 d x b o u n d s [ 0 ] [ 1 ] s 1 ⋮ d x b o u n d s [ n − 1 ] [ 1 ] s 1 d d x b o u n d s [ 0 ] [ 1 ] s 2 ⋮ d d x b o u n d s [ n − 1 ] [ 1 ] s 2 d d d x b o u n d s [ 1 ] Δ s s 2 ⋮ d d d x b o u n d s [ 1 ] Δ s s 2 0 ⋮ 0 0 ⋮ 0 x i n i t [ 0 ] s 0 x i n i t [ 1 ] s 1 x i n i t [ 2 ] s 2 ) upperbound= \begin{pmatrix} x_{bounds}[0][1]s_0 \\ \vdots \\ x_{bounds}[n-1][1]s_0 \\ \hdashline dx_{bounds}[0][1]s_1 \\ \vdots \\ dx_{bounds}[n-1][1]s_1 \\ \hdashline ddx_{bounds}[0][1]s_2 \\ \vdots \\ ddx_{bounds}[n-1][1]s_2 \\ \hdashline dddx_{bounds}[1]\Delta{s}s_2 \\ \vdots \\ dddx_{bounds}[1]\Delta{s}s_2 \\ \hdashline 0 \\ \vdots \\ 0 \\ \hdashline 0 \\ \vdots \\ 0 \\ \hdashline x_{init}[0]s_0 \\ x_{init}[1]s_1 \\ x_{init}[2]s_2 \\ \end{pmatrix} upperbound=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛xbounds[0][1]s0⋮xbounds[n−1][1]s0dxbounds[0][1]s1⋮dxbounds[n−1][1]s1ddxbounds[0][1]s2⋮ddxbounds[n−1][1]s2dddxbounds[1]Δss2⋮dddxbounds[1]Δss20⋮00⋮0xinit[0]s0xinit[1]s1xinit[2]s2⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
3.2.3 CalculateOffset()
3.2.3.1 源码
void PiecewiseJerkSpeedProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int n = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * n;
q->resize(kNumParam);
for (int i = 0; i < n; ++i) {
if (has_x_ref_) {
q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0];
}
if (has_dx_ref_) {
q->at(n + i) += -2.0 * weight_dx_ref_ * dx_ref_ / scale_factor_[1];
}
}
if (has_end_state_ref_) {
q->at(n - 1) +=
-2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0];
q->at(2 * n - 1) +=
-2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1];
q->at(3 * n - 1) +=
-2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2];
}
}
3.2.3.2 压缩格式矩阵q
q为二次规划目标函数中的一次项系数矩阵
q中esref指
end_state_ref_，sf指scale_factor_
q = ( − 2 w x − r e f x r e f [ 0 ] / s 0 ⋮ − 2 w x − r e f x r e f [ n − 1 ] / s 0 − 2 w e n d − s t a t e [ 0 ] e s r e f [ 0 ] / s 0 − 2 w d x − r e f d x r e f / s 1 ⋮ − 2 w d x − r e f d x r e f / s 1 − 2 w e n d − s t a t e [ 1 ] e s r e f [ 1 ] / s 1 0 ⋮ 0 0 − 2 w e n d − s t a t e [ 2 ] e s r e f [ 2 ] / s 2 ) q= \begin{pmatrix} -2w_{x-ref}x_{ref}[0]/s_0 \\ \vdots \\ -2w_{x-ref}x_{ref}[n-1]/s_0-2w_{end-state}[0]esref[0]/s_0 \\ \hdashline -2w_{dx-ref}dx_{ref}/s_1 \\ \vdots \\ -2w_{dx-ref}dx_{ref}/s_1-2w_{end-state}[1]esref[1]/s_1 \\ \hdashline 0 \\ \vdots \\ 0 \\ 0-2w_{end-state}[2]esref[2]/s_2 \\ \end{pmatrix} q=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛−2wx−refxref[0]/s0⋮−2wx−refxref[n−1]/s0−2wend−state[0]esref[0]/s0−2wdx−refdxref/s1⋮−2wdx−refdxref/s1−2wend−state[1]esref[1]/s10⋮00−2wend−state[2]esref[2]/s2⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
3.2.4 csc_matrix()
3.2.4.1 源码
/**
* Create Compressed-Column-Sparse matrix from existing arrays
(no MALLOC to create inner arrays x, i, p)
* @param m First dimension
* @param n Second dimension
* @param nzmax Maximum number of nonzero elements
* @param x Vector of data
* @param i Vector of row indices
* @param p Vector of column pointers
* @return New matrix pointer
*/
csc* csc_matrix(c_int m,
c_int n,
c_int nzmax,
c_float *x,
c_int *i,
c_int *p);
3.2.4.2 P矩阵的还原
在速度规划的FormulateProblem()接口中：
data->P = csc_matrix(m = kernel_dim, n = kernel_dim, nzmax = P_data.size(), P_data = CopyData(P_data),
x = CopyData(P_indices), i = CopyData(P_indptr));
csc_matrix()是一种稀疏矩阵的存储方式（Compressed Sparse Column marix），其中：
m、n表示该矩阵的维度，在此为3n*3nnzmax为非零元素的个数，在此为P_data.size()=4n-1P_data为元素值x描述P_data中各元素在矩阵中所在的行i描述P_data中各元素所在的列- 注意
x和i的描述方法有所不同：x的大小为4n-1，与P_data元素个数相同，x中第k个元素的值 x k x_k xk表明P_data中的第k个元素所在行数为 x k x_k xki的大小为3n，与稀疏矩阵列的大小相同，i中第k个元素的值 i k i_k ik和第k+1个元素的值 i k + 1 i_{k+1} ik+1表明P_data中的第 i k i_k ik至第 i k + 1 − 1 i_{k+1}-1 ik+1−1个元素在稀疏矩阵的第k列中。
通过csc_matrix()可以还原稀疏矩阵：
p = 2 ( w x − r e f s 0 2 0 ⋯ 0 0 ⋱ ⋮ ⋮ w x − r e f s 0 2 0 0 ⋯ 0 w x − r e f + w e n d − s t a t e [ 0 ] s 0 2 w d x − r e f + p d x [ 0 ] s 1 2 0 ⋯ 0 0 ⋱ ⋮ ⋮ w d x − r e f + p d x [ n − 2 ] s 1 2 0 0 ⋯ 0 w d x − r e f + p d x [ n − 1 ] + w e n d − s t a t e [ 1 ] s 1 2 w d d x + w d d d x / ( Δ s ) 2 s 2 2 0 ⋯ ⋯ ⋯ 0 − 2 w d d d x / ( Δ s ) 2 s 2 2 w d d x + 2 w d d d x / ( Δ s ) 2 s 2 2 ⋮ 0 − 2 w d d d x / ( Δ s ) 2 s 2 2 ⋱ ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋱ w d d x + 2 w d d d x / ( Δ s ) 2 s 2 2 0 0 ⋯ ⋯ 0 − 2 w d d d x / ( Δ s ) 2 s 2 2 w d d x + w d d d x / ( Δ s ) 2 + w e n d − s t a t e [ 2 ] s 2 2 ) p=2 \begin{pmatrix} \frac{w_{x-ref}}{s_0^2} & 0 & \cdots & 0 \\ 0 &\ddots & & \vdots \\ \vdots & & \frac{w_{x-ref}}{s_0^2} & 0 \\ 0 & \cdots & 0 & \frac{w_{x-ref}+w_{end-state}[0]}{s_0^2} \\ & & & & \frac{w_{dx-ref}+p_{dx}[0]}{s_1^2} & 0 & \cdots & 0 \\ & & & & 0 &\ddots & & \vdots \\ & & & & \vdots & & \frac{w_{dx-ref}+p_{dx}[n-2]}{s_1^2} & 0 \\ & & & & 0 & \cdots & 0 & \frac{w_{dx-ref}+p_{dx}[n-1]+w_{end-state}[1]}{s_1^2} \\ & & & & & & & & \frac{w_{ddx}+w_{dddx}/(\Delta{s})^2}{s_2^2} & 0 & \cdots & \cdots & \cdots & 0 \\ & & & & & & & & \frac{-2w_{dddx}/(\Delta{s})^2}{s_2^2} & \frac{w_{ddx}+2w_{dddx}/(\Delta{s})^2}{s_2^2} & & & & \vdots \\ & & & & & & & & 0 & \frac{-2w_{dddx}/(\Delta{s})^2}{s_2^2} & \ddots & & & \vdots \\ & & & & & & & & \vdots & & \ddots & \ddots & & \vdots \\ & & & & & & & & \vdots & & & \ddots & \frac{w_{ddx}+2w_{dddx}/(\Delta{s})^2}{s_2^2} & 0 \\ & & & & & & & & 0 & \cdots & \cdots & 0 & \frac{-2w_{dddx}/(\Delta{s})^2}{s_2^2} & \frac{w_{ddx}+w_{dddx}/(\Delta{s})^2+w_{end-state}[2]}{s_2^2} \\ \end{pmatrix} p=2⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛s02wx−ref0⋮00⋱⋯⋯s02wx−ref00⋮0s02wx−ref+wend−state[0]s12wdx−ref+pdx[0]0⋮00⋱⋯⋯s12wdx−ref+pdx[n−2]00⋮0s12wdx−ref+pdx[n−1]+wend−state[1]s22wddx+wdddx/(Δs)2s22−2wdddx/(Δs)20⋮⋮00s22wddx+2wdddx/(Δs)2s22−2wdddx/(Δs)2⋯⋯⋱⋱⋯⋯⋱⋱0⋯s22wddx+2wdddx/(Δs)2s22−2wdddx/(Δs)20⋮⋮⋮0s22wddx+wdddx/(Δs)2+wend−state[2]⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞
（可参考https://zhuanlan.zhihu.com/p/325645742）
3.2.4.3 A矩阵的还原
data->A =
csc_matrix(num_affine_constraint, kernel_dim, A_data.size(),
CopyData(A_data), CopyData(A_indices), CopyData(A_indptr));
其中：
- 矩阵大小6n*3n
- 非零元素为14n-8
通过csc_matrix()可还原矩阵:
A = ( 1 0 ⋯ ⋯ ⋯ 0 0 1 ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ 0 0 ⋯ ⋯ ⋯ 0 1 1 0 ⋯ ⋯ ⋯ 0 0 1 ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ 0 0 ⋯ ⋯ ⋯ 0 1 1 0 ⋯ ⋯ ⋯ 0 0 1 ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ 0 0 ⋯ ⋯ ⋯ 0 1 − 1 1 0 ⋯ ⋯ 0 0 − 1 1 ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ 0 0 ⋯ ⋯ 0 − 1 1 − s 2 s 2 0 ⋯ ⋯ 0 − Δ s s 1 2 − Δ s s 1 2 0 ⋯ ⋯ 0 0 − s 2 s 2 ⋮ 0 − Δ s s 1 2 − Δ s s 1 2 ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ 0 ⋮ ⋱ ⋱ 0 0 ⋯ ⋯ 0 − s 2 s 2 0 ⋯ ⋯ 0 − Δ s s 1 2 − Δ s s 1 2 − s 1 s 2 s 1 s 2 0 ⋯ ⋯ 0 − Δ s s 0 s 2 0 ⋯ ⋯ ⋯ 0 − ( Δ s ) 2 s 0 s 1 3 − ( Δ s ) 2 s 0 s 1 6 0 ⋯ ⋯ 0 0 − s 1 s 2 s 1 s 2 ⋮ 0 − Δ s s 0 s 2 ⋮ 0 − ( Δ s ) 2 s 0 s 1 3 − ( Δ s ) 2 s 0 s 1 6 ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋱ ⋮ ⋮ ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ 0 ⋮ ⋱ ⋮ ⋮ ⋱ ⋱ 0 0 ⋯ ⋯ 0 − s 1 s 2 s 1 s 2 0 ⋯ ⋯ 0 − Δ s s 0 s 2 0 0 ⋯ ⋯ 0 − ( Δ s ) 2 s 0 s 1 3 − ( Δ s ) 2 s 0 s 1 6 1 1 1 ) A= \begin{pmatrix} 1 & 0 & \cdots & \cdots & \cdots & 0 & & & & & & & & & & & & \\ 0 & 1 & & & & \vdots & & & & & & & & & & & & \\ \vdots & & \ddots & & & \vdots & & & & & & & & & & & & \\ \vdots & & & \ddots & & \vdots & & & & & & & & & & & & \\ \vdots & & & & \ddots\ & 0 & & & & & & & & & & & & \\ 0 & \cdots & \cdots & \cdots & 0 & 1 & & & & & & & & & & & & \\ & & & & & & 1 & 0 & \cdots & \cdots & \cdots & 0 & & & & & & \\ & & & & & & 0 & 1 & & & & \vdots & & & & & & \\ & & & & & & \vdots & & \ddots & & & \vdots & & & & & & \\ & & & & & & \vdots & & & \ddots & & \vdots & & & & & & \\ & & & & & & \vdots & & & & \ddots\ & 0 & & & & & & \\ & & & & & & 0 & \cdots & \cdots & \cdots & 0 & 1 & & & & & & \\ & & & & & & & & & & & & 1 & 0 & \cdots & \cdots & \cdots & 0 \\ & & & & & & & & & & & & 0 & 1 & & & & \vdots \\ & & & & & & & & & & & & \vdots & & \ddots & & & \vdots \\ & & & & & & & & & & & & \vdots & & & \ddots & & \vdots \\ & & & & & & & & & & & & \vdots & & & & \ddots\ & 0 \\ & & & & & & & & & & & & 0 & \cdots & \cdots & \cdots & 0 & 1 \\ & & & & & & & & & & & & -1 & 1 & 0 & \cdots & \cdots & 0 \\ & & & & & & & & & & & & 0 & -1 & 1 & & & \vdots \\ & & & & & & & & & & & & \vdots & & \ddots & \ddots & & \vdots \\ & & & & & & & & & & & & \vdots & & & \ddots & \ddots & 0 \\ & & & & & & & & & & & & 0 & \cdots & \cdots & 0 & -1 & 1 \\ & & & & & & -s_2 & s_2 & 0 & \cdots & \cdots & 0 & \frac{-\Delta{s}s_1}{2} & \frac{-\Delta{s}s_1}{2} & 0 & \cdots & \cdots & 0 \\ & & & & & & 0 & -s_2 & s_2 & & & \vdots & 0 & \frac{-\Delta{s}s_1}{2} & \frac{-\Delta{s}s_1}{2} & & & \vdots \\ & & & & & & \vdots & & \ddots & \ddots & & \vdots & \vdots & & \ddots & \ddots & & \vdots \\ & & & & & & \vdots & & & \ddots & \ddots & 0 & \vdots & & & \ddots & \ddots & 0 \\ & & & & & & 0 & \cdots & \cdots & 0 & -s_2 & s_2 & 0 & \cdots & \cdots & 0 & \frac{-\Delta{s}s_1}{2} & \frac{-\Delta{s}s_1}{2} \\ -s_1s_2 & s_1s_2 & 0 & \cdots & \cdots & 0 & -\Delta{s}s_0s_2 & 0 & \cdots & \cdots & \cdots & 0 & \frac{-(\Delta{s})^2s_0s_1}{3} & \frac{-(\Delta{s})^2s_0s_1}{6} & 0 & \cdots & \cdots & 0 \\ 0 & -s_1s_2 & s_1s_2 & & & \vdots & 0 & -\Delta{s}s_0s_2 & & & & \vdots & 0 & \frac{-(\Delta{s})^2s_0s_1}{3} & \frac{-(\Delta{s})^2s_0s_1}{6} & & & \vdots \\ \vdots & & \ddots & \ddots & & \vdots & \vdots & & \ddots & & & \vdots & \vdots & & \ddots & \ddots & & \vdots \\ \vdots & & & \ddots & \ddots & 0 & \vdots & & & \ddots & & \vdots & \vdots & & & \ddots & \ddots & 0 \\ 0 & \cdots & \cdots & 0 & -s_1s_2 & s_1s_2 & 0 & \cdots & \cdots & 0 & -\Delta{s}s_0s_2 & 0 & 0 & \cdots & \cdots & 0 & \frac{-(\Delta{s})^2s_0s_1}{3} & \frac{-(\Delta{s})^2s_0s_1}{6} \\ 1 & & & & & & & & & & & & & & & & & \\ & & & & & & 1 & & & & & & & & & & & \\ & & & & & & & & & & & & 1 & & & & & \\ \end{pmatrix} A=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛10⋮⋮⋮0−s1s20⋮⋮0101⋯s1s2−s1s2⋯⋯⋱⋯0s1s2⋱⋯⋯⋱⋯⋯⋱⋱0⋯⋱ 0⋯⋱−s1s20⋮⋮⋮010⋮⋮0s1s210⋮⋮⋮0−s20⋮⋮0−Δss0s20⋮⋮0101⋯s2−s2⋯0−Δss0s2⋯⋯⋱⋯0s2⋱⋯⋯⋱⋯⋯⋱⋯⋯⋱⋱0⋯⋱0⋯⋱ 0⋯⋱−s2⋯−Δss0s20⋮⋮⋮010⋮⋮0s20⋮⋮⋮010⋮⋮⋮0−10⋮⋮02−Δss10⋮⋮03−(Δs)2s0s10⋮⋮0101⋯1−1⋯2−Δss12−Δss1⋯6−(Δs)2s0s13−(Δs)2s0s1⋯⋯⋱⋯