规划算法中优化问题求解

非线性规划问题是目标函数或约束条件中包含非线性函数的规划问题。一般说来，求解非线性规划问题比求解线性规划问题困难得多。而且，不像线性规划有单纯形法这一通用方法，非线性规划目前还没有适用于各种问题的一般算法，已有的各种方法都有其特定的适用范围。序列二次规划和内点法是两类非常重要的算法，也是大规模问题的利器。

利用间接法求解最优化问题的途径一般有两种。一种是在可行域内使目标函数下降的迭代算法，如可行点法；另一种是利用目标函数和约束条件构造增广目标函数，借此将约束最优化问题转化为无约束最优化问题，如罚函数法、乘子法、序列二次规划法等。

序列二次规划算法是目前公认的求解约束非线性优化问题最有效的方法之一。与其他算法相比，序列二次规划法的优点是收敛性好、计算效率高、边界搜索能力强，因此受到了广泛的重视及应用。在序列二次规划法的迭代过程中，每一步都需要求解一个或多个二次规划（QP）子问题。一般地，由于二次规划子问题的求解难以利用原问题的稀疏性、对称性等良好特性，随着问题规模的扩大，其计算工作量和所需存储量是非常大的。因此，目前的序列二次规划算法一般只适用与中小规模问题。

内点法将将约束添加到目标函数中转换为一系列无约束问题逐步求解。

不同优化器求解问题

在fem_pow_deviation_smoother.cc中，可以看到其Solve函数会根据配置文件调用QpWithOsqp，NlpWithIpopt和SqpWithOsqp三个函数。这分别是利用不同求解器实现了这个方法。
如果考虑参考线的曲率约束，其优化问题是非线性的，可以使用ipopt非线性求解器求解（内点法），也可以使用osqp二次规划求解器来用SQP方法求解；
如果不考虑曲率约束，则直接用osqp求解二次规划问题。

csc矩阵

稀疏矩阵
在矩阵中，若数值为0的元素数目远远多于非0元素的数目，并且非0元素分布没有规律时，则称该矩阵为稀疏矩阵；与之相反，若非0元素数目占大多数时，则称该矩阵为稠密矩阵。定义非零元素的总数比上矩阵所有元素的总数为矩阵的稠密度。

矩阵压缩：
存储矩阵的一般方法是采用二维数组，其优点是可以随机地访问每一个元素，因而能够较容易地实现矩阵的各种运算，如转置运算、加法运算、乘法运算等。
对于稀疏矩阵来说，采用二维数组的存储方法既浪费大量的存储单元用来存放零元素，又要在运算中花费大量的时间来进行零元素的无效计算。所以必须考虑对稀疏矩阵进行压缩存储。

最常用的稀疏矩阵存储格式主要有：COO（Coordinate Format）、CSR（Compressed Sparse Row）和CSC（Compressed Sparse Column）。

COO很简单，就是使用3个数组，分别存储全部非零元的行下标（row index）、列下标（column index）和值（value）；CSR稍复杂，对行下标进行了压缩，假设矩阵行数是m，则压缩后的数组长度为m+1，记作（row ptr），其中第i个元素（0-base）表示矩阵前i行的非零元个数。

CSR是比较标准的一种，也需要三类数据来表达：数值，列号，以及行偏移。CSR不是三元组，而是整体的编码方式。数值和列号与COO一致，表示一个元素以及其列号，行偏移表示某一行的第一个元素在values里面的起始偏移位置。如上图中，第一行元素1是0偏移，第二行元素2是2偏移，第三行元素5是4偏移，第4行元素6是7偏移。在行偏移的最后补上矩阵总的元素个数，本例中是9。

CSC是和CSR相对应的一种方式，即按列压缩的意思。

之所以使用CSC矩阵是为了方便将稀疏矩阵进行高效地存储。

>>> indptr = np.array([0, 2, 3, 6])
>>> indices = np.array([0, 2, 2, 0, 1, 2])
>>> data = 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[i+1]表示稀疏矩阵的第i列(包括i列)之前一共有多少个非零元素，这些非零元素对应的行，依次在indices 中取出来即可。
第0列的非零元素个数为indptr[0+1]-indptr[0]=2-0=2个，从indices中可知对应的非零元素在0、2行，data对应的值为1、2，则第0列为{1 0 2}；
第1列的非零元素个数为indptr[1+1]-indptr[1]=3-2=1个，从indices中可知对应的非零元素在2行，data对应的值为3，则第1列为{0 0 3}；
第2列的非零元素个数为indptr[2+1]-indptr[2]=6-3=3个，从indices中可知对应的非零元素在0、1、2行，data对应的值为4、5、6，则第2列为{4 5 6}；
得到完整的矩阵。

OSQP和qpOASES对比

OSQP相比于qpOASES求解二次规划问题时更加快速和准确。