CN103728981A - Non-linear navigation tracking control method for unmanned aerial vehicle - Google Patents

Abstract

The invention relates to a non-linear navigation tracking control method for an unmanned aerial vehicle. A fixed navigation control period is adopted, and a navigation angle is obtained according to four different flying states of the unmanned aerial vehicle and deviation conditions thereof, thus the non-linear navigation tracking control is carried out on the unmanned aerial vehicle according to the navigation angle so as to ensure that the unmanned aerial vehicle can obtain appropriate navigation angles according to the characteristics of different air lines and the degree of current deviated air line when different air line flight missions are carried out, so that the unmanned aerial vehicle can realize airline operation better, the accuracy degree for the navigation control of the unmanned aerial vehicle is improved, and the flight speed v of the unmanned aerial vehicle can be dynamically adjusted by adjusting related parameters in the navigation tracking control; therefore, the problem that PID parameters cannot be adjusted dynamically in the prior art is overcome, and the flexibility for the navigation tracking control on the unmanned aerial vehicle is improved.

Description

A kind of nonlinear navigation of unmanned plane control method that tracks

Technical field

The present invention relates to unmanned aerial vehicle system (Unmanned Aerial Vehicle, be called for short UAV) nonlinear navigation of the independent navigation control method that tracks, belong to aircraft navigation control method field, be applicable to control method and the application technical research of unmanned aerial vehicle system independent navigation.

Background technology

Unmanned spacecraft is called for short " unmanned plane ", is the not manned aircraft that utilizes radio remote-controlled telemetry equipment and the presetting apparatus of providing for oneself to handle.The equipment such as navigation and flight control, presetting apparatus and power and power supply are installed on machine.Ground remote control telemetry station personnel are by equipment such as Data-Links, to its follow the tracks of, location, remote control, remote measurement and carry out real-time Data Transmission.Compare with manned aircraft, it has the advantages that to adapt to multiple flight environment of vehicle requirement, particularly can during the out of reach long boat of underwriter, fly or excessive risk flight, the line of flight and attitude control accuracy are high, can be widely used in airborne remote sensing, meteorological research, agriculture plant seeds by airplane and the prevention and control of plant diseases, pest control; In war, have more special advantage, can be widely used in aerial reconnaissance, supervision, communication, antisubmarine, electronic interferences and weapon strike etc.

The flight controller of unmanned plane mainly comprises navigation level and controlled stage, wherein the basic task of Navigation of Pilotless Aircraft level is the position of accurately determining in UAV system horizontal space, solve aircraft if predetermined air speed flight is how in the problem of predetermined altitude, and the target problem that flies to of how turning, by algorithm, provide the angle of pitch, throttle and the roll angle that aircraft needs, and can fly by prebriefed pattern, then give controlled stage and control and resolve.Linear Navigation Control algorithm for example, to be widely used in unmanned plane during flying device: adopt traditional PID control method, the navigation angle according to driftage apart from the distance of predetermined flight path route (geographic position of current unmanned plane with) calculating aircraft.In PID control method, if when current unmanned plane continues to depart from default airline operation, the proportional at navigation angle is identical with integration item symbol, and the computing that increases integration item, makes unmanned plane quickly to default airline operation; If current unmanned plane is during near default airline operation, the proportional at navigation angle is contrary with the symbol of integration item, hindered the computing of integration item, slow down or stop unmanned plane and continue to the direction flight near default course line, the phenomenon of having avoided integration overshoot or unmanned plane to vibrate on default course line.

But traditional pid algorithm still has following weak point: (1), because the error of calculation of driftage distance is larger in complicated planning flight path task, traditional pid algorithm navigation effect is poor, presses course line effect poor, and accuracy is not high.(2) because pid algorithm is linear, in the process that pid parameter is adjusted, will inevitably run into the contradiction between system stability and accuracy, get often the compromise of ratio, integration and differentiation three part control actions, be difficult to receive best effect.And pid parameter can not dynamically be adjusted, accuracy is not high.

Summary of the invention

For above shortcomings in prior art, the object of the present invention is to provide a kind of nonlinear navigation of the unmanned plane control method that tracks, in order to improve the accuracy that Navigation of Pilotless Aircraft is controlled, make unmanned plane can realize better pressure airline operation, solve in prior art the not high problem of unmanned plane course line control accuracy.

For solving the problems of the technologies described above, realize goal of the invention, the technical solution used in the present invention is as follows:

The nonlinear navigation of the unmanned plane control method that tracks, is characterized in that, locates geographic position and the heading that obtains in real time current unmanned plane by GPS, and carries out the navigation control that tracks with the Navigation Control cycle of fixing; When the Navigation Control cycle arrives, carry out the navigation step of controlling that tracks and comprise:

(1) judge current offline mode, if the offline mode that current flight pattern is spiral fashion, execution step 8; If the offline mode that current flight pattern is orthoscopic, execution step 2;

(2) determine target range L _aC, single flight distance L _{1_dist}with deviated route coefficient X; Described target range L _aCrefer to that the geographic position of current unmanned plane is to the distance between the geographic position of default air terminal; Described single flight distance L _{1_dist}refer to the distance that unmanned plane flies in the cycle a Navigation Control; Described deviated route coefficient X refers to target range L _aCprojection on default orthoscopic course line and target range L _aCbetween ratio;

Described single flight distance L _{1_dist}pressing following formula determines:

$L_{1\_\mathrm{dist}}=\frac{1}{\mathrm{\π}}*L_{1\_\mathrm{period}}*L_{1\_\mathrm{damping}}*v,$

