自动驾驶规划中使用 OSQP 进行二次规划 代码原理详细解读

2024-06-24 06:04
文章标签 进行 代码 规划 使用 原理 解读 详细 自动 驾驶 二次 osqp

本文主要是介绍自动驾驶规划中使用 OSQP 进行二次规划 代码原理详细解读，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

目录

1 问题描述

什么是稀疏矩阵 CSC 形式

QP Path Planning 问题

1. Cost function

1.1 The first term:

1.2 The second term:

1.3 The thrid term:

1.4 The forth term:

对 Qx''' 矩阵公式的验证

整体 Q 矩阵（就是 P 矩阵，二次项的权重矩阵）

整体 P 矩阵的形式如下：

目标函数中的线性项部分，即 q 矩阵的构建

计算 q 矩阵的代码块如下：

2. Constraints

2.1 不等式约束

2.2 连续性约束

Equality constraints

不等式约束的上下边界条件为：

上下边界条件是如何计算的？

二阶导边界极值的计算：

三阶导边界极值的计算

最后，A 矩阵为：

仿射矩阵 A

1 问题描述

典型的优化问题可以用以下的数学表达式来描述：

其中，P 是一个 n x n 的半正定矩阵， x 为 n 维向量，q 为 m x n 的矩阵。

需要注意的是，二次规划只在代价函数为凸函数的时候能够收敛到最优解，因此这需要 P 矩阵为半正定矩阵，这是非常重要的一个条件。这反映在 Apollo 中的规划算法则为需要进行求解的空间为凸空间，这样二次规划才能收敛到一条最优 Path。

上面的表述源自：Apollo 二次规划算法（piecewise jerk path optimizer）解析

什么是半正定矩阵？什么是正定矩阵？

设 A 为实对称矩阵，若对于每个非零实向量 X，都有 X'AX ≥ 0，则称 A 为半正定矩阵，称 X'AX 为半正定二次型。（其中，X'表示 X 的转置。）

注 ：

在 OSQP 中，对上述的数据用结构体 OSQPData 进行封装，其定义如下：

// 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;

其中，P 和 A 都是以稀疏矩阵 CSC 的形式进行存储的。

什么是稀疏矩阵 CSC 形式

CSC - Compressed sparse column (CSC or CCS)

稀疏矩阵和稠密矩阵 - 看矩阵中非零元素占所有元素的比例

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

稀疏矩阵的常规方式

下面是最常见的一种，也很好理解，(row，col) 指向矩阵非零元素的索引，data 里为该元素的值。

稀疏矩阵的 CSC 形式 - csc_matrix

按列压缩 Compressed sparse column，顾名思义将每一列出现的非零元素的个数统计后放好。

如何保证规划算法求解的空间为凸空间？

QP Path Planning 问题

QP 问题的标准形式（下面是常见的两种表示方法）：

1. Cost function

论文中的 cost 函数实际上是如下的形式：

优化变量的形式是：

Cost function 也可以写成下面这种形式：

我们注意到第一项和第二项的内容似乎相同，但其实是有所区别的。我们通过下图来解释。

可以看到，我们要规划的离散点曲线是在 SL 坐标系下进行的。所以第一项惩罚的横向偏移也就是偏移 s 的法向的距离，可以理解为关于车道中心线的偏移。而第四项关于参考线的偏移，则是考虑了静态和低速障碍物生成的一条参考线，第四项所计算的偏移其实是关于参考线的偏移。我们可以这样理解这两项：我们规划的最优轨迹要尽量贴合原来的车道中心线，同时还要尽量贴合能够避障的参考线。

1.1 The first term:

一共有 n 个点：

假如有四个点需要优化，为 x0, x1, ... x3，则矩阵为：

1.2 The second term:

1.3 The thrid term:

1.4 The forth term:

对 Qx''' 矩阵公式的验证

假设有四个点，x1, x2... x3，则：

使用上面的 matrix form：

