CN103149937A - Transverse lateral curve flight-path tracking method based on curvature compensation - Google Patents

Abstract

The invention discloses a transverse lateral curve flight-path tracking method based on curvature compensation and belongs to the technical field of flight-path tracking control. The transverse lateral curve flight-path tracking method based on curvature compensation comprises the following steps of: obtaining discrete flight-route point data of a reference flight path by a flight-path planning module; carrying out piecewise interpolation fitting processing on the obtained discrete flight-path point data by a Lagrange piecewise polynomial interpolation method to obtain a curve flight path; calculating the lateral offset distance of an unmanned plane at the present moment, and calculating a curvature value of a point, closest to the plane, on the reference flight path by referencing the coordinates of the point, closest to the plane, on the reference flight path; and completing the whole curve flight-path tracking process by using the obtained lateral offset distance and the curvature value as input variables of a transverse lateral controller of the unmanned plane. According to the transverse lateral curve flight-path tracking method disclosed by the invention, the curve interpolation fitting processing is carried out on the reference flight path, so that the transitional section of the flight path points in the tracking process is smoother and the tracking errors are smaller, therefore, the flight path can be tracked better.

Description

A kind of horizontal side direction curve Track In Track method based on curvature compensation

Technical field

The present invention relates to a kind of aircraft Track In Track control method, belong to Track In Track control technology field, specifically, refer to a kind of horizontal side direction curve Track In Track method based on curvature compensation.

Background technology

Unmanned plane also claims unmanned aircraft or remote driving aircraft, a kind ofly to be controlled by radio robot, or the non-manned vehicle of being handled by preprogramming, have numerous manned aircrafts do not have advantages of, can carry out the tasks such as various operations and training support, have wide Military Application prospect.

Along with the application of unmanned plane constantly enlarges, the flight of unmanned plane is required also more and more harsher, it is one of them that preset flight path is followed the tracks of accurately and rapidly.The Track In Track of unmanned plane is controlled, and mainly the flight control system by unmanned plane realizes.Like this, how to make unmanned plane can follow the tracks of better faster preset flight path by the flight control system of regulating unmanned plane, the raising of unmanned plane overall performance and the development of aviation industry are all had very important significance.

The trajectory planning stage, the flight path of planning formed by one group of discrete way point, between consecutive point the distance one planning step-length.Traditional lateral deviation apart from control principle as shown in Figure 1.

By Path Planning gained track points consecutive point in twos line obtain the flight path of planning (t1-t2-t3-t4), by real-time calculating unmanned plane current location and plan that between flight path, minimum distance obtains the unmanned plane lateral deviation apart from S, under the effect of horizontal side direction control loop, unmanned plane is gradually to t ₃Track points is close, and unmanned plane arrives t ₃After track points, switch and follow the tracks of next flight path section t ₃-t ₄

Because the unmanned plane track is broken line, can there be larger tracking error in tracing process; Especially when track points is closeer, can cause unmanned plane can't follow the tracks of the planning flight path.

Summary of the invention

The objective of the invention is in order to address the above problem, a kind of horizontal side direction curve Track In Track method based on curvature compensation is proposed, by introducing the Lagrange Polynomial interpolating algorithm, to the processing that carries out curve fitting of the discrete track points of trajectory planning gained, obtain the curve flight path, and introduce curvature of curve and measure by way of compensation, add in horizontal side direction Track In Track controller design, obviously improve the tracking effect to reference track, improved tracking accuracy.

A kind of horizontal side direction curve Track In Track method based on curvature compensation of the present invention specifically comprises the steps:

Step 1: the discrete way point data that obtain reference track by flight path programming module.

Step 2: by Lagrangian piecewise polynomial interpolation method, the discrete way point data of step 1 gained are carried out the piecewise interpolation process of fitting treatment, obtain the curve flight path.

Step 3: calculate unmanned plane current time lateral deviation distance, and on reference track apart from aircraft closest approach coordinate.

Step 4: by the curvature computing formula, on calculation procedure three gained reference tracks apart from aircraft closest approach place curvature value.

Step 5: repeating step two～step 4, and with step 3 gained lateral deviation distance and step 4 gained curvature value as the input quantity of the horizontal lateral controller of unmanned plane, complete whole curve Track In Track process.

The invention has the advantages that:

(1) method provided by the invention is processed by reference track being carried out the curve interpolation fitting, makes in tracing process the transition section of way point more level and smooth, and tracking error reduces.

(2) method provided by the invention is measured curvature value by way of compensation and is added in horizontal lateral controller, and curvature of curve has represented degree of crook and the bending direction of curve, can be for horizontal lateral controller provide flight path direction tendency information, thus track better.

