5.5.c PiecewiseJerkPathOptimizer路径优化代码解析

category
type
status
slug
date
summary
tags
password
icon
I pledge my life and honor to the Night's Watch, for this night and all the nights to come. 我将生命与荣耀献给守夜人，今夜如此，夜夜皆然。 — 权利的游戏 守夜人
🏰代码及环境配置：请参考 环境配置和代码运行!
本节提供了 PiecewiseJerkPath的代码测试：
 
我们需要将问题写成矩阵形式, 调用OSQP求解器:

5.5.c.1 目标函数

为了避免因为n过大, 造成矩阵不直观, 我们以4个点为例来说明问题.
问题的决策变量是, 我们将他们写成
构造时, 我们分别构造对应3个阶数的小矩阵:
首先构造, 也就是第一项:, 只有它涉及到0阶项的向量
接下来构造:
因为, 所以包括了最后两项:

5.5.c.2 边界约束

其中是单位矩阵, 很容易写出:

5.5.c.3 连接约束

(1) 1阶连接性约束:
写出矩阵的形式:
其中
(2) 0阶连接性约束:
写成矩阵的形式:

5.5.c.4 起点约束

我们还可以根据车辆起始状态, 加上对应的约束, 如下:
至此, 约束构造完毕, 将上面的不等式, 沿着行连接起来, 分别构造矩阵即可

5.5.c.5 运行代码

将上述矩阵分别赋值给P, q, A, l, u, 输入给OSQP即可. 我们在PiecewiseJerkPathOptimizer 中实现了上述矩阵的构造, 并添加了一些边界约束.
执行代码可以看到优化出来的路径:
notion image
上一篇
动手学控制理论
下一篇
端到端-理论与实战视频课程
Loading...