Wherein, L _{1_period}represent a Navigation Control cycle, L _{1_damping}represent ratio of damping, v represents the speed of unmanned plane during flying;

Described deviated route coefficient X presses following formula and determines:

X=l/L _AC，

Wherein, l represents target range L _aCprojection on default orthoscopic course line, l presses following formula and determines:

represent that the geographic position of current unmanned plane is to the distance vector of default air terminal;

be used for representing default orthoscopic course line, specifically refer to that default course line starting point is to the distance vector of default air terminal; represent that default course line starting point is to the distance between default air terminal;

(3) judgement single flight distance L _{1_dist}whether be less than target range L _aC, if so, execution step 4; Otherwise execution step 6;

(4) judge whether deviated route coefficient X is greater than 0.7071, if so, execution step 5, otherwise, execution step 6;

(5) determine orthoscopic flight angle η _s, and perform step 7; Described orthoscopic flight angle η _srefer to from the heading of current unmanned plane to distance vector

angle between the direction of place;

Orthoscopic flight angle η _spressing following formula determines:

${\mathrm{\η}}_s={\mathrm{\α}}_0\arctan \left(\frac{v_{x1}}{v_{l1}}\right),$

Wherein, α ₀value is 1 or-1; When from the heading of current unmanned plane to distance vector

the direction at place is clockwise direction, α ₀be 1; When from the heading of current unmanned plane to distance vector

the direction at place is counterclockwise, α ₀for-1; v _l1represent that speed v is at distance vector

minute vectorial mould in the direction of place, v _x1represent that speed v is at distance vector and distance vector

institute planar with distance vector minute vectorial mould in the vertical direction of direction; v _l1and v _x1determine according to the following formula respectively:

$v_{x1}=|\stackrel{\→}{v}\×\frac{\stackrel{\→}{\mathrm{CA}}}{\left|\stackrel{\→}{\mathrm{CA}}\right|}|,V_{l1}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{CA}}}{|\stackrel{\→}{\mathrm{CA}|}}|;$

(6) by following formula, determine orthoscopic flight angle η _s; And perform step 7;

η _s=α ₁*η ₁+α ₂*η ₂，

Wherein

d _srepresent orthoscopic driftage distance, described orthoscopic driftage is apart from d _srefer to that the geographic position of current unmanned plane is to default orthoscopic course line

between distance,

α ₁value is 1 or-1; When from distance vector place direction is to default orthoscopic course line

the direction at place is clockwise direction, α ₁be 1; When from distance vector

place direction is to default orthoscopic course line

the direction at place is counterclockwise, α ₁for-1;

α ₂value is 1 or-1; When the heading from current unmanned plane is to default orthoscopic course line

the direction at place is clockwise direction, α ₂be 1; When the heading from current unmanned plane is to default orthoscopic course line

the direction at place is counterclockwise, α ₂for-1; v _l2represent that speed v is at distance vector

minute vectorial mould in the direction of place, v _x2represent that speed v is at distance vector

and distance vector institute planar with distance vector

minute vectorial mould in the vertical direction of direction; v _x2and v _l2determine according to the following formula respectively:

$v_{x2}=|\stackrel{\→}{v}\×\frac{\stackrel{\→}{\mathrm{BA}}}{\left|\stackrel{\→}{\mathrm{BA}}\right|}|,V_{l2}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{BA}}}{|\stackrel{\→}{\mathrm{BA}|}}|;$

(7) by following formula, determine the transverse acceleration a of unmanned plane _cmds, and perform step 12;

a _cmds=K _LS*v ²/L _{1_dist}*sinη _s；

K wherein _lSrepresent orthoscopic flight coefficient, K _lS=4.0*L _{1_damping}* L _{1_damping};

(8) judge the distance of spiraling

whether be less than default turn circle radius R, described in the distance of spiraling

refer to that the geographic position of current unmanned plane is to the distance between default course line orbit path; If so, execution step 9, otherwise, execution step 10;

(9) determine spiral fashion flight angle η _rcentripetal acceleration a with unmanned plane _cmdr, execution step 11; Described spiral fashion flight angle η _rrefer to from the heading of current unmanned plane to distance vector

angle between the direction of place; Described distance vector

refer to that the geographic position of current unmanned plane is to the distance vector of default course line orbit path;

Described spiral fashion flight angle η _rpressing following formula determines:

${\mathrm{\η}}_r={\mathrm{\α}}_3*\arctan \left(\frac{v_{x3}}{v_{l3}}\right);$

Wherein, α ₃value is 1 or-1; When from the heading of current unmanned plane to distance vector the direction at place is clockwise direction, α ₃be 1; When from the heading of current unmanned plane to distance vector

the direction at place is counterclockwise, α ₃for-1;

V _x3and v _l3pressing respectively following formula determines:

$v_{x3}=|\frac{\stackrel{\→}{\mathrm{CD}}}{\left|\stackrel{\→}{\mathrm{CD}}\right|}\×\stackrel{\→}{v}|,V_{l3}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{CD}}}{|\stackrel{\→}{\mathrm{CD}|}}|,$

V _l3represent that speed v is at distance vector

speed component in the direction of place, v _x3represent speed v with distance vector

speed component in the direction of place in vertical direction;

The centripetal acceleration a of described unmanned plane _cmdrby following formula, be defined as:

a _cmdr=K _LR*v ²/L _{1_dist}*sinη _r；

Wherein

