1. 建模部分（Modelling）

1.1 优化目标：最小化jerk的平方

• ：是所有三维空间（假设为 x、y、z 三个方向）上的总代价。目标是最小化“jerk”的平方和。

• ：在每个方向（x、y、z）上最小化 jerk 的平方，即加加速度的平方。

• ：表示第 kk 个方向上的 jerk，jerk 是加速度对时间的导数。

• ：总的时间跨度。

1.2 状态和输入

• 状态 ：在每个方向 k，状态包括位置 、速度 和加速度 ，即 。

• 控制输入 ：控制输入即 jerk，即加速度变化率，用 来表示。

1.3 系统模型

• ：表示状态 sks_k 的变化率，即位置、速度和加速度的导数。

• ：表示飞行器的动力学模型，其中位置的导数是速度、速度的导数是加速度、加速度的导数是 jerk（控制输入）。

2. 求解部分（Solving）

2.1 Pontryagin最小值原理

2.2 引入伴随量（Costate）

2.3 定义Hamilton函数

• ：是Hamilton函数，表示了控制输入 uu（jerk）和状态 ss 以及伴随量 λ\lambda 的加权组合。

• ：代表jerk的平方代价。

• ：这些项是系统状态（速度、加速度、jerk）与伴随量的内积，表示了与控制输入相关的惩罚项。

2.4 最优控制的必要条件

1. 状态方程（正向积分）：
s˙∗(t)=f(s∗(t),u∗(t)),s∗(0)=s(0)\dot{s}^*(t) = f(s^*(t), u^*(t)), \quad s^*(0) = s(0)
这是系统的动力学方程，表示最优轨迹 s∗(t)s^*(t) 需要满足初始条件 s∗(0)=s(0)s^*(0) = s(0)。

2. 伴随方程（反向积分）：
λ˙(t)=−∇sH(s∗(t),u∗(t),λ(t)),λ(T)=−∇sh(s∗(T))\dot{\lambda}(t) = -\nabla_s H(s^*(t), u^*(t), \lambda(t)), \quad \lambda(T) = -\nabla_s h(s^*(T))
伴随量 λ(t)\lambda(t) 是随着时间反向积分的，它表示系统状态在每个时刻对最终代价的影响。

3. 最优控制律：
u∗(t)=arg⁡min⁡uH(s∗(t),u,λ(t))u^*(t) = \arg \min_u H(s^*(t), u, \lambda(t))
最优控制输入 u∗(t)u^*(t) 是通过最小化 Hamilton 函数 H(s∗(t),u,λ(t))H(s^*(t), u, \lambda(t)) 来得到的。这通常通过求解最小化问题来得到 jerk。

2.5 最优解

小结

• 建模部分：通过定义代价函数和系统的运动学方程，将运动规划问题转化为最优边值问题（OBVP）。

• 求解部分：通过Pontryagin最小值原理，引入伴随量并定义Hamilton函数，得到求解最优控制问题的必要条件，最后通过数值方法求解最优轨迹和控制输入。