CN103728981B - A kind of nonlinear navigation trace follow control method of unmanned plane - Google Patents

Abstract

The nonlinear navigation trace follow control method of unmanned plane of the present invention, it adopts the fixing Navigation Control cycle, according to four kinds of unmanned plane different state of flights and depart from situation, draw navigation angle, thus according to navigation angle, nonlinear navigation trace follow control is performed to unmanned plane, guarantee when performing different airline operation tasks, unmanned plane can according to the feature in different course line and current off-line degree, obtain suitable navigation angle, make unmanned plane can realize better pressing airline operation, improve the accuracy that Navigation of Pilotless Aircraft is controlled, and can by the correlation parameter in adjustment navigation trace follow control, dynamic conditioning is carried out at any time according to the flying speed v of unmanned plane, overcome the problem that pid parameter in prior art can not dynamically carry out adjusting, improve dirigibility unmanned plane being carried out to navigation trace follow control.

Description

A kind of nonlinear navigation trace follow control method of unmanned plane

Technical field

The present invention relates to unmanned aerial vehicle system (UnmannedAerialVehicle, be called for short UAV) the nonlinear navigation trace follow control method of independent navigation, 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 utilizing radio remote-controlled telemetry equipment to handle with the presetting apparatus provided for oneself.Machine is provided with navigation and flight control, presetting apparatus and the equipment such as power and power supply.Ground remote control telemetry station personnel, by equipment such as Data-Links, follow the tracks of it, locate, remote control, remote measurement and carry out real-time Data Transmission.Compared with manned aircraft, it has the advantages that to adapt to multiple flight environment of vehicle requirement, particularly can underwriter's out of reach long boat time flight or excessive risk flight, the line of flight and attitude control accuracy 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, aerial reconnaissance, supervision, communication, antisubmarine, electronic interferences and weapon strike etc. can be widely used in.

The flight controller of unmanned plane mainly comprises navigation level and controlled stage, wherein the basic task of Navigation of Pilotless Aircraft level accurately determines the position in UAV system horizontal space, solve aircraft if how predetermined air speed flight is in the problem of predetermined altitude, and the target problem that flies to of how turning, the angle of pitch that aircraft needs, throttle and roll angle is provided by algorithm, and can fly by prebriefed pattern, then give controlled stage and carry out control and resolve.Linear Navigation Control algorithm, to be widely used in unmanned plane during flying device, such as: adopt traditional PID control method, calculates the navigation angle of aircraft apart from (geographic position of current unmanned plane and the distance of predetermined flight path route) according to driftage.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, increases the computing of integration item, makes unmanned plane quickly to the airline operation preset; If current unmanned plane is when the airline operation preset, the proportional at navigation angle is contrary with the symbol of integration item, hinder the computing of integration item, slow down or stop unmanned plane continuing, near the direction flight presetting course line, to avoid the phenomenon that integration overshoot or unmanned plane vibrate on the course line of presetting.

But traditional pid algorithm still has following weak point: (1), because the error of calculation of distance of going off course is comparatively large in the planning flight path task of complexity, traditional pid algorithm navigation effect is poor, and pressure course line effect is poor, and accuracy is not high.(2) because pid algorithm is linear, the contradiction between system stability and accuracy in the process that pid parameter is adjusted, will inevitably be run into, get often ratio, integration and differentiation three part control action compromise, be difficult to receive best effect.Further, pid parameter can not dynamically adjust, and 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 trace follow control method of unmanned plane, in order to improve the accuracy controlled Navigation of Pilotless Aircraft, make unmanned plane can realize better pressing airline operation, solve problem not high to the accuracy of unmanned plane flight course control in prior art.

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:

A nonlinear navigation trace follow control method for unmanned plane, is characterized in that, is located the geographic position and heading that obtain current unmanned plane in real time by GPS, and performs navigation trace follow control with the fixing Navigation Control cycle; When the Navigation Control cycle arrives, the step performing navigation trace follow control comprises:

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

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

