CN106774400A - 一种基于逆动力学的无人机三维轨迹制导方法 - Google Patents

Abstract

本发明公开了一种基于逆动力学的无人机三维轨迹制导方法，属于无人机导航制导与控制技术领域；首先，针对某无人机的某个轨迹段，通过将解析法和数值迭代法相结合，依次对无人机的位置状态方程进行一级逆动力学解算，以及对无人机的地速和航迹角状态方程进行二级逆动力学解算，得到无人机的指令推力、指令迎角和指令航迹滚转角，再将这几个指令量输入已设计好的姿态控制回路，即可控制无人机的实际地速和位置，同时采用PID控制，使无人机的实际位置收敛于航路点之间的参考轨迹上；本发明可同时精确的控制无人机的地速和三维位置，在线调整无人机的期望飞行轨迹，且控制器计算成本低。

Description

一种基于逆动力学的无人机三维轨迹制导方法

技术领域

本发明属于无人机导航制导与控制技术领域，具体涉及一种基于逆动力学的无人机三维轨迹制导方法。

背景技术

无人机又称无人驾驶飞行器，广泛应用于军用和民用领域；无人机制导是指通过指令程序，使无人机沿给定的轨迹飞行。随着无人机的执行任务日趋多样，人们对无人机机动性能的要求也日益提高，传统的二维轨迹制导律已无法满足需求，实现三维轨迹的高精度跟踪控制，能让无人机完成地形回避，编队飞行以及自主空中加油等特殊任务，具有重要的意义。

在无人机的轨迹跟踪控制中，实现无人机直接沿给定航路点飞行具有重要的实用价值，目前很多无人机的参考轨迹设计方法是将各航路点以空间三维曲线进行拟合，或将轨迹拆分成若干段分别进行拟合，这类方法虽然可得到较为光滑的参考轨迹，但算法计算量较大，特别是面对复杂参考轨迹时，其拟合难度将直线上升，这对于机载计算机设备性能较差的低成本无人机来说是非常不利的。

另外，从无人机的飞行动力学角度来看，制导回路所属的各控制量之间存在高度耦合的关系。其中，无人机的位置方程为

式中，(x,y,h)为无人机的三维坐标；

地速及航迹角状态方程为

(V_g,χ,γ)分别为无人机的地速、无人机的航迹偏角和无人机的航迹倾角；(α,β,μ)分别为无人机的迎角、无人机的侧滑角和无人机的航迹滚转角；(T,F_D,F_Y,F_L)分别为无人机的发动机推力、无人机的阻力、无人机的侧力和无人机的升力；m和g分别为无人机质量和重力加速度。

由式(1)(2)可见，(V_g,χ,γ)和(T,α,β,μ)均为相互耦合的控制量，若采用传统PID控制，控制效果必然会因耦合作用而受到严重影响。

基于上述分析，所设计的三维轨迹制导方法应满足如下要求：

(1)能够在飞行包线约束条件下，同时控制无人机的地速和位置，精确跟踪三维轨迹，并实现无人机直接沿给定航路点飞行任务；

(2)期望飞行轨迹便于线生成和调整，且所需的时间和计算成本较小；

(3)制导方法物理意义明确，形式简洁，参数整定方便，易于工程实现。

发明内容

本发明的目的是为了解决上述问题，并满足上述要求，提出一种基于逆动力学的无人机三维轨迹制导方法，具体包括如下步骤：

步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；

期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点。

步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；

步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项和纵向位移状态的微分项

初始k＝1；

首先，无人机的实际速度误差包括横侧向速度误差和纵向速度误差计算如下：

是无人机在第k段轨迹段上设定的指令跟踪横侧向速度；是无人机当前在第k段轨迹段上实际的横侧向速度；是无人机在第k段轨迹段上设定的指令跟踪纵向速度；是无人机当前在第k段轨迹段上实际的纵向速度；V_gk是第k段轨迹段上为无人机设定的指令地速；γ_k是第k个轨迹段所指向方向的航迹倾角；χ_k是第k个轨迹段所指向方向的航迹偏角；V_g是无人机当前在第k段轨迹段上实际的地速；γ是无人机当前在第k段轨迹段上实际的航迹倾角；χ是无人机当前在第k段轨迹段上实际的航迹偏角。

然后，无人机的实际位移误差包括横侧向位移误差(y_line-y)和纵向位移误差(h_line-h)；计算如下：

从无人机当前实际位置处作此时对应直线轨迹的空间垂线，得到垂足坐标(x_line,y_line,h_line)，该垂线段即为无人机的位置误差总和，分解为横侧向位移误差(y_line-y)和纵向位移误差(h_line-h)。

