一种基于最优控制问题伪谱法求解构架的次优解求解方法

摘要

本发明提供一种基于最优控制问题伪谱法求解构架的次优解快速求解方法。该方法基于最优控制问题的伪谱方法求解框架，结合参数化状态变量的动态逆优化方法的技术与放宽NLP问题求解的策略，实现约束控制问题次优可行解的快速求解。该方法第一是选定基本状态变量，利用状态方程与等式约束反推其它状态变量与控制变量，所得解满足状态方程与等式约束；第二是补充约束，以实现反推状态变量终端条件与不等式路径约束的满足；第三是对于NLP求解时不再拘泥于解的最优性，并不要求NLP问题求解成功，NLP计算结束准则放宽为满足所有约束即可；第四是对解的可行性的检验，如果可行性满足，求解结束，如果不满足，采取加强不等式路径约束或加密伪谱方法配点的措施的再行计算。

说明书

技术领域

本发明涉及工程领域中约束控制问题的次优可行解求解方法，尤其指制导问题中的路径规划方法。本方法可以应用到航天器姿态机动路径规划问题或飞行器飞行轨迹规划问题以及其它本发明适用的问题。

背景技术

[0002] 工程领域中常常会碰到各种约束控制问题，这种控制问题中含有各种约束，对于一个约束控制问题，其可能存在许多可行解，可行解即满足各种约束并能实现控制目标的解，最优（次优）解就是相应于某一种具体指标意义下最优（次优）的可行解。

对于约束控制问题，目前常用的求解策略就是优化求解方法。针对具体问题建立的最优控制问题（Optimal Control Problem, OCP）模型，既可以采用对基于Pontryagin极小值原理导出的Hamilton两点边值问题进行求解的间接方法，也可以采用对基于参数化方法建立的非线性规划(Nonlinear Programming，NLP)问题进行求解的直接方法。基于参数化方法获得的解往往是最优解一定阶次的逼近，也即次优解。最优控制问题对最优性的过度追求往往带来昂贵的求解代价，这是影响其工程应用中的重大障碍。实际控制任务中可能存在各种干扰，如初值有偏，环境变化，控制目标变化等，这会导致事先求得的最优解实际上并不是最优甚至不能应用。对于跟踪参考轨迹这一类制导方法，在线路径快速规划是提高控制任务可靠性与灵活性的有效方法。许多制导问题属于约束控制问题，由于需要满足运动学方程与动力学方程约束，并且考虑各种约束，最优解的快速求解并不容易，如果能实现最优解甚至次优解的快速求解，这将会有更大的实际应用意义。

如何实现约束控制问题的有效快速求解一直以来是许多学者致力于研究的问题。伪谱方法就是当前一类先进的最优控制求解方法，它属于同时参数化状态变量与控制的直接方法。Legendre伪谱方法采用多项式逼近最优解，利用Gauss积分对一定阶次多项式精确积分的良好特性来节点处满足状态方程约束，利用伪谱方法转化得到的非线性规划问题具有约束多、约束方程系数矩阵稀疏的特点，采用结合一定的数值求解技巧的序列二次规划方法（Sequential Quadratic Programming，SQP）可以实现NLP问题的快速求解。伪谱方法的解具有谱收敛速度，此外，伴随向量映射原理（Covector Mapping Principle，CMP)有效保证了解的正确性。伪谱方法为最优控制问题的快速求解提供了可能。然而，采用伪谱法得到的解一般只是近似满足状态方程约束，严格的说，它并不是可行解，节点越少，近似程度越差。对于较复杂的最优控制问题，要获得满意精度的最优解需要较多节点，由于节点多，其求解会很费时甚至无法求解。为了实现最优解的快速求解，有的学者提出了两步方法，第一步是通过快速随机树方法（RRT）构造处可行解，第二步是对可行解进行改造与优化。另一方面，由伪谱方法求解得是对最优解一定阶次逼近的近似解，直观想象，近似最优解与最优解相差不大，自然与可能的可行解“距离”也不远。与在可行解的基础上进一步求解最优解的想法相反，能否基于最优解求解框架导出可行解是一个值得思考的问题。根据可行解的定义，求解可行解需要解决以下问题：

1)满足状态微分方程；

2)满足状态变量终端边界条件；

3)满足路径约束。

发明内容

本发明基于基于最优控制问题的伪谱方法求解框架，结合参数化状态变量的动态逆优化方法的技术手段和放宽NLP问题求解的策略，对伪谱方法获取的近似最优解进行一定改造，实现了次优可行解的快速求解。

本发明的技术方案为：基于最优控制问题的伪谱方法求解框架，结合参数化状态变量的动态逆优化方法的技术与放宽NLP问题求解的策略，实现约束控制问题次优可行解的快速求解。

对于可行解求解中满足状态微分方程的问题，在伪谱方法求解得到的计算结果中通过选定某一状态变量，综合状态方程和等式约束对其它状态变量与控制变量进行反推，由此得到的解总称为可能可行解，选定的状态变量称为基本状态变量。选择的基本状态变量的数目等于独立控制变量的数目减去独立等式约束的个数，这里所谓独立等式约束是指等式约束本身不包含于微分方程中，反例如四元数微分方程，其中四元数模为1的等式约束是包含在微分方程信息中的。基本状态变量选取的一般原则是选取利于其它状态量与控制量的反推的状态量。基于状态方程反推得到的可能可行解自然满足状态方程约束。

对于可行解求解中满足状态变量终端边界条件的问题，在伪谱方法自身终端状态变量约束的基础上，向伪谱方法转化得到的NLP问题中补充与基本状态变量的参数相关的一定约束，以保证选定的状态变量及反推得到的其它状态变量满足指定的终端条件约束。

对于可行解求解中满足路径约束的问题，由于路径等式约束用于可能可行解的反推，因此可能可行解满足等式约束，对于路径不等式约束，利用具体伪谱法自身的路径约束技术，并补充一定约束以实现路径约束的满足。需要指出的是，由于伪谱方法仅在离散节节点处施加路径约束，同时可能可行解的路径约束函数并不一定完全与伪谱法计算结果重合，因此伪谱法计算中路径约束的满足并不能保证可能可行解在整个时间段上满足路径约束，对求得的可能可行解，需要验证不等式路径约束是否满足，如果满足，计算结束，如果不满足，需要采取约束加强或增加伪谱方法配点数目的措施，并将当前计算结果作为初值再次进行计算。

本方法发明的具体步骤为： 

1）针对具体问题，建立最优控制问题（OCP）模型； 

2）利用伪谱方法得到相应的原始非线性规划（NLP）问题； 

3）在伪谱方法约束技术的基础上，为保证终端边界条件与路径约束的满足，向原始NLP中补充一定约束形成改进NLP，补充的约束可能包括与基本状态变量相关以实现终端边界条件满足的约束，控制相关的约束或其它约束； 

4）利用SQP方法求解改进NLP问题，对于该NLP问题的求解，为加快求解速度，不要求得到最优解， NLP计算结束准则可进一步放宽为满足所有约束即可；

5）从伪谱方法的解中选取数目等于独立控制变量个数减去独立等式约束个数的基本状态变量，基于基本状态变量，利用状态方程和路径等式约束进行可能可行解反推，由于步骤3补充的约束保证了终段边界条件的满足，所以此处得到的可能可行解既满足状态方程与等式约束，又满足终端边界条件； 

6）对可能可行解的不等式路径约束满足情况进行检验，如果解可行，计算结束，否则对改进的NLP问题采取加强不等式路径约束或增加伪谱法配点数目的措施，并将当前计算结果作为初值再次进行计算。

本方法基于最优控制问题框架，求得可行解相应于给定的具体指标具有较好值，是一个次优解；本方法中选定用于其它解反推的状态变量实际上是一定阶次的多项式，具有较好的连续性与可微性，由其反推导出的控制往往具有较好的连续性，可以避免最优解中控制不连续的问题，有利于实际控制的实施；本方法利用伪谱方法的有效求解能力，首次计算时无需对初值进行特别猜测，采用简单的常数赋值或线性插值的“冷启动”即可；本方法利用有限节点情况下伪谱方法对近似最优解的快速求解能力，求取可行解过程中需要的迭代次数较少，可实现次优可行解的快速求解；本方法不拘泥于解的最优性，不要求NLP问题得到成功求解，NLP计算结束准则可进一步放宽为满足所有约束即可，据此可以进一步提高求解速度。

综上所述，本发明在最优控制问题求解框架基础上结合参数化状态变量的动态逆优化方法技术和NLP求解放宽的策略，具体有四大措施，第一是选定基本状态变量，利用状态方程与等式约束反推其它状态变量与控制变量，所得解满足状态方程与等式约束；第二是补充约束，以实现反推状态变量终端条件与不等式路径约束的满足；第三是对于NLP求解时不再拘泥于解的最优性，并不要求NLP问题求解成功，NLP计算结束准则放宽为满足所有约束即可；第四是对解的可行性的检验，如果可行性满足，求解结束，如果不满足，采取加强不等式路径约束或加密伪谱方法配点的措施的再行计算。

附图说明

图1约束控制问题次优可行解求解流程图；

图2国际空间站体坐标系图；

图3轨道坐标系图；

图4 ZPM路径规划流程图。

具体实施方式

本方法专利适用于一般的约束控制问题的可行解求解，下面结合具体应用实例“空间站零燃料机动”对本方法进行详细说明。

1、 ZPM简介

空间站零燃料机动（Zero Propellant Maneuver, ZPM）指基于动量交换性伺服机构：控制力矩陀螺（Control Momentum Gyroscopes system，CMGs），将环境力矩作为主动力矩源，实现空间站大角度姿态机动甚至CMGs角动量御载的一种先进的姿控技术。

空间站上有推力器姿控系统与动量交换姿控系统，一般空间站的大角度姿态机动是由推力器姿控系统完成，动量交换姿控系统用于短期姿态保持与动量管理，如果利用CMGs实施大角度姿态机动，由于姿态保持控制器采用的特征轴机动逻辑将引起CMGs角动量饱和，而ZPM的提出就是旨在寻求一条姿态机动路径，沿着这条路径，可以在CMGs的性能范围内完成机动任务。ZPM除了可以应用于姿态机动以节约宝贵的燃料，还可以用于速率阻尼，角动量御载等任务。

ZPM是一个复杂的姿态机动制导问题，姿态机动路径规划是其关键。NASA分别于2006.11.5和2007.3.3成功主导实施了两次ZPM任务，其通过建立ZPM最优控制模型，利用Legendre伪谱方法离线求解出一条机动路径，求得的机动路径姿态（角速率）作为离散指令上传到空间站上的计算机。由于机动路径是标称条件下的离线规划，为保证各种可能偏差下任务的成功执行，在进行ZPM前进行了大量的复核验证工作，如果能根据实际条件进行机动路径的在线规划，将有利于提高机动任务的灵活性与可靠性。

采用本专利方法可以实现ZPM次优可行解的快速求解。下面将首先给出ZPM路径规划问题的，然后介绍几种ZPM的OCP描述与利用Radau伪谱方法转化得到的NLP，再介绍介本方法应用的具体实施过程。

2、 ZPM路径规划问题

    2.1、 参考坐标系

首先介绍相关坐标系，包括体系                                                ，轨道系与惯性系。

体系与空间站固连，原点位于空间站质心，轴，轴根据具体定义方向确定，轴由右手定则确定，需要指出的是体系不一定是惯量主轴坐标系。图2以国际空间站ISS为例，示意了体系定义。

轨道系原点位于空间站质心，轴指向地心，轴在轨道平面沿速度方向并与垂直，由右手定则确定，其垂直于轨道平面。每一个轨道周期轨道系将围绕负方向轴旋转一周，若空间站原圆轨道运动，其轨道角速度幅值是一个常数，其满足下述关系：

其中，为地球引力常数， 为空间站质心到地心的距离。

惯性系 的定义为在初始时刻与轨道系重合并此后在惯性空间保持指向不变。

2.2、 数学模型

进行路径规划时，不考虑空间站柔性效应，假设空间站为一刚体。

空间站体系相对于轨道系的运动学方程为：

其中为修正Rodrigues姿态参数，为空间站相对于惯性系的角速度在体系下的表示，为轨道角速度在轨道系下的表示，为轨道系到体系的方向余弦阵。

选择修正Rodrigues参数的原因是其是姿态的最小描述，同时可以在较大范围内避免奇异（沿某轴旋转±360度）。

体系下描述空间站的姿态动力学方程为：

其中 为地球引力梯度力矩， 为气动力矩， 是其它的干扰力矩，由于干扰力矩量级较小并难以准确建模，下面在路径规划问题中将予以忽略。为由控制力矩陀螺产生的控制力矩。

由于ZPM要求控制力矩陀螺工作在性能范围之内，因此控制力矩陀螺角动量的动力学方程也需要加以考虑，下面给出相对于体系的控制力矩陀螺动力学方程：

根据角动量定理，空间站在运动过程中还将遵循下面的关系：

其中  为空间站初始总角动量在惯性系下的表示，总角动量包括空间站平台角动量与控制力矩陀螺的角动量。为惯性系到轨道系的方向余弦阵，为惯性系到体系的方向余弦阵。为空间站在惯性系下的角动量增量，其满足下面的方程：

2.3、 ZPM路径规划问题

ZPM通常应用于空间站TEA间的机动，ZPM从某一TEA状态出发,将空间站机动到另一TEA状态。

由于控制力矩陀螺的最大角动量与最大角动量变化率是一个有限值，ZPM过程中需要满足下述约束：

其中符号表示矢量的模。

ZPM路径规划问题就是寻找这样一条机动路径，沿着该路径空间站能到达指定的终端状态并且满足过程不等式，该路径也即路径规划问题的可行解。

3、ZPM OCP模型与伪谱方法转化得到的NLP

下面将给出ZPM问题中的几个最优控制模型与基于Legendre-Gauss-Radau配点的Legendre伪谱法（简称为Radau伪谱方法）转化得到的NLP:

3.1、角动量最优控制问题模型及相应NLP

角动量最优指在整个ZPM机动过程中控制力矩陀螺的峰值角动量最小，也即沿着该路径控制力矩陀螺拥有最大的角动量冗余，角动量最优控制问题模型为：

此处建立的OCP模型状态方程略有不同于前面的动力学模型，这样处理更有利于路径规划问题的计算。该最优控制问题并不是标准形式，如将视为一个导数为0的状态变量，那么这个问题是一个典型的Mayor型约束最优控制问题，之所以处理是为了更有利于减少不必要的变量。最优控制问题构型中对与路径约束进行了平方处理，这样是为了保证数值求解过程中的路径约束的可微性。

基于Radau伪谱方法转化得到的NLP为：

这里N为Legendre-Gauss-Radau (LGR)配点个数。,,（）分别为对应于第个LGR配点的待优化求解的修正Rodrigues姿态参数，角速度与角动量。在Radau伪谱方法中，进行状态变量插值拟合时除了基于个配点上的状态变量参数外，还包括归一化时间点处的状态变量值。

3.2、能量最优控制问题模型及相应NLP

能量最优指ZPM过程中消耗的能量最少。

由Rarau伪谱方法转化得到的NLP 为:

其中 为Radau伪谱方法的积分权重。

3.3、时间最优控制模型及相应NLP

时间最优指ZPM机动时间最短。

对应的NLP 为:

4、本方法的实施细节

下面将介绍本方法在ZPM路径规划问题中的具体应用：

4.1、满足状态方程约束的解的推导

将修正NLP问题的求解得到状态变量与控制变量称为伪谱法计算结果，根据方法专利的具体技术，从伪谱法结果中选择一定状态变量，综合状态方程与等式约束推导其它状态变量和控制变量。针对ZPM问题，姿态参数变量被选择作为基本状态变量来推导其它状态变量与控制。

为了区分反推变量与伪谱方法求解中的变量，反推解中变量的符号采用上波浪线加以区分，对于选定的伪谱法中计算的姿态参数，显然有：。

通过姿态运动学方程可以推导得到角速度：

根据前面的角动量关系，可以计算处控制力矩陀螺角动量：

再利用动力学方程求解控制时需要首先求解角速度的导数，利用运动学关系可以导出：

进而可求的控制：

对于这样一组满足状态方程约束的解，其相应的路径约束函数为：

上面推导中需要求解关于修正Rodrigues参数值与一二阶导数值，伪谱法利用多项式逼近最优解，从NLP问题中求解的姿态参数是离散时间点上的数值，姿态参数值与一二阶导数值既可通过lagrange多项式插值求解，也可以通过求解出姿态参数多项式的解析形式进行求解，此处选择的后一种方式。

基于以上推导，可以得到满足状态微分方程的可行解。

4.2、可行解求解中的施加补充约束

为保证基于选定状态变量的反推解满足终端边界条件约束与过程约束，在伪谱法自身约束技术的基础上，还施加了下述约束：

1）终端角速度约束

当ZPM结束后达到指定TEA状态时，空间站角速度等于轨道角速度，从运动学方程中可以看到，此时有等价条件，对于从原始NLP中求得的有限节点的近似最优解，其通常并不满足这个条件，为保证反推角速度满足指定的终端条件约束，向原始NLP中补充物理意义为终端姿态导数为0的约束：

2） 终端角动量约束

根据动量矩定理，

当控制力矩陀螺达到终端角动量时，此时有等价条件：

为保证反推控制力矩陀螺角动量的精确满足，向原始NLP中补充约束：

实际研究表明，伪谱法本身满足状态方程约束的技术较准确的求解了环境力矩，故而角动量增量约束也比较准确的得到满足，当ZPM结束空间站到达指定的角速度状态时，反推的终端角动量误差较小，如果不要求控制力矩陀螺终端角动量精确满足，可行解求解过程中终端角动量约束可以不予施加。

3）终端控制约束

基于终端时刻点控制不影响最优解的观点，Radau伪谱方法没有对终端时刻点控制进行约束。当伪谱法中的节点较少时，终端时刻点附近控制相关约束可能会不满足。

为了保证有限节点条件下终端控制的满足，需要施加终端控制约束，需要说明的是，约束的控制变量既可以是反推的控制，也可以是伪谱方法中的控制变量，为了简化约束形式，此处采用对伪谱方法中的控制变量加以约束来间接约束反推控制变量。约束形式为：

由于Radau伪谱法中没有这一参数，因此采用Lagrange外插求解，在伪谱法中对应于归一化时间。

由于反推解与伪谱法的解可能不一致，约束并不是直接施加于反推的控制上，因此可能出现伪谱法结果满足该约束但是反推解不满足的情况，这也是可行解检验中需要解决的问题。

4）终端角速度导数约束

原始的ZPM最优控制问题中对机动结束后空间站角速度导数并没有限制，为了保证终端TEA的平衡稳定，这里可以进一步约束终端角动量导数为0，由运动学方程可知，此时有等价条件，约束形式为：

其中为伪谱法中节点处Lagrange多项式的二阶导数矩阵，其与一阶导数矩阵有关系：

施加这个约束后，根据TEA自身性质，此时控制力矩陀螺角动量变化率的幅值基本为0，这将有利于空间站从机动模式到动量管理模式的过渡。实际上施加这一约束后，就可以不再施加上面的终端控制约束了。

5）初始角速度导数约束

ZPM开始阶段，空间站从一个TEA状态出发，为了使空间站较平稳的进入机动状态，要求初始时刻控制力矩陀螺的输出力矩较小，这既避免过激控制引起的柔性附件振动，同时也利于控制力矩陀螺的力矩输出，可以施加类似于终端角速度导数约束的初始角速度导数为0的约束，约束形式为：

施加该约束后，初始时刻控制力矩陀螺基本为0，这有利于控制力矩陀螺操纵率的对指令力矩的实现。

理论上讲，对于NLP问题，约束越多，越难以求解，但是本方法中增加的约束相较于伪谱法自身的海量约束可以忽略不计，并且约束合理，物理意义明确，其对NLP的计算影响很小，有利于可行解的获得。

4.3、放宽NLP求解的策略

研究表明，将NLP求解成功准则从寻求到最优解放宽为求解到可行解，即满足各种约束，可以大大减少NLP问题求解过程中的迭代次数，但是本身NLP问题的最优解也可快速求得，此处仍然求取的是NLP问题的最优解。

4.4、可行性检验

对于ZPM 问题，由于施加额外约束有效保证了终端边界条件的满足，此处仅对反推的路径约束进行检查，约束检查的方法很直观，就是在整个时间历程选取大量的离散时刻点进行检查，如果对这些离散点路径约束均满足，就认为是在整个时间段均满足。目前采用每一秒间隔进行检验，认为已足够细密。如果约束满足，可行解求解成功，计算结束，否则需要采取一定措施后再行求解。

对于ZMP问题不等式路径约束，如果解可行性不满足，采用约束加强方式继续进行计算，并将上一次计算结果作为初值再次进行计算，约束加强的方式为：针对可行性不满足的具体不等式约束，根据最大相对误差确定加强系数。以角动量路径约束为例：

其中为不等式加强系数，其初值为1，如果反推解可行性验证时不满足该约束，则系数会加强为：

其中为最大相对误差。

综上，应用本方法的ZPM可行解求解流程见图4，首先建立ZMP的OCP模型，基于伪谱法得到相应于最优控制问题的原始ZMP NLP，将上面的补充约束增加到原始ZMP NLP中形成改进ZMP NLP，利用序列二次规划方法对改进 NLP问题解进行求解，然后基于姿态参数对其它状态变量、控制变量与路径约束函数进行反推，得到可能可行解，对解的可行性检查仅针对路径不等式约束函数，如果解可行，计算成功，如果不可行，加强不等式路径约束后，在上一步计算的结果上重新计算。