CN106444822A - Space vector field guidance based stratospheric airship's trajectory tracking control method - Google Patents

Abstract

The invention provides a space vector field guidance based stratospheric airship's path tracking control method, comprising the following steps: 1) desired tracking values giving: giving a desired task path; giving a desired forward speed; calculating the resistance force Ff at the desired speed; 2) navigation calculating: calculating the desired yaw angle phi<d> and the desired pitch angle theta <d> required to remove the error between the desired position and the actual position; 3) path tracking yaw angle and pitch angle error calculating: calculating the error between the desired yaw angle and the actual yaw angle and the error between the desired pitch angle and the actual pitch angle; 4) slip mode controller calculating: calculating the error between the desired attitude angle and the actual attitude angle and the control amount required for the speed control of the airship; and 5) execution component control signal calculating: calculating the control amounts of theta el, theta er, theta r, T, and gamma required by the execution components. The control flow is referred in the figure.

Description

A kind of stratospheric airship path tracking control method based on the guidance of space vector field

Technical field

The invention provides a kind of stratospheric airship path tracking control method based on the guidance of space vector field, it is flat The path following control of fluid layer dirigible provides the parametrization tracking in a kind of space, belongs to automatic control technology field.

Background technology

Stratospheric airship is as the tradition extension in stratosphere field for the dirigible, and some technology both having remained tradition dirigible are special Point, it is also proposed new design simultaneously and requires.Aerostat, the stratosphere flight such as High Altitude UAV relative to other Device, has a low energy consumption, the advantage such as during long boat.This method control object flies for the stratosphere with cross empennage and vector screw Ship, as shown in Figure 1.The flight control method majority of stratospheric airship is derived from robot field and fixed wing aircraft field, The rare maturation method requiring with cruise for stratospheric airship self model feature, residing physical environment.Stratospheric airship Cruising altitude is at more than 20km, and thin at this level air, rudder face and propeller efficiency all ratios are relatively low, self cruising speed one As be 5m/s-15m/s, can synchronize over the ground in somewhere overhead according to mission requirements.

Current main flow path tracking algorithm uses the method directly following the tracks of certain desired point on expected path to navigate Resolve.This method uses space vector field method of guidance, different from legacy paths homing guidance method.According to task path and The dynamic characteristic of aircraft, sets up vector field around task path and resolves expectation attitude, rather than follow the tracks of on expected path Desired point.The control object of space vector field method of guidance is the attitude of dirigible, and speeds control can carry out spirit according to mission requirements The adjustment lived.

The present invention " a kind of stratospheric airship path tracking control method based on the guidance of space vector field ", it is proposed that based on The space path tracking and controlling method of dynamics nonlinear model.The path trace that the method combines based on space vector field is calculated Method and sliding mode control theory.It is bounded stability by the closed-loop system that the method controls, and there is good convergence effect, be flat The flight control of fluid layer dirigible provides engineering design method.

Content of the invention

(1) purpose：It is an object of the invention to provide a kind of based on space vector field guidance stratospheric airship path with Track control method, control engineer can in the method and combine actual parameter and realize that the autonomous cruise of stratospheric airship flies OK.

(2) technical scheme：The present invention " a kind of stratospheric airship controlling of path thereof based on the guidance of space vector field ", its Main contents and program are：

The cruise track of stratospheric airship is generally-straight or regular curve, and control and change in conjunction with height can divide Solve is parameterized space line and helix.(include space line first with space vector field theory at given expected path And spiral path) near set up navigation vector field, generate expectation attitude angle；Then utilize sliding mode control theory design path with Track controller so that it is tracking error levels off to zero in finite time.In actual application, the position of stratospheric airship, attitude, speed The quantity of states such as degree are obtained by airborne sensor measurements such as combined inertial nevigations, will be by the transmission of the method calculated controlled quentity controlled variable to control Rudder face and vector screw etc. perform the path trace facility that device can realize stratospheric airship.

The present invention " a kind of stratospheric airship path tracking control method based on the guidance of space vector field ", its concrete steps As follows：

Step one given expectation pursuit gain：Given expectation task path；Given expectation forward speed.