l _{1_period}represent a Navigation Control cycle, L _{1_damping}represent ratio of damping, K _lRthe rotating flight coefficient of indicating panel, K _lS=4.0*L _{1_damping}* L _{1_damping};

(10) by following formula, determine the centripetal acceleration a of unmanned plane _cmdr, execution step 11;

$a_{\mathrm{cmdr}}={\stackrel{\‾}{d}}_r*k_d+d_r*k_p+v_{x3}^2/(d_r+R);$

Wherein, k _drotating flight the first coefficient of indicating panel, k _d=2*L _{1_damping}* L _{1_damping}; k _protating flight the second coefficient of indicating panel, k _p=(2 π/L _{1_period}) ²; d _rthe rotating driftage distance of indicating panel, described spiral fashion driftage is apart from d _rrefer to that the geographic position of current unmanned plane is to the bee-line between default spiral fashion course line,

the rotating driftage of indicating panel is apart from d _rrate of change,

pressing following formula determines:

${\stackrel{\‾}{d}}_r=\frac{|d_{r\_\mathrm{last}}-d_{r\_\mathrm{curr}}|}{L_{1\_\mathrm{period}}};$

Wherein, d _{r_last}represent the spiral fashion driftage distance that when this previous Navigation Control cycle arrives, execution navigation tracks and controls; d _{r_curr}represent current spiral fashion driftage distance;

(11) by the centripetal acceleration a of unmanned plane _cmdrbring the navigation angle θ that following formula determines that this time navigation tracks and controls into; Execution step 13;

θ=arctan(a _cmdr/g)；

G is wherein the value of acceleration of gravity;

(12) by the transverse acceleration a of unmanned plane _cmdsbring the navigation angle θ that following formula determines that this time navigation tracks and controls into; Execution step 13;

θ=arctan(a _cmds/g)；

G is wherein the value of acceleration of gravity;

(13) according to navigation angle θ, control the heading of unmanned plane.

As optimization, described Navigation Control period L _{1_period}span be 10～50ms; Described ratio of damping L _{1_damping}span be 0.6～1.

As optimization, when described Navigation Control period L _{1_period}every change 1ms, described ratio of damping L _{1_damping}value correspondingly change 0.05.

Than prior art, tool of the present invention has the following advantages:

1, the nonlinear navigation of unmanned plane of the present invention tracks in control method, adopt the fixing Navigation Control cycle, according to four of unmanned plane kinds of different state of flights and depart from situation, draw navigation angle, thereby according to navigation angle, unmanned plane is carried out to the nonlinear navigation control that tracks.Utilize this nonlinear navigation control method that tracks, guaranteed when carrying out different airline operation tasks, unmanned plane can be according to the feature in different course lines and current off-line degree, obtain suitable navigation angle, make unmanned plane can realize better pressure airline operation, improved the accuracy that Navigation of Pilotless Aircraft is controlled.

2, the nonlinear navigation of unmanned plane of the present invention tracks in control method, unmanned plane is adjusted navigation angle in real time according to the transverse acceleration under different flying speeds or centripetal acceleration, when unmanned plane is closer apart from course line, only produce smaller navigation angle, when unmanned plane is distant apart from course line, produce larger navigation angle, thereby improve the accuracy that navigation angle is controlled, make unmanned plane can realize better pressure airline operation.

3, the nonlinear navigation of unmanned plane of the present invention tracks in control method, parameter L _{1_damping}and L _{1_period}can dynamically adjust according to the flying speed v of unmanned plane, overcome the problem that in prior art, pid parameter can not dynamically be adjusted, improve the dirigibility that unmanned plane is navigated and tracks and control.

Accompanying drawing explanation

Fig. 1 is the nonlinear navigation of the unmanned plane of the present invention a kind of situation that offline mode is orthoscopic in control method that tracks.

Fig. 2 is the nonlinear navigation of the unmanned plane of the present invention another kind of situation that offline mode is orthoscopic in control method that tracks.

Fig. 3 is the nonlinear navigation of the unmanned plane of the present invention a kind of situation that offline mode is spiral fashion in control method that tracks.

Fig. 4 is the nonlinear navigation of the unmanned plane of the present invention another kind of situation that offline mode is spiral fashion in control method that tracks.

Embodiment

The nonlinear navigation of the unmanned plane of the present invention control method that tracks, by obtaining geographic position and the heading of current unmanned plane, in conjunction with default course line, adopt nonlinear navigation to track and control transverse acceleration or the centripetal acceleration of calculating in real time unmanned plane, and obtain the navigation angle of unmanned plane, guarantee that unmanned plane, when the airline operation task of carry out setting, can realize pressure airline operation better.Solve the traditional pid algorithm of available technology adopting and navigate while tracking control, pid parameter is got the compromise of ratio, integration and differentiation three part control actions often, makes the navigation angle calculating be linear change.Therefore in complicated airline operation task, corresponding adjustment can not be made according to concrete airline operation task in the navigation angle that traditional pid algorithm is tried to achieve, and accuracy is not high.

The nonlinear navigation of unmanned plane of the present invention tracks in control method, tracks and controls in the process at differentiate boat angle carrying out navigation, mainly according to four kinds of different state of flight differentiates boat angles.The first, the offline mode that unmanned plane is orthoscopic at offline mode, and unmanned plane is current near default airline operation.The second, the offline mode that unmanned plane is orthoscopic at offline mode, but unmanned plane is current away from default airline operation.The third, the offline mode that unmanned plane is spiral fashion at offline mode, and unmanned plane flies in turn circle radius.The 4th kind, the offline mode that unmanned plane is spiral fashion at offline mode, and unmanned plane flies outward at turn circle radius.The present invention divides the course line of unmanned plane during flying for four kinds of different course lines, and according to different course lines and depart from situation, draws navigation angle.The present invention, by current any situation that belongs to above-mentioned four kinds of state of flights of monitoring unmanned plane, tries to achieve navigation angle thereby set up corresponding model.Although make in complicated airline operation task, unmanned plane also can make unmanned plane press better airline operation according to the calculating at state of flight adjustment navigation angle concrete in this aerial mission, improves the accuracy that Navigation of Pilotless Aircraft is controlled.

Below in conjunction with drawings and Examples, technical scheme of the present invention is further illustrated.

The nonlinear navigation of the unmanned plane control method that tracks, by GPS(Global Positioning System, GPS) location obtains geographic position and the heading of current unmanned plane in real time, and the Navigation Control cycle to fix, and carries out as follows the control that tracks of navigating:

(1) judge current offline mode, if the offline mode that current flight pattern is spiral fashion, execution step 8; If the offline mode that current flight pattern is orthoscopic, execution step 2.

(2) determine target range L _aC, single flight distance L _{1_dist}with deviated route coefficient X; Described target range L _aCrefer to that the geographic position of current unmanned plane is to the distance between the geographic position of default air terminal; Described single flight distance L _{1_dist}refer to the distance that unmanned plane flies in the cycle a Navigation Control; Described deviated route coefficient X refers to target range L _aCprojection on default orthoscopic course line and target range L _aCbetween ratio.

Described single flight distance L _{1_dist}pressing following formula determines:

$L_{1\_\mathrm{dist}}=\frac{1}{\mathrm{\π}}*L_{1\_\mathrm{period}}*L_{1\_\mathrm{damping}}*v,$

Wherein, L _{1_period}represent a Navigation Control cycle, L _{1_damping}represent ratio of damping, v represents the speed of unmanned plane during flying;

Described deviated route coefficient X presses following formula and determines:

X=l/L _AC，

Wherein, l represents target range L _aCprojection on default orthoscopic course line, l presses following formula and determines:

represent that the geographic position of current unmanned plane is to the distance vector of default air terminal;

be used for representing default orthoscopic course line, specifically refer to that default course line starting point is to the distance vector of default air terminal;

represent that default course line starting point is to the distance between default air terminal.

Suppose that the geographic position of locating the current unmanned plane getting by GPS represents with C point, C point coordinate is expressed as (c1, c2); Under the prerequisite of the offline mode that is orthoscopic in unmanned plane current flight pattern, starting point and the terminal in orthoscopic course line is set; Course line starting point represents with B point, and the coordinates table that B is ordered is shown (b1, b2); Air terminal represents with A point, and the coordinates table that A is ordered is shown (a1, a2); Described target range L _aCrefer to that the geographic position of current unmanned plane is to the distance between the geographic position of the air terminal of establishing, L _aCcomputing formula be

suppose that unmanned plane is to fly at a constant speed, single flight distance L _{1_dist}for constant, according to default Navigation Control period L _{1_period}, ratio of damping L _{1_damping}determine with the speed of unmanned plane.The mathematics implication that deviated route coefficient X represents is unmanned plane current flight direction and distance vector

the cosine value of angle, when calculating deviated route coefficient X,

coordinate be ((a1-c1), (a2-c2)), coordinate be ((a1-b1), (a2-b2)), wherein

$\left|\stackrel{\→}{\mathrm{BA}}\right|=\sqrt{{(a1-b1)}^2+{(a2-b2)}^2}.$

(3) judgement single flight distance L _{1_dist}whether be less than target range L _aC, if so, execution step 4; Otherwise execution step 6.

(4) judge whether deviated route coefficient X is greater than 0.7071, if so, execution step 5, otherwise, execution step 6.

Step 3 and step 4 be used for judging unmanned plane current be whether near default airline operation.If single flight distance L _{1_dist}be less than target range L _aC, illustrate that just unmanned plane can reach home in the next Navigation Control cycle, if deviated route coefficient X is greater than at 0.7071 o'clock, unmanned plane current flight direction and distance vector are described

angle be less than 45 °, when single flight distance L _{1_dist}be less than target range L _aCand unmanned plane current flight direction and distance vector

angle while being less than 45 °, think that current unmanned plane is near default airline operation, its flight situation as shown in Figure 1, is now asked orthoscopic flight angle η according to the method in step 5 _s.Otherwise think that current unmanned plane is away from default airline operation, its flight situation as shown in Figure 2, is now asked orthoscopic flight angle η according to the method in step 6 _s.

(5) determine orthoscopic flight angle η _s, and perform step 7; Described orthoscopic flight angle η _srefer to from the heading of current unmanned plane to distance vector angle between the direction of place.

Orthoscopic flight angle η _spressing following formula determines:

${\mathrm{\η}}_s={\mathrm{\α}}_0\arctan \left(\frac{v_{x1}}{v_{l1}}\right),$

Wherein, α ₀value is 1 or-1; When from the heading of current unmanned plane to distance vector

the direction at place is clockwise direction, α ₀be 1; When from the heading of current unmanned plane to distance vector

the direction at place is counterclockwise, α ₀for-1; v _l1represent that speed v is at distance vector

minute vectorial mould in the direction of place, v _x1represent that speed v is at distance vector

and distance vector

