翼伞系统航迹规划方法和系统、航迹跟踪控制方法和系统与流程

本发明属于翼伞航迹规划和跟踪控制
技术领域：
，具体涉及一种翼伞系统的航迹规划方法及系统，以及跟踪规划航迹的方法及系统。
背景技术：
：冲压式翼伞是一种由纺织材料构成的柔性飞行器，开伞后空气由翼伞前缘切口进入气室，在气室内形成滞止压力，使翼伞能保持较为稳定的翼形并产生升力和阻力，因此翼伞具有较高升阻比、优良的滑翔性能和可控性。拉拽伞衣后缘可以调整翼伞飞行方向和速度，实现精确着陆，克服了传统圆形降落伞飞行轨迹随风飘、落点散布大的缺点，同时翼伞在着陆时可以以雀降方式无损着陆，在战场物资精确空投、自然灾害救灾物资精确空投、航天器回收等领域有广泛应用前景，得到了国内外许多研究者的关注。为翼伞系统规划出合适归航航迹是能否实现精确空投的前提之一，很大程度上决定了翼伞的着陆精度和归航控制方式，只有在合适的规划航迹的基础上才能设计合适的航迹跟踪控制器，因此航迹规划对实现翼伞精确空投具有重要意义。翼伞系统航迹规划是指在翼伞动力学约束基础上，为翼伞系统规划出从初始空投点到目标点的、满足特定性能指标的归航航迹。早期归航主要为径向归航和锥型归航，由于其归航精度较低，现已较少采用，目前主要的翼伞系统航迹规划方法为最优控制归航法和分段归航法。最优控制归航法以准确、安全、控制能量小等为优化目标，求解算法主要包括间接法和直接法，文献：翼伞系统动力学与归航方案研究[d].国防科学技术大学,2005，采用间接法求解了翼伞系统的最优航迹规划问题，利用极小值原理将最优控制问题转化为两点边值问题再求解，但该方法需要先对状态方程进行正向积分，再对协态方程进行反向积分，过程较为繁琐复杂。文献：基于伪谱法的翼伞系统归航轨迹容错设计[j].控制理论与应用,2013(06):702-708、基于高斯伪谱法的翼伞系统复杂多约束轨迹规划[j].航空学报,2017(03):220-230等运用直接法求解了翼伞最优控制归航航迹规划问题，主要利用伪谱法将最优控制航迹规划问题转化为非线性规划问题，再利用序列二次规划算法求解该问题。文献：multi-objectivetrajectoryoptimizationmethodofparafoilbasedonparticleswarmalgorithm[c]//2019chinesecontrolconference(ccc),27-30july2019利用伪谱法求解了翼伞航迹规划问题，并利用pso法确定pareto优化点，进一步改进了航迹规划效果。文献trajectoryoptimizationviaparticleswarmsforrobustparafoilguidance[c]//2018aiaaguidance,navigation,andcontrolconference,8–12january2018.直接利用pso算法实现了翼伞鲁棒制导，规划的航迹可有效降低翼伞着陆误差和着地冲击速度。文献：基于遗传算法带约束的翼伞系统归航轨迹设计[j].中南大学学报(自然科学版),2017(02):404-410中将最优控制航迹规划问题转化为b样条基函数控制顶点的参数优化问题，然后采用遗传算法或量子遗传算法等智能算法进行目标函数寻优。文献：翼伞系统最优归航轨迹设计的敏感度分析方法[j].控制理论与应用,2015(08):1003-1011基于敏感度分析方法，利用控制变量参数化与时间尺度相结合的优化算法，将航迹规划问题转化为一系列参数优化问题再进行数值求解。需要指出的是，这类最优控制航迹规划方法获得的最优控制量往往都是连续的，工程上不易实施。分段归航法由于操纵过程简单、鲁棒性强，因此在x-38等系统中得到了实际应用。文献：翼伞系统分段归航轨迹的优化设计[j].航天返回与遥感,2004(03):11-16中基于遗传算法，研究了翼伞系统的分段归航航迹设计问题，规划的航迹被分段为目标接近段、能量控制段和逆风着陆段，目标接近段和逆风主路段主要实施滑翔运动、能量管理段主要为螺旋线下降的转弯运动。文献：基于能量约束的翼伞系统分段归航设计与仿真[j].航天控制,2011,29(5):43-47基于改进粒子群算法设计了翼伞系统分段归航航迹，仿真表明设计的归航轨迹简单实用，满足落点精度要求。郑成基于iaga改进遗传算法有效求解了分段归航轨迹问题，提出的算法可有效防止早熟，收敛速度更快，规划的航迹满足定点和逆风着陆的要求。翼伞系统在较大风场中的归航控制[j].控制理论与应用,2016(12):1630-1638基于粒子群算法优化了分段归航轨迹，并采用ladrc控制器对航迹进行了修正，仿真结果表明该归航控制可提高抗风性能和归航精度。文献：基于iafsa的四自由度翼伞分段归航设计[j].火力与指挥控制,2017(02):64-68.基于人工鱼群算法对分段航迹目标函数进行了参数寻优，提出的算法可加快算法收敛速度，规划航迹满足精确落点和逆风着陆的要求。上述分段归航算法采用的目标函数大同小异，都是将航迹规划问题转化为进入点(entrypoint)参数的优化问题，尽管采取的算法在收敛速度方面有差异，但得到的结果是大致相同的，都能满足精确着陆和逆风着陆要求。需要注意的是，分段归航算法的优化函数中一般不包含能耗指标，因此分段归航法在能量消耗方面不占优势，此外，获得的航迹中包含多个半径最小的过渡转弯段，此时需要突然将控制绳下拉到最大量，然后短时间内又将其恢复到较小控制量，这对翼伞的实际操控提出了更高的要求。技术实现要素：发明目的：针对现有技术中存在的问题，本发明提供了一种翼伞系统航迹规划方法，该方法得到的规划航迹满足精确且逆风着陆的要求，且航迹能量消耗较低，同时翼伞归航时的操控简单，控制量变化幅度较小，为系统提供了更大的控制量冗余，航迹平滑合理，便于跟踪。技术方案：本发明一方面公开了一种翼伞系统航迹规划方法，包括：s11、建立风固定坐标系下的翼伞降阶质点模型：其中(x,y,h)分别为翼伞系统在风固定坐标系中水平面x方向、y方向和垂直方向的位置分量，vs为翼伞系统水平方向速度，vz为垂直方向速度，ψ为航向角，为航向角速率，u为与翼伞非对称下偏量对应的控制量；s12、确定翼伞系统在空投初始时刻t0的位置(x0,y0,h0)和航向角ψ0、翼伞系统在着陆时刻tf的期望位置(xf,yf,hf)和期望航向角ψf、翼伞操纵绳的最大下拉量umax；s13、建立翼伞系统航迹规划的目标函数：j＝f1j1+f2j2+f3j3；其中j1＝(x(tf)-xf)2+(y(tf)-yf)2，为着陆点水平位置误差目标；j2＝cos(ψ(tf))+1，为着陆点航向误差目标；为控制能耗目标；f1、f2和f3为加权因子；x(tf)、y(tf)、ψ(tf)分别为规划航迹上翼伞在着陆时刻的水平面x方向、y方向的坐标和航向角；s14、将翼伞飞行时段[t0,tf]划分为n个相邻区间，每个子区间内u(t)取常数值以控制翼伞系统，即：其中：将u(t)表示为序列[σk]，k＝1,…,n；s15、求解翼伞飞行时段每个区间的最优控制量σk，使目标函数值j取最小值，得到最优控制量序列s16、根据最优控制量序列和翼伞的初始状态和速度，推导出规划的航迹path(t)＝(x(t),y(t),h(t))，t∈[t0,tf]；其中：h(t)＝h0+vzt，步骤s15中采用梯度下降法求解翼伞飞行时段的最优控制量序列，具体包括：s151、设置学习率λ，容许误差e，迭代次数最大值l，控制量步长△σk；在每个子区间控制量的上下界内随机初始化控制量序列为根据计算目标函数的初始值j0；初始化迭代次数l＝0，初始化每个子区间的负梯度s152、更新每个子区间的控制量如果则令如果则令根据更新后的控制量序列计算目标函数值jl+1；计算当前迭代的负梯度：s153、判断是否达到迭代结束条件，所述迭代结束条件为：当前迭代次数l≥l或目标函数值变化量|jl+1-jl|<e；如达到迭代结束条件，则控制量序列为最优控制量序列，结束迭代；如不满足迭代结束条件，令迭代次数l＝l+1，跳转至步骤s152继续下一次迭代。根据控制量序列[σk]计算目标函数值j的步骤为：计算翼伞飞行时长：根据质点模型得到规划航迹上翼伞系统着陆时刻的航向角和水平面位置：其中计算着陆点水平位置误差目标值j1和着陆点航向误差目标值j2；根据翼伞飞行时段[t0,tf]内翼伞的控制量u(t)，计算控制能耗目标值j3；计算目标函数值j＝f1j1+f2j2+f3j3。另一方面，本发明还公开了一种航迹跟踪控制方法，包括：s21、根据上述翼伞系统航迹规划方法得到翼伞的规划航迹pref(t)，t∈[t0,tf]；s22、在时刻t计算水平面内翼伞系统与规划航迹的位置偏差向量计算翼伞侧偏距：计算翼伞的航向角偏差计算翼伞的下滑角偏差：γerror(t)＝γref(t)-γ0(t)计算翼伞的高度误差：其中pref(t).x、pref(t).y为规划航迹上时刻t的水平面内x方向、y方向的坐标值；xt、yt为翼伞系统在时刻t的实际位置；为规划航迹上时刻t的航向角；为与x轴的夹角；为与规划航迹点pref(t)处切线的夹角；为翼伞系统在时刻t的实际航向角；γref(t)为规划航迹点pref(t)处的下滑角；γ0(t)为翼伞系统中领航翼伞在时刻t的下滑角；s23、计算时刻t的水平面跟踪控制量和纵向跟踪控制量：其中u1为翼伞系统的水平面跟踪控制量，通过非对称下拉来控制翼伞的水平转弯角速率实现水平面航向调节；u2为翼伞系统的纵向跟踪控制量，通过对称下拉来控制翼伞的下滑角速率实现纵向高度调节；u1max和u2max分别为翼伞非对称下拉和对称下拉的最大值，k1、k2、k3、k4分别为侧偏系数、航向角偏差系数、高度误差系数、下滑角偏差系数。在翼伞飞行过程中，循环执行步骤s22和s23，直到翼伞系统着陆。本发明还公开了实现上述翼伞系统航迹规划方法的系统，包括：翼伞降价质点模型建立模块11，用于建立风固定坐标系下的翼伞降阶质点模型：其中(x,y,h)分别为翼伞系统在风固定坐标系中水平面x方向、y方向和垂直方向的位置分量，vs为翼伞系统水平方向速度，vz为垂直方向速度，ψ为航向角，为航向角速率，u为与翼伞非对称下偏量对应的控制量；翼伞系统初始状态和着陆期望状态确定模块12，用于确定翼伞系统在空投初始时刻t0的位置(x0,y0,h0)和航向角ψ0、翼伞系统在着陆时刻tf的期望位置(xf,yf,hf)和期望航向角ψf、翼伞操纵绳的最大下拉量umax；目标函数建立模块13，用于建立翼伞系统航迹规划的目标函数：j＝f1j1+f2j2+f3j3；其中j1＝(x(tf)-xf)2+(y(tf)-yf)2，为着陆点水平位置误差目标；j2＝cos(ψ(tf))+1，为着陆点航向误差目标；为控制能耗目标；f1、f2和f3为加权因子；x(tf)、y(tf)、ψ(tf)分别为规划航迹上翼伞在着陆时刻的水平面x方向、y方向的坐标和航向角；飞行时段区间划分模块14，用于将翼伞飞行时段[t0,tf]划分为n个相邻区间，每个子区间内u(t)取常数值以控制翼伞系统，即：其中：将u(t)表示为序列[σk]，k＝1,…,n；最优控制量计算模块15，用于求解翼伞飞行时段每个区间的最优控制量σk，使目标函数值j取最小值，得到最优控制量序列规划航迹推导模块16，用于根据最优控制量序列和翼伞的初始状态和速度，推导出规划的航迹path(t)＝(x(t),y(t),h(t))，t∈[t0,tf]。本发明还公开了实现上述航迹跟踪控制方法的系统，包括：航迹规划模块21，用于根据上述翼伞系统航迹规划方法得到翼伞的规划航迹pref(t)，t∈[t0,tf]；偏差计算模块22，用于计算时刻t水平面内翼伞系统与规划航迹的位置偏差向量翼伞侧偏距lxy(t)、航向角偏差翼伞的下滑角偏差γerror(t)、翼伞的高度误差herror(t)；跟踪控制量计算模块23，用于计算时刻t的水平面跟踪控制量和纵向跟踪控制量。有益效果：与现有技术相比，本发明公开的翼伞系统航迹规划方法在考虑翼伞初值约束、终点逆风精确着陆约束、控制约束情况下，将控制变量参数化为一系列分段常值，在翼伞质点模型状态方程约束下，通过梯度下降法求解使翼伞系统水平位置误差、着陆点航向误差以及控制能耗的加权和最小的控制量，从而得到规划航迹。该方法得到规划航迹能够满足精确、逆风着陆的要求，且航迹能量消耗更低，控制量由分段常值构成，简化了翼伞归航时的操控，同时控制量的值在一个较小范围内变化，为系统提供了更大的控制量冗余，有利于航迹跟踪控制时的偏差修正。附图说明图1为本发明公开的翼伞航迹规划方法流程图；图2为飞行时段分段常值控制示意图；图3为实施例一中规划航迹对比示意图；图4为实施例一中规划航迹航向角的对比示意图；图5为实施例一控制量对对比示意图；图6为实施例一中目标函数值随迭代次数的变化示意图；图7为实施例一中翼伞航迹规划系统的组成示意图；图8为实施例二中公开的航迹跟踪控制方法流程图；图9为实施例二中叠加随机阵风干扰后的翼伞速度变化示意图；图10为实施例二中翼伞水平面航迹跟踪结果示意图；图11为实施例二中翼伞纵向航迹跟踪结果示意图；图12为实施例二中翼伞航迹跟踪过程中的侧向误差曲线图；图13为实施例二中翼伞航迹跟踪过程中的纵向误差曲线图；图14为实施例二中翼伞航迹跟踪过程中控制量的变化曲线图；图15为实施例二中公开的航迹跟踪控制系统的组成示意图。具体实施方式下面结合附图和具体实施方式，进一步阐明本发明。实施例1：如图1所示，本发明公开了一种翼伞系统航迹规划方法，包括：s11、建立风固定坐标系下的翼伞降阶质点模型：其中(x,y,h)分别为翼伞系统在风固定坐标系中水平面x方向、y方向和垂直方向的位置分量，vs为翼伞系统水平方向速度，vz为垂直方向速度，ψ为航向角，为航向角速率，u为与翼伞非对称下偏量对应的控制量；s12、确定翼伞系统在空投初始时刻t0的位置(x0,y0,h0)和航向角ψ0、翼伞系统在着陆时刻tf的期望位置(xf,yf,hf)和期望航向角ψf、翼伞操纵绳的最大下拉量umax；翼伞系统着陆的期望位置根据着陆要求确定，其期望航向角ψf需要满足逆风着陆的要求，即ψf为着陆目标点风向的反方向，假定着陆点风向与x轴正向一致，即ψf＝180°，或ψf＝-180°，则逆风着陆条件可转化为cosψf＝-1，此时若快速下拉翼伞两侧操纵绳，可实现翼伞的逆风雀降。s13、建立翼伞系统航迹规划的目标函数：j＝f1j1+f2j2+f3j3(2)其中j1＝(x(tf)-xf)2+(y(tf)-yf)2，为着陆点水平位置误差目标；j2＝cos(ψ(tf))+1，为着陆点航向误差目标；为控制能耗目标；f1、f2和f3为加权因子，可根据不同任务的不同侧重点，选择不同的取值；x(tf)、y(tf)、ψ(tf)分别为规划航迹上翼伞在着陆时刻的水平面x方向、y方向的坐标和航向角；s14、将翼伞飞行时段[t0,tf]划分为n个相邻区间，每个子区间内u(t)取常数值以控制翼伞系统，如图2所示，即：其中：将u(t)表示为序列[σk]，k＝1,…,n；s15、求解翼伞飞行时段每个区间的最优控制量σk，使目标函数值j取最小值，得到最优控制量序列步骤s14使控制量离散化，则航迹规划转换为控制序列优化选择问题，本发明采用梯度下降法求解翼伞飞行时段的最优控制量序列。函数j(u(t))在某u(t)取值处的梯度方向，是j下降最快的方向，梯度下降法理论简单，编程较容易实现。在求目标函数的最小值时，从定义域内任意初始点出发，沿着负梯度方向走可以最快到达极小值点。具体包括步骤s151-s153：s151、设置学习率λ，容许误差e，迭代次数最大值l，控制量步长△σk；在每个子区间控制量的上下界内随机初始化控制量序列为根据计算目标函数的初始值j0；初始化迭代次数l＝0，初始化每个子区间的负梯度s152、更新每个子区间的控制量如果则令如果则令根据更新后的控制量序列计算目标函数值jl+1；计算当前迭代的负梯度：s153、判断是否达到迭代结束条件，所述迭代结束条件为：当前迭代次数l≥l或目标函数值变化量|jl+1-jl|<e；如达到迭代结束条件，则控制量序列为最优控制量序列，结束迭代；如不满足迭代结束条件，令迭代次数l＝l+1，跳转至步骤s152继续下一次迭代。s16、根据最优控制量序列和翼伞的初始状态和速度，推导出规划的航迹path(t)＝(x(t),y(t),h(t))，t∈[t0,tf]；其中：h(t)＝h0+vzt，步骤s15中根据控制量序列[σk]计算目标函数值j的步骤为：计算翼伞飞行时长：根据质点模型得到规划航迹上翼伞系统着陆时刻的航向角和水平面位置：其中计算着陆点水平位置误差目标值j1和着陆点航向误差目标值j2；根据翼伞飞行时段[t0,tf]内翼伞的控制量u(t)，计算控制能耗目标值j3；计算目标函数值j＝f1j1+f2j2+f3j3。本实施例对上述航迹规划方法进行了仿真，并与基于高斯伪谱法的最优控制归航算法、基于遗传算法的分段归航算法进行了对比。其中基于伪谱最优航迹算法参考了文献：(1)高海涛，张利民，孙青林，等.基于伪谱法的翼伞系统归航轨迹容错设计[j].控制理论与应用.2013(06):702-708；(2)罗淑贞，孙青林，檀盼龙，等.基于高斯伪谱法的翼伞系统复杂多约束轨迹规划[j].航空学报.2017(03):220-230。基于遗传算法的分段航迹算法参考了文献：(1)熊菁，秦子增，程文科，等.翼伞系统分段归航轨迹的优化设计[j].航天返回与遥感.2004(03):11-16.、(2)郑成，吴庆宪，姜长生，等.基于iaga的翼伞系统分段归航轨迹的优化[j].电光与控制.2011(02):69-72、(3)陶金，孙青林，陈增强，等.翼伞系统在较大风场中的归航控制[j].控制理论与应用.2016(12):1630-1638、(4)胡文治，陈建平，张红英，等.翼伞系统分段归航轨迹的优化设计[j].航空计算技术.2017(06):55-59。翼伞空投初始点位置设为(1500，1000，2000)，着陆点的期望水平位置为(0,0)，翼伞的速度设为v＝10m/s，初始航向角设为45°，则可将v分解为水平速度vs＝9.5m/s，垂直方向速度vz＝3.1m/s，滑翔比约为3.1，umax＝0.18。本实施例将常值风的影响视为空投初始位置偏移，飞行时段[t0,tf]的分段数目n设为6，△σk＝0.002，学习率λ设为0.01，迭代最大次数设为6000次，目标函数系数f1、f2和f3分别设为0.01、16、4，得到的航迹规划结果如图3所示，其中图3-(a)为水平面航迹对比，图3-(b)为三维空间内的航迹对比。从图3可以看出，基于遗传算法的分段航迹算法、基于高斯伪谱法的最优控制航迹算法，以及本发明公开的基于梯度下降法的最优分段常值航迹算法，都可以有效的为翼伞系统规划出可行航迹，三者从同一初始点出发，经过不同航迹都能到达同一个目标点，同时还满足逆风精确着陆的要求。在分段归航航迹中，翼伞经过了目标接近段、能量控制段和逆风着陆段到达目标点；在高斯伪谱法最优航迹中，翼伞先向远端飞行，消耗掉一定高度后再转向目标点飞行；而在本实施例的规划航迹中，翼伞通过一个较大半径的迂回转弯消耗高度再着陆到目标点，规划的目标点位置为(0.0990，0.2542)，实现了精确归航。图4为规划航迹航向角的对比示意图；从中可以看出，从相同45°初始航向角出发，分段归航、最优归航和本文归航算法在着陆时的航向角基本上都在180°左右，实现了以180°角逆风着陆的目标。图5为控制量对对比示意图；从图中的控制量的变化曲线可以看到，3种航迹规划算法下翼伞的控制量都小于允许的最大值，规划的航迹满足翼伞控制特性，是可飞的。其中最优控制归航着陆精度高、控制量小，但控制过程是连续变化的曲线，控制电机需要连续不断调整才可以实现控制目标，控制难度较大；而分段归航的控制量为分段常值，主要涉及转弯、滑翔、雀降等几个简单操作，其控制操作比最优归航要简单，从工程实用性角度出发，分段归航方式比最优控制归航方式更容易实现，但可以发现分段归航控制量较大，此外在初始方向调整段、目标接近段到能量管理段之间的过渡段、从能量管理段到逆风着陆段之间的过渡段，翼伞都需要从一个较小控制量突然增大到最大控制量，然后又从最大控制量降低到较小控制量，这增加了操纵的难度，同时由于翼伞控制存在较大的滞后，当控制还没有完全起作用时，很快又将控制量从最大控制量降低到较小控制量，这种操纵会带来较大的跟踪误差。跟前两种航迹规划算法相比，本实施例的航迹规划算法结合了最优控制航迹规划和分段航迹规划算法的优点，首先是跟前两者一样，规划的航迹着陆精度高，实现了精确着陆；其次是控制能量消耗较；再次是控制是分段常值，操纵容易；最后，从图5中可以看到，本发明算法设计的控制量主要在基准值附近变化，且其变化不大，这为后续的航迹跟踪提供了更大的控制量冗余。图6为本实施例中目标函数随迭代次数的变化值，可以看到在迭代次数为1200左右时，梯度下降法基本就已完全收敛，梯度下降法的目标函数最小值为0.3092。表1航迹规划算法指标对比序号归航方式距离偏差逆风误差能量消耗目标总值1遗传分段1.34e-0501.85217.40832伪谱最优000.19760.79043最优分段0.27270.00150.07060.3092表1进一步给出了三种航迹规划算法的规划指标结果对比，从表中数据可以看到，分段归航只要求控制量在约束范围内即可，因此分段归航航迹跟最优归航航迹相比，分段归航能量消耗较大；最优控制航迹规划算法在着陆距离偏差和逆风着陆方面的表现最好，且最优归航控制的能量消耗总值要比分段归航小一个数量级，这是由于最优控制归航将控制能量最小作为了目标函数设计指标之一；而本发明提出的最优分段常值归航在距离偏差和逆风着陆方面有所折衷，但能量消耗最小，从目标总值看，本发明提出的航迹规划方法目标函数总值是最低的。如图7所示，为实现上述航迹规划方法的系统，包括：翼伞降价质点模型建立模块11，用于建立风固定坐标系下的翼伞降阶质点模型；翼伞系统初始状态和着陆期望状态确定模块12，用于确定翼伞系统在空投初始时刻t0的位置(x0,y0,h0)和航向角ψ0、翼伞系统在着陆时刻tf的期望位置(xf,yf,hf)和期望航向角ψf、翼伞操纵绳的最大下拉量umax；目标函数建立模块13，用于建立翼伞系统航迹规划的目标函数：j＝f1j1+f2j2+f3j3；其中j1＝(x(tf)-xf)2+(y(tf)-yf)2，为着陆点水平位置误差目标；j2＝cos(ψ(tf))+1，为着陆点航向误差目标；为控制能耗目标；f1、f2和f3为加权因子；x(tf)、y(tf)、ψ(tf)分别为规划航迹上翼伞在着陆时刻的水平面x方向、y方向的坐标和航向角；飞行时段区间划分模块14，用于将翼伞飞行时段[t0,tf]划分为n个相邻区间，每个子区间内u(t)取常数值以控制翼伞系统，即：其中：将u(t)表示为序列[σk]，k＝1,…,n；最优控制量计算模块15，用于按照步骤151-153求解翼伞飞行时段每个区间的最优控制量σk，使目标函数值j取最小值，得到最优控制量序列规划航迹推导模块16，用于根据最优控制量序列和翼伞的初始状态和速度，推导出规划的航迹path(t)＝(x(t),y(t),h(t))，t∈[t0,tf]。实施例二：本实施例公开了对实施例一规划的航迹进行跟踪控制的方法，如图8所示，包括：s21、根据实施例一中所述的翼伞系统航迹规划方法得到翼伞的规划航迹pref(t)，t∈[t0,tf]；s22、在时刻t计算水平面内翼伞系统与规划航迹的位置偏差向量计算翼伞侧偏距：计算翼伞的航向角偏差计算翼伞的下滑角偏差：γerror(t)＝γref(t)-γ0(t)计算翼伞的高度误差：其中pref(t).x、pref(t).y为规划航迹上时刻t的水平面内x方向、y方向的坐标值；xt、yt为翼伞系统在时刻t的实际位置；为规划航迹上时刻t的航向角；为与x轴的夹角；为与规划航迹点pref(t)处切线的夹角；为翼伞系统在时刻t的实际航向角；γref(t)为规划航迹点pref(t)处的下滑角；γ0(t)为翼伞系统中领航翼伞在时刻t的下滑角；s23、计算时刻t的水平面跟踪控制量和纵向跟踪控制量：其中u1为翼伞系统的水平面跟踪控制量，通过非对称下拉来控制翼伞的水平转弯角速率实现水平面航向调节；u2为翼伞系统的纵向跟踪控制量，通过对称下拉来控制翼伞的下滑角速率实现纵向高度调节；u1max和u2max分别为翼伞非对称下拉和对称下拉的最大值，k1、k2、k3、k4分别为侧偏系数、航向角偏差系数、高度误差系数、下滑角偏差系数。在翼伞飞行过程中，循环执行步骤s22和s23，直到翼伞系统着陆。本实施例对上述航迹跟踪控制方法进行了仿真。具体采用的模型为翼伞6自由度模型，模型参数如下表2。表2翼伞参数参数载荷质量(kg)翼伞质量(kg)展长(m)弦长(m)厚度(m)伞绳长度(m)值13513730.37.5翼伞实际初始空投位置为(1550，950，2050)，初始航向角为50°，与规划的初始投放点位置(1500，1000，2000)相比，在xyz三轴的每个方向都存在50m的初始位置误差，与规划的初始航向角45°存在5°的角度误差。为了验证航迹跟踪控制方法的抗风性能，本实施例还在100秒到200秒之间加上了阵风干扰，由于已将常值风干扰考虑为初始规划位置的偏移，因此本实施例在去掉常值风影响后，将随机阵风建模为均值为0、标准差为1m/s的高斯随机过程，叠加至翼伞飞行速度后得到的阵风干扰如图9所示。在存在上述初始位置误差、航向角误差和随机阵风干扰的情况下，设置跟踪控制器，所示跟踪控制器包括水平面和纵向两个通道，在时刻t该跟踪控制器如公式(4)的定义。本实施例中，式(4)中的参数k1、k2、k3、k4分别为5/57.3、3、2/57.3、2，得到如图10、图11所示的翼伞航迹跟踪结果。从图10，图11可以看到，在该航迹跟踪控制器的作用下，翼伞从初始投放点出发之后不断的调整运动方向，快速向规划航迹靠拢，然后沿着规划的航迹精确逆风着陆到了目标点。图12和图13分半为翼伞航迹跟踪过程中的侧向误差和纵向误差，从图12、13可以看到，刚开始时翼伞在侧向和纵向都存在较大的误差，但航迹跟踪误差迅速的收敛到了稳定状态，此外由于在100秒到200秒之间存在随机阵风干扰，因此在这段时间内侧向和高度方向误差存在一定波动，但侧向误差和纵向误差总的趋势还是快速减小的，说明本发明公开的航迹跟踪控制方法具有较强的干扰抑制能力，而且计算量较小，实现较为容易。图14为翼伞航迹跟踪过程中的控制量变化过程，其中对称控制量用于修正高度误差；非对称控制量，即差分控制量，用于修正侧向误差。对比图5中最优分段常值控制量和图14中实际差分控制量，可以发现两者的趋势是一致的，控制量基本上为常值，差分控制量刚开始为正，说明翼伞刚开始在不断的向左偏转，在这个过程中控制量在逐渐阶梯状的变大，说明转弯半径在逐渐变小，然后控制量变为负值，此时翼伞向右偏转，最终以逆风180°着陆到目标点。本实施例还公开了实现上述航迹跟踪控制的系统，如图15所示，包括：航迹规划模块21，用于根据实施例一中所述的翼伞系统航迹规划方法得到翼伞的规划航迹pref(t)，t∈[t0,tf]；偏差计算模块22，用于计算时刻t水平面内翼伞系统与规划航迹的位置偏差向量翼伞侧偏距lxy(t)、航向角偏差翼伞的下滑角偏差γerror(t)、翼伞的高度误差herror(t)；跟踪控制量计算模块23，用于根据公式4计算时刻t的水平面跟踪控制量和纵向跟踪控制量。当前第1页12