自动驾驶小白说 🏎️5.5.c PiecewiseJerkPathOptimizer路径优化代码解析
type
status
slug
date
summary
tags
category
password
icon
I pledge my life and honor to the Night's Watch, for this night and all the nights to come. 我将生命与荣耀献给守夜人，今夜如此，夜夜皆然。 — 权利的游戏 守夜人
🏰代码及环境配置：请参考0.2 环境配置和代码运行 | 动手学运动规划!
本节提供了 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 中实现了上述矩阵的构造, 并添加了一些边界约束. 执行代码可以看到优化出来的路径:
Loading...
