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

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

A three dimensional trajectory guidance method for Unmanned Aerial Vehicle Based on inverse dynamics

The invention discloses a UAV 3D trajectory guidance method based on inverse dynamics, which belongs to the UAV navigation guidance and control technology; first, on a trajectory of UAV, by combining analytical method and numerical iteration method, in order to position state equations of UAV. A solution of inverse dynamics count, and the speed and track UAV angle equation two inverse dynamics calculation, get command thrust, UAV command and attack angle command track roll angle, then the instruction input has several design good posture control loop, you can control the actual speed and position of the UAV, and at the same time using PID control, the actual position of the reference trajectory of the UAV to route convergence between points; the invention can accurately control the UAV speed and three-dimensional position, online adjustment The expected flight path of the UAV, and the controller has low computational cost.

【技术实现步骤摘要】
一种基于逆动力学的无人机三维轨迹制导方法
本专利技术属于无人机导航制导与控制
，具体涉及一种基于逆动力学的无人机三维轨迹制导方法。
技术介绍
无人机又称无人驾驶飞行器，广泛应用于军用和民用领域；无人机制导是指通过指令程序，使无人机沿给定的轨迹飞行。随着无人机的执行任务日趋多样，人们对无人机机动性能的要求也日益提高，传统的二维轨迹制导律已无法满足需求，实现三维轨迹的高精度跟踪控制，能让无人机完成地形回避，编队飞行以及自主空中加油等特殊任务，具有重要的意义。在无人机的轨迹跟踪控制中，实现无人机直接沿给定航路点飞行具有重要的实用价值，目前很多无人机的参考轨迹设计方法是将各航路点以空间三维曲线进行拟合，或将轨迹拆分成若干段分别进行拟合，这类方法虽然可得到较为光滑的参考轨迹，但算法计算量较大，特别是面对复杂参考轨迹时，其拟合难度将直线上升，这对于机载计算机设备性能较差的低成本无人机来说是非常不利的。另外，从无人机的飞行动力学角度来看，制导回路所属的各控制量之间存在高度耦合的关系。其中，无人机的位置方程为式中，(x,y,h)为无人机的三维坐标；地速及航迹角状态方程为(Vg,χ,γ)分别为无人机的地速、无人机的航迹偏角和无人机的航迹倾角；(α,β,μ)分别为无人机的迎角、无人机的侧滑角和无人机的航迹滚转角；(T,FD,FY,FL)分别为无人机的发动机推力、无人机的阻力、无人机的侧力和无人机的升力；m和g分别为无人机质量和重力加速度。由式(1)(2)可见，(Vg,χ,γ)和(T,α,β,μ)均为相互耦合的控制量，若采用传统PID控制，控制效果必然会因耦合作用而受到严重影响。基于上述分析，所设计的三维轨迹制导方法应满足如下要求：(1)能够在飞行包线约束条件下，同时控制无人机的地速和位置，精确跟踪三维轨迹，并实现无人机直接沿给定航路点飞行任务；(2)期望飞行轨迹便于线生成和调整，且所需的时间和计算成本较小；(3)制导方法物理意义明确，形式简洁，参数整定方便，易于工程实现。
技术实现思路
本专利技术的目的是为了解决上述问题，并满足上述要求，提出一种基于逆动力学的无人机三维轨迹制导方法，具体包括如下步骤：步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点。步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项和纵向位移状态的微分项初始k＝1；首先，无人机的实际速度误差包括横侧向速度误差和纵向速度误差计算如下：是无人机在第k段轨迹段上设定的指令跟踪横侧向速度；是无人机当前在第k段轨迹段上实际的横侧向速度；是无人机在第k段轨迹段上设定的指令跟踪纵向速度；是无人机当前在第k段轨迹段上实际的纵向速度；Vgk是第k段轨迹段上为无人机设定的指令地速；γk是第k个轨迹段所指向方向的航迹倾角；χk是第k个轨迹段所指向方向的航迹偏角；Vg是无人机当前在第k段轨迹段上实际的地速；γ是无人机当前在第k段轨迹段上实际的航迹倾角；χ是无人机当前在第k段轨迹段上实际的航迹偏角。然后，无人机的实际位移误差包括横侧向位移误差(yline-y)和纵向位移误差(hline-h)；计算如下：从无人机当前实际位置处作此时对应直线轨迹的空间垂线，得到垂足坐标(xline,yline,hline)，该垂线段即为无人机的位置误差总和，分解为横侧向位移误差(yline-y)和纵向位移误差(hline-h)。最后，PID控制律加和具体为：对无人机的位移误差(yline-y)和(hline-h)采用PI控制，将速度误差和作为微分项，并将PI控制与微分项求和，PID的参数即为速度误差和位移误差在飞行制导律中分别所占的权重，得到期望的横侧向位移状态的微分项和纵向位移状态的微分项步骤四、将期望的微分项和微分项分别带入横侧向位移状态和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对微分方程进行第一级逆动力学解算，输出指令航迹偏角χc和航迹倾角γc；首先，将期望的微分项带入纵向位移状态的微分方程中，采用解析法解算出指令航迹倾角γc，计算如下：然后，将期望的微分项和指令航迹倾角γc带入到横侧向位移状态的微分方程中，利用数值迭代法解算出指令航迹偏角χc，计算如下：步骤五、将指令航迹偏角χc和指令航迹倾角γc以及无人机在该航路段上的指令地速Vgk作为下一级逆动力学解算的输入指令值，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力Tc、指令迎角αc和指令航迹滚转角μc；具体步骤如下：步骤501、将指令航迹偏角χc，指令航迹倾角γc和指令地速Vgk分别与当前无人机实际的航迹偏角χ，航迹倾角γ和地速Vg作差，并采用PD控制，分别得到期望的航迹偏角微分值期望的航迹倾角微分值和期望的地速微分值步骤502、利用当前无人机的实际地速Vg和航迹倾角γ，以及期望的航迹偏角微分值和期望的航迹倾角微分值计算指令航迹滚转角μc的解析解；步骤503、利用指令航迹滚转角μc的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角αc的非线性一元方程；步骤504、用数值迭代法对关于指令迎角αc的非线性一元方程进行解算，得到指令迎角αc。步骤505、将指令迎角αc代入期望地速状态方程中，解算出指令推力Tc；期望地速状态方程如下：步骤六、将输出的指令推力Tc、指令迎角αc、侧滑角0°和航迹滚转角μc作为无人机姿态控制回路的输入，即可实现对无人机三维轨迹的跟踪控制；步骤七、当无人机飞行至第k段轨迹段对应的转换半径d的范围内时，则无人机飞到所对应的航路点位置，继续将跟踪下一段轨迹，返回至步骤三，直至追踪至最后一个航路点。本专利技术的优点在于：(1)一种基于逆动力学的无人机三维轨迹制导方法，可同时精确的控制无人机的地速和三维位置。(2)一种基于逆动力学的无人机三维轨迹制导方法，可在线调整无人机的期望飞行轨迹，且控制器计算成本低。(3)一种基于逆动力学的无人机三维轨迹制导方法，控制结构简单，各部分物理意义明确，便于参数整定。附图说明图1是本专利技术整个制导控制器的结构框图；图2是本专利技术一种基于逆动力学的无人机三维轨迹制导方法流程图；图3是本专利技术采用的三维制导方法中存在的几何关系示意图；图4是本专利技术采用解析法和数值迭代法相结合进行第二级逆动力学解算的流程图；图5a是无人机实际位置与对应直线航路段之间的距离误差图；图5b是无人机地速误差图；图5c是无人机XY平面轨迹跟踪效果图；图5d是无人机XZ平面轨迹跟踪效果图；图5e是无人机YZ平面轨迹跟踪效果图。具体实施方式下面将结合附图和实施例对本专利技术作进一步的详细说明。本专利技术一种基于逆动力学的无人机三维轨迹制导方法，通过将解析法和数值迭代法相结合，依次对无人机的位置状态方程进行一级逆动力学解算，以及对无人机的地速和航迹角状态方程进行二级逆动力学解算，得到无人机的指令推力、指令迎角和指令航本文档来自技高网...

