自动驾驶规划中使用OSQP进行二次规划
1. 引言
自动驾驶中, QP被用作曲线的平滑, 通过对路径曲线的平滑保证控制实现的可行性, 通过对速度曲线的平滑保证乘车的舒适性.
附赠自动驾驶最全的学习资料和量产经验以及100T的资源分享:链接
2. 相关数学概念
// the location of file: /usr/local/include/osqp/types.h
typedef struct {
c_int n; ///< number of variables n
c_int m; ///< number of constraints m
csc *P; ///< the upper triangular part of the quadratic cost matrix P
csc *A; ///< linear constraints matrix A in csc format (size m x n)
c_float *q; ///< dense array for linear part of cost function (size n)
c_float *l; ///< dense array for lower bound (size m)
c_float *u; ///< dense array for upper bound (size m)
} OSQPData;
3. OSQP的安装和使用
3.1 OSQP的安装
git clone --recursive https://github.com/osqp/osqp
cd osqp
mkdir build
cd build
cmake --build . --target install
3.2 OSQP的使用例子
OSQP的例子可以在下载下的源码中找到: osqp>examples>osqp_demo.c.
#include "osqp.h"
int main(int argc, char **argv) {
// Load problem data
c_float P_x[3] = { 4.0, 1.0, 2.0, };
c_int P_nnz = 3;
c_int P_i[3] = { 0, 0, 1, };
c_int P_p[3] = { 0, 1, 3, };
c_float q[2] = { 1.0, 1.0, };
c_float A_x[4] = { 1.0, 1.0, 1.0, 1.0, };
c_int A_nnz = 4;
c_int A_i[4] = { 0, 1, 0, 2, };
c_int A_p[3] = { 0, 2, 4, };
c_float l[3] = { 1.0, 0.0, 0.0, };
c_float u[3] = { 1.0, 0.7, 0.7, };
c_int n = 2;
c_int m = 3;
// Exitflag
c_int exitflag = 0;
// Workspace structures
OSQPWorkspace *work;
OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
OSQPData *data = (OSQPData *)c_malloc(sizeof(OSQPData));
// Populate data
if (data) {
data->n = n;
data->m = m;
data->P = csc_matrix(data->n, data->n, P_nnz, P_x, P_i, P_p);
data->q = q;
data->A = csc_matrix(data->m, data->n, A_nnz, A_x, A_i, A_p);
data->l = l;
data->u = u;
}
// Define solver settings as default
if (settings) osqp_set_default_settings(settings);
// Setup workspace
exitflag = osqp_setup(&work, data, settings);
// Solve Problem
osqp_solve(work);
// Clean workspace
osqp_cleanup(work);
if (data) {
if (data->A) c_free(data->A);
if (data->P) c_free(data->P);
c_free(data);
}
if (settings) c_free(settings);
return exitflag;
}
CMakeLists.txt可以参考官网Wiki.
cmake_minimum_required(VERSION 3.10)
# set the project name
project(OSQP)
# add the executable
add_executable(OSQP osqp_example.c)
# Find OSQP library and headers
find_package(osqp REQUIRED)
# Link the OSQP shared library
target_link_libraries(OSQP PRIVATE osqp::osqp)
最后运行的结果如下: