一种基于模型预测控制的精准空投系统自主归航控制方法

1.本发明涉及导航、制导控制技术领域，尤其是涉及一种基于模型预测控制的精准空投系统自主归航控制方法。


背景技术：

2.精确空投技术能够携带载荷实现远距离、安全、精确自主定点着陆，具有很高的应用价值和广阔的发展前景，在民用、军用领域已得到了广泛的应用。由于翼伞具有飞行灵活，易于部署，载荷能力强，气动稳定性好，滑翔比高等优良特性，已成为精准空投技术的主要实现工具。相较于传统的空投系统，精确空投系统利用翼伞代替圆形降落伞，并配备了先进自主归航控制系统，多种传感器与控制舵机，可以实现精准自主归航。精准空投系统具有重要的应用价值和广泛的应用领域：它可以实现准确投送物资和装备，提高救援和补给行动的效率和安全性。该技术可应用于抗震救灾援助、环境探测以及航天器回收等多种重要战略场合。
3.飞行控制问题是精准空投实现的关键和难点，主要包括动力学建模、轨迹规划和跟踪三个主要子问题：其中动力学建模的目的是描述精准空投系统运动特性，主要用于仿真验证，同时一些轨迹规划和轨迹跟踪方法也是基于相应的简化模型进行设计的。自上个世纪末起，已有大量针对翼伞系统建模的文献，现在比较常用的有六自由度刚体模型、八/九自由度多体动力学模型以及刚柔耦合动力学模型；翼伞系统的归航目前普遍采用分段归航方案，将整个归航轨迹按照不同的阶段和特点分为三段：目标接近段、能量消耗段和着陆段。其中研究较多的是螺旋归航方案，即能量消耗段采用螺旋下降的轨迹，因为这种方案已经在x-38飞行实验中得到成功验证；路径/轨迹跟踪通过设计控制算法，使翼伞能够精准跟踪规划的路径/轨迹，目前普遍采用的算法有pid控制器等算法。现有的控制算法主要分为路径跟踪和轨迹跟踪两类，其中路径上的点不带有期望到达时间信息，而轨迹上的点是和时间一一对应的。由于考虑了时间信息，轨迹跟踪的控制设计往往比路径跟踪更为复杂。
4.飞行器建模、轨迹规划和轨迹/路径跟踪控制三个子问题在衔接时往往存在一些难点，且由于空投系统归航轨迹特有的几何特征，使得模型预测控制在预测时域内所有参考点的计算变得尤为复杂。
5.目前精准空投系统的归航方案研究主要集中在螺旋消高的多段归航方案，与一般的飞行器制导不同，大量的盘旋轨迹会为控制器计算角度带来挑战：由于角度存在周期性，在轨迹持续盘旋时期望偏航角会不断发散，因此需要将角度归一到一个合理的区间，一般的角度归一方法是通过对2π取余，将所有角度归一[0,2π)的区间内。但这种方法很可能带来控制器的发散，尤其是在期望偏航角和实际偏航角都非常接近于0但相互异号，此时偏航角误差实际应为一个小量，但通过将角度归一[0,2π)的区间内，会使得从负方向接近0的偏航角被归一到接近2π的量，使得偏航角误差结果从接近0的小量突然变为接近2π的大量。翼伞作为大惯性的飞行器，这种偏差的大突变是控制器无法迅速消除的，因此会出现一个现象：翼伞系统在盘旋过程中，期望偏航角在所定义的坐标下经过0 rad时，会出现原地盘旋
或者严重偏离预定轨迹的行为。
[0006]
另一个限制精准空投系统轨迹规划和跟踪控制结合的难点在于时间信息的匹配。现有的规划轨迹生成方案主要由四自由度质点模型生成，且轨迹上的点和期望抵达时间一一对应。但现有的轨迹/路径跟踪控制的研究，一部分将参考轨迹简化为没有时间信息的路径进行处理，另一部分将参考轨迹任意指定一段直线或者折线进行跟踪，很少有直接针对带时序信息的真实规划轨迹进行跟踪控制的研究。但由于四自由度模型建立在翼伞飞行过程中速度恒定的假设上，而翼伞在实际飞行过程中，滑翔和盘旋的速度会有小的偏差，这会导致参考轨迹的时间信息和实际飞行的时间信息不匹配，会使得参考点时而在实际位置之前（此时偏航角误差为接近于0的小量），时而在实际位置之后（此时偏航角误差突变为接近于
±
π的值）。这会导致基于偏航角误差的控制器的输入出现频繁的严重突变，也会导致空投系统在归航过程中突然失控。
[0007]
除了难点外，模型预测控制所采用的模型选取与各模型间的参数匹配，以及优化指标设计的统一也需要解决。