Described single flight distance L _{1_dist}determine by following formula:

$L_{1\_dist}=\frac{1}{\mathrm{\π}}*L_{1\_period}*L_{1\_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 determines by following formula:

X＝l/L _AC，

Wherein, l represents target range L _aCprojection on the orthoscopic course line of presetting, l determines by following formula:

represent the distance vector of the geographic position of current unmanned plane to the air terminal preset; be used for representing the orthoscopic course line of presetting, specifically refer to the distance vector of default course line starting point to the air terminal preset; represent the distance between the course line starting point preset to default air terminal;

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

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

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

Orthoscopic flight angle η _sdetermine by following formula:

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

Wherein, α ₀value is 1 or-1; When the heading from current unmanned plane is to distance vector the direction at place is clockwise direction, α ₀be 1; When the heading from current unmanned plane is to distance vector the direction at place is counterclockwise, α ₀for-1; v _l1represent that speed v is at distance vector point vector field homoemorphism on direction, place, v _x1represent that speed v is at distance vector and distance vector institute planar with distance vector point vector field homoemorphism on the direction that direction is vertical; v _l1and v _x1determine according to the following formula respectively:

(6) by following formula determination 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 the orthoscopic course line of presetting between distance, α ₁value is 1 or-1; When from distance vector direction, place is to the orthoscopic course line of presetting the direction at place is clockwise direction, α ₁be 1; When from distance vector direction, place is to the orthoscopic course line of presetting the direction at place is counterclockwise, α ₁for-1; α ₂value is 1 or-1; When the heading from current unmanned plane is to the orthoscopic course line of presetting the direction at place is clockwise direction, α ₂be 1; When the heading from current unmanned plane is to the orthoscopic course line of presetting the direction at place is counterclockwise, α ₂for-1; v _l2represent that speed v is at distance vector point vector field homoemorphism on direction, place, v _x2represent that speed v is at distance vector and distance vector institute planar with distance vector point vector field homoemorphism on the direction that direction is vertical; v _x2and v _l2determine according to the following formula respectively:

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

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

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

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

(9) spiral fashion flight angle η is determined _rwith the centripetal acceleration a of unmanned plane _cmdr, perform step 11; Described spiral fashion flight angle η _rrefer to from the heading of current unmanned plane to distance vector angle between direction, place; Described distance vector refer to the distance vector of the geographic position of current unmanned plane to the course line orbit path preset;

Described spiral fashion flight angle η _rdetermine by following formula:

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

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

V _x3and v _l3determine by following formula respectively:

V _l3represent that speed v is at distance vector speed component on direction, place, v _x3represent speed v with distance vector speed component on direction vertical on direction, place;

The centripetal acceleration a of described unmanned plane _cmdrbe defined as by following formula:

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 _lR=4.0*L _{1_damping}* L _{1_damping};

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

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

Wherein, k _dindicating panel rotating flight first coefficient, k _d=2*L _{1_damping}* L _{1_damping}; k _pindicating panel rotating flight second coefficient, k _p=(2 π/L _{1_period}) ²; d _rthe rotating driftage distance of indicating panel, described spiral fashion driftage is apart from d _rrefer to the bee-line between the geographic position of current unmanned plane to the spiral fashion course line of presetting, the rotating driftage of indicating panel is apart from d _rrate of change, determine by following formula:

${\stackrel{\‾}{d}}_r=\frac{\left|d_{r\_last}-d_{r\_curr}\right|}{L_{1\_period}};$

Wherein, d _{r_last}represent that this previous Navigation Control cycle performs the spiral fashion driftage distance of navigation trace follow control when arriving; 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 this time navigation trace follow control into; Perform 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 this time navigation trace follow control into; Perform step 13;

θ＝arctan(a _cmds/g)；

G is wherein the value of acceleration of gravity;

(13) heading of unmanned plane is controlled according to navigation angle θ.

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}often change 1ms, described ratio of damping L _{1_damping}value correspondingly change 0.05.

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