institute planar with distance vector

minute vectorial mould in the vertical direction of direction; v _l1and v _x1determine according to the following formula respectively:

$v_{x1}=|\stackrel{\→}{v}\×\frac{\stackrel{\→}{\mathrm{CA}}}{\left|\stackrel{\→}{\mathrm{CA}}\right|}|,V_{l1}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{CA}}}{|\stackrel{\→}{\mathrm{CA}|}}|.$

Step 5 is under orthoscopic offline mode, and current unmanned plane, when near default airline operation, is asked orthoscopic flight angle η _ssituation.Because unmanned plane is closer apart from course line, so unmanned plane only needs a smaller navigation angle to adjust heading.Step 5 is decomposed into v by speed v _l1and v _x1during two components, the mathematical model of foundation is: take the geographic position of current unmanned plane is true origin, with distance vector

the direction at place is X-axis, with distance vector and distance vector

institute planar with distance vector

the vertical direction of direction is Y-axis, and speed v is decomposed into the component v in X-axis _l1with the component v in Y-axis _x1.Draw thus from the heading of current unmanned plane to distance vector

angle between the direction of place ${\mathrm{\η}}_s=\arctan \left(\frac{v_{x1}}{v_{l1}}\right).$

(6) by following formula, determine orthoscopic flight angle η _s; And perform step 7.

η _s=α ₁*η ₁+α ₂*η ₂，

Wherein

d _srepresent orthoscopic driftage distance, described orthoscopic driftage is apart from d _srefer to that the geographic position of current unmanned plane is to default orthoscopic course line

between distance, α ₁value is 1 or-1; When from distance vector

place direction is to default orthoscopic course line

the direction at place is clockwise direction, α ₁be 1; When from distance vector

place direction is to default orthoscopic course line

the direction at place is counterclockwise, α ₁for-1;

α ₂value is 1 or-1; When the heading from current unmanned plane is to default orthoscopic course line

the direction at place is clockwise direction, α ₂be 1; When the heading from current unmanned plane is to default orthoscopic course line

the direction at place is counterclockwise, α ₂for-1; v _l2represent that speed v is at distance vector

minute vectorial mould in the direction of place, v _x2represent that speed v is at distance vector

and distance vector

institute planar with distance vector

minute vectorial mould in the vertical direction of direction; v _x2and v _l2determine according to the following formula respectively:

$v_{x2}=|\stackrel{\→}{v}\×\frac{\stackrel{\→}{\mathrm{BA}}}{\left|\stackrel{\→}{\mathrm{BA}}\right|}|,V_{l2}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{BA}}}{|\stackrel{\→}{\mathrm{BA}|}}|.$

Step 6 is under orthoscopic offline mode, and current unmanned plane, when away from default airline operation, is asked orthoscopic flight angle η _ssituation.Because unmanned plane is distant apart from course line, i.e. driftage is apart from larger, and now unmanned plane needs a large yaw angle to make unmanned plane better press course line.Therefore step 6 is by the orthoscopic angle η that flies _sbe divided into η ₁and η ₂calculate, the mathematical model of its foundation is: orthoscopic flight angle η _sby angle separated time, be divided into η ₁and η ₂, wherein angle separated time and distance vector parallel, can find out η ₁by orthoscopic, gone off course apart from d _s, single flight distance L _{1_dist}with distance to

in the triangle forming, by inverse trigonometric function, can be obtained

η ₂in the process of calculating, the mathematical model of foundation is: take the geographic position of current unmanned plane is true origin, with distance vector

the direction at place is X-axis, with distance vector

and distance vector

institute planar with distance vector

the vertical direction of direction is Y-axis, and speed v is decomposed into the component v in X-axis _l2with the component v in Y-axis _x2.Draw thus

(7) by following formula, determine the transverse acceleration a of unmanned plane _cmds, and perform step 12.

a _cmds=K _LS*v ²/L _{1_dist}*sinη _s；

K wherein _lSrepresent orthoscopic flight coefficient, K _lS=4.0*L _{1_damping}* L _{1_damping}.

(8) judge the distance of spiraling whether be less than default turn circle radius R, described in the distance of spiraling refer to that the geographic position of current unmanned plane is to the distance between default course line orbit path; If so, execution step 9, otherwise, execution step 10.

Step 8 is used for judging that unmanned plane is current whether within the radius in the default course line of spiraling, flies.If the distance of spiraling

whether be less than default turn circle radius R, illustrate within the current radius presetting the course line of spiraling of unmanned plane and fly, its flight situation as shown in Figure 3, is now asked spiral fashion flight angle η according to the method in step 9 _r.Otherwise, illustrating outside the current radius presetting the course line of spiraling of unmanned plane and fly, its flight situation as shown in Figure 4, is now asked spiral fashion flight angle η according to the method in step 10 _r.

(9) determine spiral fashion flight angle η _rcentripetal acceleration a with unmanned plane _cmdr, execution step 11; Described spiral fashion flight angle η _rrefer to from the heading of current unmanned plane to distance vector

angle between the direction of place; Described distance vector refer to that the geographic position of current unmanned plane is to the distance vector of default course line orbit path.

Described spiral fashion flight angle η _rpressing following formula determines:

${\mathrm{\η}}_r={\mathrm{\α}}_3*\arctan \left(\frac{v_{x3}}{v_{l3}}\right);$

Wherein, α ₃value is 1 or-1; When from the heading of current unmanned plane to distance vector

the direction at place is clockwise direction, α ₃be 1; When from the heading of current unmanned plane to distance vector