技术实现要素：

[0008]
为了解决上述技术问题，本发明提供了一种基于模型预测控制的精准空投系统自主归航控制方法。
[0009]
为实现上述目的，本发明提供了一种基于模型预测控制的精准空投系统自主归航控制方法，包括以下步骤：s1、选取三个不同的模型描述伞物系统运动，包括九自由度多体动力学模型，四自由度质点模型以及面向模型预测控制的三自由度降阶模型，并对三种模型之间的参数进行匹配；s2、轨迹规划算法：对螺旋消高的分段归航轨迹进行几何描述；设计目标函数，利用优化算法进行求解，得到关键轨迹参数；利用s1中四自由度质点模型生成优化后的三维参考轨迹；s3、设计轨迹规划和轨迹跟踪的匹配算法：针对生成的复杂螺旋归航轨迹，提出基于matlab的偏航角计算函数，针对轨迹时序失配，提出动态参考点选择机制；s4、轨迹跟踪算法：利用s1中三自由度降阶模型，设计指标函数，利用模型预估控制进行优化与预测，根据s3中的偏航角计算函数和动态参考点选择机制对s2中的规划轨迹进行跟踪控制。
[0010]
优选的，所述s1中，九自由度多体动力学模型的运动方程为： ；；
；其中，表示连接点的惯性位置，表示欧拉角，表示翼伞，表示载荷；为的简写形式，为的简写形式，为的简写形式；、、是系统速度的三个分量，、、是系统角速度的三个分量，代表俯仰角，代表滚转角，代表偏航角。
[0011]
九自由度多体动力学模型的动力学方程为： ；其中，表示载荷的质量，表示翼伞的总质量，表示翼伞的质量，表示流体惯性的质量，下角标表示流体惯性力对应的物理量，表示翼伞的总惯性，表示翼伞惯性，表示流体惯性，表示从惯性参考系到载荷参考系的转换矩阵，表示从惯性参考系到翼伞参考系的转换矩阵，表示连接点到翼伞质心的矢径、表示从连接点到载荷质心的矢径，带上标“~”的矢量表示其对应的反对称矩阵，表示载荷在其坐标系中的角速度、表示翼伞在其坐标系中的角速度；表示系统铰接点的线速度，表示铰接点受到的约束力；
[0012]
气动力、，气动力矩和重力力矩、表示如下： ；；；其中，向量是翼伞参考系中翼伞的质心速度，是载荷质量，是翼伞质量，表示载荷的面积，表示翼伞的面积，表示升力系数、表示阻力系数，表示四分之一弦长处的俯仰力矩系数；是空气密度，是重力加速度，是翼伞受到的来流动压。
[0013]
优选的，所述s1中，四自由度质点模型方程为：
ꢀ
；其中，是水平方向上的稳态速度，是垂直方向上的稳态速度，是空投系统的偏航角，为系统的偏航速率。
[0014]
优选的，所述s1中，三自由度降阶模型的方程为： ；其中，是降阶三阶系统的振荡角频率，是降阶三阶系统的阻尼比，和是权重因子，是非对称舵偏。
[0015]
优选的，所述s2中，归航轨迹描述为：第1段过渡圆弧的圆心坐标为： ；其中，是初始偏航角，是旋转标志位，当表示空投系统顺时针转动，表示逆时针转动；a是空投点坐标，详见图3，下文b、c、d、e、f、g同，表示翼伞的最小转弯半径。
[0016]
第2段过渡圆弧的圆心坐标为： ；其中，是螺旋段的半径，是在以目标点为原点的坐标系中点d的极角。
[0017]
向量及其与轴夹角 ； ；其中，为bc向量在轴的分量，为bc向量在轴的分量；四段过渡圆弧的圆心角、、、分别为：
ꢀ
； ；其中， ； ；其中， ； ；各特征点对应的临界时间为： ；其中，带下标1~6的时间对应于特征点a、b、c、d、e、f、g；为空投初始高度，、分别表示翼伞系统的水平和垂直稳态速度，、、是中间变量，量纲为弧度。
[0018]
优选的，所述s2中，目标函数为： ；设计，使能量消耗最小 ；通过加权，将和合并为一个优化指标： ；其中，p和q分别为和的加权因子。
[0019]
优选的，所述s2中，三维参考轨迹生成过程为：系统的偏航速率u的表达式为：
ꢀ
；用u作为s1中四自由度质点模型。
[0020]
优选的，所述s3中，偏航角计算函数具体为：通过s2中的优化轨迹的平面坐标计算偏航角 ；其中，atan使用matlab计算反正切函数的计算规则，、是s2中规划轨迹的平面坐标与翼伞实际位置之差，，函数输出的结果是翼伞需要跟踪的期望偏航角，；基于matlab的计算规则，期望偏航角与实际偏航角之差为： ；其中，mod是matlab的取余函数，是空投系统的实际偏航角；如果两者为互为异号的小角度，进行下一步处理：。
[0021]
优选的，所述s3中，动态参考点选择机制为： ；其中，是标志位参数，是s2中生成的参考轨迹对应的参考偏航角，是伞物系统的实际偏航角；当标志 时，立即生成一个点空间，并保存在时间间隔内的参考轨迹中的所有参考点，其中是预设的时间区间；点空间中每个点与实际位置之间的期望偏航角进行计算；选择点空间内第一个满足的点进行跟踪。
[0022]
优选的，所述s4中，将三自由度降阶模型写作：
ꢀ
；其中， ；i
3*3
是3阶单位矩阵，h
mpc
为三自由度模型的采样时间；省略下角标mpc，将上述三自由度降阶模型写作： ；设计优化指标函数： ；其中 ； ； ；和是大小为的对称正半定矩阵，是预测时域，是参考值，在轨迹跟踪问题中指参考偏航角，是预测时域内的输出预测值向量，在轨迹跟踪问题中指预测的实际偏航角向量；u是预测输入向量； ； ；将上述表达式代入mpc的控制指标函数中，化简为： ；为使j最小，对应的控制输入为
ꢀ
；其中，是系统在时间步骤k的状态。
[0023]
本发明所述的一种基于模型预测控制的精准空投系统自主归航控制方法的优点和积极效果是：1、由于在轨迹规划和轨迹跟踪两个环节均考虑了控制精度和能量消耗，在保证控制精度外，还具有更好的节能效果，提高了空投系统的续航能力。
[0024]
2、采用了三种不同的模型，包括一个高保真的多体动力学模型，以及两种不同的简化模型，在准确描述每个子问题中最重要的特征的同时，大大减少了计算量，释放机载计算机的计算压力。
[0025]
3、提出的偏航角计算方法和动态参考点选择机制，实现了轨迹规划和轨迹跟踪的衔接，分别解决了规划轨迹大量盘旋的角度计算问题和与实际轨迹之间的时序失配问题，从而实现轨迹规划和跟踪的整合。
[0026]
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
[0027]
图1为本发明一种基于模型预测控制的精准空投系统自主归航控制方法的流程图；图2为本发明一种基于模型预测控制的精准空投系统自主归航控制方法的翼伞-载荷系统示意图；图3为本发明一种基于模型预测控制的精准空投系统自主归航控制方法的螺旋消高的平面分段归航轨迹示意图；图4为本发明一种基于模型预测控制的精准空投系统自主归航控制方法的优化算法covidoa的流程图；图5为本发明一种基于模型预测控制的精准空投系统自主归航控制方法的动态参考点选择机制的流程图；图6为本发明一种基于模型预测控制的精准空投系统自主归航控制方法实施例的s2生成的规划轨迹；图7为本发明一种基于模型预测控制的精准空投系统自主归航控制方法的实施例的s4中基于mpc的轨迹跟踪效果。
具体实施方式
[0028]
以下通过附图和实施例对本发明的技术方案作进一步说明。
[0029]
如图1所示，一种基于模型预测控制的精准空投系统自主归航控制方法，包括以下步骤：s1、选取三个不同的模型描述伞物系统运动，包括九自由度多体动力学模型，四自由度质点模型以及面向模型预测控制的三自由度降阶模型，并对三种模型之间的参数进行匹配。
[0030]
（1）九自由度多体动力学模型
翼伞-载荷系统的九自由度模型如图2所示，其中点c是翼伞和载荷的连接点。p、b分别是翼伞和载荷的质心。系统参考坐标系的原点固定在连接点c上，所有轴线与惯性坐标系的轴线平行。翼伞和载荷的机体坐标系的原点分别固定在p和b上。
[0031]
翼伞-载荷系统的九自由度多体动力学模型的运动方程为： ；；；其中，表示连接点的惯性位置，表示欧拉角，表示翼伞，表示载荷；为的简写形式，为的简写形式，为的简写形式；九自由度多体动力学模型的动力学方程为： ；其中，表示载荷的质量，表示翼伞的总质量，表示翼伞的总惯性，表示从惯性参考系到载荷参考系的转换矩阵，表示从惯性参考系到翼伞参考系的转换矩阵，表示连接点到翼伞质心的矢径、表示从连接点到载荷质心的矢径。带上标“~”的矢量表示其对应的反对称矩阵，用于方便地表示向量的叉乘运算。表示载荷在其坐标系中的角速度、表示翼伞在其坐标系中的角速度，气动力、，气动力矩和重力力矩、表示如下： ；；
；其中，向量是翼伞参考系中翼伞的质心速度，非对称舵偏通过影响模型中的横向气动参数、、来操控翼伞的转向。
[0032]
（2）四自由度质点模型由于系统主要处于稳态飞行状态，其姿态在生成期望轨迹时可以忽略，则系统可视为一个具有偏航角的质点。
[0033]
四自由度质点模型方程为： ；其中，是水平方向上的稳态速度，是垂直方向上的稳态速度，是需要预先规定的常数，由翼伞-载荷系统的规格唯一确定。是空投系统的偏航角，为系统的偏航速率，用于描述舵机的能量消耗。
[0034]
（3）三自由度降阶模型在横向轨迹跟踪中，主要关注的是偏航角ψ与非对称舵偏δa之间的变化；另外，由于九自由度多体动力学模型的动态过程十分复杂，简化后的模型一般都很难对动态过程进行准确描述，故此处直接将其简化为一个降阶的三阶动力学过程，在保证稳态性能匹配的前提下对动态过程进行大致近似。
[0035]
三自由度降阶模型的方程为： ；其中，是降阶三阶系统的振荡角频率，是降阶三阶系统的阻尼比，和是权重因子，用于描述偏航角速率的导数对输入的影响。是非对称舵偏。
[0036]
（4）三种模型之间的参数匹配九自由度多体动力学模型与四自由度质点模型的匹配：只需要确定和，其物理意义分别是水平和垂直方向上的稳态速度，通过对九自由度多体动力学模型进行仿真，在运动进入稳态后计算水平速度和垂直速度，代入四自由度质点模型中即可完成匹配。
[0037]
九自由度多体动力学模型与三自由度降阶模型的匹配：参数和描述的是非对称舵偏对的二阶导和一阶导的影响，则和最终会影响的斜率，由于a是直接作用在二阶导上，因此对斜率的影响更大，和的值越大，的
斜率越大。而和的物理意义是三阶系统振荡角频率和阻尼比，两者共同影响过渡过程的震荡周期和震荡幅值，但对震荡周期的影响更大，其值越大震荡周期越短，的增大会略微增大震荡周期，但更主要的作用是降低震荡幅值。
[0038]
在匹配三自由度降阶模型的参数的具体步骤是：1）九自由度模型的仿真中绘制非对称舵偏对的曲线，和三自由度降阶模型的输出曲线对比；2）首先调节稳态参数和，按照上述匹配原则，使两个模型稳态时的曲线尽量重合；3）最后调节过渡过程参数和，使得过渡过程变化趋势大致相同。
[0039]
因为九自由度的复杂性，三阶系统是不可能完全在动态过程上与之匹配的，只能关注大体变化趋势。
[0040]
s2、轨迹规划算法：对螺旋消高的分段归航轨迹进行几何描述；设计目标函数，利用优化算法进行求解，得到关键轨迹参数；利用s1中四自由度质点模型生成优化后的三维参考轨迹。
[0041]
（1）归航轨迹描述为：轨迹平面示意图如图3所示。其中a点为初始点，g为目标点。归航过程主要分为几个部分： ab、cd和ef为过渡段圆弧，半径为根据翼伞转弯极限预设的最小转弯半径。 bc是滑翔段，de是用于消耗冗余高度的螺旋段。fg是着段，其航向是迎风的，消耗降落伞速度，实现软着陆。、、、是过渡圆弧的中心角。是螺旋段的半径,是在以目标点为原点的坐标系中点d的极角。
[0042]
第1段过渡圆弧的圆心坐标为： ；其中，是初始偏航角，是旋转标志位，当表示空投系统顺时针转动，表示逆时针转动；第2段过渡圆弧的圆心坐标为： ；其中，是螺旋段的半径，是在以目标点为原点的坐标系中点d的极角，bc向量及其与轴夹角 ； ；
其中，为bc向量在轴的分量，为bc向量在轴的分量；四段过渡圆弧的圆心角、、、分别为： ； ；其中，； ；其中，； ；各特征点对应的临界时间为： ；其中，带下标1~6的对应于特征点a、b、c、d、e、f、g。
[0043]
（2）平面优化规划路径要优化的变量是和，通过这两个参数可以唯一确定多段归航方案的平面路径。
[0044]
首先设计目标函数，使得规划轨迹准确降落在目标点上： ；设计，使能量消耗最小 ；通过加权，将和合并为一个优化指标：
其中，和分别为和的加权因子，描述对降落精度和能量消耗的权衡取舍。
[0045]
如图4所示，采用冠状病毒优化算法（covid optimization algorithm, covidoa）对指标函数j（在covioa中被称为适应度函数）进行优化。covidoa 是一种模拟冠状病毒劫持人类细胞机制的进化搜索策略。
[0046]
（3）三维轨迹生成将平面路径转化为三维轨迹的核心思想是：从之前设计的平面路径中搜索四自由度模型的信息，并将其记录下来，然后应用于四自由度模型。
[0047]
由于被定义为系统的偏航速率，通过简化，可以得到的表达式如下： ；用表达式给出的u作用于s1中的四自由度质点模型，即可获得包含时序信息的精准空投系统三维归航轨迹。
[0048]
s3、设计轨迹规划和轨迹跟踪的匹配算法：针对生成的复杂螺旋归航轨迹，提出基于matlab的偏航角计算函数，以解决螺旋过程中角度周期变化带来的控制器发散问题；针对轨迹时序失配，提出动态参考点选择机制，可以解决规划轨迹和实际轨迹时序失配的问题。
[0049]
（1）偏航角计算函数具体为：基于matlab的计算规则，首先提出一种函数，通过s2中的优化轨迹的平面坐标计算参考偏航角 ；其中，atan使用matlab计算反正切函数的计算规则，、是s2中规划轨迹的平面坐标与翼伞实际位置之差，，函数输出的结果是翼伞需要跟踪的期望偏航角，。
[0050]
基于matlab的计算规则，期望偏航角与实际偏航角之差为： ；其中，mod是matlab的取余函数，是空投系统的实际偏航角。
[0051]
这样做的目的是为了把期望偏航角和实际偏航角都归一到 [0,2π) 的区间内，但如果两者为互为异号的小角度，依然会导致偏航角误差计算错误，因此进行下一步处理：
ꢀ
；这样做的目的是：若发现期望偏航角位于的区间内，而实际偏航角位于的区间内，则计算偏航角误差时会由于角度的周期性产生计算错误（因为这种情况下根据原本的偏航角误差计算公式，实际偏航角应该选择为是。
[0052]
另外需要注意的是，这种判断方法的界在这里是以和给出的，但根据螺旋消高段的角速不同，这个条件可以略微的收紧。
[0053]
这种偏航角误差计算方法是面向mpc的，因此不仅仅用于计算当前时刻的偏航角误差，而是要用于整个mpc预测时域内偏航角误差的计算。
[0054]
（2）为了解决当实际位置和规划轨迹时序失配的问题，给出了一个用于判断参考点和实际点相对位置的标志位参数： ；其中，是s2中生成的参考轨迹对应的参考偏航角，是伞物系统的实际偏航角。
[0055]
表示参考点在实际点前方，而表示参考点落后于实际点。当时，控制器会被误导以跟踪实际位置后方的点，而实际上应该继续前进。因此，选择的参考点应进行修正。由于计划轨迹和实际轨迹在时间上大致对应，当出现这种情况时，正确的参考点不会偏离太远。
[0056]
如图5所示，根据标志位参数，动态参考点选择机制为：当标志时，立即生成一个点空间，并保存在时间间隔内的参考轨迹中的所有参考点，其中是预设的时间区间；点空间中每个点与实际位置之间的期望偏航角进行计算。
[0057]
接下来有两种方法来解决这个问题：在初/中制导段，系统的续航更为重要，因此应选择偏航角误差最小的点进行跟踪，以便在较小的舵偏下跟踪规划轨迹，使能量消耗最小；终制导段是精准空投系统落地的临门一脚，更注重准确性，因此应选择点空间内第一个满足的点进行跟踪，以便在最短的时间内追踪参考轨迹。
[0058]
s4、轨迹跟踪算法：利用s1中三自由度降阶模型，设计指标函数，利用模型预估控制（model predictive control，mpc）进行优化与预测，根据s3中的偏航角计算函数和动态参考点选择机制对s2中的规划轨迹进行跟踪控制。
[0059]
将三自由度降阶模型写作： ；其中，
ꢀ
；是3阶单位矩阵，为三自由度模型的采样时间；省略下角标mpc，将上述三自由度降阶模型写作： ；设计优化指标函数： ；其中 ； ； ；和是大小为的对称正半定矩阵，是预测时域，是参考值，在轨迹跟踪问题中指参考偏航角，是预测时域内的输出预测值向量，在轨迹跟踪问题中指预测的实际偏航角向量；u是预测输入向量；mpc的优化指标j中的的物理意义是预测试域内的偏航角误差向量，其中的每个元素都需要采用s3中的偏航角误差计算方法，否则会因为角度的周期性导致mpc失效。
[0060] ； ；将上述表达式代入mpc的控制指标函数中，化简为： ；为使j最小，对应的控制输入为
ꢀ
；其中，w是参考信号，xk是系统在时间步骤k的状态。
[0061]
在mpc中只有u的第一个元素uk被应用于控制系统，其物理意义是非对称舵偏δa的控制信号。
[0062]
实施例以一种展长为7.5m的中型翼伞为例，翼伞具体参数如下，主要包括几何参数和气动参数： ；另外，升力系数、阻力系数以及四分之一弦长处的俯仰力矩系数随翼伞攻角的变化以表格形式给出，便于matlab插值使用。
[0063]
；s1，将翼伞的关键参数带入九自由度多体动力学模型的表达式中，可得到用于描述翼伞-载荷系统运动特性的高精度模型，此模型主要用于仿真验证。
[0064]
接下来确定另外两个模型，由s1提出的模型参数对应关系，有：四自由度质点模型中：；三自由度线性模型中：;
将上述参数代入s1中的模型表达式中即可唯一确定模型。到此，s1中的三个模型已经确定，s1完成。
[0065]
接下来进行s2，生成精准空投系统归航的期望轨迹。
[0066]
首先给定初始条件：以空投的目标点为原点建立坐标系，空投初始坐标a = (1000,
ꢀ‑
700,
ꢀ‑
1200) m，初试偏航角，目标点g = (0,0,0) m, 最小转弯半径。
[0067]
优化指标的权重因子选取为p = 100, q = 0.1，将上述参数结合s2中的轨迹几何描述的公式，代入s2中优化指标函数j的具体表达式中，使用covidoa对优化指标进行优化，得到平面轨迹的最优解，即螺旋下降段的半径和进入角参数r
et = 223.175, θ
et =
ꢀ−
3.1274。
[0068]
根据s2中给出偏航角速的计算公式，可从平面轨迹中求得s1中提出的四自由度质点模型的输入。在matlab中将该数据保存，代入四自由度模型中，其生成的三维轨迹即为上述最优平面路径对应的带有时间信息的最优三维轨迹。得到的轨迹如图6所示。
[0069]
遵循s3中的偏航角计算规则和动态点选择机制，取时间区间。遵循步骤s4中mpc的设计方法，取预测时域, mpc更新步长为。其中预测时域内的每一个预测点（包括预测输出和参考点）都需要采用s3中的偏航角计算规则和动态点选择机制进行处理，否则会造成参考轨迹和实际轨迹不匹配而控制失效。
[0070]
将s1中的三自由度降阶线性化模型和上述数据带入s4中的mpc公式中，可求解出最优预测控制率，令其作用于九自由度模型中的非对称舵偏，则可跟踪s2中生成的规划轨迹，可获得完整过程的仿真效果。跟踪效果如图7所示。
[0071]
因此，本发明采用上述一种基于模型预测控制的精准空投系统自主归航控制方法，减小了计算量，实现了轨迹规划和跟踪的整合；具有控制精度高，节能效果好的优点。
[0072]
最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。

技术特征：
1.一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，包括以下步骤：s1、选取三个不同的模型描述伞物系统运动，包括九自由度多体动力学模型，四自由度质点模型以及面向模型预测控制的三自由度降阶模型，并对三种模型之间的参数进行匹配；s2、轨迹规划算法：对螺旋消高的分段归航轨迹进行几何描述；设计目标函数，利用优化算法进行求解，得到关键轨迹参数；利用s1中四自由度质点模型生成优化后的三维参考轨迹；s3、设计轨迹规划和轨迹跟踪的匹配算法：针对生成的复杂螺旋归航轨迹，提出基于matlab的偏航角计算函数，针对轨迹时序失配，提出动态参考点选择机制；s4、轨迹跟踪算法：利用s1中三自由度降阶模型，设计指标函数，利用模型预估控制进行优化与预测，根据s3中的偏航角计算函数和动态参考点选择机制对s2中的规划轨迹进行跟踪控制。2.根据权利要求1所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s1中，九自由度多体动力学模型的运动方程为： ；；；其中，表示连接点的惯性位置，表示欧拉角，表示翼伞，表示载荷；为的简写形式，为的简写形式，为的简写形式；、、是系统速度的三个分量，、、是系统角速度的三个分量，代表俯仰角，代表滚转角，代表偏航角；九自由度多体动力学模型的动力学方程为：
ꢀ
；其中，表示载荷的质量，表示翼伞的总质量，表示翼伞的质量，表示流体惯性的质量，下角标表示流体惯性力对应的物理量，表示翼伞的总惯性，表示翼伞惯性，表示流体惯性，表示从惯性参考系到载荷参考系的转换矩阵，表示从惯性参考系到翼伞参考系的转换矩阵，表示连接点到翼伞质心的矢径、表示从连接点到载荷质心的矢径，带上标“~”的矢量表示其对应的反对称矩阵，表示载荷在其坐标系中的角速度、表示翼伞在其坐标系中的角速度；表示系统铰接点的线速度，表示铰接点受到的约束力；气动力、，气动力矩和重力力矩、表示如下： ；；；其中，向量是翼伞参考系中翼伞的质心速度，是载荷质量，是翼伞质量，表示载荷的体积，表示载荷的面积，表示翼伞的面积，表示升力系数、表示阻力系数，表示四分之一弦长处的俯仰力矩系数，是空气密度，是重力加速度，是翼伞受到的来流动压。3.根据权利要求2所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s1中，四自由度质点模型方程为：
ꢀ
；其中，是水平方向上的稳态速度，是垂直方向上的稳态速度，是空投系统的偏航角，为系统的偏航速率。4.根据权利要求3所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s1中，三自由度降阶模型的方程为： ；其中，是降阶三阶系统的振荡角频率，是降阶三阶系统的阻尼比，和是权重因子，是非对称舵偏。5.根据权利要求4所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s2中，归航轨迹描述为：第1段过渡圆弧的圆心坐标为： ；其中，是初始偏航角，是旋转标志位，当表示空投系统顺时针转动，表示逆时针转动；是空投点坐标，表示翼伞的最小转弯半径；第2段过渡圆弧的圆心坐标为： ；其中，是螺旋段的半径， 是在以目标点为原点的坐标系中点的极角；向量及其与轴夹角 ；
ꢀ
；其中，为向量在轴的分量，为向量在轴的分量；四段过渡圆弧的圆心角、、、分别为： ； ；其中， ； ；其中， ； ；各特征点对应的临界时间为： ；其中，带下标1~6的时间对应于特征点a、b、c、d、e、f、g；为空投初始高度，表示翼伞系统的水平稳态速度，表示翼伞系统的垂直稳态速度，、、是中间变量，量纲为弧度。
6.根据权利要求5所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s2中，目标函数为： ；设计，使能量消耗最小 ；通过加权，将和合并为一个优化指标： ；其中，和分别为和的加权因子。7.根据权利要求6所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s2中，三维参考轨迹生成过程为：系统的偏航速率的表达式为： ；用作为s1中四自由度质点模型。8.根据权利要求7所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s3中，偏航角计算函数具体为：通过s2中的优化轨迹的平面坐标计算偏航角 ；其中，使用计算反正切函数的计算规则，、是s2中规划轨迹的平面坐标与翼伞实际位置之差，，函数输出的结果是翼伞需要跟踪的期望偏航角，；
基于的计算规则，期望偏航角与实际偏航角之差为： ；其中，是的取余函数，是空投系统的实际偏航角；如果两者为互为异号的小角度，进行下一步处理：。9.根据权利要求8所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s3中，动态参考点选择机制为： ；其中，是标志位参数，是s2中生成的参考轨迹对应的参考偏航角，是伞物系统的实际偏航角；当标志
ꢀꢀ
时，立即生成一个点空间，并保存在时间间隔内的参考轨迹中的所有参考点，其中是预设的时间区间；点空间中每个点与实际位置之间的期望偏航角进行计算；选择点空间内第一个满足的点进行跟踪。10.根据权利要求9所述的一种基于模型预测控制的精准空投系统自主归航控制方法，其特征在于，所述s4中，将三自由度降阶模型写作： ；其中，；是3阶单位矩阵，为三自由度模型的采样时间；省略下角标，将上述三自由度降阶模型写作：
ꢀ
；设计优化指标函数： ；其中 ； ； ；和是大小为的对称正半定矩阵，是预测时域，是参考值，在轨迹跟踪问题中指参考偏航角，是预测时域内的输出预测值向量，在轨迹跟踪问题中指预测的实际偏航角向量；是预测输入向量； ； ；将上述表达式代入mpc的控制指标函数中，化简为： ；为使最小，对应的控制输入为 ；其中，是系统在时间步骤的状态。

技术总结
本发明公开了一种基于模型预测控制的精准空投系统自主归航控制方法，属于导航、制导控制技术领域。该控制方法包括以下步骤：S1、选取三个不同的模型描述伞物系统运动，并对三种模型之间的参数进行匹配；S2、轨迹规划算法，设计目标函数，得到关键轨迹参数，利用四自由度质点模型生成优化后的三维参考轨迹；S3、设计轨迹规划和轨迹跟踪的匹配算法，提出基于MATLAB的偏航角计算函数，提出动态参考点选择机制；S4、根据偏航角计算函数和动态参考点选择机制对规划轨迹进行跟踪控制。本发明采用上述一种基于模型预测控制的精准空投系统自主归航控制方法，减小了计算量，实现了轨迹规划和跟踪的整合；具有控制精度高，节能效果好的优点。优点。优点。


技术研发人员：冉茂鹏 刘乃源 王振乾 吕金虎
受保护的技术使用者：北京航空航天大学
技术研发日：2023.09.13
技术公布日：2023/10/20