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)和初始状态(array<double, 3> x_init,包括x, dx, ddx的初始状态)FormulateProblem()
:将速度规划、路径规划问题转换成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<c_float> P_data
中:
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<c_int> P_indices
中:
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,