【技术保护点】
一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，包括如下步骤：步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点；步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项

【技术特征摘要】
1.一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，包括如下步骤：步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点；步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项和纵向位移状态的微分项初始k＝1，1≤k≤n；步骤四、将期望的微分项和微分项分别带入横侧向位移状态和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对微分方程进行第一级逆动力学解算，输出指令航迹偏角χc和航迹倾角γc；步骤五、将指令航迹偏角χc和指令航迹倾角γc以及无人机在该航路段上的指令地速Vgk作为下一级逆动力学解算的输入指令值，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力Tc、指令迎角αc和指令航迹滚转角μc；步骤六、将输出的指令推力Tc、指令迎角αc、侧滑角0°和航迹滚转角μc作为无人机姿态控制回路的输入，实现对无人机三维轨迹的跟踪控制；步骤七、当无人机飞行至第k段轨迹段对应的转换半径d的范围内时，则无人机飞到所对应的航路点位置，继续将跟踪下一段轨迹，返回至步骤三，直至追踪至最后一个航路点。2.如权利要求1所述的一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，所述的步骤三具体为：首先，无人机的实际速度误差包括横侧向速度误差和纵向速度误差计算如下：是无人机在第k段轨迹段上设定的指令跟踪横侧向速度；是无人机当前在第k段轨迹段上实际的横侧向速度；是无人机在第k段轨迹段上设定的指令跟踪纵向速度；是无人机当前在第k段轨迹段上实际的纵向速度；Vgk是第k段轨迹段上为无人机设定的指令地速；γk是第k个轨迹段所指向方向的航迹倾角；χk是第k个轨迹段所指向方向的航迹偏角；Vg是无人机当前在第k段轨迹段上实际的地速；γ是无人机当前在第k段轨迹段上实际的航迹倾角；χ是无人机当前在第k段轨迹段上实际的航迹偏角；然后，无人机的实际位移误差包括横侧向位移误差(yline-y)和纵向位移误差(hline-h)；计算如下：从无人机当前实际位置处作此时...

【专利技术属性】
技术研发人员：王宏伦，吴健发，李娜，姚鹏，苏子康，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：北京,11