CN104765370A - UUV trajectory sight guiding method with sideslip angle considered under condition of environmental disturbance - Google Patents

Abstract

The invention discloses an unmanned underwater vehicle (UUV) trajectory sight guiding method with the sideslip angle considered under the condition of environmental disturbance and relates to under-actuated UUV trajectory tracking control methods. When a traditional UUV trajectory sight guiding method is used under the condition of environmental disturbance, stable trajectory errors exist. To eliminate the stable trajectory errors, the sideslip angle B=a tan (v/u) is directly introduced to correct the expected heading instruction, the corrected heading expected angle Qd=Bi-a(t)-B is used for adjusting the heading angle, and then the stable trajectory errors can be eliminated and accurate tracking of the UUV trajectory is achieved. The method is suitable for under-actuated UUV trajectory tracking control.

Description

The UUV flight path line of sight guidance method of yaw angle is considered under having environmental interference

Technical field

The present invention relates to drive lacking underwater unmanned vehicle and carry out Trajectory Tracking Control method.

Background technology

Owing to underwater unmanned vehicle (UUV) surface level can only providing longitudinal thrust and turning bow moment, can not lateral thrust be provided, namely laterally directly not controlled, therefore belong to drive lacking UUV.For this class drive lacking UUV, in order to realize accurate tracking tracing control, line of sight guidance method can address this problem very well.The method by viewpoint before design forward sight vector sum, the two-dimentional desired locations point of Track In Track is mapped to one dimension expect bow to, by expect bow to tracking reach the accurate tracking of flight path.For straight-line segment line of sight guidance method, for the waters that a slice is open, the flight path of UUV can be expressed as usually by a series of track points { P ₁..., P _k-1, P _k..., P _nsuccessively be connected form.Suppose that the straight-line segment that certain adjacent two track points is formed is P _k-1p _k, wherein P _k-1and P _kbe respectively starting point and the terminal of this straight-line segment.Here P is defined _kfor the turning point of UUV flight path, namely when UUV current location is in P _kfor the center of circle, with R _acceptfor in the circle shaped neighborhood region of radius time, the desired track of UUV switches to P _kp _k+1straight-line segment, namely now the starting point of UUV is updated to P _k, terminal is updated to P _k+1.Circulation like this, thus the tracking realizing whole desired track.

When there being ocean current to disturb, if the expectation bow directly above-mentioned line of sight method drawn to angle as the expectation bow of control system to, finally can follow the tracks of this expectation bow to, but stable course error can be there is.

Summary of the invention

The problem stablizing course error is there is in the present invention in order to solve existing line of sight guidance method.

Consider the UUV flight path line of sight guidance method of yaw angle under having environmental interference, comprise the steps:

Step one:

The flight path of UUV can be expressed as by a series of track points { P ₁..., P _k-1, P _k..., P _nsuccessively be connected form; Suppose that the straight-line segment that certain adjacent two track points is formed is P _k-1p _k, wherein P _k-1and P _kbe respectively starting point and the terminal of this straight-line segment; Here P is defined _kfor the turning point of UUV flight path, namely when UUV current location is in P _kfor the center of circle, with R _acceptfor in the circle shaped neighborhood region of radius time, the desired track of UUV switches to P _kp _k+1straight-line segment, namely now the starting point of UUV is updated to P _k, terminal is updated to P _k+1;

According to line of sight guidance method parameters relation, obtain bow to expectation angle ψ _d:

ψ _d＝β _i-α(t) (1)

Wherein, β _ifor directed line segment p _k-1p _kwith the angle of north orientation coordinate axis, α (t) is forward sight vector and the angle in path.

Step 2: definition yaw angle β=atan (v/u); According to formula (2), expectation bow is revised to instruction

ψ _d＝β _i-α(t)-β (2)

Wherein, u and v is respectively the transverse velocity of UUV under carrier coordinate system and longitudinal velocity, and longitudinal velocity is the speed that UUV navigates by water along fore direction, and transverse velocity is vertical with longitudinal velocity, and atan (v/u) is arctan function.

The present invention directly introduces yaw angle and revises to instruction expectation bow, not only can be applied to desirable marine environment, even if when there is ocean current interference, accurate bow in real time can be provided equally to angle; Utilize the present invention, in UUV tracking after desired track, stable course error is zero, namely eliminates stable course error, realizes the accurate tracking to UUV flight path.

Accompanying drawing explanation

Fig. 1 is straight-line segment line of sight guidance method schematic diagram;

Fig. 2 is classic method UUV course error simulation result;

Fig. 3 is classic method UUV Track In Track simulation result;

Fig. 4 is UUV course error simulation result of the present invention;

Fig. 5 UUV Track In Track of the present invention simulation result.

Embodiment

Embodiment one: composition graphs 1 illustrates present embodiment, considers the UUV flight path line of sight guidance method of yaw angle, comprises the steps: under having environmental interference

Step one:

For the waters that a slice is open, the flight path of UUV can be expressed as by a series of track points { P ₁..., P _k-1, P _k..., P _nsuccessively be connected form; Suppose that the straight-line segment that certain adjacent two track points is formed is P _k-1p _k, wherein P _k-1and P _kbe respectively starting point and the terminal of this straight-line segment; Here P is defined _kfor the turning point of UUV flight path, namely when UUV current location is in P _kfor the center of circle, with R _acceptfor in the circle shaped neighborhood region of radius time, the desired track of UUV switches to P _kp _k+1straight-line segment, namely now the starting point of UUV is updated to P _k, terminal is updated to P _k+1; Circulation like this, thus the tracking realizing whole desired track;

According to line of sight guidance method parameters relation, obtain bow to expectation angle ψ _d:

ψ _d＝β _i-α(t) (1)

Wherein, β _ifor directed line segment p _k-1p _kwith the angle of north orientation coordinate axis, α (t) is forward sight vector and the angle in path.

Step 2: definition yaw angle β=atan (v/u); According to formula (2), expectation bow is revised to instruction

ψ _d＝β _i-α(t)-β (2)

Wherein, u and v is respectively the transverse velocity of UUV under carrier coordinate system and longitudinal velocity, and longitudinal velocity is the speed that UUV navigates by water along fore direction, and transverse velocity is vertical with longitudinal velocity, and atan (v/u) is arctan function;

Embodiment two: the line of sight guidance method parameters relation described in the step 1 described in present embodiment is as follows:

β _i＝atan2(y _k-y _k-1,x _k-x _k-1)

δ(t)＝β _i-atan2(y _k-y _t,x _k-x _t)

$d\left(t\right)=\sqrt{{(x_k-x_t)}^2+{(y_k-y_t)}^2}$

ε(t)＝d(t)*sin(δ(t))

ψ _d＝β _i-α(t)

Line of sight guidance method principle as shown in Figure 1, β _ifor directed line segment p _k-1p _kwith the angle of north orientation coordinate axis, (x _t, y _t) be the real-time location coordinates of UUV, the real-time bow that ψ (t) is UUV is to angle, the lateral error that ε (t) is path trace, δ (t) is for UUV current location is to the angle in path end points line and path, d (t) is the distance between UUV current location and path end points, and △ is that (△ chooses the UUV length being generally 2 ~ 6 times to the forward sight vector chosen, and too conference causes tracking response slack-off, too little meeting causes system instability), p _los(x _los, y _los) be viewpoint before the generation of sight line guidance method, ψ _dfor bow is to expectation angle, α (t) is forward sight vector and the angle in path.

Other step is identical with embodiment one.

Embodiment

The method is used for the matlab emulation of UUV Track In Track, choosing △ is 4 (UUV length 1.929 meters), arranges initial value (x ₀, y ₀) be (0,0), (v ₀, u ₀) be (0,0).(all the other parameters do not set initial value, are all obtained a result by the derivation of equation, and are real-time update along with UUV travels)

Simulation result is as follows:

Having under ocean current disturbed condition, traditional UUV flight path line of sight guidance method, simulation result is as shown in Fig. 2-Fig. 3;

The track of Fig. 3 cathetus is artificially given desired track, dashed trace is actual flight path, can find out that UUV can follow the tracks of upper desired trajectory from simulation result, but as can be seen from Figure 2 UUV tracking also exist a stable course error after ship trajectory always.

Under having ocean current disturbed condition, line of sight guidance method of the present invention, simulation result is as shown in fig. 4-5;

As can be seen from simulation result, when condition is identical, in simulation result of the present invention, desired track in UUV tracking in Fig. 5; Can find out in Fig. 4, in UUV tracking after desired track, stable course error is zero.

Claims (2)

1. consider the UUV flight path line of sight guidance method of yaw angle under having environmental interference, it is characterized in that it comprises the steps:

Step one:

The flight path of UUV can be expressed as by a series of track points { P ₁..., P _k-1, P _k..., P _nsuccessively be connected form; Suppose that the straight-line segment that certain adjacent two track points is formed is P _k-1p _k, wherein P _k-1and P _kbe respectively starting point and the terminal of this straight-line segment; Here P is defined _kfor the turning point of UUV flight path, namely when UUV current location is in P _kfor the center of circle, with R _acceptfor in the circle shaped neighborhood region of radius time, the desired track of UUV switches to P _kp _k+1straight-line segment, namely now the starting point of UUV is updated to P _k, terminal is updated to P _k+1;

According to line of sight guidance method parameters relation, obtain bow to expectation angle ψ _d:

ψ _d＝β _i-α(t) (1)

Wherein, β _ifor directed line segment p _k-1p _kwith the angle of north orientation coordinate axis, α (t) is forward sight vector and the angle in path;

Step 2: definition yaw angle β=a tan (v/u); According to formula (2), expectation bow is revised to instruction

ψ _d＝β _i-α(t)-β (2)

Wherein, u and v is respectively the transverse velocity of UUV under carrier coordinate system and longitudinal velocity, and longitudinal velocity is the speed that UUV navigates by water along fore direction, and transverse velocity is vertical with longitudinal velocity, and a tan (v/u) is arctan function.

2. according to claim 1 have environmental interference under consider the UUV flight path line of sight guidance method of yaw angle to it is characterized in that the line of sight guidance method parameters relation described in step 1 is as follows:

β _i＝a tan2(y _k-y _k-1,x _k-x _k-1)

δ(t)＝β _i-a tan2(y _k-y _t,x _k-x _t)

$d\left(t\right)=\sqrt{{(x_k-x_t)}^2+{(y_k-y_t)}^2}$

ε(t)＝d(t)*sin(δ(t))

ψ _d＝β _i-α(t)

β _ifor directed line segment p _k-1p _kwith the angle of north orientation coordinate axis, (x _t, y _t) be the real-time location coordinates of UUV, the lateral error that ε (t) is path trace, δ (t) is for UUV current location is to the angle in path end points line and path, d (t) is the distance between UUV current location and path end points, △ is the forward sight vector chosen, ψ _dfor bow is to expectation angle, α (t) is forward sight vector and the angle in path.