Step 2 navigation calculates：Calculate the expectation yaw angle needed for error eliminating between desired locations and physical location ψ^dWith expectation pitching angle theta^d.

Step 3 path trace yaw angle and angle of pitch error calculation：Calculate between expectation yaw angle and actual yaw angle ErrorExpect the error between the angle of pitch and the actual angle of pitch

Step 4 sliding mode controller calculates：Calculate and eliminate error between expectation attitude angle and actual attitude angle, and dirigible Controlled quentity controlled variable U needed for speeds control.

Step 5 each execution unit control signal calculates：Calculate the execution unit controlled quentity controlled variable realizing needed for sliding-mode control law U δ_el,δ_er,δ_r,T,γ.

Wherein, it is divided into space line and two kinds of helix, straight line path in the given expectation task path described in step one Footpath is by straight line and north orientation angle ξ^ψAngle ξ with straight line and horizontal plane^θDetermine, be denoted as p_l(ξ^ψ,ξ^θ)；Spiral path is by path The beginning center of circle [c^x,c^y,c^z], radius c^rAnd climb rate c^λDetermine, be denoted as p_o(c^x,c^y,c^z,c^r,c^λ).Described given desired speed For υ_c=[u_c, v_c, w_c]^T=[C, 0,0]^T, C ＞ 0 is constant, u_c,v_c,w_cFor desired speed along the decomposition amount of hull coordinate system.

Wherein, the expectation needed for error calculating between elimination desired locations and physical location described in step 2 is inclined Boat angle ψ^d, it is desirable to yaw angle θ^d, its computational methods are as follows：

Wherein ψ^∞For the initial yaw angle setting, θ^∞ For the initial pitch angle setting, d^ψ,d^θIt is respectively the throwing at horizontal plane and vertical direction for the site error between body and task path Shadow component；K ＞ 0 is for determining the parameter of direction vector conversion speed in vector field；On agreed assignment path herein away from body away from It is desired point P from nearest point_m, task path is L at the tangent line of desired point, χ^ψFor tangent line L and north orientation angle, χ^θFor tangent line L with The angle of horizontal plane；

Space line：d^ψ,d^θCan be by path planning starting point coordinate P_A=[x_Ay_Az_A]^T, body position coordinates P_o=[x₀ y₀z₀]^T, and straight line path and north orientation angle ξ^ψAnd the angle ξ with horizontal plane^θDetermine；χ in such cases^ψ=ξ^ψ,χ^θ=ξ^θ；

Helix：d^ψ,d^θCan be by spiral path p_o(c^x,c^y,c^z,c^r,c^λ), body position coordinates P_o=[x₀y₀z₀]^T Determine；Now χ^ψ,χ^θCalculated by detailed geometry.

Wherein, the path trace yaw angle error described in step 3With tracking angle of pitch errorIts calculating side Method is as follows：

Wherein ψ is the current yaw angle of stratospheric airship, and θ is the current angle of pitch of stratospheric airship.

Wherein, the controlled quentity controlled variable needed for error eliminating between expectation yaw angle and actual yaw angle described in step 4 U, its computational methods are as follows：

Drive lacking dirigible kinetic model in this example is：

Wherein：X₁=[x z φ θ ψ]^T,X₂=[u w p q r]^TIt is position and attitude quantity of state and the speed of dirigible respectively Degree angular speed quantity of state, owing to being under-actuated systems, so displacement y does not directly control.R, A, N, B are related coefficients Matrix, U is and execution unit δ_el,δ_er,δ_r, the related controlled quentity controlled variable of T, γ.

The sliding-mode surface of sliding formwork control is：S=E₁+HE₂, wherein H=diag{h₁,h₂,…,h₅, E₁,E₂It is X respectively₁,X₂Right Answer the error of desired value.

Setting up liapunov function isAnd sliding formwork boundary conditionWherein M, K is diagonal coefficient matrix,

Finally give the accounting equation with regard to controlled quentity controlled variable U

Wherein, execution unit controlled quentity controlled variable δ realizing needed for sliding-mode control law U described in step 5_el,δ_er,δ_r,T, γ, its computational methods are as follows：

[δ_el,δ_er,δ_r,T,γ]^T=B U

(3) advantage and effect：

The present invention " a kind of stratospheric airship path tracking control method based on the guidance of space vector field ", with prior art Ratio, its advantage is：

1) the method utilizes virtual point on surrounding vectors field, path rather than track path to carry out path trace, by the time with Spatially decoupled, the control purpose of other and time correlation can be realized, such as the collaborative flight under time-constrain.

2) the method ensure that the Asymptotic Stability performance of closed-loop system, and convergence rate and sliding manifolds boundary layer thickness Can be adjusted according to actual requirement；

3) the method uses sliding-mode control, can overcome the uncertainty of system, to interference and Unmarried pregnancy tool There is very strong robustness, especially there is to the control of nonlinear system good control effect.

4) the method simple in construction, guides process stabilization, is particularly suited for low dynamic body, it is easy to accomplish extensive engineering Application.

Control engineer can give any desired cruise path according to actual stratospheric airship in application process, and will The calculated controlled quentity controlled variable of the method is directly transferred to executing agency's realizing route following function.

Brief description

Fig. 1 is control method FB(flow block) of the present invention；

Fig. 2 is stratospheric airship schematic diagram of the present invention；

Fig. 3 is vector field straight line path of the present invention navigation computational geometry graph of a relation；

Fig. 4 is vector field spiral path of the present invention navigation computational geometry graph of a relation；

Symbol description is as follows：

P_AP_A=[x_Ay_Az_A]^TPlan initial point position for straight line expected path；

P_oP_o=[x₀y₀z₀]^TFor current location under inertial coodinate system for the dirigible；

ξ^ψExpect straight line path and north orientation angle；

ξ^θExpect straight line path and horizontal plane angle；

ψ, θ stratospheric airship yaw angle and the angle of pitch；

ψ^d,θ^dStratospheric airship expectation yaw angle and the expectation angle of pitch；

Stratospheric airship yaw angle error and angle of pitch error；

Stratospheric airship yaw rate and rate of pitch；

δ_el,δ_erTwo elevator drift angles；

δ_rRudder；

The single screw of T produces thrust；

The vector drift angle of γ vector device；

V_gStratospheric airship speed in inertial system；

Linear velocity under [u v w] stratospheric airship body axis system；

Angular speed under [p q r] stratospheric airship body axis system；

ψ^∞,θ^∞Infinite point yaw angle and the angle of pitch, vector field parameter, for adjustable positive number；

(c^x,c^y,c^z) spiral path initiates home position coordinate；

c^rSpiral path radius；

c^λThe spiral path climb rate；

P_mReference point on task path；

d^ψDirigible and P_mHorizontal range；

d^θDirigible and P_mVertical range；

Tangent line at L reference point；

χ^ψTangent line L and north orientation angle；

χ^θTangent line L and horizontal plane angle；

Detailed description of the invention

Below in conjunction with the accompanying drawings, each several part method for designing in the present invention is further described：

The present invention " a kind of stratospheric airship path tracking control method based on the guidance of space vector field ", as shown in Figure 1, It comprises the following steps that：Step one：Given expectation pursuit gain

1) as in figure 2 it is shown, set up hull coordinate system O with stratospheric airship centre of buoyancy for initial point_xyz；It with any point on ground is Initial point sets up inertial coodinate system O_gx_gy_gz_g, wherein initial point O_gFor ground any point, O_gx_gPoint to north, O_gy_gPoint to east, O_gz_gRefer to To the earth's core.

2) give expectation and invite task path, including straight line and helix.Wherein, as it is shown on figure 3, straight line path by straight line with Plane O_gx_gy_gAngle ξ^θ, and with plane O_gy_gz_gAngle ξ^ψDetermine；As shown in Figure 4, spiral path is by initiateing home position P_c =[c^x,c^y,c^z]^TWith radius c^rWith climb rate c^λDetermine, be denoted as p_o(c^x,c^y,c^z,c^r,c^λ).

3) desired speed υ is given_c=[u, v, w]^T=[C, 0,0]^T, C ＞ 0 is constant, and u, v, w are that desired speed is along body The decomposition amount of coordinate system.In stratospheric airship working environment, without vertical direction wind speed, it is believed that dirigible forward speed and ground velocity phase Deng i.e. V_g=u=C.

