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 PDFInfo
- 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
- track
- unmanned aerial
- aerial vehicle
- command
- angle
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000004364 calculation method Methods 0.000 claims abstract description 18
- 238000004458 analytical method Methods 0.000 claims abstract description 13
- 238000006073 displacement reaction Methods 0.000 claims description 56
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 2
- 230000000694 effects Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000001808 coupling effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
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
The invention discloses an unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics, belonging to the technical field of unmanned aerial vehicle navigation guidance and control; firstly, aiming at a certain track section of an unmanned aerial vehicle, sequentially carrying out primary inverse dynamics solution on a position state equation of the unmanned aerial vehicle and carrying out secondary inverse dynamics solution on a ground speed and track angle state equation of the unmanned aerial vehicle by combining an analytic method and a numerical iteration method to obtain a command thrust, a command attack angle and a command track roll angle of the unmanned aerial vehicle, inputting the command quantities into a designed attitude control loop, so that the actual ground speed and position of the unmanned aerial vehicle can be controlled, and meanwhile, PID control is adopted to enable the actual position of the unmanned aerial vehicle to be converged on a reference track between route points; the invention can simultaneously and accurately control the ground speed and the three-dimensional position of the unmanned aerial vehicle, adjust the expected flight track of the unmanned aerial vehicle on line, and the controller has low calculation cost.
Description
Technical Field
The invention belongs to the technical field of unmanned aerial vehicle navigation guidance and control, and particularly relates to an unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics.
Background
Unmanned aerial vehicles are also called unmanned aircrafts and are widely applied to military and civil fields; unmanned aerial vehicle guidance means that an unmanned aerial vehicle flies along a given track through a command program. Along with the fact that the execution tasks of the unmanned aerial vehicle are diversified day by day, the requirements of people on the maneuverability of the unmanned aerial vehicle are increased day by day, the traditional two-dimensional track guidance law cannot meet the requirements, high-precision tracking control of three-dimensional tracks is achieved, the unmanned aerial vehicle can complete special tasks such as terrain avoidance, formation flight and autonomous aerial refueling, and the unmanned aerial vehicle control system has important significance.
In the track tracking control of the unmanned aerial vehicle, the unmanned aerial vehicle can directly fly along a given route point, and the method has important practical value, the current reference track design method of many unmanned aerial vehicles is to fit each route point by a space three-dimensional curve, or to split the track into a plurality of sections for fitting respectively, although the smooth reference track can be obtained by the method, the algorithm calculation amount is large, particularly when the method faces a complex reference track, the fitting difficulty of the method is increased linearly, and the method is very unfavorable for the low-cost unmanned aerial vehicle with poor performance of an onboard computer device.
In addition, from the viewpoint of flight dynamics of the unmanned aerial vehicle, a highly coupled relationship exists between the control quantities to which the guidance loop belongs. Wherein the position equation of the unmanned aerial vehicle is
In the formula, (x, y, h) is a three-dimensional coordinate of the unmanned aerial vehicle;
the equation of state of the ground speed and the track angle is
(Vgχ, γ) the ground speed of the drone, the track yaw angle of the drone and the track inclination angle of the drone, (α, μ) the attack angle of the drone, the sideslip angle of the drone and the track roll angle of the drone, (T, F)D,FY,FL) The engine thrust of the unmanned aerial vehicle, the resistance of the unmanned aerial vehicle, the lateral force of the unmanned aerial vehicle and the lift force of the unmanned aerial vehicle are respectively; and m and g are the mass and the gravity acceleration of the unmanned aerial vehicle respectively.
As can be seen from the formulae (1) (2), (V)gChi, gamma) and (T, α, mu) are mutually coupled control quantities, and if the traditional PID control is adopted, the control effect is inevitably seriously influenced by the coupling action.
Based on the analysis, the designed three-dimensional trajectory guidance method should meet the following requirements:
(1) the ground speed and the position of the unmanned aerial vehicle can be simultaneously controlled under the constraint condition of the flight envelope, the three-dimensional track can be accurately tracked, and the task of directly flying the unmanned aerial vehicle along a given route point can be realized;
(2) the flight trajectory is expected to be convenient for line generation and adjustment, and the required time and calculation cost are small;
(3) the guidance method has the advantages of clear physical significance, concise form, convenient parameter setting and easy engineering realization.
Disclosure of Invention
The invention aims to solve the problems and meet the requirements, and provides an unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics, which specifically comprises the following steps:
setting coordinates of waypoints through which an unmanned aerial vehicle flies according to an expected track and dividing track sections for a certain unmanned aerial vehicle;
the expected track is formed by route sections formed by connecting a plurality of route points; the navigation path section has n (n is more than or equal to 1) sections, and n +1 navigation path points are correspondingly arranged.
Step two, aiming at each route point, setting the conversion radius corresponding to the route point as d according to the maneuvering performance of the unmanned aerial vehicle and the set route condition;
step three, aiming at the k-th track segment, respectively calculating the actual speed error and the actual displacement error of the track segment by utilizing the expected track of the unmanned aerial vehicle, and adding differential terms forming the expected transverse and lateral displacement states of the track segment by adopting a PID control law
And differential term of longitudinal displacement state
Initial k is 1;
first, the actual speed error of the drone includes lateral speed error
And longitudinal velocity error
The calculation is as follows:
the unmanned aerial vehicle tracks the transverse lateral speed according to the instruction set on the k-th track segment;
the actual transverse lateral speed of the unmanned aerial vehicle on the k-th track segment at present;the unmanned aerial vehicle tracks the longitudinal speed according to the instruction set on the k-th track segment;
the actual longitudinal speed of the unmanned aerial vehicle on the k-th track segment at present; vgkThe command ground speed set for the unmanned aerial vehicle on the kth track section; gamma raykIs the track inclination in the direction pointed by the kth track segment; chi shapekIs the track declination of the direction in which the kth track segment points; vgThe current actual ground speed of the unmanned aerial vehicle on the k-th track segment; gamma is the actual track inclination angle of the unmanned aerial vehicle on the k-th track segment; χ is the current actual flight path deviation angle of the unmanned aerial vehicle in the k-th track segment.
Then, the actual displacement error of the drone includes the lateral displacement error (y)line-y) and longitudinal displacement error (h)line-h); the calculation is as follows:
making a space perpendicular line of the corresponding straight line track at the moment from the current actual position of the unmanned aerial vehicle to obtain a foot coordinate (x)line,yline,hline) The vertical line segment is the sum of the position errors of the unmanned aerial vehicle and is decomposed into transverse lateral displacement errors (y)line-y) and longitudinal displacement error (h)line-h)。
Finally, the PID control law addition specifically comprises: displacement error (y) to droneline-y) and (h)line-h) using PI control, error of speed
And
taking the PI control as a differential term, summing the PI control and the differential term, wherein the PID parameter is the weight of the speed error and the displacement error respectively in the flight guidance law, and obtaining the differential term of the expected transverse lateral displacement state
And differential term of longitudinal displacement state
Step four, desired differential terms are divided intoAnd derivative term
Respectively carrying out first-stage inverse dynamic solution on the differential equations in the transverse and lateral displacement states and the longitudinal displacement state by adopting a method of combining an analytic method and a numerical iteration method, and outputting a command track drift angle xcAnd track inclination angle gammac；
First, the desired derivative term is divided
The differential equation brought into the longitudinal displacement state is adopted to solve and calculate the command track inclination angle gamma by an analytic methodcThe calculation is as follows:
then, the desired microItemizing
And the commanded track inclination angle gammacThe deviation angle chi of the command track is solved and calculated by numerical iteration method by being brought into a differential equation of a transverse lateral displacement statecThe calculation is as follows:
step five, deviating the deviation angle x of the command trackcAnd the commanded track inclination angle gammacAnd the command ground speed V of the unmanned aerial vehicle on the navigation road sectiongkAs an input instruction value of next stage inverse dynamics solution, a method combining an analytic method and a numerical iteration method is adopted to carry out second stage inverse dynamics solution on state equations of ground speed, track inclination angle and track deflection angle of the unmanned aerial vehicle, and instruction thrust T is outputcCommand angle of attack αcAnd commanded track roll angle muc；
The method comprises the following specific steps:
step 501, the deviation angle χ of the command trackcCommand track inclination angle gammacAnd commanded ground speed VgkRespectively corresponding to the actual track drift angle x, track inclination angle gamma and ground speed V of the current unmanned aerial vehiclegMaking difference and adopting PD control to respectively obtain the desired track declination differential valuesExpected track inclination differential value
And a desired ground speed differential value
Step 502, utilizing actual ground speed V of current unmanned aerial vehiclegAnd track inclination angle gamma, and the expected track drift differential value
And the expected track inclination angle differential value
Calculating commanded track roll angle mucThe analytic solution of (2);
step 503, utilizing the commanded track roll angle μcIs combined with the expected ground speed state equation and is substituted into the expected track inclination angle state equation to obtain α about the command attack anglecA non-linear unary equation of (2);
step 504, using numerical iteration method to compare α with the command attack anglecThe nonlinear unitary equation is solved to obtain an instruction attack angle αc。
Step 505, commanding the attack angle αcSubstituting into the expected ground speed state equation to calculate the command thrust Tc；
The desired ground speed equation of state is as follows:
step six, outputting the command thrust TcCommand angle of attack αcSide slip angle of 0 DEG and track roll angle mucThe three-dimensional trajectory tracking control of the unmanned aerial vehicle can be realized by being used as the input of an unmanned aerial vehicle attitude control loop;
and step seven, when the unmanned aerial vehicle flies to the range of the conversion radius d corresponding to the k-th track section, the unmanned aerial vehicle flies to the corresponding waypoint position, the next track section is tracked continuously, and the step three is returned until the last waypoint is tracked.
The invention has the advantages that:
(1) an unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics can simultaneously and accurately control the ground speed and the three-dimensional position of an unmanned aerial vehicle.
(2) An unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics can adjust the expected flight track of an unmanned aerial vehicle on line, and the calculation cost of a controller is low.
(3) An unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics is simple in control structure, clear in physical significance of each part and convenient to set parameters.
Drawings
FIG. 1 is a block diagram of the overall guidance controller of the present invention;
FIG. 2 is a flow chart of the unmanned aerial vehicle three-dimensional trajectory guidance method based on inverse dynamics;
FIG. 3 is a schematic representation of the geometric relationships that exist in the three-dimensional guidance method employed by the present invention;
FIG. 4 is a flow chart of a second stage inverse dynamics solution using a combination of analytical and numerical iterative methods in accordance with the present invention;
fig. 5a is a distance error map between the actual position of the drone and the corresponding straight-line flight segment;
FIG. 5b is a plot of the velocity error of the drone;
FIG. 5c is an XY plane trajectory tracking effect diagram of the unmanned aerial vehicle;
FIG. 5d is a diagram of the unmanned aerial vehicle XZ plane trajectory tracking effect;
fig. 5e is a diagram of the effect of tracking the YZ plane of the drone.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
The invention relates to an unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics, which combines an analytic method and a numerical iteration method, sequentially carries out primary inverse dynamics solution on a position state equation of an unmanned aerial vehicle, and carries out secondary inverse dynamics solution on a ground speed and track angle state equation of the unmanned aerial vehicle to obtain a command thrust, a command attack angle and a command track roll angle of the unmanned aerial vehicle, and then inputs the command quantities into a designed attitude control loop to control the actual ground speed and position of the unmanned aerial vehicle, and meanwhile, PID control is adopted to enable the actual position of the unmanned aerial vehicle to be converged on a reference track between waypoints.
The structural block diagram of the whole guidance controller is shown in fig. 1, and the whole work flow is as follows:
for a certain unmanned aerial vehicle, firstly setting coordinates of each route point, acquiring and calculating an expected track angle according to the current position (x, y, h) of the unmanned aerial vehicle and the coordinates of the command route points, and setting command ground speeds of the unmanned aerial vehicle on each route section; according to the command ground speed and the expected track angle, the command tracking ground speed component of the unmanned aerial vehicle in the transverse direction and the longitudinal direction can be calculated
And
make it respectively in the actual ground speed component of unmanned aerial vehicle
And
respectively making difference to obtain actual speed error of unmanned aerial vehicleAnd
multiplied by a factor KDAs a derivative of the PID controller;
meanwhile, generating corresponding linear track sections according to the coordinates of the command waypoints, making a perpendicular line from the current position of the unmanned aerial vehicle to the current corresponding linear waypoint section, wherein the vertical foot coordinates are (x)line,yline,hline) Let ylineAnd hlineRespectively calculating the difference between the actual transverse and lateral coordinates y and the longitudinal height h of the unmanned aerial vehicle, and calculating the actual displacement error (y) of the unmanned aerial vehicle on each linear track sectionline-y) and (h)line-h); and multiplying by coefficient K respectively by PI control lawPAnd KI；
PID control is then applied to form differential values of the desired lateral and longitudinal displacement states
Andas the input of the first stage inverse dynamics calculation, the PI control item and the differential item are respectively and correspondingly added to formAndas the input of the first stage inverse dynamics calculation, the inverse dynamics method combining the analytic method and the numerical iteration method is adopted to calculate the command track angle, and as the input of the second stage inverse dynamics calculation, the command thrust T is calculatedcCommand angle of attack αcAnd commanded track roll angle muc. Will TcInput to the unmanned power control module αcAnd mucAnd inputting the control signal into an attitude control loop of the unmanned aerial vehicle, and further generating a corresponding rudder angle instruction of the unmanned aerial vehicle so as to simultaneously control the attitude and the ground speed of the unmanned aerial vehicle.
As shown in fig. 2, the method specifically includes the following steps:
setting coordinates of waypoints through which an unmanned aerial vehicle flies according to an expected track and dividing track sections for a certain unmanned aerial vehicle;
forming an expected track of the unmanned aerial vehicle according to set waypoint coordinates which the unmanned aerial vehicle needs to pass through for flying, and dividing the track into a plurality of track sections; the expected track of the unmanned aerial vehicle is formed by route sections formed by connecting a plurality of route points, the route points can be connected into n (n is more than or equal to 1) sections of straight routes according to the serial number, and n +1 route points are correspondingly arranged.
Step two, aiming at each route point, setting the conversion radius corresponding to the route point as d according to the maneuvering performance of the unmanned aerial vehicle and the set route condition;
in the present embodiment, the transition radius of each waypoint is set to d.
Step three, aiming at the k-th track segment, respectively calculating the actual speed error and the actual displacement error of the track segment by utilizing the expected track of the unmanned aerial vehicle, and adding differential terms forming the expected transverse and lateral displacement states of the track segment by adopting a PID control law
And differential term of longitudinal displacement state
Dividing the problem of expected track tracking of the unmanned aerial vehicle based on the waypoints into a speed error convergence problem and a position error convergence problem, respectively designing corresponding PID control strategies, and adding the designed PID control laws to form differential terms of an expected transverse displacement state and an expected longitudinal displacement state;
the method specifically comprises the following steps:
(1) speed error convergence problem of unmanned aerial vehicle
As shown in FIG. 3, when the UAV converges on the kth (k is more than or equal to 1 and less than or equal to n) th flight path, the three-dimensional coordinate of the UAV is (x, y, h), and the kth-1 st flight path point WPk-1(xk-1,yk-1,hk-1) To the kth waypoint WPk(xk,yk,hk) Flight path deviation angle x betweenkE [0,2 π) and track dip γkThe epsilon (-pi/2, pi/2) is the expected track deflection angle and the track inclination angle of the unmanned aerial vehicle at the moment, and the expression is as follows:
wherein y isk-1And ykPlays a role of instructing horizontal lateral displacement, hk-1And hkPlays the role of longitudinal displacement of the command, chikAnd gammakNamely the virtual track angle to be tracked; chi shapekIs the track declination of the direction in which the kth track segment points; gamma raykIs the direction pointed to by the k-th track segmentTrack inclination of;
note that: when k is 1, the k-1 th waypoint is (x)k-1,yk-1,hk-1)＝(x0,y0,h0) This point is defined as the start of flight.
The virtual track angle x to be tracked of the kth track segmentkAnd gammakAnd the command ground speed V set by the unmanned aerial vehicle at the momentgkSubstituting expression (1) to obtain the command tracking lateral velocity at this timeAnd longitudinal velocity
The error from the actual speed is respectively
And
is expressed as
(2) Problem of convergence of position error of unmanned aerial vehicle
As shown in fig. 3, the vertical line of the k-th route corresponding to the current actual position of the drone can be used to obtain the coordinates FP (x) of the vertical footline,yline,hline) The vertical line segment is the sum of the position errors of the unmanned aerial vehicle and can be decomposed into transverse and lateral displacement errors (y)line-y) and longitudinal displacement error (h)line-h). Wherein, ylineActing as a command for lateral displacement, hlineActing to command longitudinal displacement.
That is, yk-1，hk-1，yk，hk，ylineAnd hlineAll function as command positions, where ykAnd hkDrives the unmanned aerial vehicle to converge to the position of the command transverse and longitudinal speedInstruction, ylineAnd hlineThe method is a position instruction for driving the unmanned aerial vehicle to converge to an instruction track, the geometric relation existing in the guidance method is shown in figure 1, and at the moment, the position instruction needs to be subjected to weighted summation processing.
For the convergence problem of the command trajectory, its displacement error (y)line-y) and (h)line-h) employing PI control, thereby enabling the drone to converge on the command trajectory without difference; for the convergence problem of the command speed, the lateral and longitudinal speed errors have been obtained by the equations (3) and (4)
And
the term may be directly referred to as a derivative term. The structure of PI control is summed with the differential terms, wherein the parameters of PID are the weight of the velocity error convergence problem and the position error convergence problem in the flight guidance law respectively, and the differential terms of the expected transverse lateral displacement state can be obtained
And differential term of longitudinal displacement state
Step four: differentiating the expected state obtained in the third step
And
respectively carrying out first-stage inverse dynamics calculation on the lateral displacement state equation and the longitudinal displacement state equation of the unmanned aerial vehicle by adopting a method of combining an analytic method and a numerical iteration method in the differential equation of the lateral displacement state and the differential equation of the longitudinal displacement state, and outputting a command track drift angle xcAnd track inclination angle gammac。
By differential terms of desired lateral displacement stateAnd differential term of longitudinal displacement state
Corresponding differential terms in alternative (1)
And
firstly, aiming at a state equation of expected longitudinal displacement, calculating a command track inclination angle gamma by an analytic methodcAnalytic solution of (2):
then, the calculated gamma is comparedcBy substituting into the differential equation of state for the desired lateral displacement, only the unknown χ can be obtainedcThe unitary equation of (a):
calculating commanded track yaw angle χ using numerical methods, e.g. Newton's iterationc。
Step five: solving the track drift angle x calculated in the step fourcAnd track inclination angle gammacAnd set unmanned aerial vehicle instruction ground speed VgkAs input of next stage inverse dynamics calculation, the state equations of the ground speed, the track inclination angle and the track deflection angle of the unmanned aerial vehicle are subjected to second stage inverse dynamics calculation by adopting a method of combining an analytic method and a numerical iteration method, and command thrust T is outputcCommand angle of attack αcAnd commanded track roll angle muc。
As shown in fig. 4, the specific steps are as follows:
step (ii) of501. Deviation angle x of command trackcCommand track inclination angle gammacAnd commanded ground speed VgkRespectively corresponding to the actual track drift angle x, track inclination angle gamma and ground speed V of the current unmanned aerial vehiclegMaking difference and adopting PD control to respectively obtain the desired track declination differential values
Expected track inclination differential value
And a desired ground speed differential value
Step 502, utilizing actual ground speed V of current unmanned aerial vehiclegAnd track inclination angle gamma, and the expected track drift differential valueAnd the expected track inclination angle differential value
Calculating commanded track roll angle mucThe analytic solution of (2);
because the unmanned aerial vehicle adopts the BTT mode to turn, sideslip angle β is approximately equal to 0, and side force F is equal to this momentYAlso approximately equal to 0, so that the three equations of equation (2) can be reduced to
Differential value in the above equation
Andis measured by its expected value
And
alternatively, coupled equations (8) and (9), may yield an analytical solution for the commanded track roll angle:
step 503, utilizing the commanded track roll angle μcIs combined with the expected ground speed state equation and is substituted into the expected track inclination angle state equation to obtain α about the command attack anglecA non-linear unary equation of (2);
the calculated command track rolling angle mucThe sum formula (7) is substituted in the formula (9) to obtain the command attack angle α containing only unknown numbercThe non-linear unitary equation of (2):
note: resistance FDAnd lift force FLAll of which may be considered variable α at this timecA function of (a);
step 504, using numerical iteration method to compare α with the command attack anglecThe nonlinear unitary equation is solved to obtain an instruction attack angle αc。
Step 505, commanding the attack angle αcSubstituting into the expected ground speed state equation to calculate the command thrust Tc；
The desired ground speed equation of state is as follows:
α will be mixedcBy substituting formula (7), the command thrust T can be calculatedc。
Step six, outputting the command thrust TcCommand angle of attack αcSide slip angle of 0 DEG and track roll angle mucThe three-dimensional trajectory tracking control of the unmanned aerial vehicle can be realized by being used as the input of an unmanned aerial vehicle attitude control loop;
step seven: when the unmanned aerial vehicle flies to the position of the corresponding waypoint d away from the current tracked track section, the unmanned aerial vehicle can be considered to fly to the position of the corresponding waypoint, the unmanned aerial vehicle can track the next section of track at the moment, and the whole guidance algorithm is circulated to the third step until the last waypoint is tracked.
In the embodiment, the unmanned aerial vehicle is set to fly at the ground speed of 200m/s, and the initial height is 7010 m; under the wind disturbance condition by adopting the method, an unmanned aerial vehicle three-dimensional trajectory tracking effect graph is obtained;
as shown in fig. 5a, it is a distance error graph between the actual position of the unmanned aerial vehicle and the corresponding straight-line navigation road section, and it can be seen that when the unmanned aerial vehicle climbs at a constant speed and turns, the position inevitably has a certain degree of overshoot, but the position error can be converged to a degree close to 0 soon;
as shown in fig. 5b, it is a ground speed error diagram of the unmanned aerial vehicle, and it can be seen that when the unmanned aerial vehicle climbs at a constant speed and turns, a certain speed loss inevitably exists, but the speed error can be converged to a degree close to 0 soon, and the speed loss is not large;
as shown in fig. 5c, 5d and 5e, the XY plane trajectory tracking effect graph, the XZ plane trajectory tracking effect graph and the YZ plane trajectory tracking effect graph are respectively provided, and these three graphs intuitively reflect the actual flight process of the unmanned aerial vehicle, and it can be seen that: in the climbing-leveling stage, overshoot of about 100m initially exists in the longitudinal height, but the unmanned aerial vehicle is restored to the instruction track soon; when the unmanned aerial vehicle horizontally turns at a large angle of 45 degrees, the height change is only about 10m-20m, after the overshoot of the transverse lateral position reaches about 400m, the unmanned aerial vehicle can be quickly converged to the instruction position, and the overshoot of the position is considerable relative to the 200m/s high speed of the unmanned aerial vehicle.
Claims (3)
1. An unmanned aerial vehicle three-dimensional track guidance method based on inverse dynamics is characterized by comprising the following steps:
setting coordinates of waypoints through which an unmanned aerial vehicle flies according to an expected track and dividing track sections for a certain unmanned aerial vehicle;
the expected track is formed by route sections formed by connecting a plurality of route points; the navigation path section has n (n is more than or equal to 1) sections, and n +1 navigation path points are correspondingly arranged;
step two, aiming at each route point, setting the conversion radius corresponding to the route point as d according to the maneuvering performance of the unmanned aerial vehicle and the set route condition;
step three, aiming at the k-th track segment, respectively calculating the actual speed error and the actual displacement error of the track segment by utilizing the expected track of the unmanned aerial vehicle, and adding differential terms forming the expected transverse and lateral displacement states of the track segment by adopting a PID control lawAnd differential term of longitudinal displacement state
Initial k is 1, and k is more than or equal to 1 and less than or equal to n;
the method specifically comprises the following steps:
first, the actual speed error of the drone includes lateral speed error
And longitudinal velocity error
The calculation is as follows:
then, the actual displacement error of the drone includes the lateral displacement error (y)line-y) and longitudinal displacement error (h)line-h); the calculation is as follows:
making a space perpendicular line of the corresponding straight line track at the moment from the current actual position of the unmanned aerial vehicle to obtain a foot coordinate (x)line,yline,hline) The vertical line segment is the sum of the position errors of the unmanned aerial vehicle and is decomposed into transverse lateral displacement errors (y)line-y) and longitudinal displacement error (h)line-h)；
Finally, the PID control law addition specifically comprises: displacement error (y) to droneline-y) and (h)line-h) using PI control, error of speed
And
taking the PI control as a differential term, summing the PI control and the differential term, wherein the PID parameter is the weight of the speed error and the displacement error respectively in the flight guidance law, and obtaining the differential term of the expected transverse lateral displacement state
And differential term of longitudinal displacement state
Step four, desired differential terms are divided into
And derivative term
Respectively carrying out first-stage inverse dynamic solution on the differential equations in the transverse and lateral displacement states and the longitudinal displacement state by adopting a method of combining an analytic method and a numerical iteration method, and outputting a command track drift angle xcAnd track inclination angle gammac；
Step five, deviating the deviation angle x of the command trackcAnd the commanded track inclination angle gammacAnd the command ground speed V of the unmanned aerial vehicle on the navigation road sectiongkAs an input instruction value of next stage inverse dynamics solution, a method combining an analytic method and a numerical iteration method is adopted to carry out second stage inverse dynamics solution on state equations of ground speed, track inclination angle and track deflection angle of the unmanned aerial vehicle, and instruction thrust T is outputcCommand angle of attack αcAnd commanded track roll angle muc；
Step six, outputting the command thrust TcCommand angle of attack αcSide slip angle of 0 DEG and track roll angle mucAs the input of an unmanned aerial vehicle attitude control loop, the unmanned aerial vehicle is realizedTracking control of the three-dimensional track;
and step seven, when the unmanned aerial vehicle flies to the range of the conversion radius d corresponding to the k-th track section, the unmanned aerial vehicle flies to the corresponding waypoint position, the next track section is tracked continuously, and the step three is returned until the last waypoint is tracked.
2. The unmanned aerial vehicle three-dimensional trajectory guidance method based on inverse dynamics as claimed in claim 1, wherein the fourth step is specifically:
first, the desired derivative term is divided
The differential equation brought into the longitudinal displacement state is adopted to solve and calculate the command track inclination angle gamma by an analytic methodcThe calculation is as follows:
then, the desired derivative term is added
And the commanded track inclination angle gammacThe deviation angle chi of the command track is solved and calculated by numerical iteration method by being brought into a differential equation of a transverse lateral displacement statecThe calculation is as follows:
3. the unmanned aerial vehicle three-dimensional trajectory guidance method based on inverse dynamics as claimed in claim 1, wherein the fifth step is specifically:
the method comprises the following specific steps:
step 501, the deviation angle χ of the command trackcCommand track inclination angle gammacAnd commanded ground speed VgkRespectively with the actual track drift angle x of the current unmanned plane,track inclination angle gamma and ground speed VgMaking difference and adopting PD control to respectively obtain the desired track declination differential values
Expected track inclination differential value
And a desired ground speed differential value
Step 502, utilizing actual ground speed V of current unmanned aerial vehiclegAnd track inclination angle gamma, and the expected track drift differential valueAnd the expected track inclination angle differential valueCalculating commanded track roll angle mucThe analytic solution of (2);
step 503, utilizing the commanded track roll angle μcIs combined with the expected ground speed state equation and is substituted into the expected track inclination angle state equation to obtain α about the command attack anglecA non-linear unary equation of (2);
FLfor unmanned aerial vehicle lift, m and g are respectively the mass and gravitational acceleration of the unmanned aerial vehicle, FDIs the resistance of the drone;
step 504, using numerical iteration method to compare α with the command attack anglecThe nonlinear unitary equation is solved to obtain an instruction attack angle αc；
Step 505, commanding the attack angle αcSubstituting into the expected ground speed state equation to calculate the command thrust Tc；
The desired ground speed equation of state is as follows:
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)
| 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 | 北京航空航天大学 | Method and system for autonomous landing of fixed-wing aircraft |
| CN109189079B (en) * | 2018-11-05 | 2021-07-23 | 南京理工大学 | Mobile robot navigation control method 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 unmanned aerial vehicle |
| CN114326793B (en) * | 2021-12-09 | 2023-09-15 | 北京机电工程研究所 | Unmanned aerial vehicle flight control device, system and unmanned aerial vehicle based on PLC |
Citations (3)
| 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 |
-
2016
- 2016-12-28 CN CN201611233604.6A patent/CN106774400B/en active Active
Patent Citations (3)
| 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)
| 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 | |
| Lee et al. | Obstacle avoidance using image-based visual servoing integrated with nonlinear model predictive control | |
| CN108845588B (en) | Trajectory tracking control method of four-rotor aircraft based on nonlinear guidance | |
| CN105425812B (en) | Unmanned aerial vehicle automatic landing trajectory control method based on dual models | |
| CN106444822B (en) | A kind of stratospheric airship path tracking control method based on space vector field guidance | |
| CN104199303B (en) | Stratospheric satellite planar path tracking control method based on vector field guidance | |
| CN105116914B (en) | A kind of stratospheric airship analytic modell analytical model predicted path tracking and controlling method | |
| CN105652879A (en) | Autonomous flight control method for unmanned plane without ailerons | |
| Schopferer et al. | Performance-aware flight path planning for unmanned aircraft in uniform wind fields | |
| CN112198885B (en) | Unmanned aerial vehicle control method capable of meeting autonomous landing requirement of maneuvering platform | |
| CN117389312B (en) | Model-based anti-roll three-dimensional tracking control method for underwater vehicle | |
| CN110456816A (en) | A kind of quadrotor Trajectory Tracking Control method based on continuous terminal sliding mode | |
| Liang et al. | Active disturbance rejection attitude control for a bird-like flapping wing micro air vehicle during automatic landing | |
| Brezoescu et al. | Straight-line path following in windy conditions | |
| Safwat et al. | Robust Nonlinear Flight Controller For Small Unmanned Aircraft Vehicle based on Incremental BackStepping | |
| Yang et al. | A decentralised control strategy for formation flight of unmanned aerial vehicles | |
| CN107450313A (en) | UAV autopilot control system based on Self Adaptive Control | |
| Karimoddini et al. | Hierarchical control design of a UAV helicopter | |
| Sedlmair et al. | Design and experimental validation of UAV control laws-3D spline-path-following and easy-handling remote control | |
| Mills et al. | Vision based control for fixed wing UAVs inspecting locally linear infrastructure using skid-to-turn maneuvers | |
| Bhardwaj et al. | Unified Trajectory Control for Vertical Take-Off and Landing Transition Aircraft | |
| CN112161626B (en) | High-flyability route planning method based on route tracking mapping network | |
| CN116203840A (en) | Adaptive gain scheduling control method for reusable carrier | |
| Fang et al. | Adaptive nonlinear path following method for fix‐wing micro aerial vehicle | |
| Verma et al. | A novel trajectory tracking methodology using structured adaptive model inversion for uninhabited aerial vehicles |
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 |