[发明专利]一种基于逆动力学的无人机三维轨迹制导方法

权利要求书

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

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

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

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

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

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

具体为：

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

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

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

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

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

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

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

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

具体步骤如下：

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

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

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

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

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

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

期望地速状态方程如下：