Step 2：Calculate expectation yaw angle and the angle of pitch

1) the straight line path expectation yaw angle angle of pitch calculates：

First, parameter P of straight line path is determined_A=[x_Ay_Az_A]^T, p_l(ξ^ψ,ξ^θAfter), calculate straight line by method of geometry On path, the nearest some P of body_m=[x_m,y_m,z_m]；

Secondly, straight line path is calculated at a P_mThe tangent line L at place, and calculate the angle χ of tangent line L^ψ=ξ^ψ,χ^θ=ξ^θ；

Then, closest approach P on computer body distance straight line_mDistanceAnd d^θ=z₀-z_m, The position of dirigible is P_o=[x₀y₀z₀]^T, as shown in Figure 3；

Afterwards, infinite point yaw angle ψ is given^∞And pitching angle theta^∞；

Finally, straight line path expectation yaw angle ψ is calculated^dWith expectation pitching angle theta^d：

2) the spiral path expectation yaw angle angle of pitch calculates：

First, spiral path parameter p is determined_o(c^x,c^y,c^z,c^r,c^λ), spiral path is projected extremely with body position coordinates O_gx_gy_gPlane, as shown in Figure 4；

Secondly, body is obtained on the projection surface to the closest approach P of curved path_m0, draw dirigible to spiral path simultaneously Horizontal rangeAnd the angle with north orientation of tangent line L

Then, corresponding subpoint P_m0, spiral path finds closest approach P_m, dirigible can be obtained to P_mVertical range d^θ=z₀-z_m.

For spiral path, the angle χ with horizontal plane of tangent line L^θIt is a constant value,

Afterwards, infinite point yaw angle ψ is given^∞And pitching angle theta^∞；

Finally, spiral path expectation yaw angle ψ is calculated^dWith expectation pitching angle theta^d：

Step 3：Calculate path trace attitude error

1)Wherein ψ is the current yaw angle of stratospheric airship, and θ is the current angle of pitch of stratospheric airship.

Step 4：Design sliding formwork control path following control device

For non-linear dirigible model, its kinetic model can be expressed as：

Owing to being under-actuated systems, so lateral displacement y and side velocity v cannot directly control, need by dirigible certainly The shipping-direction stability of body and direction are controlled.Setting up sliding formwork chain of command S is：

Wherein [x₀z₀φ₀θ₀ψ₀],[u₀w₀p₀q₀r₀] it is corresponding desired value.

Setting up liapunov function isAnd sliding formwork boundary condition：

Under this condition, enable to liapunov function to meet：

Now airframe systems is stable.Can be obtained by sliding formwork boundary condition and kinetics equation：

The accounting equation of controlled quentity controlled variable U can be obtained after arranging：

Step 5：Calculate each execution unit control signal

After obtaining controlled quentity controlled variable U, by U=B [δ in dirigible kinetics equation_el,δ_er,δ_r,T,γ]^T, each portion can be obtained The control signal amount of part, respectively left elevator δ_el, right elevator δ_er, rudder δ_r, single screw thrust T, vectored thrust side To γ.

[δ_el,δ_er,δ_r,T,γ]^T=B U.

Claims (6)

1. the stratospheric airship path tracking control method based on the guidance of space vector field, it is characterised in that：Concrete steps As follows：

Step one given expectation pursuit gain：Given expectation task path；Given expectation forward speed.

Step 2 navigation calculates：Calculate the expectation yaw angle ψ needed for error eliminating between desired locations and physical location^dAnd the phase Hope pitching angle theta^d.

Step 3 path trace yaw angle and angle of pitch error calculation：Calculate expectation yaw angle and actual yaw angleBetween mistake Difference, it is desirable to the error between the angle of pitch and the actual angle of pitch

Step 4 sliding mode controller calculates：Calculate and eliminate error between expectation attitude angle and actual attitude angle, and the speed of dirigible Required controlled quentity controlled variable U of degree control.

Step 5 each execution unit control signal calculates：Calculate execution unit controlled quentity controlled variable δ realizing needed for sliding-mode control law U_el, δ_er,δ_r,T,γ.