the direction at place is counterclockwise, α ₃for-1.

V _x3and v _l3pressing respectively following formula determines:

$v_{x3}=|\frac{\stackrel{\→}{\mathrm{CD}}}{\left|\stackrel{\→}{\mathrm{CD}}\right|}\×\stackrel{\→}{v}|,V_{l3}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{CD}}}{|\stackrel{\→}{\mathrm{CD}|}}|,$

V _l3represent that speed v is at distance vector

speed component in the direction of place, v _x3represent speed v with distance vector

speed component in the direction of place in vertical direction.

The centripetal acceleration a of described unmanned plane _cmdrby following formula, be defined as:

a _cmdr=K _LR*v ²/L _{1_dist}*sinη _r；

Wherein

l _{1_period}represent a Navigation Control cycle, L _{1_damping}represent ratio of damping, K _lRthe rotating flight coefficient of indicating panel, K _lS=4.0*L _{1_damping}* L _{1_damping}.

Step 9 is under spiral fashion offline mode, when current unmanned plane flies in turn circle radius, and the rotating flight angle of calculating dial η _rsituation.At the rotating flight angle of calculating dial η _rtime, the mathematical model of foundation is: take the geographic position of current unmanned plane is true origin, with distance vector

the direction at place is X-axis, with spiral course line institute planar with distance vector the vertical direction of direction is Y-axis, and speed v is decomposed into the component v in X-axis _l3with the component v in Y-axis _x3.Can draw thus suppose that the geographic position of locating the current unmanned plane getting by GPS represents with C point, C point coordinate is expressed as (c1, c2); Under the prerequisite of the offline mode that is spiral fashion in unmanned plane current flight pattern, orbit path and the turn circle radius R in spiral fashion course line is set; Course line central point represents with D point, and the coordinates table that D is ordered is shown (d1, d2); ?

coordinate be ((d1-c1), (d2-c2)),

the centripetal acceleration a of unmanned plane _cmdrthe direction of direction sensing course line, the geographic position central point that is current unmanned plane.

(10) by following formula, determine the centripetal acceleration a of unmanned plane _cmdr, execution step 11.

$a_{\mathrm{cmdr}}={\stackrel{\‾}{d}}_r*k_d+d_r*k_p+v_{x3}^2/(d_r+R);$

Wherein, k _drotating flight the first coefficient of indicating panel, k _d=2*L _{1_damping}* L _{1_damping}; k _protating flight the second coefficient of indicating panel, k _p=(2 π/L _{1_period}) ²; d _rthe rotating driftage distance of indicating panel, described spiral fashion driftage is apart from d _rrefer to that the geographic position of current unmanned plane is to the bee-line between default spiral fashion course line,

the rotating driftage of indicating panel is apart from d _rrate of change,

pressing following formula determines:

${\stackrel{\‾}{d}}_r=\frac{|d_{r\_\mathrm{last}}-d_{r\_\mathrm{curr}}|}{L_{1\_\mathrm{period}}};$

Wherein, d _{r_last}represent the spiral fashion driftage distance that when this previous Navigation Control cycle arrives, execution navigation tracks and controls; d _{r_curr}represent current spiral fashion driftage distance.

Step 10 is under spiral fashion offline mode, current unmanned plane when turn circle radius flies outward, the rotating flight angle of calculating dial η _rsituation.Wherein current spiral fashion driftage is apart from d _{r_curr}equal current d _rvalue.Suppose that the geographic position of locating the current unmanned plane getting by GPS represents with C point, C point coordinate is expressed as (c1, c2); Under the prerequisite of the offline mode that is spiral fashion in unmanned plane current flight pattern, orbit path and the turn circle radius R in spiral fashion course line is set; Course line central point represents with D point, and the coordinates table that D is ordered is shown (d1, d2); ?

(11) by the centripetal acceleration a of unmanned plane _cmdrbring the navigation angle θ that following formula determines that this time navigation tracks and controls into; Execution step 13.

θ=arctan(a _cmdr/g)；

G is wherein the value of acceleration of gravity.

(12) by the transverse acceleration a of unmanned plane _cmdsbring the navigation angle θ that following formula determines that this time navigation tracks and controls into; Execution step 13.

θ=arctan(a _cmds/g)；

G is wherein the value of acceleration of gravity.

(13) according to navigation angle θ, control the heading of unmanned plane.

Can find out that the nonlinear navigation of the unmanned plane of the present embodiment control method that tracks divides the calculating of navigation angle θ for four kinds of situations.For different situations, by the course line set and the geographic position of current unmanned plane, calculate navigation angle θ, when unmanned plane away from set course line time, spiral fashion flight angle η _ror orthoscopic flight angle η _sangle is larger, causes corresponding centripetal acceleration a _cmdror transverse acceleration a _cmdsvalue larger, so will produce a larger navigation angle, make unmanned plane approach fast course line.Otherwise unmanned plane goes to the navigation angle with less to approach course line.Utilize this nonlinear navigation control method that tracks, even if make unmanned plane when carrying out complicated aerial mission, also can according to concrete state of flight, change the computing method of navigation angle θ, make the navigation angle θ obtaining more accurate.Meanwhile, the present embodiment is calculated suitable navigation angle θ by setting course line and the current geographic position of unmanned plane, makes unmanned function complete better the aerial mission of pressing course line.

