CN106774400B - Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics - Google Patents

Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics Download PDF

Info

Publication number
CN106774400B
CN106774400B CN201611233604.6A CN201611233604A CN106774400B CN 106774400 B CN106774400 B CN 106774400B CN 201611233604 A CN201611233604 A CN 201611233604A CN 106774400 B CN106774400 B CN 106774400B
Authority
CN
China
Prior art keywords
uav
track
angle
trajectory
commanded
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201611233604.6A
Other languages
Chinese (zh)
Other versions
CN106774400A (en
Inventor
王宏伦
吴健发
李娜
姚鹏
苏子康
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beijing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing University of Aeronautics and Astronautics filed Critical Beijing University of Aeronautics and Astronautics
Priority to CN201611233604.6A priority Critical patent/CN106774400B/en
Publication of CN106774400A publication Critical patent/CN106774400A/en
Application granted granted Critical
Publication of CN106774400B publication Critical patent/CN106774400B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

The invention discloses a three-dimensional trajectory guidance method for an unmanned aerial vehicle based on inverse dynamics, which belongs to the technical field of unmanned aerial vehicle navigation, guidance and control. Combined with the method, the first-order inverse dynamics solution is performed on the position state equation of the UAV in turn, and the second-order inverse dynamics solution is performed on the UAV's ground speed and track angle state equations to obtain the command of the UAV. Thrust, command angle of attack and command track roll angle, and then input these command quantities into the designed attitude control loop to control the actual ground speed and position of the drone. At the same time, PID control is used to make the drone The actual position of the drone converges on the reference trajectory between the waypoints; the present invention can simultaneously precisely control the ground speed and three-dimensional position of the drone, adjust the desired flight trajectory of the drone online, and the controller has low computational cost.

Description

一种基于逆动力学的无人机三维轨迹制导方法A three-dimensional trajectory guidance method for UAV based on inverse dynamics

技术领域technical field

本发明属于无人机导航制导与控制技术领域，具体涉及一种基于逆动力学的无人机三维轨迹制导方法。The invention belongs to the technical field of navigation, guidance and control of unmanned aerial vehicles, in particular to a three-dimensional trajectory guidance method of unmanned aerial vehicles based on inverse dynamics.

背景技术Background technique

无人机又称无人驾驶飞行器，广泛应用于军用和民用领域；无人机制导是指通过指令程序，使无人机沿给定的轨迹飞行。随着无人机的执行任务日趋多样，人们对无人机机动性能的要求也日益提高，传统的二维轨迹制导律已无法满足需求，实现三维轨迹的高精度跟踪控制，能让无人机完成地形回避，编队飞行以及自主空中加油等特殊任务，具有重要的意义。UAV, also known as unmanned aerial vehicle, is widely used in military and civilian fields; UAV guidance refers to making the UAV fly along a given trajectory through an instruction program. With the increasing variety of tasks performed by UAVs, people's requirements for UAV maneuverability are also increasing. The traditional two-dimensional trajectory guidance law can no longer meet the needs. The realization of high-precision tracking control of three-dimensional trajectory can make UAVs It is of great significance to complete special tasks such as terrain avoidance, formation flight and autonomous aerial refueling.

在无人机的轨迹跟踪控制中，实现无人机直接沿给定航路点飞行具有重要的实用价值，目前很多无人机的参考轨迹设计方法是将各航路点以空间三维曲线进行拟合，或将轨迹拆分成若干段分别进行拟合，这类方法虽然可得到较为光滑的参考轨迹，但算法计算量较大，特别是面对复杂参考轨迹时，其拟合难度将直线上升，这对于机载计算机设备性能较差的低成本无人机来说是非常不利的。In the trajectory tracking control of UAV, it has important practical value to realize UAV flying directly along a given waypoint. At present, the reference trajectory design method of many UAVs is to fit each waypoint with a three-dimensional curve in space. Or split the trajectory into several segments for fitting. Although this method can obtain a relatively smooth reference trajectory, the algorithm requires a large amount of calculation, especially when facing complex reference trajectories, the fitting difficulty will increase linearly. It is very disadvantageous for low-cost drones with poor on-board computer equipment.

另外，从无人机的飞行动力学角度来看，制导回路所属的各控制量之间存在高度耦合的关系。其中，无人机的位置方程为In addition, from the perspective of the flight dynamics of the UAV, there is a highly coupled relationship between the various control variables to which the guidance loop belongs. Among them, the position equation of the UAV is

式中，(x,y,h)为无人机的三维坐标；In the formula, (x, y, h) are the three-dimensional coordinates of the UAV;

地速及航迹角状态方程为The state equations of ground speed and track angle are:

Figure BDA0001194925160000021
Figure BDA0001194925160000021

(Vg,χ,γ)分别为无人机的地速、无人机的航迹偏角和无人机的航迹倾角；(α,β,μ)分别为无人机的迎角、无人机的侧滑角和无人机的航迹滚转角；(T,FD,FY,FL)分别为无人机的发动机推力、无人机的阻力、无人机的侧力和无人机的升力；m和g分别为无人机质量和重力加速度。(V g , χ, γ) are the ground speed of the UAV, the track declination angle of the UAV and the track inclination angle of the UAV; (α, β, μ) are the angle of attack of the UAV, The sideslip angle of the UAV and the track roll angle of the UAV; (T, F D , F Y , F L ) are the engine thrust of the UAV, the resistance of the UAV, and the side force of the UAV, respectively and the lift of the drone; m and g are the mass of the drone and the acceleration of gravity, respectively.

由式(1)(2)可见，(Vg,χ,γ)和(T,α,β,μ)均为相互耦合的控制量，若采用传统PID控制，控制效果必然会因耦合作用而受到严重影响。It can be seen from equations (1) and (2) that (V g , χ, γ) and (T, α, β, μ) are control variables that are coupled with each other. If traditional PID control is used, the control effect will inevitably be affected by the coupling effect. severely affected.

基于上述分析，所设计的三维轨迹制导方法应满足如下要求：Based on the above analysis, the designed 3D trajectory guidance method should meet the following requirements:

(1)能够在飞行包线约束条件下，同时控制无人机的地速和位置，精确跟踪三维轨迹，并实现无人机直接沿给定航路点飞行任务；(1) Under the constraints of the flight envelope, the ground speed and position of the UAV can be simultaneously controlled, the three-dimensional trajectory can be accurately tracked, and the UAV can fly directly along a given waypoint;

(2)期望飞行轨迹便于线生成和调整，且所需的时间和计算成本较小；(2) It is expected that the flight trajectory is convenient for line generation and adjustment, and the required time and computational cost are small;

(3)制导方法物理意义明确，形式简洁，参数整定方便，易于工程实现。(3) The guidance method has clear physical meaning, concise form, convenient parameter setting, and easy engineering implementation.

发明内容SUMMARY OF THE INVENTION

本发明的目的是为了解决上述问题，并满足上述要求，提出一种基于逆动力学的无人机三维轨迹制导方法，具体包括如下步骤：The purpose of the present invention is to solve the above problems and meet the above requirements, and propose a three-dimensional trajectory guidance method for unmanned aerial vehicles based on inverse dynamics, which specifically includes the following steps:

步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；Step 1. For a certain UAV, set the coordinates of the waypoints that the UAV flies through according to the desired trajectory, and divide the trajectory segments;

期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点。The desired trajectory is composed of flight segments connected by several waypoints; the flight segment has n (n≥1) segments, corresponding to n+1 waypoints.

步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；Step 2: For each waypoint, according to the maneuverability of the UAV and the set route conditions, set the conversion radius corresponding to the waypoint to d;

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

Figure BDA0001194925160000022
和纵向位移状态的微分项
Figure BDA0001194925160000031
Step 3: For the k-th trajectory segment, use the expected trajectory of the UAV to calculate the actual velocity error and the actual displacement error of the trajectory segment respectively, and use the PID control law to sum up the differential to form the desired lateral and lateral displacement state of the trajectory segment. item
Figure BDA0001194925160000022
and the differential term of the longitudinal displacement state
Figure BDA0001194925160000031

初始k＝1；initial k=1;

首先，无人机的实际速度误差包括横侧向速度误差

Figure BDA0001194925160000032
和纵向速度误差
Figure BDA00011949251600000316
计算如下：First, the actual speed error of the UAV includes the lateral and lateral speed error
Figure BDA0001194925160000032
and longitudinal velocity error
Figure BDA00011949251600000316
The calculation is as follows:

Figure BDA0001194925160000034
Figure BDA0001194925160000034

是无人机在第k段轨迹段上设定的指令跟踪横侧向速度；

Figure BDA0001194925160000036
是无人机当前在第k段轨迹段上实际的横侧向速度；是无人机在第k段轨迹段上设定的指令跟踪纵向速度；
Figure BDA0001194925160000038
是无人机当前在第k段轨迹段上实际的纵向速度；Vgk是第k段轨迹段上为无人机设定的指令地速；γk是第k个轨迹段所指向方向的航迹倾角；χk是第k个轨迹段所指向方向的航迹偏角；Vg是无人机当前在第k段轨迹段上实际的地速；γ是无人机当前在第k段轨迹段上实际的航迹倾角；χ是无人机当前在第k段轨迹段上实际的航迹偏角。 is the command tracking lateral speed set by the UAV on the k-th trajectory segment;
Figure BDA0001194925160000036
is the actual lateral speed of the UAV on the k-th trajectory segment; is the command tracking longitudinal speed set by the UAV on the k-th trajectory segment;
Figure BDA0001194925160000038
is the current actual longitudinal speed of the drone on the kth trajectory segment; V gk is the command ground speed set for the drone on the kth trajectory segment; γk is the flight direction in the direction pointed by the kth trajectory segment. track inclination; χ k is the track declination angle in the direction pointed by the kth trajectory segment; Vg is the actual ground speed of the UAV on the kth trajectory segment; γ is the current UAV on the kth trajectory segment is the actual track inclination angle on the segment; χ is the actual track declination angle of the UAV currently on the k-th track segment.

然后，无人机的实际位移误差包括横侧向位移误差(yline-y)和纵向位移误差(hline-h)；计算如下：Then, the actual displacement error of the UAV includes the lateral and lateral displacement error (y line -y) and the longitudinal displacement error (h line -h); the calculation is as follows:

从无人机当前实际位置处作此时对应直线轨迹的空间垂线，得到垂足坐标(xline,yline,hline)，该垂线段即为无人机的位置误差总和，分解为横侧向位移误差(yline-y)和纵向位移误差(hline-h)。From the current actual position of the UAV, draw the space vertical line corresponding to the straight line trajectory at this time, and obtain the vertical foot coordinates (x line , y line , h line ), and the vertical line segment is the sum of the position errors of the UAV, decomposed into horizontal Lateral displacement error (y line -y) and longitudinal displacement error (h line -h).

最后，PID控制律加和具体为：对无人机的位移误差(yline-y)和(hline-h)采用PI控制，将速度误差

Figure BDA0001194925160000039
和
Figure BDA00011949251600000310
作为微分项，并将PI控制与微分项求和，PID的参数即为速度误差和位移误差在飞行制导律中分别所占的权重，得到期望的横侧向位移状态的微分项
Figure BDA00011949251600000311
和纵向位移状态的微分项
Figure BDA00011949251600000312
Finally, the PID control law summation is as follows: PI control is adopted for the displacement errors (y line -y) and (h line -h) of the UAV, and the speed error is
Figure BDA0001194925160000039
and
Figure BDA00011949251600000310
As the differential term, the PI control and the differential term are summed. The parameters of the PID are the weights of the velocity error and the displacement error in the flight guidance law respectively, and the differential term of the desired lateral displacement state is obtained.
Figure BDA00011949251600000311
and the differential term of the longitudinal displacement state
Figure BDA00011949251600000312

步骤四、将期望的微分项和微分项

Figure BDA00011949251600000314
分别带入横侧向位移状态和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对微分方程进行第一级逆动力学解算，输出指令航迹偏角χc和航迹倾角γc；Step 4. Put the desired differential term and the differential term
Figure BDA00011949251600000314
The differential equations of the lateral displacement state and the longitudinal displacement state are respectively brought into the differential equations, and the first-order inverse dynamics solution is carried out to the differential equations by the combination of the analytical method and the numerical iteration method, and the commanded track declination angle χ c and the flight path are output. track inclination γ c ;

首先，将期望的微分项

Figure BDA00011949251600000315
带入纵向位移状态的微分方程中，采用解析法解算出指令航迹倾角γc，计算如下：First, set the desired differential term
Figure BDA00011949251600000315
Bringing into the differential equation of the longitudinal displacement state, the commanded track inclination angle γ c is calculated by the analytical method, and the calculation is as follows:

Figure BDA0001194925160000041
Figure BDA0001194925160000041

然后，将期望的微分项

Figure BDA0001194925160000042
和指令航迹倾角γc带入到横侧向位移状态的微分方程中，利用数值迭代法解算出指令航迹偏角χc，计算如下：Then, the desired differential term
Figure BDA0001194925160000042
and the commanded track inclination angle γ c are brought into the differential equation of the lateral displacement state, and the commanded track declination angle χ c is calculated by the numerical iterative method, and the calculation is as follows:

Figure BDA0001194925160000043
Figure BDA0001194925160000043

步骤五、将指令航迹偏角χc和指令航迹倾角γc以及无人机在该航路段上的指令地速Vgk作为下一级逆动力学解算的输入指令值，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力Tc、指令迎角αc和指令航迹滚转角μc；Step 5. Take the commanded track declination angle χc and commanded track inclination angle γc and the commanded ground speed Vgk of the UAV on the flight section as the input command value of the next-level inverse dynamics solution, and use the analytical method. The method combined with the numerical iterative method performs the second-level inverse dynamics solution to the state equation of the UAV ground speed, track inclination and track declination, and outputs the command thrust T c , command angle of attack α c and command track roll angle μ c ;

具体步骤如下：Specific steps are as follows:

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

Figure BDA0001194925160000045
和期望的地速微分值
Figure BDA0001194925160000046
Step 501, make the difference between the commanded track declination angle χ c , the commanded track inclination angle γ c and the commanded ground speed V gk respectively with the actual track declination angle χ of the current UAV, the track inclination angle γ and the ground speed V g , And adopt PD control to obtain the desired track declination differential value respectively Desired track inclination differential value
Figure BDA0001194925160000045
and the desired ground speed differential
Figure BDA0001194925160000046

步骤502、利用当前无人机的实际地速Vg和航迹倾角γ，以及期望的航迹偏角微分值

Figure BDA0001194925160000047
和期望的航迹倾角微分值
Figure BDA0001194925160000048
计算指令航迹滚转角μc的解析解；Step 502, use the actual ground speed V g and the track inclination angle γ of the current UAV, and the desired differential value of the track declination angle
Figure BDA0001194925160000047
and the desired differential value of track inclination
Figure BDA0001194925160000048
Calculate the analytical solution of the commanded track roll angle μ c ;

Figure BDA0001194925160000049
Figure BDA0001194925160000049

步骤503、利用指令航迹滚转角μc的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角αc的非线性一元方程；Step 503, using the analytical solution of the commanded track roll angle μc , combined with the desired ground speed state equation, and substituted into the desired track inclination angle state equation to obtain a nonlinear one-dimensional equation about the commanded angle of attack αc;

Figure BDA00011949251600000410
Figure BDA00011949251600000410

步骤504、用数值迭代法对关于指令迎角αc的非线性一元方程进行解算，得到指令迎角αc。Step 504 , using the numerical iteration method to solve the nonlinear one-dimensional equation about the commanded angle of attack α c to obtain the commanded angle of attack α c .

步骤505、将指令迎角αc代入期望地速状态方程中，解算出指令推力Tc；Step 505: Substitute the commanded angle of attack αc into the desired ground speed state equation, and solve the commanded thrust Tc ;

期望地速状态方程如下：The desired ground speed equation of state is as follows:

Figure BDA00011949251600000411
Figure BDA00011949251600000411

步骤六、将输出的指令推力Tc、指令迎角αc、侧滑角0°和航迹滚转角μc作为无人机姿态控制回路的输入，即可实现对无人机三维轨迹的跟踪控制；Step 6. Use the output command thrust T c , command angle of attack α c , sideslip angle 0° and track roll angle μ c as the input of the UAV attitude control loop, so as to realize the tracking of the three-dimensional trajectory of the UAV control;

步骤七、当无人机飞行至第k段轨迹段对应的转换半径d的范围内时，则无人机飞到所对应的航路点位置，继续将跟踪下一段轨迹，返回至步骤三，直至追踪至最后一个航路点。Step 7. When the drone flies to the range of the conversion radius d corresponding to the k-th trajectory segment, the drone flies to the corresponding waypoint position, and continues to track the next trajectory, returning to step 3 until Track to the last waypoint.

本发明的优点在于：The advantages of the present invention are:

(1)一种基于逆动力学的无人机三维轨迹制导方法，可同时精确的控制无人机的地速和三维位置。(1) A three-dimensional trajectory guidance method of UAV based on inverse dynamics, which can accurately control the ground speed and three-dimensional position of UAV at the same time.

(2)一种基于逆动力学的无人机三维轨迹制导方法，可在线调整无人机的期望飞行轨迹，且控制器计算成本低。(2) A three-dimensional trajectory guidance method of UAV based on inverse dynamics, which can adjust the desired flight trajectory of the UAV online, and the computational cost of the controller is low.

(3)一种基于逆动力学的无人机三维轨迹制导方法，控制结构简单，各部分物理意义明确，便于参数整定。(3) A three-dimensional trajectory guidance method of UAV based on inverse dynamics, the control structure is simple, the physical meaning of each part is clear, and the parameter setting is convenient.

附图说明Description of drawings

图1是本发明整个制导控制器的结构框图；Fig. 1 is the structural block diagram of the whole guidance controller of the present invention;

图2是本发明一种基于逆动力学的无人机三维轨迹制导方法流程图；Fig. 2 is a kind of flow chart of the UAV three-dimensional trajectory guidance method based on inverse dynamics of the present invention;

图3是本发明采用的三维制导方法中存在的几何关系示意图；3 is a schematic diagram of the geometric relationship existing in the three-dimensional guidance method adopted by the present invention;

图4是本发明采用解析法和数值迭代法相结合进行第二级逆动力学解算的流程图；Fig. 4 is the flow chart that the present invention adopts the combination of analytical method and numerical iterative method to carry out the second-level inverse dynamics solution;

图5a是无人机实际位置与对应直线航路段之间的距离误差图；Fig. 5a is the distance error diagram between the actual position of the UAV and the corresponding straight route segment;

图5b是无人机地速误差图；Figure 5b is the ground speed error map of the UAV;

图5c是无人机XY平面轨迹跟踪效果图；Figure 5c is the effect diagram of UAV XY plane trajectory tracking;

图5d是无人机XZ平面轨迹跟踪效果图；Figure 5d is the effect diagram of UAV XZ plane trajectory tracking;

图5e是无人机YZ平面轨迹跟踪效果图。Figure 5e is the effect of UAV YZ plane trajectory tracking.

具体实施方式Detailed ways

下面将结合附图和实施例对本发明作进一步的详细说明。The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments.

本发明一种基于逆动力学的无人机三维轨迹制导方法，通过将解析法和数值迭代法相结合，依次对无人机的位置状态方程进行一级逆动力学解算，以及对无人机的地速和航迹角状态方程进行二级逆动力学解算，得到无人机的指令推力、指令迎角和指令航迹滚转角，再将这几个指令量输入已设计好的姿态控制回路，即可控制无人机的实际地速和位置，同时采用PID控制，使无人机的实际位置收敛于航路点之间的参考轨迹上。The present invention is a three-dimensional trajectory guidance method for unmanned aerial vehicles based on inverse dynamics. By combining the analytical method and the numerical iterative method, the first-order inverse dynamics solution of the position state equation of the unmanned aerial vehicle is successively performed, and the unmanned aerial vehicle The ground speed and track angle state equations of the UAV are calculated by the second-level inverse dynamics, and the commanded thrust, commanded angle of attack and commanded track roll angle of the UAV are obtained, and then these command quantities are input into the designed attitude control. The loop can control the actual ground speed and position of the UAV, and at the same time use PID control to make the actual position of the UAV converge on the reference trajectory between the waypoints.

整个制导控制器的结构框图如图1，其整体工作流程为：The structural block diagram of the entire guidance controller is shown in Figure 1, and its overall workflow is:

针对某个无人机，首先设定好各航路点的坐标，根据指令航路点坐标，根据无人机当前位置(x,y,h)获取计算出期望航迹角，并设定好无人机在各航路段上的指令地速；根据指令地速和期望航迹角，可计算出无人机在横侧向与纵向的指令跟踪地速分量

Figure BDA0001194925160000061
和
Figure BDA0001194925160000062
令其分别于无人机实际的地速分量
Figure BDA0001194925160000063
和
Figure BDA0001194925160000064
分别作差，获取无人机的实际速度误差和
Figure BDA00011949251600000613
再乘以系数KD，作为PID控制器的微分项；For a certain drone, first set the coordinates of each waypoint, according to the commanded waypoint coordinates, obtain and calculate the desired track angle according to the current position (x, y, h) of the drone, and set the unmanned The commanded ground speed of the drone on each flight segment; according to the commanded ground speed and the desired track angle, the commanded tracking ground speed components of the drone in the lateral and longitudinal directions can be calculated
Figure BDA0001194925160000061
and
Figure BDA0001194925160000062
Let it be different from the actual ground speed component of the drone
Figure BDA0001194925160000063
and
Figure BDA0001194925160000064
Make the difference separately to obtain the actual speed error of the UAV and
Figure BDA00011949251600000613
Multiply by the coefficient K D as the differential term of the PID controller;

同时根据指令航路点坐标，生成对应的各直线轨迹段，从无人机的当前位置向当前对应的直线航路段作垂线，垂足坐标为(xline,yline,hline)，令yline和hline分别与无人机的实际横侧向坐标y与纵向高度h作差，计算无人机在各直线轨迹段上的实际位移误差(yline-y)和(hline-h)；并并采用PI控制律，分别乘以系数KP和KI；At the same time, according to the commanded waypoint coordinates, the corresponding straight line trajectory segments are generated, and a vertical line is drawn from the current position of the UAV to the current corresponding straight line segment. The vertical foot coordinates are (x line , y line , h line ), let y line and h line are respectively the difference between the actual lateral coordinate y and the vertical height h of the UAV, and the actual displacement errors (y line -y) and (h line -h) of the UAV on each straight trajectory segment are calculated. ; And adopt the PI control law, multiply the coefficients K P and K I respectively;

接下来要采用PID控制，构成期望的横侧向和纵向位移状态的微分值

Figure BDA0001194925160000067
和作为第一级逆动力学解算的输入，将PI控制项与微分项分别对应加和构成和作为第一级逆动力学解算的输入，采用解析法与数值迭代法相结合的逆动力学方法解算出指令航迹角，作为第二级逆动力学解算的输入，解算出指令推力Tc、指令迎角αc和指令航迹滚转角μc。将Tc输入至无人机动力控制模块，将αc和μc输入至无人机的姿态控制回路中，进一步生成无人机的相应舵角指令，从而同时控制无人机的姿态与地速。Next, PID control is used to form the differential values of the desired lateral and longitudinal displacement states
Figure BDA0001194925160000067
and As the input of the first-level inverse dynamics solution, the PI control term and the differential term are correspondingly added and formed. and As the input of the first-level inverse dynamics solution, the commanded track angle is calculated by the inverse dynamics method combining the analytical method and the numerical iterative method. As the input of the second-level inverse dynamics solution, the commanded thrust T c is calculated. , the commanded angle of attack α c and the commanded track roll angle μ c . Input T c to the power control module of the UAV, input α c and μ c into the attitude control loop of the UAV, and further generate the corresponding rudder angle command of the UAV, so as to control the attitude and ground of the UAV at the same time. speed.

如图2所示，具体包括如下步骤：As shown in Figure 2, it specifically includes the following steps:

步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；Step 1. For a certain UAV, set the coordinates of the waypoints that the UAV flies through according to the desired trajectory, and divide the trajectory segments;

根据设定的无人机飞行所需要经过的航路点坐标，构成无人机的期望轨迹，并将轨迹划分为若干轨迹段；无人机的期望轨迹由若干航路点所联结成的航路段构成，设航路点按序号可联结为n(n≥1)段直线航路，对应有n+1个航路点。According to the coordinates of the waypoints that the UAV needs to pass through for flight, the desired trajectory of the UAV is formed, and the trajectory is divided into several trajectory segments; the desired trajectory of the UAV is composed of flight segments connected by several waypoints , the waypoints can be connected to n (n≥1) straight lines according to the serial number, corresponding to n+1 waypoints.

步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；Step 2: For each waypoint, according to the maneuverability of the UAV and the set route conditions, set the conversion radius corresponding to the waypoint to d;

本实施例中将每个航路点的转换半径均设定为d。In this embodiment, the conversion radius of each waypoint is set as d.

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

Figure BDA00011949251600000611
和纵向位移状态的微分项
Figure BDA0001194925160000071
Step 3: For the k-th trajectory segment, use the expected trajectory of the UAV to calculate the actual velocity error and the actual displacement error of the trajectory segment respectively, and use the PID control law to sum up the differential to form the desired lateral and lateral displacement state of the trajectory segment. item
Figure BDA00011949251600000611
and the differential term of the longitudinal displacement state
Figure BDA0001194925160000071

将无人机基于航路点的期望轨迹跟踪问题划分为速度误差收敛问题和位置误差收敛问题，分别设计相应的PID控制策略，再将所设计的PID控制律加和构成期望横侧向位移状态和纵向位移状态的微分项；The UAV's desired trajectory tracking problem based on waypoints is divided into the speed error convergence problem and the position error convergence problem, the corresponding PID control strategies are designed respectively, and the designed PID control laws are added to form the desired lateral displacement state and Differential term of longitudinal displacement state;

具体为：Specifically:

(1)无人机速度误差收敛问题(1) UAV speed error convergence problem

如图3所示，当无人机UAV收敛于第k(1≤k≤n)段航路上时，无人机UAV的三维坐标为(x,y,h)，第k-1个航路点WPk-1(xk-1,yk-1,hk-1)与第k个航路点WPk(xk,yk,hk)之间的航迹偏角χk∈[0,2π)和航迹倾角γk∈(-π/2,π/2)即为此时无人机的期望航迹偏角和航迹倾角，其表达式为：As shown in Figure 3, when the UAV UAV converges on the k-th (1≤k≤n) route, the three-dimensional coordinates of the UAV UAV are (x, y, h), and the k-1th waypoint The track declination angle χ k ∈ [ 0 ,2π) and the track inclination γ k ∈(-π/2,π/2) are the expected track declination and track inclination of the UAV at this time, and their expressions are:

Figure BDA0001194925160000072
Figure BDA0001194925160000072

其中yk-1和yk起到了指令横侧向位移的作用，hk-1和hk起到了指令纵向位移的作用，χk和γk即为所要跟踪的虚拟航迹角；χk是第k个轨迹段所指向方向的航迹偏角；γk是第k个轨迹段所指向方向的航迹倾角；Among them, y k-1 and y k play the role of commanded lateral displacement, h k-1 and h k play the role of commanded longitudinal displacement, χ k and γ k are the virtual track angles to be tracked; χ k is the track declination in the direction pointed by the kth trajectory segment; γ k is the track inclination in the direction pointed by the kth trajectory segment;

注意：当k＝1时，第k-1个航路点为(xk-1,yk-1,hk-1)＝(x0,y0,h0)，该点被定义为飞行的起点。Note: When k=1, the k-1th waypoint is (x k-1 , y k-1 , h k-1 )=(x 0 , y 0 , h 0 ), which is defined as a flight starting point.

将第k个轨迹段所要跟踪的虚拟航迹角χk和γk以及无人机此时设定的指令地速Vgk代入式(1)，得到此时的指令跟踪横侧向速度和纵向速度

Figure BDA0001194925160000074
其与实际速度的误差分别为
Figure BDA0001194925160000075
和
Figure BDA0001194925160000076
表达式为Substitute the virtual track angles χ k and γ k to be tracked by the kth track segment and the command ground speed V gk set by the UAV at this time into Equation (1) to obtain the command tracking lateral speed at this time. and longitudinal velocity
Figure BDA0001194925160000074
The error from the actual speed is
Figure BDA0001194925160000075
and
Figure BDA0001194925160000076
The expression is

Figure BDA0001194925160000077
Figure BDA0001194925160000077

(2)无人机位置误差收敛问题(2) Unmanned aerial vehicle position error convergence problem

如图3所示，从无人机当前实际位置处作此时所对应第k段航路的垂线，可得垂足坐标FP(xline,yline,hline)，该垂线段即为无人机的位置误差总和，可分解为横侧向位移误差(yline-y)和纵向位移误差(hline-h)。其中，yline起着指令横侧向位移的作用，hline起着指令纵向位移的作用。As shown in Figure 3, from the current actual position of the UAV, draw the vertical line of the corresponding k-th route at this time, and the vertical foot coordinate FP (x line , y line , h line ) can be obtained, and the vertical line segment is no The sum of the position error of man-machine can be decomposed into lateral displacement error (y line -y) and longitudinal displacement error (h line -h). Among them, y line plays the role of commanding lateral displacement, and h line plays the role of commanding longitudinal displacement.

也就是说，yk-1，hk-1，yk，hk，yline和hline均起到了指令位置的作用，其中，yk和hk是驱使无人机收敛到指令横侧向和纵向速度的位置指令，yline和hline是驱使无人机收敛到指令轨迹的位置指令，制导方法中存在的几何关系如图1所示，此时需要对位置指令进行加权求和处理，本发明引入了PID控制的思路来解决这一问题。That is to say, y k-1 , h k-1 , y k , h k , y line and h line all play the role of the command position, where y k and h k are to drive the drone to converge to the lateral side of the command The position commands of the direction and longitudinal velocity, the y line and the h line are the position commands that drive the UAV to converge to the command trajectory. The geometric relationship in the guidance method is shown in Figure 1. At this time, the weighted summation processing of the position commands is required. , the present invention introduces the idea of PID control to solve this problem.

对于指令轨迹的收敛问题，其位移误差(yline-y)和(hline-h)采用PI控制，从而使无人机能够无差收敛到指令轨迹上；对于指令速度的收敛问题，通过式(3)和(4)已经求得了横侧向和纵向速度误差

Figure BDA0001194925160000081
和
Figure BDA0001194925160000082
该项可直接作为微分项。将PI控制的结构与微分项求和，其中PID的参数即为速度误差收敛问题和位置误差收敛问题分别在飞行制导律中所占的权重，可得期望的横侧向位移状态的微分项
Figure BDA0001194925160000083
和纵向位移状态的微分项
Figure BDA0001194925160000084
For the convergence problem of the command trajectory, the displacement errors (y line -y) and (h line -h) are controlled by PI, so that the UAV can converge to the command trajectory without error; for the convergence problem of the command speed, the formula (3) and (4) have obtained the lateral and longitudinal velocity errors
Figure BDA0001194925160000081
and
Figure BDA0001194925160000082
This term can be used directly as a derivative term. The structure of PI control and the differential term are summed, where the parameters of PID are the weights of the speed error convergence problem and the position error convergence problem in the flight guidance law respectively, and the differential term of the desired lateral displacement state can be obtained.
Figure BDA0001194925160000083
and the differential term of the longitudinal displacement state
Figure BDA0001194925160000084

步骤四：将步骤三中所得的期望状态的微分项

Figure BDA0001194925160000085
和
Figure BDA0001194925160000086
分别带入横侧向位移状态的微分方程和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对无人机的横侧向位移和纵向位移状态方程进行第一级逆动力学解算，输出指令航迹偏角χc和航迹倾角γc。Step 4: Differentiate the desired state obtained in Step 3
Figure BDA0001194925160000085
and
Figure BDA0001194925160000086
The differential equations of the lateral displacement state and the longitudinal displacement state are respectively brought into the differential equations of the lateral displacement state and the differential equation of the longitudinal displacement state, and the first-order inverse dynamics are carried out on the lateral and longitudinal displacement state equations of the UAV by a combination of analytical method and numerical iterative method. Learn to solve, output command track declination χ c and track inclination γ c .

用期望横侧向位移状态的微分项和纵向位移状态的微分项

Figure BDA0001194925160000088
替代式(1)中的对应的微分项
Figure BDA0001194925160000089
和
Figure BDA00011949251600000810
with the differential term of the desired lateral displacement state and the differential term of the longitudinal displacement state
Figure BDA0001194925160000088
Substitute the corresponding differential term in Eq. (1)
Figure BDA0001194925160000089
and
Figure BDA00011949251600000810

首先，针对期望纵向位移的状态方程，通过解析法解算出指令航迹倾角γc的解析解：First, according to the state equation of the desired longitudinal displacement, the analytical solution of the commanded track inclination angle γ c is solved by the analytical method:

Figure BDA00011949251600000811
Figure BDA00011949251600000811

然后，将解算出的γc带入到期望横侧向位移的微分状态方程中，可得仅包含未知数χc的一元方程：Then, the calculated γ c is brought into the differential state equation of the expected lateral displacement, and the unary equation containing only the unknown χ c can be obtained:

Figure BDA00011949251600000812
Figure BDA00011949251600000812

利用数值方法，例如牛顿迭代法解算出指令航迹偏角χc。The commanded track declination angle χ c is calculated by numerical methods, such as Newton iteration method.

步骤五：将步骤四解算出的航迹偏角χc和航迹倾角γc以及设定的无人机指令地速Vgk作为下一级逆动力学解算的输入，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力Tc、指令迎角αc和指令航迹滚转角μc。Step 5: Use the track declination angle χ c and the track inclination angle γ c calculated in step 4 and the set UAV command ground speed V gk as the input of the next-level inverse dynamics solution, using the analytical method and numerical value The iterative method is used to solve the second-order inverse dynamics of the state equation of the UAV ground speed, track inclination and track declination, and output the command thrust T c , command angle of attack α c and command track roll angle μ c .

如图4所示，具体步骤如下：As shown in Figure 4, the specific steps are as follows:

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

Figure BDA0001194925160000091
期望的航迹倾角微分值
Figure BDA0001194925160000092
和期望的地速微分值
Figure BDA0001194925160000093
Step 501, make the difference between the commanded track declination angle χ c , the commanded track inclination angle γ c and the commanded ground speed V gk respectively with the actual track declination angle χ of the current UAV, the track inclination angle γ and the ground speed V g , And adopt PD control to obtain the desired track declination differential value respectively
Figure BDA0001194925160000091
Desired track inclination differential value
Figure BDA0001194925160000092
and the desired ground speed differential
Figure BDA0001194925160000093

步骤502、利用当前无人机的实际地速Vg和航迹倾角γ，以及期望的航迹偏角微分值和期望的航迹倾角微分值

Figure BDA0001194925160000095
计算指令航迹滚转角μc的解析解；Step 502, use the actual ground speed V g and the track inclination angle γ of the current UAV, and the desired differential value of the track declination angle and the desired differential value of track inclination
Figure BDA0001194925160000095
Calculate the analytical solution of the commanded track roll angle μ c ;

由于无人机采用BTT方式转弯，侧滑角β约等于0，此时侧力FY也约等于0，因此式(2)的三个方程可化简为Since the UAV turns by the BTT method, the sideslip angle β is approximately equal to 0, and the side force F Y is also approximately equal to 0 at this time, so the three equations of Equation (2) can be simplified as

Figure BDA0001194925160000096
Figure BDA0001194925160000096

Figure BDA0001194925160000097
Figure BDA0001194925160000097

Figure BDA0001194925160000098
Figure BDA0001194925160000098

上式中的微分值

Figure BDA0001194925160000099
和被其期望值
Figure BDA00011949251600000911
Figure BDA00011949251600000912
和
Figure BDA00011949251600000913
所替换，联立式(8)和(9)，可得指令航迹滚转角的解析解：The differential value in the above formula
Figure BDA0001194925160000099
and by its expected value
Figure BDA00011949251600000911
Figure BDA00011949251600000912
and
Figure BDA00011949251600000913
Substituting, combining equations (8) and (9), the analytical solution of the commanded track roll angle can be obtained:

Figure BDA00011949251600000914
Figure BDA00011949251600000914

步骤503、利用指令航迹滚转角μc的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角αc的非线性一元方程；Step 503, using the analytical solution of the commanded track roll angle μc , combined with the desired ground speed state equation, and substituted into the desired track inclination angle state equation to obtain a nonlinear one-dimensional equation about the commanded angle of attack αc;

将解算出的指令航迹滚转角μc和式(7)代入式(9)中，得只含未知数指令迎角αc的非线性一元方程：Substituting the calculated commanded track roll angle μ c and equation (7) into equation (9), the nonlinear one-dimensional equation containing only the unknown commanded angle of attack α c is obtained:

Figure BDA00011949251600000915
Figure BDA00011949251600000915

注：阻力FD和升力FL在此时均可视为变量αc的函数；Note: Both the drag FD and the lift FL can be regarded as functions of the variable α c at this time;

步骤504、用数值迭代法对关于指令迎角αc的非线性一元方程进行解算，得到指令迎角αc。Step 504 , using the numerical iteration method to solve the nonlinear one-dimensional equation about the commanded angle of attack α c to obtain the commanded angle of attack α c .

步骤505、将指令迎角αc代入期望地速状态方程中，解算出指令推力Tc；Step 505: Substitute the commanded angle of attack αc into the desired ground speed state equation, and solve the commanded thrust Tc ;

期望地速状态方程如下：The desired ground speed equation of state is as follows:

Figure BDA00011949251600000916
Figure BDA00011949251600000916

将αc代入式(7)，即可解算出指令推力Tc。The command thrust T c can be calculated by substituting α c into Equation (7).

步骤六、将输出的指令推力Tc、指令迎角αc、侧滑角0°和航迹滚转角μc作为无人机姿态控制回路的输入，即可实现对无人机三维轨迹的跟踪控制；Step 6. Use the output command thrust T c , command angle of attack α c , sideslip angle 0° and track roll angle μ c as the input of the UAV attitude control loop, so as to realize the tracking of the three-dimensional trajectory of the UAV control;

步骤七：当无人机飞行至距离目前所跟踪轨迹段的对应航路点d的位置时，即可认为无人机已飞到所对应的航路点位置，此时无人机将跟踪下一段轨迹，整个制导算法循环至步骤三，直至追踪至最后一个航路点。Step 7: When the drone flies to the position of the corresponding waypoint d from the currently tracked track segment, it can be considered that the drone has flown to the corresponding waypoint position, and the drone will track the next track. , the entire guidance algorithm loops to step 3 until the last waypoint is tracked.

本实施例中，设定无人机以200m/s的地速飞行，初始高度为7010m；采用本发明方法的风扰条件下，获取的无人机三维轨迹跟踪效果图；In this embodiment, the drone is set to fly at a ground speed of 200m/s, and the initial height is 7010m; under the wind disturbance condition of the method of the present invention, the obtained three-dimensional trajectory tracking effect diagram of the drone;

如图5a所示，是无人机实际位置与对应直线航路段之间的距离误差图，可以看出无人机在进行恒速爬升及转弯动作时，位置不可避免的存在一定程度的超调，但很快位置误差就能收敛到接近0的程度；As shown in Figure 5a, it is a map of the distance error between the actual position of the UAV and the corresponding straight line segment. It can be seen that when the UAV performs constant-speed climbing and turning actions, the position inevitably has a certain degree of overshoot. , but the position error will soon converge to a level close to 0;

如图5b所示，是无人机地速误差图，可以看出无人机在进行恒速爬升及转弯动作时，会不可避免的存在一定的速度损失，但很快速度误差就能收敛到接近0的程度，且速度损失不大；As shown in Figure 5b, it is the ground speed error map of the UAV. It can be seen that when the UAV performs constant-speed climbing and turning actions, there will inevitably be a certain speed loss, but the speed error will soon converge to The degree is close to 0, and the speed loss is not large;

如图5c，5d和5e所示，分别是XY平面轨迹跟踪效果图，XZ平面轨迹跟踪效果图和YZ平面轨迹跟踪效果图，这三个图直观的体现了无人机的实际飞行过程，可以看出：在爬升-拉平阶段，纵向高度上一开始存在约100m左右的超调，但是很快无人机就恢复到指令轨迹上了；在45°大角度水平转弯时，高度变化仅在10m-20m左右，横侧向位置超调达到约400m后，能较快收敛到指令位置，相对于无人机200m/s的高速，该位置超调是较为可观的。As shown in Figures 5c, 5d and 5e, they are the XY plane trajectory tracking effect diagram, the XZ plane trajectory tracking effect diagram and the YZ plane trajectory tracking effect diagram respectively. These three diagrams intuitively reflect the actual flight process of the UAV. It can be seen that: in the climb-levelling stage, there is an overshoot of about 100m in the longitudinal height at first, but the drone soon returns to the command trajectory; when turning horizontally at a large angle of 45°, the height change is only 10m. About -20m, after the lateral and lateral position overshoot reaches about 400m, it can quickly converge to the command position. Compared with the high speed of the UAV of 200m/s, the position overshoot is quite considerable.

Claims (3)

1.一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，包括如下步骤：1. an unmanned aerial vehicle three-dimensional trajectory guidance method based on inverse dynamics, is characterized in that, comprises the steps: 步骤一、针对某个无人机，根据期望轨迹将无人机飞行经过的航路点设定坐标，并划分轨迹段；Step 1. For a certain UAV, set the coordinates of the waypoints that the UAV flies through according to the desired trajectory, and divide the trajectory segments; 期望轨迹由若干航路点联结成的航路段构成；航路段共有n(n≥1)段，对应有n+1个航路点；The desired trajectory is composed of flight segments connected by several waypoints; the flight segment has n (n≥1) segments, corresponding to n+1 waypoints; 步骤二、针对每个航路点，根据无人机的机动性能和所设定好的航路情况，设定该航路点对应的转换半径为d；Step 2: For each waypoint, according to the maneuverability of the UAV and the set route conditions, set the conversion radius corresponding to the waypoint to d; 步骤三、针对第k段轨迹段，利用无人机的期望轨迹分别计算该轨迹段的实际速度误差和实际位移误差，采用PID控制律加和构成该轨迹段期望的横侧向位移状态的微分项和纵向位移状态的微分项
Figure FDA0002156435200000012
Step 3: For the k-th trajectory segment, use the expected trajectory of the UAV to calculate the actual velocity error and the actual displacement error of the trajectory segment respectively, and use the PID control law to sum up the differential to form the desired lateral and lateral displacement state of the trajectory segment. item and the differential term of the longitudinal displacement state
Figure FDA0002156435200000012
 初始k＝1，1≤k≤n；Initial k=1, 1≤k≤n; 具体为：Specifically: 首先，无人机的实际速度误差包括横侧向速度误差
Figure FDA0002156435200000013
和纵向速度误差
Figure FDA0002156435200000014
计算如下：First, the actual speed error of the UAV includes the lateral and lateral speed error
Figure FDA0002156435200000013
and longitudinal velocity error
Figure FDA0002156435200000014
The calculation is as follows:
Figure FDA0002156435200000015
Figure FDA0002156435200000015
Figure FDA0002156435200000016
是无人机在第k段轨迹段上设定的指令跟踪横侧向速度；是无人机当前在第k段轨迹段上实际的横侧向速度；是无人机在第k段轨迹段上设定的指令跟踪纵向速度；
Figure FDA0002156435200000019
是无人机当前在第k段轨迹段上实际的纵向速度；Vgk是第k段轨迹段上为无人机设定的指令地速；γk是第k个轨迹段所指向方向的航迹倾角；χk是第k个轨迹段所指向方向的航迹偏角；Vg是无人机当前在第k段轨迹段上实际的地速；γ是无人机当前在第k段轨迹段上实际的航迹倾角；χ是无人机当前在第k段轨迹段上实际的航迹偏角；
Figure FDA0002156435200000016
is the command tracking lateral speed set by the UAV on the k-th trajectory segment; is the actual lateral speed of the UAV on the k-th trajectory segment; is the command tracking longitudinal speed set by the UAV on the k-th trajectory segment;
Figure FDA0002156435200000019
is the current actual longitudinal speed of the drone on the kth trajectory segment; V gk is the command ground speed set for the drone on the kth trajectory segment; γk is the flight direction in the direction pointed by the kth trajectory segment. track inclination; χ k is the track declination angle in the direction pointed by the kth trajectory segment; Vg is the actual ground speed of the UAV on the kth trajectory segment; γ is the current UAV on the kth trajectory segment is the actual track inclination angle on the segment; χ is the actual track declination angle of the UAV on the k-th track segment; 然后，无人机的实际位移误差包括横侧向位移误差(yline-y)和纵向位移误差(hline-h)；计算如下：Then, the actual displacement error of the UAV includes the lateral and lateral displacement error (y line -y) and the longitudinal displacement error (h line -h); the calculation is as follows: 从无人机当前实际位置处作此时对应直线轨迹的空间垂线，得到垂足坐标(xline,yline,hline)，该垂线段即为无人机的位置误差总和，分解为横侧向位移误差(yline-y)和纵向位移误差(hline-h)；From the current actual position of the UAV, draw the space vertical line corresponding to the straight line trajectory at this time, and obtain the vertical foot coordinates (x line , y line , h line ), and the vertical line segment is the sum of the position errors of the UAV, decomposed into horizontal Lateral displacement error (y line -y) and longitudinal displacement error (h line -h); 最后，PID控制律加和具体为：对无人机的位移误差(yline-y)和(hline-h)采用PI控制，将速度误差
Figure FDA0002156435200000021
和
Figure FDA0002156435200000022
作为微分项，并将PI控制与微分项求和，PID的参数即为速度误差和位移误差在飞行制导律中分别所占的权重，得到期望的横侧向位移状态的微分项
Figure FDA0002156435200000023
和纵向位移状态的微分项 Finally, the PID control law summation is as follows: PI control is adopted for the displacement errors (y line -y) and (h line -h) of the UAV, and the speed error is
Figure FDA0002156435200000021
and
Figure FDA0002156435200000022
As the differential term, the PI control and the differential term are summed. The parameters of the PID are the weights of the velocity error and the displacement error in the flight guidance law respectively, and the differential term of the desired lateral displacement state is obtained.
Figure FDA0002156435200000023
and the differential term of the longitudinal displacement state 步骤四、将期望的微分项
Figure FDA0002156435200000025
和微分项
Figure FDA0002156435200000026
分别带入横侧向位移状态和纵向位移状态的微分方程中，采用解析法和数值迭代法相结合的方法对微分方程进行第一级逆动力学解算，输出指令航迹偏角χc和航迹倾角γc；Step 4. Put the desired differential term
Figure FDA0002156435200000025
and the differential term
Figure FDA0002156435200000026
The differential equations of the lateral displacement state and the longitudinal displacement state are respectively brought into the differential equations, and the first-order inverse dynamics solution is carried out to the differential equations by the combination of the analytical method and the numerical iteration method, and the commanded track declination angle χ c and the flight path are output. track inclination γ c ; 步骤五、将指令航迹偏角χc和指令航迹倾角γc以及无人机在该航路段上的指令地速Vgk作为下一级逆动力学解算的输入指令值，采用解析法和数值迭代法相结合的方法对无人机地速、航迹倾角和航迹偏角的状态方程进行第二级逆动力学解算，输出指令推力Tc、指令迎角αc和指令航迹滚转角μc；Step 5. Take the commanded track declination angle χc and commanded track inclination angle γc and the commanded ground speed Vgk of the UAV on the flight section as the input command value of the next-level inverse dynamics solution, and use the analytical method. The method combined with the numerical iterative method performs the second-level inverse dynamics solution to the state equation of the UAV ground speed, track inclination and track declination, and outputs the command thrust T c , command angle of attack α c and command track roll angle μ c ; 步骤六、将输出的指令推力Tc、指令迎角αc、侧滑角0°和航迹滚转角μc作为无人机姿态控制回路的输入，实现对无人机三维轨迹的跟踪控制；Step 6: Use the output command thrust T c , command angle of attack α c , sideslip angle 0° and track roll angle μ c as the input of the UAV attitude control loop, so as to realize the tracking control of the three-dimensional trajectory of the UAV; 步骤七、当无人机飞行至第k段轨迹段对应的转换半径d的范围内时，则无人机飞到所对应的航路点位置，继续将跟踪下一段轨迹，返回至步骤三，直至追踪至最后一个航路点。Step 7. When the drone flies to the range of the conversion radius d corresponding to the k-th trajectory segment, the drone flies to the corresponding waypoint position, and continues to track the next trajectory, returning to step 3 until Track to the last waypoint. 2.如权利要求1所述的一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，所述的步骤四具体为：2. a kind of UAV three-dimensional trajectory guidance method based on inverse dynamics as claimed in claim 1 is characterized in that, described step 4 is specifically: 首先，将期望的微分项
Figure FDA0002156435200000027
带入纵向位移状态的微分方程中，采用解析法解算出指令航迹倾角γc，计算如下：First, set the desired differential term
Figure FDA0002156435200000027
Bringing into the differential equation of the longitudinal displacement state, the commanded track inclination angle γ c is calculated by the analytical method, and the calculation is as follows:
Figure FDA0002156435200000028
Figure FDA0002156435200000028
 然后，将期望的微分项
Figure FDA0002156435200000029
和指令航迹倾角γc带入到横侧向位移状态的微分方程中，利用数值迭代法解算出指令航迹偏角χc，计算如下：Then, the desired differential term
Figure FDA0002156435200000029
and the commanded track inclination angle γ c are brought into the differential equation of the lateral displacement state, and the commanded track declination angle χ c is calculated by the numerical iterative method, and the calculation is as follows:
Figure FDA00021564352000000210
Figure FDA00021564352000000210
 3.如权利要求1所述的一种基于逆动力学的无人机三维轨迹制导方法，其特征在于，所述的步骤五具体为：3. a kind of unmanned aerial vehicle three-dimensional trajectory guidance method based on inverse dynamics as claimed in claim 1 is characterized in that, described step 5 is specifically: 具体步骤如下：Specific steps are as follows: 步骤501、将指令航迹偏角χc，指令航迹倾角γc和指令地速Vgk分别与当前无人机实际的航迹偏角χ，航迹倾角γ和地速Vg作差，并采用PD控制，分别得到期望的航迹偏角微分值
Figure FDA00021564352000000211
期望的航迹倾角微分值
Figure FDA0002156435200000031
和期望的地速微分值
Figure FDA0002156435200000032
Step 501, make the difference between the commanded track declination angle χ c , the commanded track inclination angle γ c and the commanded ground speed V gk respectively with the actual track declination angle χ of the current UAV, the track inclination angle γ and the ground speed V g , And adopt PD control to obtain the desired track declination differential value respectively
Figure FDA00021564352000000211
Desired track inclination differential value
Figure FDA0002156435200000031
and the desired ground speed differential
Figure FDA0002156435200000032
 步骤502、利用当前无人机的实际地速Vg和航迹倾角γ，以及期望的航迹偏角微分值和期望的航迹倾角微分值计算指令航迹滚转角μc的解析解；Step 502, use the actual ground speed V g and the track inclination angle γ of the current UAV, and the desired differential value of the track declination angle and the desired differential value of track inclination Calculate the analytical solution of the commanded track roll angle μ c ;
Figure FDA0002156435200000035
Figure FDA0002156435200000035
 步骤503、利用指令航迹滚转角μc的解析解，结合期望地速状态方程，代入期望航迹倾角状态方程中，得到关于指令迎角αc的非线性一元方程；Step 503, using the analytical solution of the commanded track roll angle μc , combined with the desired ground speed state equation, and substituted into the desired track inclination angle state equation to obtain a nonlinear one-dimensional equation about the commanded angle of attack αc;
Figure FDA0002156435200000036
Figure FDA0002156435200000036
 FL为无人机的升力，m和g分别为无人机的质量和重力加速度，FD为无人机的阻力； FL is the lift of the UAV, m and g are the mass and gravitational acceleration of the UAV, respectively, and F D is the resistance of the UAV; 步骤504、用数值迭代法对关于指令迎角αc的非线性一元方程进行解算，得到指令迎角αc；Step 504: Solve the nonlinear one-dimensional equation about the commanded angle of attack αc by using the numerical iterative method to obtain the commanded angle of attack αc ; 步骤505、将指令迎角αc代入期望地速状态方程中，解算出指令推力Tc；Step 505: Substitute the commanded angle of attack αc into the desired ground speed state equation, and solve the commanded thrust Tc ; 期望地速状态方程如下：The desired ground speed equation of state is as follows:
CN201611233604.6A 2016-12-28 2016-12-28 Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics Active CN106774400B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611233604.6A CN106774400B (en) 2016-12-28 2016-12-28 Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611233604.6A CN106774400B (en) 2016-12-28 2016-12-28 Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics

Publications (2)

Publication Number Publication Date
CN106774400A CN106774400A (en) 2017-05-31
CN106774400B true CN106774400B (en) 2020-02-07

Family

ID=58921346

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611233604.6A Active CN106774400B (en) 2016-12-28 2016-12-28 Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics

Country Status (1)

Country Link
CN (1) CN106774400B (en)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107992069B (en) * 2017-11-29 2021-01-19 上海无线电设备研究所 Guidance law design method for unmanned aerial vehicle path tracking control
CN109828602B (en) * 2018-01-19 2020-09-04 北京航空航天大学 Track loop nonlinear model transformation method based on observation compensation technology
CN108873929B (en) * 2018-05-24 2020-05-29 北京航空航天大学 A method and system for autonomous landing of a fixed-wing aircraft
CN109189079B (en) * 2018-11-05 2021-07-23 南京理工大学 Navigation control method of mobile robot based on GPS positioning
CN111580542B (en) * 2019-02-15 2024-07-19 北京京东乾石科技有限公司 Dynamic unmanned aerial vehicle formation control method, device and storage medium
CN110345945B (en) * 2019-05-29 2022-02-15 深圳市道通智能航空技术股份有限公司 Transmission method and device of reference flight point data and unmanned aerial vehicle
CN110879611B (en) * 2019-11-01 2022-12-16 中国电子科技集团公司电子科学研究院 Unmanned aerial vehicle cluster three-dimensional curve path tracking method and device
CN111487966B (en) * 2020-04-13 2022-09-09 哈尔滨工程大学 Self-adaptive path tracking control method for unmanned surface vehicle based on waypoints
CN112099528B (en) * 2020-09-18 2023-02-28 中国航空无线电电子研究所 Guidance method based on chemical reaction optimization algorithm
CN112686921B (en) * 2021-01-08 2023-12-01 西安羚控电子科技有限公司 Multi-interference unmanned aerial vehicle detection tracking method based on track characteristics
CN113419557B (en) * 2021-06-17 2022-07-19 哈尔滨工业大学 Audio synthesis method for sports drone
CN114326793B (en) * 2021-12-09 2023-09-15 北京机电工程研究所 PLC-based unmanned aerial vehicle flight control device, system and unmanned aerial vehicle

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103135546A (en) * 2013-01-28 2013-06-05 北京航空航天大学 Unmanned aerial vehicle flying leg switching method based on leg distance and relative location vector dot product
CN105159305A (en) * 2015-08-03 2015-12-16 南京航空航天大学 Four-rotor flight control method based on sliding mode variable structure
CN105259760A (en) * 2015-10-23 2016-01-20 天津工业大学 Method for designing small quadrocopter controller

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103135546A (en) * 2013-01-28 2013-06-05 北京航空航天大学 Unmanned aerial vehicle flying leg switching method based on leg distance and relative location vector dot product
CN105159305A (en) * 2015-08-03 2015-12-16 南京航空航天大学 Four-rotor flight control method based on sliding mode variable structure
CN105259760A (en) * 2015-10-23 2016-01-20 天津工业大学 Method for designing small quadrocopter controller

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
《Adaptive Flight Control Design for Quadrotor UAV Based on Dynamic Inversion and Neural Networks》;Qing Lin等;《2013 Third International conference on Instrumentation, Measurement, Computer, Communication and Control》;20140623;全文 *
《Attitude control of UAV based on PI dynamic inversion》;Hai Chen等;《2016 35th Chinese Control Conference》;20160829;全文 *
《基于改进动态逆的四旋翼无人机三维路径跟踪控制》;卢晓东等;《仪表技术》;20150715;第1节-第2节 *
《采用逆动力学法的无人机跟踪飞行速度控制系统设计》;王永寿等;《控制与制导》;20101231;全文 *

Also Published As

Publication number Publication date
CN106774400A (en) 2017-05-31

Similar Documents

Publication Publication Date Title
CN106774400B (en) Unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics
Rubí et al. A survey of path following control strategies for UAVs focused on quadrotors
CN103760906B (en) Neural network and non-linear continuous depopulated helicopter attitude control method
Zheng et al. Planning and tracking in image space for image-based visual servoing of a quadrotor
CN110806756B (en) Autonomous guidance and control method of UAV based on DDPG
CN103885450B (en) Depopulated helicopter attitude nonlinear control method and verification platform
CN109445449B (en) A high subsonic UAV ultra-low altitude flight control system and method
CN113156985B (en) Robust anti-disturbance flight control method for fixed-wing UAV obstacle avoidance based on preset performance
CN106681345A (en) Crowd-searching-algorithm-based active-disturbance-rejection control method for unmanned plane
CN111338374B (en) Unmanned aerial vehicle cluster formation control method
CN106527462A (en) Unmanned aerial vehicle (UAV) control device
Guan et al. Moving path following with prescribed performance and its application on automatic carrier landing
CN109991992A (en) Model-free adaptive control method for small unmanned helicopter based on immersion invariant set
CN112161626B (en) High-flyability route planning method based on route tracking mapping network
Brezoescu et al. Straight-line path following in windy conditions
CN107678442A (en) A kind of four rotors based under dual model it is autonomous ship control method
CN113821054B (en) Unmanned plane track tracking guidance method based on pigeon intelligent optimization dynamic inverse control
CN108958278B (en) A fast anti-jamming guidance method for the cruise segment of an aerospace vehicle
CN107450313A (en) UAV autopilot control system based on Self Adaptive Control
Dong et al. Economic model-predictive control for aircraft forced landing: Framework and two-level implementation
Zhang et al. Small fixed-wing unmanned aerial vehicle path following under low altitude wind shear disturbance
Kuś et al. Helicopter control algorithms from the set orientation to the set geographical Location
Tingting et al. Formation control of multiple UAVs via pigeon inspired optimisation
CN113377122B (en) Adaptive control method for switching of motor-driven variant aircraft capable of perching
Regina et al. Fixed-wing UAV guidance law for surface-target tracking and overflight

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant