19、使用克罗托夫函数进行快速合成轨迹优化

使用克罗托夫函数进行快速合成轨迹优化

1. 扩张原理与不变嵌入方法概述

扩张原理与不变嵌入方法的核心思想是将初始任务纳入一组优化任务中（即不变嵌入）。在这个过程中，各个任务之间可能存在简单的关系，并且在这组任务中，有一个任务可以通过克罗托夫方法轻松求解。之后，利用这个易解任务的解以及任务之间的关联，就能得到原任务的解。

为了解决任务，需要考虑一个最小化序列 ${\nu^l}$，其选择的目的是最小化任务求解的准则：
$I = S_0(^1X^l(t_0^{l+}), t_0^{l+}) + \sum_{i = 1}^{N} J_i$
其中：
- $J_i = S_i(^iX^l(t_i^{l-}), ^{i + 1}X^l(t_0^{l+}), t_i^l) + \int_{t_N^{l+}}^{t_N^{l-}} \Phi_i(^iX^l, ^iU^l, t)dt$，$i = 1, \cdots, N$
- $J_N = S_N(^NX^l(t_N^{l-}), t_N^{l-}) + \int_{t_N^{l+}}^{t_N^{l-}} \Phi_N(^NX^l, ^NU^l, t)dt$

这些计算需要满足条件 (3)、(4) 以及：
- $(^1X^l(t_0^{l+}), t_0^{l+}) \in B_0$
- $(^NX^l(t_N^{l-}), t_N^l) \in B_N$
- $(^iX^l(t_i^{l-}), ^{i + 1}X^l(t_0^{l+}), t_i^l) \in B_i$，$i = 1, \cdots, N - 1$

这里的 $B_0$、$B_N$、$B_i