During concrete application, parameter L _{1_damping}and L _{1_period}can dynamically adjust according to the flying speed v of unmanned plane, overcome the problem that pid parameter can not dynamically be adjusted.Navigation Control period L _{1_period}span be 10～50ms, ratio of damping L _{1_damping}span be 0.6～1.In addition, in order to make unmanned plane reach best performance, L interval time that tracks and control when described execution navigation _{1_period}every change 1ms, described ratio of damping L _{1_damping}value correspondingly change 0.05.When unmanned plane is when turning, if it is too slow to turn, can be by L _{1_period}value reduce 5ms; If unmanned plane vibrates after turning on the line of flight, so by L _{1_period}increase 1ms or 2ms.

In sum, the nonlinear navigation of the unmanned plane of the present invention control method that tracks, by obtaining geographic position and the heading of current unmanned plane, in conjunction with default course line, adopt nonlinear navigation to track and control transverse acceleration or the centripetal acceleration of calculating in real time unmanned plane, and obtain the navigation angle of unmanned plane, guarantee that unmanned plane, when carrying out the airline operation task of setting, can realize pressure airline operation better.The calculating of navigation angle θ is divided for four kinds of situations simultaneously, for different situations, by different computing method, draw navigation angle θ.Although make in complicated airline operation task, unmanned plane also can, according to the calculating at state of flight adjustment navigation angle concrete in this aerial mission, guarantee the accuracy of navigation angle θ.Meanwhile, parameter L _{1_damping}and L _{1_period}can dynamically adjust according to the flying speed v of unmanned plane, overcome the problem that pid parameter can not dynamically be adjusted, improve the dirigibility that unmanned plane is navigated and tracks and control.

Finally explanation is, above embodiment is only unrestricted in order to technical scheme of the present invention to be described, although the present invention is had been described in detail with reference to preferred embodiment, those of ordinary skill in the art is to be understood that, can modify or be equal to replacement technical scheme of the present invention, and not departing from aim and the scope of technical solution of the present invention, it all should be encompassed in the middle of claim scope of the present invention.

Claims (3)

1. the nonlinear navigation of the unmanned plane control method that tracks, is characterized in that, locates geographic position and the heading that obtains in real time current unmanned plane by GPS, and carries out the navigation control that tracks with the Navigation Control cycle of fixing; When the Navigation Control cycle arrives, carry out the navigation step of controlling that tracks and comprise:

(1) judge current offline mode, if the offline mode that current flight pattern is spiral fashion, execution step 8; If the offline mode that current flight pattern is orthoscopic, execution step 2;

(2) determine target range L _aC, single flight distance L _{1_dist}with deviated route coefficient X; Described target range L _aCrefer to that the geographic position of current unmanned plane is to the distance between the geographic position of default air terminal; Described single flight distance L _{1_dist}refer to the distance that unmanned plane flies in the cycle a Navigation Control; Described deviated route coefficient X refers to target range L _aCprojection on default orthoscopic course line and target range L _aCbetween ratio;

Described single flight distance L _{1_dist}pressing following formula determines:

$L_{1\_\mathrm{dist}}=\frac{1}{\mathrm{\π}}*L_{1\_\mathrm{period}}*L_{1\_\mathrm{damping}}*v,$

Wherein, L _{1_period}represent a Navigation Control cycle, L _{1_damping}represent ratio of damping, v represents the speed of unmanned plane during flying;

Described deviated route coefficient X presses following formula and determines:

X=l/L _AC，

Wherein, l represents target range L _aCprojection on default orthoscopic course line, l presses following formula and determines:

represent that the geographic position of current unmanned plane is to the distance vector of default air terminal;

be used for representing default orthoscopic course line, specifically refer to that default course line starting point is to the distance vector of default air terminal; represent that default course line starting point is to the distance between default air terminal;

(3) judgement single flight distance L _{1_dist}whether be less than target range L _aC, if so, execution step 4; Otherwise execution step 6;

(4) judge whether deviated route coefficient X is greater than 0.7071, if so, execution step 5, otherwise, execution step 6;

(5) determine orthoscopic flight angle η _s, and perform step 7; Described orthoscopic flight angle η _srefer to from the heading of current unmanned plane to distance vector

angle between the direction of place;

Orthoscopic flight angle η _spressing following formula determines:

${\mathrm{\η}}_s={\mathrm{\α}}_0\arctan \left(\frac{v_{x1}}{v_{l1}}\right),$

Wherein, α ₀value is 1 or-1; When from the heading of current unmanned plane to distance vector the direction at place is clockwise direction, α ₀be 1; When from the heading of current unmanned plane to distance vector

the direction at place is counterclockwise, α ₀for-1; v _l1represent that speed v is at distance vector

minute vectorial mould in the direction of place, v _x1represent that speed v is at distance vector

and distance vector

institute planar with distance vector

minute vectorial mould in the vertical direction of direction; v _l1and v _x1determine according to the following formula respectively:

$v_{x1}=|\stackrel{\→}{v}\×\frac{\stackrel{\→}{\mathrm{CA}}}{\left|\stackrel{\→}{\mathrm{CA}}\right|}|,V_{l1}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{CA}}}{|\stackrel{\→}{\mathrm{CA}|}}|;$

(6) by following formula, determine orthoscopic flight angle η _s; And perform step 7;

η _s=α ₁*η ₁+α ₂*η ₂，

Wherein

d _srepresent orthoscopic driftage distance, described orthoscopic driftage is apart from d _srefer to that the geographic position of current unmanned plane is to default orthoscopic course line

between distance,

α ₁value is 1 or-1; When from distance vector