最后，PID控制律加和具体为：对无人机的位移误差(y_line-y)和(h_line-h)采用PI控制，将速度误差和作为微分项，并将PI控制与微分项求和，PID的参数即为速度误差和位移误差在飞行制导律中分别所占的权重，得到期望的横侧向位移状态的微分项和纵向位移状态的微分项

步骤四、将期望的微分项和微分项分别带入横侧向位移状态和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对微分方程进行第一级逆动力学解算，输出指令航迹偏角χ_c和航迹倾角γ_c；

首先，将期望的微分项带入纵向位移状态的微分方程中，采用解析法解算出指令航迹倾角γ_c，计算如下：

然后，将期望的微分项和指令航迹倾角γ_c带入到横侧向位移状态的微分方程中，利用数值迭代法解算出指令航迹偏角χ_c，计算如下：

步骤五、将指令航迹偏角χ_c和指令航迹倾角γ_c以及无人机在该航路段上的指令地速V_gk作为下一级逆动力学解算的输入指令值，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力T_c、指令迎角α_c和指令航迹滚转角μ_c；

具体步骤如下：

步骤501、将指令航迹偏角χ_c，指令航迹倾角γ_c和指令地速V_gk分别与当前无人机实际的航迹偏角χ，航迹倾角γ和地速V_g作差，并采用PD控制，分别得到期望的航迹偏角微分值期望的航迹倾角微分值和期望的地速微分值

步骤502、利用当前无人机的实际地速V_g和航迹倾角γ，以及期望的航迹偏角微分值和期望的航迹倾角微分值计算指令航迹滚转角μ_c的解析解；

步骤503、利用指令航迹滚转角μ_c的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角α_c的非线性一元方程；

步骤504、用数值迭代法对关于指令迎角α_c的非线性一元方程进行解算，得到指令迎角α_c。

步骤505、将指令迎角α_c代入期望地速状态方程中，解算出指令推力T_c；

期望地速状态方程如下：

步骤六、将输出的指令推力T_c、指令迎角α_c、侧滑角0°和航迹滚转角μ_c作为无人机姿态控制回路的输入，即可实现对无人机三维轨迹的跟踪控制；

步骤七、当无人机飞行至第k段轨迹段对应的转换半径d的范围内时，则无人机飞到所对应的航路点位置，继续将跟踪下一段轨迹，返回至步骤三，直至追踪至最后一个航路点。

本发明的优点在于：

(1)一种基于逆动力学的无人机三维轨迹制导方法，可同时精确的控制无人机的地速和三维位置。

(2)一种基于逆动力学的无人机三维轨迹制导方法，可在线调整无人机的期望飞行轨迹，且控制器计算成本低。

(3)一种基于逆动力学的无人机三维轨迹制导方法，控制结构简单，各部分物理意义明确，便于参数整定。

附图说明

图1是本发明整个制导控制器的结构框图；

图2是本发明一种基于逆动力学的无人机三维轨迹制导方法流程图；

图3是本发明采用的三维制导方法中存在的几何关系示意图；

图4是本发明采用解析法和数值迭代法相结合进行第二级逆动力学解算的流程图；

图5a是无人机实际位置与对应直线航路段之间的距离误差图；

图5b是无人机地速误差图；

图5c是无人机XY平面轨迹跟踪效果图；

图5d是无人机XZ平面轨迹跟踪效果图；

图5e是无人机YZ平面轨迹跟踪效果图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明一种基于逆动力学的无人机三维轨迹制导方法，通过将解析法和数值迭代法相结合，依次对无人机的位置状态方程进行一级逆动力学解算，以及对无人机的地速和航迹角状态方程进行二级逆动力学解算，得到无人机的指令推力、指令迎角和指令航迹滚转角，再将这几个指令量输入已设计好的姿态控制回路，即可控制无人机的实际地速和位置，同时采用PID控制，使无人机的实际位置收敛于航路点之间的参考轨迹上。

整个制导控制器的结构框图如图1，其整体工作流程为：

针对某个无人机，首先设定好各航路点的坐标，根据指令航路点坐标，根据无人机当前位置(x,y,h)获取计算出期望航迹角，并设定好无人机在各航路段上的指令地速；根据指令地速和期望航迹角，可计算出无人机在横侧向与纵向的指令跟踪地速分量和令其分别于无人机实际的地速分量和分别作差，获取无人机的实际速度误差和再乘以系数K_D，作为PID控制器的微分项；