2. a kind of stratospheric airship path tracking control method based on the guidance of space vector field according to claim 1, It is characterized in that：

Described in step one given expectation task path be divided into space line and two kinds of helix, straight line path by straight line with North orientation angle ξ^ψAngle ξ with straight line and horizontal plane^θDetermine, be denoted as p_l(ξ^ψ,ξ^θ)；Spiral path is initiateed the center of circle [c by path^x, c^y,c^z], radius c^rAnd climb rate c^λDetermine, be denoted as p_o(c^x,c^y,c^z,c^r,c^λ).Described given desired speed is υ_c=[u_c, v_c,w_c]^T=[C, 0,0]^T, C ＞ 0 is constant, u_c,v_c,w_cFor desired speed along the decomposition amount of hull coordinate system.

3. a kind of stratospheric airship path tracking control method based on the guidance of space vector field according to claim 1, It is characterized in that：

The expectation yaw angle ψ needed for error calculating between elimination desired locations and physical location described in step 2^d, the phase Hope yaw angle θ^d, its computational methods are as follows：

Wherein ψ^∞For the initial yaw angle setting, θ^∞For setting Fixed initial pitch angle, d^ψ,d^θThe site error being respectively between body and task path is divided in the projection of horizontal plane and vertical direction Amount；K ＞ 0 is for determining the parameter of direction vector conversion speed in vector field；On agreed assignment path herein away from body distance Near point is desired point P_m, task path is L at the tangent line of desired point, χ^ψFor tangent line L and north orientation angle, χ^θFor tangent line L and level The angle in face；

Space line：d^ψ,d^θCan be by path planning starting point coordinate P_A=[x_Ay_Az_A]^T, body position coordinates P_o=[x₀y₀ z₀]^T, and straight line path and north orientation angle ξ^ψAnd the angle ξ with horizontal plane^θDetermine；χ in such cases^ψ=ξ^ψ,χ^θ=ξ^θ；

Helix：d^ψ,d^θCan be by spiral path p_o(c^x,c^y,c^z,c^r,c^λ), body position coordinates P_o=[x₀y₀z₀]^TDetermine； Now χ^ψ,χ^θCalculated by detailed geometry.

4. a kind of stratospheric airship path tracking control method based on the guidance of space vector field according to claim 1, It is characterized in that：

Path trace yaw angle error described in step 3With tracking angle of pitch errorIts computational methods are as follows：Wherein ψ is the current yaw angle of stratospheric airship, and θ is the current angle of pitch of stratospheric airship.

5. a kind of stratospheric airship path tracking control method based on the guidance of space vector field according to claim 1, It is characterized in that：

Controlled quentity controlled variable U needed for error eliminating between expectation yaw angle and actual yaw angle described in step 4, it calculates Method is as follows：

Drive lacking dirigible kinetic model in this example is：

Wherein：X₁=[x z φ θ ψ]^T,X₂=[u w p q r]^TIt is position and attitude quantity of state and the speed angle speed of dirigible respectively Degree quantity of state, owing to being under-actuated systems, so displacement y does not directly control.R, A, N, B are related coefficient matrixes, U It is and execution unit δ_el,δ_er,δ_r, the related controlled quentity controlled variable of T, γ.

The sliding-mode surface of sliding formwork control is：S=E₁+HE₂, wherein H=diag{h₁,h₂,…,h₅, E₁,E₂It is X respectively₁,X₂The corresponding phase The error of prestige value.

Setting up liapunov function isAnd sliding formwork boundary conditionWherein M, K are equal For diagonal coefficient matrix,

Finally give the accounting equation with regard to controlled quentity controlled variable U

$U=A\&lsqb;{\stackrel{\&CenterDot;}{X}}_{20}-H^{-1}(MS+K\mathrm{sgn}(S)+({\mathrm{RX}}_2-X_{10}\left)\right)\&rsqb;-N$

6. a kind of stratospheric airship path tracking control method based on the guidance of space vector field according to claim 1, It is characterized in that：

Execution unit controlled quentity controlled variable δ realizing needed for sliding-mode control law U described in step 5_el,δ_er,δ_r, T, γ, its calculating side Method is as follows：

[δ_el,δ_er,δ_r,T,γ]^T=B U.