place direction is to default orthoscopic course line the direction at place is clockwise direction, α ₁be 1; When from distance vector

place direction is to default orthoscopic course line

the direction at place is counterclockwise, α ₁for-1;

α ₂value is 1 or-1; When the heading from current unmanned plane is to default orthoscopic course line

the direction at place is clockwise direction, α ₂be 1; When the heading from current unmanned plane is to default orthoscopic course line

the direction at place is counterclockwise, α ₂for-1; v _l2represent that speed v is at distance vector

minute vectorial mould in the direction of place, v _x2represent that speed v is at distance vector

and distance vector

institute planar with distance vector

minute vectorial mould in the vertical direction of direction; v _x2and v _l2determine according to the following formula respectively:

$v_{x2}=|\stackrel{\→}{v}\×\frac{\stackrel{\→}{\mathrm{BA}}}{\left|\stackrel{\→}{\mathrm{BA}}\right|}|,V_{l2}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{BA}}}{|\stackrel{\→}{\mathrm{BA}|}}|;$

(7) by following formula, determine the transverse acceleration a of unmanned plane _cmds, and perform step 12;

a _cmds=K _LS*v ²/L _{1_dist}*sinη _s；

K wherein _lSrepresent orthoscopic flight coefficient, K _lS=4.0*L _{1_damping}* L _{1_damping};

(8) judge the distance of spiraling

whether be less than default turn circle radius R, described in the distance of spiraling

refer to that the geographic position of current unmanned plane is to the distance between default course line orbit path; If so, execution step 9, otherwise, execution step 10;

(9) determine spiral fashion flight angle η _rcentripetal acceleration a with unmanned plane _cmdr, execution step 11; Described spiral fashion flight angle η _rrefer to from the heading of current unmanned plane to distance vector

angle between the direction of place; Described distance vector refer to that the geographic position of current unmanned plane is to the distance vector of default course line orbit path;

Described spiral fashion flight angle η _rpressing following formula determines:

${\mathrm{\η}}_r={\mathrm{\α}}_3*\arctan \left(\frac{v_{x3}}{v_{l3}}\right);$

Wherein, α ₃value is 1 or-1; When from the heading of current unmanned plane to distance vector the direction at place is clockwise direction, α ₃be 1; When from the heading of current unmanned plane to distance vector

the direction at place is counterclockwise, α ₃for-1;

V _x3and v _l3pressing respectively following formula determines:

$v_{x3}=|\frac{\stackrel{\→}{\mathrm{CD}}}{\left|\stackrel{\→}{\mathrm{CD}}\right|}\×\stackrel{\→}{v}|,V_{l3}=|\stackrel{\→}{v}\·\frac{\stackrel{\→}{\mathrm{CD}}}{|\stackrel{\→}{\mathrm{CD}|}}|,$

V _l3represent that speed v is at distance vector speed component in the direction of place, v _x3represent speed v with distance vector

speed component in the direction of place in vertical direction;

The centripetal acceleration a of described unmanned plane _cmdrby following formula, be defined as:

a _cmdr=K _LR*v ²/L _{1_dist}*sinη _r；

Wherein

l _{1_period}represent a Navigation Control cycle, L _{1_damping}represent ratio of damping, K _lRthe rotating flight coefficient of indicating panel, K _lS=4.0*L _{1_damping}* L _{1_damping};

(10) by following formula, determine the centripetal acceleration a of unmanned plane _cmdr, execution step 11;

$a_{\mathrm{cmdr}}={\stackrel{\‾}{d}}_r*k_d+d_r*k_p+v_{x3}^2/(d_r+R);$

Wherein, k _drotating flight the first coefficient of indicating panel, k _d=2*L _{1_damping}* L _{1_damping}; k _protating flight the second coefficient of indicating panel, k _p=(2 π/L _{1_period}) ²; d _rthe rotating driftage distance of indicating panel, described spiral fashion driftage is apart from d _rrefer to that the geographic position of current unmanned plane is to the bee-line between default spiral fashion course line,

the rotating driftage of indicating panel is apart from d _rrate of change,

pressing following formula determines:

${\stackrel{\‾}{d}}_r=\frac{|d_{r\_\mathrm{last}}-d_{r\_\mathrm{curr}}|}{L_{1\_\mathrm{period}}};$

Wherein, d _{r_last}represent the spiral fashion driftage distance that when this previous Navigation Control cycle arrives, execution navigation tracks and controls; d _{r_curr}represent current spiral fashion driftage distance;

(11) by the centripetal acceleration a of unmanned plane _cmdrbring the navigation angle θ that following formula determines that this time navigation tracks and controls into; Execution step 13;

θ=arctan(a _cmdr/g)；

G is wherein the value of acceleration of gravity;

(12) by the transverse acceleration a of unmanned plane _cmdsbring the navigation angle θ that following formula determines that this time navigation tracks and controls into; Execution step 13;

θ=arctan(a _cmds/g)；

G is wherein the value of acceleration of gravity;

(13) according to navigation angle θ, control the heading of unmanned plane.

2. the nonlinear navigation of the unmanned plane as claimed in claim 1 control method that tracks, is characterized in that described Navigation Control period L _{1_period}span be 10～50ms; Described ratio of damping L _{1_damping}span be 0.6～1.

3. the nonlinear navigation of the unmanned plane as claimed in claim 2 control method that tracks, is characterized in that, when described Navigation Control period L _{1_period}every change 1ms, described ratio of damping L _{1_damping}value correspondingly change 0.05.

Images