同时根据指令航路点坐标，生成对应的各直线轨迹段，从无人机的当前位置向当前对应的直线航路段作垂线，垂足坐标为(x_line,y_line,h_line)，令y_line和h_line分别与无人机的实际横侧向坐标y与纵向高度h作差，计算无人机在各直线轨迹段上的实际位移误差(y_line-y)和(h_line-h)；并并采用PI控制律，分别乘以系数K_P和K_I；

接下来要采用PID控制，构成期望的横侧向和纵向位移状态的微分值和作为第一级逆动力学解算的输入，将PI控制项与微分项分别对应加和构成和作为第一级逆动力学解算的输入，采用解析法与数值迭代法相结合的逆动力学方法解算出指令航迹角，作为第二级逆动力学解算的输入，解算出指令推力T_c、指令迎角α_c和指令航迹滚转角μ_c。将T_c输入至无人机动力控制模块，将α_c和μ_c输入至无人机的姿态控制回路中，进一步生成无人机的相应舵角指令，从而同时控制无人机的姿态与地速。

如图2所示，具体包括如下步骤：

步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；

根据设定的无人机飞行所需要经过的航路点坐标，构成无人机的期望轨迹，并将轨迹划分为若干轨迹段；无人机的期望轨迹由若干航路点所联结成的航路段构成，设航路点按序号可联结为n(n≥1)段直线航路，对应有n+1个航路点。

步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；

本实施例中将每个航路点的转换半径均设定为d。

步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项和纵向位移状态的微分项

将无人机基于航路点的期望轨迹跟踪问题划分为速度误差收敛问题和位置误差收敛问题，分别设计相应的PID控制策略，再将所设计的PID控制律加和构成期望横侧向位移状态和纵向位移状态的微分项；

具体为：

(1)无人机速度误差收敛问题

如图3所示，当无人机UAV收敛于第k(1≤k≤n)段航路上时，无人机UAV的三维坐标为(x,y,h)，第k-1个航路点WP_k-1(x_k-1,y_k-1,h_k-1)与第k个航路点WP_k(x_k,y_k,h_k)之间的航迹偏角χ_k∈[0,2π)和航迹倾角γ_k∈(-π/2,π/2)即为此时无人机的期望航迹偏角和航迹倾角，其表达式为：

其中y_k-1和y_k起到了指令横侧向位移的作用，h_k-1和h_k起到了指令纵向位移的作用，χ_k和γ_k即为所要跟踪的虚拟航迹角；χ_k是第k个轨迹段所指向方向的航迹偏角；γ_k是第k个轨迹段所指向方向的航迹倾角；

注意：当k＝1时，第k-1个航路点为(x_k-1,y_k-1,h_k-1)＝(x₀,y₀,h₀)，该点被定义为飞行的起点。

将第k个轨迹段所要跟踪的虚拟航迹角χ_k和γ_k以及无人机此时设定的指令地速V_gk代入式(1)，得到此时的指令跟踪横侧向速度和纵向速度其与实际速度的误差分别为和表达式为

(2)无人机位置误差收敛问题

如图3所示，从无人机当前实际位置处作此时所对应第k段航路的垂线，可得垂足坐标FP(x_line,y_line,h_line)，该垂线段即为无人机的位置误差总和，可分解为横侧向位移误差(y_line-y)和纵向位移误差(h_line-h)。其中，y_line起着指令横侧向位移的作用，h_line起着指令纵向位移的作用。

也就是说，y_k-1，h_k-1，y_k，h_k，y_line和h_line均起到了指令位置的作用，其中，y_k和h_k是驱使无人机收敛到指令横侧向和纵向速度的位置指令，y_line和h_line是驱使无人机收敛到指令轨迹的位置指令，制导方法中存在的几何关系如图1所示，此时需要对位置指令进行加权求和处理，本发明引入了PID控制的思路来解决这一问题。

对于指令轨迹的收敛问题，其位移误差(y_line-y)和(h_line-h)采用PI控制，从而使无人机能够无差收敛到指令轨迹上；对于指令速度的收敛问题，通过式(3)和(4)已经求得了横侧向和纵向速度误差和该项可直接作为微分项。将PI控制的结构与微分项求和，其中PID的参数即为速度误差收敛问题和位置误差收敛问题分别在飞行制导律中所占的权重，可得期望的横侧向位移状态的微分项和纵向位移状态的微分项

步骤四：将步骤三中所得的期望状态的微分项和分别带入横侧向位移状态的微分方程和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对无人机的横侧向位移和纵向位移状态方程进行第一级逆动力学解算，输出指令航迹偏角χ_c和航迹倾角γ_c。

用期望横侧向位移状态的微分项和纵向位移状态的微分项替代式(1)中的对应的微分项和