Description of drawings

Fig. 1 is that broken line flight path lateral deviation is apart from control principle drawing;

Fig. 2 is the discrete way point of flight path programming module gained in the present invention;

Fig. 3 is discrete way point curve fit process process schematic diagram in the present invention;

Fig. 4 is unmanned aerial vehicle flight path tracking control unit structural representation in the present invention.

Embodiment

The present invention is described in further detail below in conjunction with drawings and Examples.

the present invention proposes a kind of horizontal side direction curve Track In Track method based on curvature compensation, at first obtain the discrete way point data of reference track by flight path programming module, then by Lagrangian piecewise polynomial interpolation method, discrete way point is carried out the piecewise interpolation process of fitting treatment, thereby obtain the curve flight path, thereby the minimum distance that calculates in real time unmanned plane current location distance reference curve flight path section obtains unmanned plane lateral deviation distance, can obtain simultaneously on reference track the coordinate apart from aircraft closest approach place, can obtain this corresponding point position curvature value by the curvature computing formula, with above-mentioned gained unmanned plane lateral deviation distance and the closest approach place curvature input quantity as the horizontal lateral controller of unmanned plane, completing the curve Track In Track controls.

A kind of horizontal side direction curve Track In Track method based on curvature compensation of the present invention comprises following step:

Step 1: the discrete way point data that obtain reference track by flight path programming module.

According to unmanned plane starting point and impact point, pass through flight path programming module, obtain unmanned plane and arrive the discrete way point data of impact point from starting point, as P in Fig. 2 (1), P (2) ... flight path shown in P (n), P (1) is starting point, and p (n) is impact point, and n is way point quantity on flight path.

Step 2: by Lagrangian piecewise polynomial interpolation method, the discrete way point data of step 1 gained are carried out the piecewise interpolation process of fitting treatment, obtain the curve flight path.

Adopt segmentation quadratic polynomial interpolation method in the present invention, so-called segmentation quadratic polynomial interpolation is namely chosen three adjacent way point data as interpolation knot at every turn, and then is tried to achieve the Lagrange interpolation polynomial.If three adjacent way point coordinates are (x _i-1, y _i-1), (x _i, y _i), (x _i+1, y _i+1), the Lagrange interpolation polynomial is:

$L_2\left(x\right)=\frac{(x-x_i)(x-x_{i+1})}{(x_{i-1}-x_i)(x_{i-1}-x_{i+1})}{y}_{i-1}+\frac{(x-x_{i-1})(x-x_{i+1})}{(x_i-x_{i-1})(x_i-x_{i+1})}{y}_i+\frac{(x-x_{i-1})(x-x_i)}{(x_{i+1}-x_{i-1})(x_{i+1}-x_i)}{y}_{i+1}---\left(1\right)$

If flight path programming module gained way point information is P ₁, P ₂... P _n, the way point number is n, the processing procedure of the way point data being carried out segmentation quadratic polynomial interpolation is specially as shown in Figure 3:

(1) whenever way point of unmanned plane arrival, establishing this way point is P _Count

(2) if count≤n-2 utilizes P _Count, P _Count+1, P _Count+2Three way point data are carried out the quadratic polynomial interpolation; Otherwise finish, complete the tracking of whole piece flight path.

(3) if count=n-2 draws P _Count-P _Count+2Section match air route; Otherwise draw P _Count-P _Count+1Air route section curve flight path, and with this curve flight path as the reference flight path;

(4) make count=count+1, return to step (2).

Step 3: put the computing formula of curve bee-line by derivation, calculate unmanned plane current time lateral deviation apart from (being the minimum distance S of current some distance reference flight path of unmanned plane), on the computing reference flight path apart from aircraft closest approach coordinate.

What adopt due to the present invention is segmentation quadratic polynomial interpolation method, thus take two-dimensional curve as example, derive point to curve apart from computing formula.

If curvilinear equation is f (x), on curve, any point coordinate can be expressed as A=(x, f (x)).In the plane, certain 1 B coordinate is (a, b), and putting B is F (x)=(x-a) to the square distance of putting A ²+ [f (x)-b] ²Differentiate gets F'(x to F (x))=2 (x-a)+2f'(x) [f (x)-b], make F'(x)=0, can be in the hope of a C coordinate (x ₀, f (x ₀)), putting C is the nearest point of range points B on curve.To put the C coordinate and bring F (x) into, so can try to achieve a B to the bee-line of curve square.

Be L with step 2 gained Lagrange quadratic polynomial the Representation Equation ₂(x)=p ₁x ²+ p ₂x+p ₃, p ₁, p ₂, p ₃Be respectively polynomial every coefficient, use said method, with L ₂(x) be brought into F'(x as f (x)) in, can in the hope of:

F'(x)＝2(x-a)+2(p ₁x ²+p ₂x+p ₃-b)(2p ₁x+p ₂)(2)

By finding the solution, can obtain on reference track apart from aircraft closest approach coordinate and the aircraft bee-line apart from reference track.

Step 4: by the curvature computing formula, calculation procedure three gained with reference on the section of air route apart from aircraft closest approach place curvature value.

Be L with gained Lagrange quadratic polynomial the Representation Equation in step 2 ₂(x)=p ₁x ²+ p ₂x+p ₃, its differentiate is obtained L ₂' (x)=2p ₁x+p ₂, L ₂" (x)=2p ₁, can obtain the curvature of curve expression formula:

$K=\frac{{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA00002858952800012.png,HDA00002858952800013.png"\; state="\{\{state\}\}">L</figure-callout>}}_2}^{\&prime;\&prime;}\left(x\right)}{{(1+{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA00002858952800012.png,HDA00002858952800013.png"\; state="\{\{state\}\}">L</figure-callout>}}_2}^{\&prime;}{\left(x\right)}^2)}^{\frac{3}{2}}}=\frac{{2p}_1}{{[1+{(2p_{1}x+p_2)}^2]}^{\frac{3}{2}}}---\left(3\right)$

Step 5: repeating step two is to step 4, and with step 3 gained lateral deviation distance and step 4 gained curvature value as the input quantity of the horizontal lateral controller of unmanned plane, complete whole curve Track In Track process.

With step 3 gained lateral deviation distance and the step 4 gained curvature value input quantity as the horizontal lateral controller of unmanned plane, design unmanned aerial vehicle flight path tracking control unit.As shown in Figure 4, lateral deviation apart from controller according to the unmanned plane lateral deviation apart from information output aircraft roll angle set-point, make the instruction of roll angle controller output aileron movement by feeding back roll angle and compensating flight path curvature, this instruction is controlled aileron in conjunction with the feedback roll angle via the aileron servo control mechanism, realizes that unmanned plane is to the tracking of flight path.The feedback of roll angle speed is in order to increase system damping, to improve the control effect.

Claims (3)

1. the horizontal side direction curve Track In Track method based on curvature compensation, is characterized in that, comprises the steps:

Step 1: the discrete way point data that obtain reference track by flight path programming module;

Step 2: by Lagrangian piecewise polynomial interpolation method, the discrete way point data of step 1 gained are carried out the piecewise interpolation process of fitting treatment, obtain the curve flight path;

Step 3: calculate unmanned plane current time lateral deviation distance, and on reference track apart from aircraft closest approach coordinate;

Step 4: by the curvature computing formula, on calculation procedure three gained reference tracks apart from aircraft closest approach place curvature value;

Step 5: repeating step two～step 4, and with step 3 gained lateral deviation distance and step 4 gained curvature value as the input quantity of the horizontal lateral controller of unmanned plane, complete whole curve Track In Track process.

2. a kind of horizontal side direction curve Track In Track method based on curvature compensation according to claim 1 is characterized in that: described piecewise polynomial interpolation method, and processing procedure is specially:

(1) whenever way point of unmanned plane arrival, establishing this way point is P _Count

(2) if count≤n-2 utilizes P _Count, P _Count+1, P _Count+2Three way point data are carried out the quadratic polynomial interpolation; Otherwise

Finish, complete the tracking of whole piece flight path;

(3) if count=n-2 draws P _Count-P _Count+2Section match air route; Otherwise draw P _Count-P _Count+1Air route section curve boat

Mark, and with this curve flight path as the reference flight path;

(4) make count=count+1, return to step (2).

3. a kind of horizontal side direction curve Track In Track method based on curvature compensation according to claim 1 is characterized in that: described step 3 gained with reference on the section of air route apart from the aircraft closest approach curvature value K of place expression formula:

$K=\frac{{L_2}^{\&prime;\&prime;}\left(x\right)}{{(1+{L_2}^{\&prime;}{\left(x\right)}^2)}^{\frac{3}{2}}}=\frac{{2p}_1}{{[1+{(2p_{1}x+p_2)}^2]}^{\frac{3}{2}}}---\left(3\right)$

Be L with Lagrangian quadratic polynomial the Representation Equation ₂(x)=p ₁x ²+ p ₂x+p ₃, p ₁, p ₂, p ₃Be respectively polynomial every coefficient, its differentiate is obtained L ₂' (x)=2p ₁x+p ₂, L ₂" (x)=2p ₁

Images