2基于hp自适应伪谱法的航迹规划策略

近几年来，以伪谱法为代表的直接法在计算速度上的优势，广泛应用于处理非线性系统最优控制问题 ^[11] 。在Radau伪谱法基础上形成的hp-Radau伪谱法即hp自适应伪谱法，能够根据误差要求，对差值多项式阶数进行自适应调整或者对网格进行细化，在满足既定精度下，能够提高求解速度，适用于航迹规划问题的求解。

2.1时域转换

采用hp自适应伪谱法求解最优控制问题，需要对时间区间进行变换，把 t ∈[ t ₀ , t _f ]划分为 K 个网格区间[ t _k-1 , t _k ]（其中 k =1,2,…, K ），总共包括 K +1个分割点 t ₀ < t ₁ <…< t _K = t _f 。对划分的时间网格区间做如下变换：

通过此变换将每个 t ∈[ t _k-1 , t _k ]都转换到 τ ∈ [−1,1]区域内，并对 t 求导可得

2.2最优化问题转化

hp自适应伪谱法的基本原理为：首先将状态、控制变量在配点上进行多项式近似，然后将状态方程、状态变量、约束方程和代价函数在配点上离散化，将连续最优控制问题转化为非线性规划问题。本节将航迹规划分为两段，各段处理方式是一致的。假设 X ^{( k )} ( τ )和 U ^{( k )} ( τ )为第 k 个网格区间的状态变量和控制变量。

2.2.1状态变量及控制变量的多项式近似

定义Lagrange插值多项式形式为

式中，第 k 个网格时间区间的配点用 表示， N _k 表示第 k 个时间网格内的配点数。第 k 个时间网格中的状态变量用 N _k +1个Lagrange插值多项式 ( τ )( j =1,…, N _k , N _k +1)作为基函数进行近似处理，状态变量离散为如下形式：

采用同样方式，对控制变量进行离散化处理，有：

2.2.2状态方程离散化

由上述可知，状态变量对时间的导数近似于对式（25）求导，从而用一组代数约束代替微分方程约束。式（25）对时间求导为

将式（27）代入运动学微分方程即式（11）中，并通过在配点上离散处理，可得

2.2.3路径约束离散化

在 N _k 个配点处将过程约束 C [ X ( t ), U ( t ), t ] 离散处理，得到如下形式：

2.2.4末端约束离散化

在 N _k 个配点处将过程约束 C [ X ( t ), U ( t ), t ] 离散处理，得到如下形式：

2.2.5连接约束离散化

2.2.6代价函数离散化

用Lagrange积分替代代价函数积分项，代价函数离散处理后的形式如下：

式中， （ j =1,…, N _k , k =1,…, K ）为Lagrange多项式的系数。

通过以上变换，连续最优控制问题转化为了非线性规划问题。离散化的表达式如下：

针对分段处理的问题，需要增加连接约束的离散化方程：

2.3参数自适应调整

hp自适应伪谱法能够对是否增加Lagrange插值多项式的阶数（ p -细化）或者网格重规划（ h -细化）进行自适应处理，来满足既定的离散偏差要求。

设状态微分方程与约束离散处理的允许偏差为 e ，在第 k 个时间网格中，状态微分方程和离散处理后的状态间误差为 ，约束条件离散处理后的偏差为 。在算法运行过程中，hp自适应伪谱法将对 与 进行实时解算，表达式为

式中，1≤ l ≤ L ， l ≤ i ≤ n ， l ≤ j ≤ m ，L为各网格区间内的配点数， n 为计算过程中的状态微分方程个数， m 为约束条件个数。

将状态变量的每一数值及其相对应的控制变量数值代入式（35）和式（36）计算实时误差 和 ，如果在当前时间网格内，满足 ≤ e 和 ≤ e ，则说明当前的处理方式满足精度要求。如果 ≤ e 与 ≤ e 无法同时满足，则说明需要进行时间网格的重新调整（ h -细化）或增加Lagrange差值多项式的阶数（ p -细化），以满足精度要求。

定义 为自适应系数，判别原则为 RX4QGjiAg36STciCPu9lsuVo1HN8bhYtZ0oZpseGwlpIDEesSYx82kkZfZoBsKY2