首先，针对期望纵向位移的状态方程，通过解析法解算出指令航迹倾角γ_c的解析解：

然后，将解算出的γ_c带入到期望横侧向位移的微分状态方程中，可得仅包含未知数χ_c的一元方程：

利用数值方法，例如牛顿迭代法解算出指令航迹偏角χ_c。

步骤五：将步骤四解算出的航迹偏角χ_c和航迹倾角γ_c以及设定的无人机指令地速V_gk作为下一级逆动力学解算的输入，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力T_c、指令迎角α_c和指令航迹滚转角μ_c。

如图4所示，具体步骤如下：

步骤501、将指令航迹偏角χ_c，指令航迹倾角γ_c和指令地速V_gk分别与当前无人机实际的航迹偏角χ，航迹倾角γ和地速V_g作差，并采用PD控制，分别得到期望的航迹偏角微分值期望的航迹倾角微分值和期望的地速微分值

步骤502、利用当前无人机的实际地速V_g和航迹倾角γ，以及期望的航迹偏角微分值和期望的航迹倾角微分值计算指令航迹滚转角μ_c的解析解；

由于无人机采用BTT方式转弯，侧滑角β约等于0，此时侧力F_Y也约等于0，因此式(2)的三个方程可化简为

上式中的微分值和被其期望值 和所替换，联立式(8)和(9)，可得指令航迹滚转角的解析解：

步骤503、利用指令航迹滚转角μ_c的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角α_c的非线性一元方程；

将解算出的指令航迹滚转角μ_c和式(7)代入式(9)中，得只含未知数指令迎角α_c的非线性一元方程：

注：阻力F_D和升力F_L在此时均可视为变量α_c的函数；

步骤504、用数值迭代法对关于指令迎角α_c的非线性一元方程进行解算，得到指令迎角α_c。

步骤505、将指令迎角α_c代入期望地速状态方程中，解算出指令推力T_c；

期望地速状态方程如下：

将α_c代入式(7)，即可解算出指令推力T_c。

步骤六、将输出的指令推力T_c、指令迎角α_c、侧滑角0°和航迹滚转角μ_c作为无人机姿态控制回路的输入，即可实现对无人机三维轨迹的跟踪控制；

步骤七：当无人机飞行至距离目前所跟踪轨迹段的对应航路点d的位置时，即可认为无人机已飞到所对应的航路点位置，此时无人机将跟踪下一段轨迹，整个制导算法循环至步骤三，直至追踪至最后一个航路点。

本实施例中，设定无人机以200m/s的地速飞行，初始高度为7010m；采用本发明方法的风扰条件下，获取的无人机三维轨迹跟踪效果图；

如图5a所示，是无人机实际位置与对应直线航路段之间的距离误差图，可以看出无人机在进行恒速爬升及转弯动作时，位置不可避免的存在一定程度的超调，但很快位置误差就能收敛到接近0的程度；

如图5b所示，是无人机地速误差图，可以看出无人机在进行恒速爬升及转弯动作时，会不可避免的存在一定的速度损失，但很快速度误差就能收敛到接近0的程度，且速度损失不大；

如图5c，5d和5e所示，分别是XY平面轨迹跟踪效果图，XZ平面轨迹跟踪效果图和YZ平面轨迹跟踪效果图，这三个图直观的体现了无人机的实际飞行过程，可以看出：在爬升-拉平阶段，纵向高度上一开始存在约100m左右的超调，但是很快无人机就恢复到指令轨迹上了；在45°大角度水平转弯时，高度变化仅在10m-20m左右，横侧向位置超调达到约400m后，能较快收敛到指令位置，相对于无人机200m/s的高速，该位置超调是较为可观的。

Claims (4)

1.一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，包括如下步骤：

步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；

期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点；

步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；

步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项和纵向位移状态的微分项

初始k＝1，1≤k≤n；

步骤四、将期望的微分项和微分项分别带入横侧向位移状态和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对微分方程进行第一级逆动力学解算，输出指令航迹偏角χ_c和航迹倾角γ_c；

步骤五、将指令航迹偏角χ_c和指令航迹倾角γ_c以及无人机在该航路段上的指令地速V_gk作为下一级逆动力学解算的输入指令值，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力T_c、指令迎角α_c和指令航迹滚转角μ_c；

步骤六、将输出的指令推力T_c、指令迎角α_c、侧滑角0°和航迹滚转角μ_c作为无人机姿态控制回路的输入，实现对无人机三维轨迹的跟踪控制；

步骤七、当无人机飞行至第k段轨迹段对应的转换半径d的范围内时，则无人机飞到所对应的航路点位置，继续将跟踪下一段轨迹，返回至步骤三，直至追踪至最后一个航路点。