说明上述的 Qx''' 的公式是正确的。

整体 Q 矩阵（就是 P 矩阵，二次项的权重矩阵）

所以，cost function 的 Q 矩阵为：

整体 P 矩阵的形式如下：
 

注意：代码中 Apollo 中的 Q 矩阵（代码中是 P 矩阵）形式与上述推导有一定的差异：

Apollo 6.0 代码中的 P 矩阵形式如下：

参见这篇文章：

https://zhuanlan.zhihu.com/p/480298921

目标函数的形式如下：

下面就是代码中的 P 矩阵的实际形式：

构建 P 矩阵的函数：

void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,

                                               std::vector<c_int>* P_indices,

                                               std::vector<c_int>* P_indptr)

P 矩阵的零阶导数（蓝色）部分对应的代码块：

// x(i)^2 * (w_x + w_x_ref[i]), w_x_ref might be a uniform value for all x(i)

  // or piecewise values for different x(i)

  // P 矩阵的零阶导数部分

  for (int i = 0; i < n - 1; ++i) {

    columns[i].emplace_back(i, (weight_x_ + weight_x_ref_vec_[i]) /

                                   (scale_factor_[0] * scale_factor_[0])); // scale_factor - 缩放系数

    ++value_index;

  }

  // x(n-1)^2 * (w_x + w_x_ref[n-1] + w_end_x) - 最后一个节点增加了一个末状态的权重 w_end_x

  columns[n - 1].emplace_back(

      n - 1, (weight_x_ + weight_x_ref_vec_[n - 1] + weight_end_state_[0]) /

                 (scale_factor_[0] * scale_factor_[0]));

  ++value_index;

P 矩阵的一阶导数（粉色）部分对应的代码块：

  // x(i)'^2 * w_dx - P 矩阵的一阶导数部分

  for (int i = 0; i < n - 1; ++i) {

    columns[n + i].emplace_back(

        n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));

    ++value_index;

  }

  // x(n-1)'^2 * (w_dx + w_end_dx)

  columns[2 * n - 1].emplace_back(2 * n - 1,

                                  (weight_dx_ + weight_end_state_[1]) /

                                      (scale_factor_[1] * scale_factor_[1]));

  ++value_index;

P 矩阵的二阶导数（绿色）部分对应的代码块：

 auto delta_s_square = delta_s_ * delta_s_;

  // P 矩阵的二阶导数部分的第一个对角线元素

  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)

  columns[2 * n].emplace_back(2 * n,

                              (weight_ddx_ + weight_dddx_ / delta_s_square) /

                                  (scale_factor_[2] * scale_factor_[2]));

  ++value_index;

  // P 矩阵的二阶导数部分的对角线元素（除了对角线上的第一个元素和最后一个元素）

  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;

  }

  // P 矩阵的二阶导数部分的最后一个对角线元素

  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;

  // P 矩阵的二阶导数部分的次对角线上的元素

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''

  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;

目标函数中的线性项部分，即 q 矩阵的构建

这部分代码在 PiecewiseJerkPathProblem::CalculateOffset 这个函数

void PiecewiseJerkPathProblem::CalculateOffset(std::vector<c_float>* q)

q 矩阵形式如下：

计算 q 矩阵的代码块如下：

  if (has_x_ref_) {

    for (int i = 0; i < n; ++i) {

      q->at(i) += -2.0 * weight_x_ref_vec_.at(i) * x_ref_[i] / scale_factor_[0];

    }

  }

  //

  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];

  }

2. Constraints

2.1 不等式约束

2.2 连续性约束

用每个轨迹点处在 l 方向的 jerk 相等作为连续性约束。

上面的推导和下面的推导是一致的：

将 piecewise jerk 的条件带入：

得：

Now we generate constraints in matrix form.

For constrains a - lateral offset - 0 阶导数

For constrains b - lateral velocity - 一阶导数

For constrains c - lateral acceleration - 二阶导

For constraints d - lateral jerk - 三阶导数

这里有一些问题，对于 jerk 的不等式约束，应该是下面的形式：

For constraints e - 连续性约束

下图是有四个点的时候仿射矩阵的具体形式：

For Constraints f:

约束可以写成：

具体形式：

Equality constraints

不等式约束的上下边界条件为：

上下边界条件是如何计算的？

关于边界约束的详细介绍参见这篇文章：https://zhuanlan.zhihu.com/p/481835121

二阶导边界极值的计算：

对应代码如下：

const double lat_acc_bound =

std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) /

veh_param.wheel_base();

std::vector<std::pair<double, double>> ddl_bounds;

for (size_t i = 0; i < path_boundary_size; ++i) {

double s = static_cast<double>(i) * path_boundary.delta_s() +

path_boundary.start_s();

double kappa = reference_line.GetNearestReferencePoint(s).kappa();

ddl_bounds.emplace_back(-lat_acc_bound - kappa, lat_acc_bound - kappa);

}

三阶导边界极值的计算

最后，A 矩阵为：

仿射矩阵 A

至此，P 矩阵，q 矩阵，A 矩阵，b 矩阵均可以表示出来，放入 OSQP 求解器中，可以进行迭代求解了。

The Principle of Apollo Path Planning using Quadratic Programming

这里是 Piecewise Jerk Path Optimizer 的代码讲解。

这篇关于自动驾驶规划中使用 OSQP 进行二次规划 代码原理详细解读的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1089366。

相关文章

中文分词jieba库的使用与实景应用（一）

中文分词jieba库的使用与实景应用（一）

知识星球：https://articles.zsxq.com/id_fxvgc803qmr2.html 目录 一.定义： 精确模式（默认模式）： 全模式： 搜索引擎模式： paddle 模式（基于深度学习的分词模式）： 二 自定义词典 三.文本解析   调整词出现的频率 四. 关键词提取 A. 基于TF-IDF算法的关键词提取 B. 基于TextRank算法的关键词提取
阅读更多...
使用SecondaryNameNode恢复NameNode的数据

使用SecondaryNameNode恢复NameNode的数据

1）需求： NameNode进程挂了并且存储的数据也丢失了，如何恢复NameNode 此种方式恢复的数据可能存在小部分数据的丢失。 2）故障模拟 （1）kill -9 NameNode进程 [lytfly@hadoop102 current]$ kill -9 19886 （2）删除NameNode存储的数据（/opt/module/hadoop-3.1.4/data/tmp/dfs/na
阅读更多...
Hadoop数据压缩使用介绍

Hadoop数据压缩使用介绍

一、压缩原则 （1）运算密集型的Job，少用压缩 （2）IO密集型的Job，多用压缩 二、压缩算法比较 三、压缩位置选择 四、压缩参数配置 1）为了支持多种压缩/解压缩算法，Hadoop引入了编码/解码器 2）要在Hadoop中启用压缩，可以配置如下参数
阅读更多...
Makefile简明使用教程

Makefile简明使用教程

文章目录 规则makefile文件的基本语法：加在命令前的特殊符号：.PHONY伪目标: Makefilev1 直观写法v2 加上中间过程v3 伪目标v4 变量 make 选项-f-n-C Make 是一种流行的构建工具，常用于将源代码转换成可执行文件或者其他形式的输出文件(如库文件、文档等)。Make 可以自动化地执行编译、链接等一系列操作。 规则 makefile文件
阅读更多...
深入探索协同过滤：从原理到推荐模块案例

深入探索协同过滤：从原理到推荐模块案例

文章目录 前言一、协同过滤1. 基于用户的协同过滤（UserCF）2. 基于物品的协同过滤（ItemCF）3. 相似度计算方法 二、相似度计算方法1. 欧氏距离2. 皮尔逊相关系数3. 杰卡德相似系数4. 余弦相似度 三、推荐模块案例1.基于文章的协同过滤推荐功能2.基于用户的协同过滤推荐功能 前言     在信息过载的时代，推荐系统成为连接用户与内容的桥梁。本文聚焦于
阅读更多...
使用opencv优化图片（画面变清晰）

使用opencv优化图片（画面变清晰）

文章目录 需求影响照片清晰度的因素 实现降噪测试代码 锐化空间锐化Unsharp Masking频率域锐化对比测试 对比度增强常用算法对比测试 需求 对图像进行优化，使其看起来更清晰，同时保持尺寸不变，通常涉及到图像处理技术如锐化、降噪、对比度增强等 影响照片清晰度的因素 影响照片清晰度的因素有很多，主要可以从以下几个方面来分析 1. 拍摄设备 相机传感器：相机传
阅读更多...
动态规划---打家劫舍

动态规划---打家劫舍

题目： 你是一个专业的小偷，计划偷窃沿街的房屋。每间房内都藏有一定的现金，影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统，如果两间相邻的房屋在同一晚上被小偷闯入，系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组，计算你 不触动警报装置的情况下 ，一夜之内能够偷窃到的最高金额。 思路： 动态规划五部曲： 1.确定dp数组及含义 dp数组是一维数组，dp[i]代表
阅读更多...
【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯，先赞后看哦~🎈🎈 🏆 作者简介：景天科技苑 🏆《头衔》：大厂架构师，华为云开发者社区专家博主，阿里云开发者社区专家博主，CSDN全栈领域优质创作者，掘金优秀博主，51CTO博客专家等。 🏆《博客》：Python全栈，前后端开发，小程序开发，人工智能，js逆向，App逆向，网络系统安全，数据分析，Django，fastapi
阅读更多...
hdu4407(容斥原理)

hdu4407(容斥原理)

题意：给一串数字1,2，......n，两个操作:1、修改第k个数字，2、查询区间[l,r]中与n互质的数之和。 解题思路：咱一看，像线段树，但是如果用线段树做，那么每个区间一定要记录所有的素因子，这样会超内存。然后我就做不来了。后来看了题解，原来是用容斥原理来做的。还记得这道题目吗？求区间[1,r]中与p互质的数的个数，如果不会的话就先去做那题吧。现在这题是求区间[l,r]中与n互质的数的和
阅读更多...
活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2