In the nonlinear navigation trace follow control method of 1, unmanned plane of the present invention, adopt the fixing Navigation Control cycle, according to four kinds of unmanned plane different state of flights and depart from situation, draw navigation angle, thus according to navigation angle, nonlinear navigation trace follow control is performed to unmanned plane.Utilize this nonlinear navigation trace follow control method, ensure that when performing different airline operation tasks, unmanned plane can according to the feature in different course line and current off-line degree, obtain suitable navigation angle, make unmanned plane can realize better pressing airline operation, improve the accuracy that Navigation of Pilotless Aircraft is controlled.

In the nonlinear navigation trace follow control method of 2, unmanned plane of the present invention, unmanned plane adjusts navigation angle in real time according to the transverse acceleration under different flying speed or centripetal acceleration, when unmanned plane distance course line is closer, only produce smaller navigation angle, when unmanned plane distance course line is distant, produce larger navigation angle, thus improve the accuracy of navigation angle control, make unmanned plane can realize better pressing airline operation.

In the nonlinear navigation trace follow control method of 3, unmanned plane of the present invention, parameter L _{1_damping}and L _{1_period}dynamic conditioning can be carried out according to the flying speed v of unmanned plane, overcome the problem that pid parameter in prior art can not dynamically carry out adjusting, improve dirigibility unmanned plane being carried out to navigation trace follow control.

Accompanying drawing explanation

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

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

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

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

Embodiment

The nonlinear navigation trace follow control method of unmanned plane of the present invention, by obtaining geographic position and the heading of current unmanned plane, in conjunction with the course line of presetting, nonlinear navigation trace follow control is adopted to calculate transverse acceleration or the centripetal acceleration of unmanned plane in real time, and obtain the navigation angle of unmanned plane, guaranteeing that unmanned plane is when performing the airline operation task of setting, can realize better pressing airline operation.Solve in prior art when adopting traditional pid algorithm to carry out navigation trace follow control, pid parameter gets ratio, the trading off of integration and differentiation three part control action often, and the navigation angle calculated linearly is changed.Therefore, in the airline operation task of complexity, 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.

In the nonlinear navigation trace follow control method of unmanned plane of the present invention, in the process performing navigation trace follow control differentiate boat angle, mainly according to four kinds of different state of flight differentiate boat angles.The first, unmanned plane is the offline mode of orthoscopic at offline mode, and unmanned plane is current near presetting airline operation.The second, unmanned plane is the offline mode of orthoscopic at offline mode, but unmanned plane is current away from default airline operation.The third, unmanned plane is the offline mode of spiral fashion at offline mode, and unmanned plane flies in turn circle radius.4th kind, unmanned plane is the offline mode of spiral fashion at offline mode, and unmanned plane flies outward at turn circle radius.The course line of unmanned plane during flying is divided in order to four kinds of different course lines by the present invention, and according to different course lines and depart from situation, draws navigation angle.The present invention by current any one situation belonging to above-mentioned four kinds of state of flights of monitoring unmanned plane, thus sets up corresponding model to try to achieve navigation angle.Although make in the airline operation task of complexity, unmanned plane also according to the calculating at state of flight concrete in this aerial mission adjustment navigation angle, can make unmanned plane press airline operation better, improves the accuracy controlled Navigation of Pilotless Aircraft.

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

The nonlinear navigation trace follow control method of unmanned plane, by GPS (GlobalPositioningSystem, GPS) locate the geographic position and the heading that obtain current unmanned plane in real time, and with the fixing Navigation Control cycle, perform navigation trace follow control as follows:

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

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

Described single flight distance L _{1_dist}determine by following formula:

$L_{1\_dist}=\frac{1}{\mathrm{\π}}*L_{1\_period}*L_{1\_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 determines by following formula:

X＝l/L _AC，

Wherein, l represents target range L _aCprojection on the orthoscopic course line of presetting, l determines by following formula:

represent the distance vector of the geographic position of current unmanned plane to the air terminal preset; be used for representing the orthoscopic course line of presetting, specifically refer to the distance vector of default course line starting point to the air terminal preset; represent the distance between the course line starting point preset to default air terminal.

Suppose that the geographic position of being located the current unmanned plane got by GPS is represented with C point, C point coordinate is expressed as (c1, c2); Under unmanned plane current flight pattern is the prerequisite of the offline mode of orthoscopic, starting point and the terminal in orthoscopic course line is set; Starting point B point in course line represents, the coordinate of B point is expressed as (b1, b2); Air terminal A point represents, the coordinate of A point is expressed as (a1, a2); Described target range L _aCrefer to the geographic position of current unmanned plane to the air terminal established geographic position between distance, then L _aCcomputing formula be suppose that unmanned plane flies at a constant speed, then single flight distance L _{1_dist}for constant, according to the Navigation Control period L preset _{1_period}, ratio of damping L _{1_damping}determine with the speed of unmanned plane.The mathematical sense 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

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

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

Step 3 and step 4 be used for judging unmanned plane current whether be near presetting airline operation.If single flight distance L _{1_dist}be less than target range L _aC, illustrate that unmanned plane just can be reached home in the next Navigation Control cycle, if when deviated route coefficient X is greater than 0.7071, unmanned plane current flight direction and distance vector are described angle be less than 45 °, as single flight distance L _{1_dist}be less than target range L _aCand unmanned plane current flight direction and distance vector angle when being less than 45 °, then think that current unmanned plane is near presetting airline operation, its flight progress as shown in Figure 1, now asks orthoscopic to fly angle η according to the method in step 5 _s.Otherwise think that current unmanned plane is away from default airline operation, its flight progress as shown in Figure 2, now asks orthoscopic to fly angle η according to the method in step 6 _s.

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

Orthoscopic flight angle η _sdetermine by following formula:

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

Wherein, α ₀value is 1 or-1; When the heading from current unmanned plane is to distance vector the direction at place is clockwise direction, α ₀be 1; When the heading from current unmanned plane is to distance vector the direction at place is counterclockwise, α ₀for-1; v _l1represent that speed v is at distance vector point vector field homoemorphism on direction, place, v _x1represent that speed v is at distance vector and distance vector institute planar with distance vector point vector field homoemorphism on the direction that direction is vertical; v _l1and v _x1determine according to the following formula respectively:

Step 5 is under orthoscopic offline mode, and current unmanned plane, in time presetting airline operation, asks orthoscopic flight angle η _ssituation.Because unmanned plane distance course line is closer, so unmanned plane only needs a smaller navigation angle to adjust heading.Speed v is decomposed into v by step 5 _l1and v _x1during two components, the mathematical model of foundation is: with the geographic position of current unmanned plane for true origin, with distance vector the direction at place is X-axis, with distance vector and distance vector institute planar with distance vector vertical direction, direction is Y-axis, 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 direction, place ${\mathrm{\η}}_s=arctan\left(\frac{v_{x1}}{v_{l1}}\right).$

(6) by following formula determination 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 the orthoscopic course line of presetting between distance, α ₁value is 1 or-1; When from distance vector direction, place is to the orthoscopic course line of presetting the direction at place is clockwise direction, α ₁be 1; When from distance vector direction, place is to the orthoscopic course line of presetting the direction at place is counterclockwise, α ₁for-1; α ₂value is 1 or-1; When the heading from current unmanned plane is to the orthoscopic course line of presetting the direction at place is clockwise direction, α ₂be 1; When the heading from current unmanned plane is to the orthoscopic course line of presetting the direction at place is counterclockwise, α ₂for-1; v _l2represent that speed v is at distance vector point vector field homoemorphism on direction, place, v _x2represent that speed v is at distance vector and distance vector institute planar with distance vector point vector field homoemorphism on the direction that direction is vertical; v _x2and v _l2determine according to the following formula respectively:

Step 6 is under orthoscopic offline mode, and current unmanned plane, when away from default airline operation, asks orthoscopic flight angle η _ssituation.Because unmanned plane distance course line is distant, namely driftage is apart from comparatively large, and now unmanned plane needs a large yaw angle better to press course line to make unmanned plane.Therefore orthoscopic is flown angle η by step 6 _sbe divided into η ₁and η ₂calculate, its mathematical model set up is: orthoscopic flight angle η _sη is divided into by angle separated time ₁and η ₂, wherein angle separated time and distance vector parallel, can find out, η ₁gone off course apart from d by orthoscopic _s, single flight distance L _{1_dist}with distance to in the triangle of composition, can be obtained by inverse trigonometric function η ₂in the process calculated, the mathematical model of foundation is: with the geographic position of current unmanned plane for true origin, with distance vector the direction at place is X-axis, with distance vector and distance vector institute planar with distance vector vertical direction, direction is Y-axis, speed v is decomposed into the component v in X-axis _l2with the component v in Y-axis _x2.Draw thus

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

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

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

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

Whether step 8 is used for judging that unmanned plane is current and flies within the radius presetting course line of spiraling.If the distance of spiraling whether be less than default turn circle radius R, illustrate and fly within the current radius presetting course line of spiraling of unmanned plane, its flight progress as shown in Figure 3, now asks spiral fashion flight angle η according to the method in step 9 _r.Otherwise illustrate and fly outside the current radius presetting course line of spiraling of unmanned plane, its flight progress as shown in Figure 4, now asks spiral fashion flight angle η according to the method in step 10 _r.

(9) spiral fashion flight angle η is determined _rwith the centripetal acceleration a of unmanned plane _cmdr, perform step 11; Described spiral fashion flight angle η _rrefer to from the heading of current unmanned plane to distance vector angle between direction, place; Described distance vector refer to the distance vector of the geographic position of current unmanned plane to the course line orbit path preset.

Described spiral fashion flight angle η _rdetermine by following formula:

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

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

V _x3and v _l3determine by following formula respectively:

V _l3represent that speed v is at distance vector speed component on direction, place, v _x3represent speed v with distance vector speed component on direction vertical on direction, place.

The centripetal acceleration a of described unmanned plane _cmdrbe defined as by following formula:

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 _lR=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 calculating dial rotating flight angle η _rsituation.At calculating dial rotating flight angle η _rtime, the mathematical model of foundation is: with the geographic position of current unmanned plane for true origin, with distance vector the direction at place is X-axis, with spiral course line institute planar with distance vector vertical direction, direction is Y-axis, 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 being located the current unmanned plane got by GPS is represented with C point, C point coordinate is expressed as (c1, c2); Under unmanned plane current flight pattern is the prerequisite of the offline mode of spiral fashion, the orbit path in the rotating course line of setting dish and turn circle radius R; Line center point D point represents, the coordinate of D point is expressed as (d1, d2); Then coordinate be ((d1-c1), (d2-c2)), the centripetal acceleration a of unmanned plane _cmdrdirection be the direction that line center point is pointed in the geographic position of current unmanned plane.

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

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

Wherein, k _dindicating panel rotating flight first coefficient, k _d=2*L _{1_damping}* L _{1_damping}; k _pindicating panel rotating flight second coefficient, k _p=(2 π/L _{1_period}) ²; d _rthe rotating driftage distance of indicating panel, described spiral fashion driftage is apart from d _rrefer to the bee-line between the geographic position of current unmanned plane to the spiral fashion course line of presetting, the rotating driftage of indicating panel is apart from d _rrate of change, determine by following formula:

${\stackrel{\‾}{d}}_r=\frac{\left|d_{r\_last}-d_{r\_curr}\right|}{L_{1\_period}};$

Wherein, d _{r_last}represent that this previous Navigation Control cycle performs the spiral fashion driftage distance of navigation trace follow control when arriving; 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, calculating dial rotating flight angle η _rsituation.Wherein current spiral fashion driftage is apart from d _{r_curr}equal current d _rvalue.Suppose that the geographic position of being located the current unmanned plane got by GPS is represented with C point, C point coordinate is expressed as (c1, c2); Under unmanned plane current flight pattern is the prerequisite of the offline mode of spiral fashion, the orbit path in the rotating course line of setting dish and turn circle radius R; Line center point D point represents, the coordinate of D point is expressed as (d1, d2); Then

(11) by the centripetal acceleration a of unmanned plane _cmdrbring the navigation angle θ that following formula determines this time navigation trace follow control into; Perform 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 this time navigation trace follow control into; Perform step 13.

θ＝arctan(a _cmds/g)；

G is wherein the value of acceleration of gravity.

(13) heading of unmanned plane is controlled according to navigation angle θ.

Can find out, the nonlinear navigation trace follow control method of the unmanned plane of the present embodiment, the calculating of navigation angle θ is divided in order to four kinds of situations.For different situations, calculate navigation angle θ by the course line of setting and the geographic position of current unmanned plane, when unmanned plane is away from the course line set, spiral fashion flight angle η _ror orthoscopic flight angle η _sangle changing rate is large, causes corresponding centripetal acceleration a _cmdror transverse acceleration a _cmdsvalue larger, so a larger navigation angle will be produced, make unmanned plane approach course line fast.Otherwise unmanned plane goes to approach course line by with less navigation angle.Utilize this nonlinear navigation trace follow control method, even if making unmanned plane when performing complicated aerial mission, also can change the computing method of navigation angle θ according to concrete state of flight, making the navigation angle θ obtained more accurate.Meanwhile, the present embodiment calculates suitable navigation angle θ by setting course line and the current geographic position of unmanned plane, makes unmanned function complete the aerial mission in pressure course line better.

During embody rule, parameter L _{1_damping}and L _{1_period}dynamically can adjust according to the flying speed v of unmanned plane, overcome the problem that pid parameter can not dynamically carry out adjusting.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 the performance making unmanned plane reach best, as described L interval time performing navigation trace follow control _{1_period}often 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 by L _{1_period}value reduce 5ms; If unmanned plane vibrates, so by L after turning on the line of flight _{1_period}increase 1ms or 2ms.

In sum, the nonlinear navigation trace follow control method of unmanned plane of the present invention, by obtaining geographic position and the heading of current unmanned plane, in conjunction with the course line of presetting, nonlinear navigation trace follow control is adopted to calculate transverse acceleration or the centripetal acceleration of unmanned plane in real time, and obtain the navigation angle of unmanned plane, guaranteeing that unmanned plane is when performing the airline operation task of setting, can realize better pressing airline operation.The calculating of navigation angle θ is divided in order to four kinds of situations simultaneously, for different situations, draw navigation angle θ by different computing method.Although make in the airline operation task of complexity, unmanned plane also according to the calculating at state of flight concrete in this aerial mission adjustment navigation angle, can ensure that the accuracy of navigation angle θ.Meanwhile, parameter L _{1_damping}and L _{1_period}dynamically can adjust according to the flying speed v of unmanned plane, overcome the problem that pid parameter can not dynamically carry out adjusting, improve dirigibility unmanned plane being carried out to navigation trace follow control.

What finally illustrate is, above embodiment is only in order to illustrate technical scheme of the present invention and unrestricted, although with reference to preferred embodiment to invention has been detailed description, those of ordinary skill in the art is to be understood that, can modify to technical scheme of the present invention or equivalent replacement, and not departing from aim and the scope of technical solution of the present invention, it all should be encompassed in the middle of right of the present invention.

Claims (3)

1. a nonlinear navigation trace follow control method for unmanned plane, is characterized in that, is located the geographic position and heading that obtain current unmanned plane in real time by GPS, and performs navigation trace follow control with the fixing Navigation Control cycle; When the Navigation Control cycle arrives, the step performing navigation trace follow control comprises:

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

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

Described single flight distance L _{1_dist}determine by following formula:

$L_{1\_dist}=\frac{1}{\mathrm{\π}}*L_{1\_period}*L_{1\_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 determines by following formula:

X＝l/L _AC，

Wherein, l represents target range L _aCprojection on the orthoscopic course line of presetting, l determines by following formula:

represent the distance vector of the geographic position of current unmanned plane to the air terminal preset; be used for representing the orthoscopic course line of presetting, specifically refer to the distance vector of default course line starting point to the air terminal preset; represent the distance between the course line starting point preset to default air terminal;

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

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

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

Orthoscopic flight angle η _sdetermine by following formula:

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

Wherein, α ₀value is 1 or-1; When the heading from current unmanned plane is to distance vector the direction at place is clockwise direction, α ₀be 1; When the heading from current unmanned plane is to distance vector the direction at place is counterclockwise, α ₀for-1; v _l1represent that speed v is at distance vector point vector field homoemorphism on direction, place, v _x1represent that speed v is at distance vector and distance vector institute planar with distance vector point vector field homoemorphism on the direction that direction is vertical; v _l1and v _x1determine according to the following formula respectively:

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

(6) by following formula determination 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 the orthoscopic course line of presetting between distance, α ₁value is 1 or-1; When from distance vector direction, place is to the orthoscopic course line of presetting the direction at place is clockwise direction, α ₁be 1; When from distance vector direction, place is to the orthoscopic course line of presetting the direction at place is counterclockwise, α ₁for-1; α ₂value is 1 or-1; When the heading from current unmanned plane is to the orthoscopic course line of presetting the direction at place is clockwise direction, α ₂be 1; When the heading from current unmanned plane is to the orthoscopic course line of presetting the direction at place is counterclockwise, α ₂for-1; v _l2represent that speed v is at distance vector point vector field homoemorphism on direction, place, v _x2represent that speed v is at distance vector and distance vector institute planar with distance vector point vector field homoemorphism on the direction that direction is vertical; v _x2and v _l2determine according to the following formula respectively:

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

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

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

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

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

(9) spiral fashion flight angle η is determined _rwith the centripetal acceleration a of unmanned plane _cmdr, perform step 11; Described spiral fashion flight angle η _rrefer to from the heading of current unmanned plane to distance vector angle between direction, place; Described distance vector refer to the distance vector of the geographic position of current unmanned plane to the course line orbit path preset;

Described spiral fashion flight angle η _rdetermine by following formula:

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

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

V _x3and v _l3determine by following formula respectively:

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

V _l3represent that speed v is at distance vector speed component on direction, place, v _x3represent speed v with distance vector speed component on direction vertical on direction, place;

The centripetal acceleration a of described unmanned plane _cmdrbe defined as by following formula:

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 _lR=4.0*L _{1_damping}* L _{1_damping};

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

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

Wherein, k _dindicating panel rotating flight first coefficient, k _d=2*L _{1_damping}* L _{1_damping}; k _pindicating panel rotating flight second coefficient, k _p=(2 π/L _{1_period}) ²; d _rthe rotating driftage distance of indicating panel, described spiral fashion driftage is apart from d _rrefer to the bee-line between the geographic position of current unmanned plane to the spiral fashion course line of presetting, the rotating driftage of indicating panel is apart from d _rrate of change, determine by following formula:

${\stackrel{\‾}{d}}_r=\frac{|d_{r\_last}-d_{r\_curr}|}{L_{1\_period}};$

Wherein, d _{r_last}represent that this previous Navigation Control cycle performs the spiral fashion driftage distance of navigation trace follow control when arriving; 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 this time navigation trace follow control into; Perform 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 this time navigation trace follow control into; Perform step 13;

θ＝arctan(a _cmds/g)；

G is wherein the value of acceleration of gravity;

(13) heading of unmanned plane is controlled according to navigation angle θ.

2. the nonlinear navigation trace follow control method of unmanned plane as claimed in claim 1, 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 trace follow control method of unmanned plane as claimed in claim 2, is characterized in that, when described Navigation Control period L _{1_period}often change 1ms, described ratio of damping L _{1_damping}value correspondingly change 0.05.