2.如权利要求1所述的一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，所述的步骤三具体为：

首先，无人机的实际速度误差包括横侧向速度误差和纵向速度误差计算如下：

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_r-\stackrel{\·}{y}=V_{gk}{\mathrm{cos\γ}}_k{\mathrm{sin\χ}}_k-V_{g}cos\mathrm{\γ}sin\mathrm{\χ}\\ {\stackrel{\·}{h}}_r-\stackrel{\·}{h}=V_{gk}{\mathrm{sin\γ}}_k-V_{g}sin\mathrm{\γ}\end{array}\right.$

是无人机在第k段轨迹段上设定的指令跟踪横侧向速度；是无人机当前在第k段轨迹段上实际的横侧向速度；是无人机在第k段轨迹段上设定的指令跟踪纵向速度；是无人机当前在第k段轨迹段上实际的纵向速度；V_gk是第k段轨迹段上为无人机设定的指令地速；γ_k是第k个轨迹段所指向方向的航迹倾角；χ_k是第k个轨迹段所指向方向的航迹偏角；V_g是无人机当前在第k段轨迹段上实际的地速；γ是无人机当前在第k段轨迹段上实际的航迹倾角；χ是无人机当前在第k段轨迹段上实际的航迹偏角；

然后，无人机的实际位移误差包括横侧向位移误差(y_line-y)和纵向位移误差(h_line-h)；计算如下：

从无人机当前实际位置处作此时对应直线轨迹的空间垂线，得到垂足坐标(x_line,y_line,h_line)，该垂线段即为无人机的位置误差总和，分解为横侧向位移误差(y_line-y)和纵向位移误差(h_line-h)；

最后，PID控制律加和具体为：对无人机的位移误差(y_line-y)和(h_line-h)采用PI控制，将速度误差和作为微分项，并将PI控制与微分项求和，PID的参数即为速度误差和位移误差在飞行制导律中分别所占的权重，得到期望的横侧向位移状态的微分项和纵向位移状态的微分项

3.如权利要求1所述的一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，所述的步骤四具体为：

首先，将期望的微分项带入纵向位移状态的微分方程中，采用解析法解算出指令航迹倾角γ_c，计算如下：

${\mathrm{\γ}}_c={\sin }^{-1}\left(\frac{{\stackrel{\·}{h}}_d}{V_g}\right)$

然后，将期望的微分项和指令航迹倾角γ_c带入到横侧向位移状态的微分方程中，利用数值迭代法解算出指令航迹偏角χ_c，计算如下：

${\stackrel{\·}{y}}_d-V_g{\mathrm{cos\γ}}_c{\mathrm{sin\χ}}_c=0.$

4.如权利要求1所述的一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，所述的步骤五具体为：

具体步骤如下：

步骤501、将指令航迹偏角χ_c，指令航迹倾角γ_c和指令地速V_gk分别与当前无人机实际的航迹偏角χ，航迹倾角γ和地速V_g作差，并采用PD控制，分别得到期望的航迹偏角微分值期望的航迹倾角微分值和期望的地速微分值

步骤502、利用当前无人机的实际地速V_g和航迹倾角γ，以及期望的航迹偏角微分值和期望的航迹倾角微分值计算指令航迹滚转角μ_c的解析解；

${\mathrm{\μ}}_c={\tan }^{-1}\left(\frac{V_g{\stackrel{\·}{\mathrm{\χ}}}_{d}cos\mathrm{\γ}}{V_g{\stackrel{\·}{\mathrm{\γ}}}_d+gcos\mathrm{\γ}}\right)$

步骤503、利用指令航迹滚转角μ_c的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角α_c的非线性一元方程；

$F_L{\mathrm{cos\μ}}_c+{\mathrm{tan\α}}_c{\mathrm{cos\μ}}_c(m{\stackrel{\·}{V}}_{gd}+F_D+mgsin\mathrm{\γ})-{\mathrm{mV}}_g{\stackrel{\·}{\mathrm{\γ}}}_d-mgcos\mathrm{\γ}=0$

F_L为无人机的升力，m和g分别为无人机的质量和重力加速度，F_D为无人机的阻力；

步骤504、用数值迭代法对关于指令迎角α_c的非线性一元方程进行解算，得到指令迎角α_c；

步骤505、将指令迎角α_c代入期望地速状态方程中，解算出指令推力T_c；

期望地速状态方程如下：

${\stackrel{\·}{V}}_{gd}=\frac{1}{m}(T_c{\mathrm{cos\α}}_c-F_D-mgsin\mathrm{\γ}).$