CN105807767A - Self-adaption filtering method tracking environmental force sudden change in dynamic positioning - Google Patents

Abstract

The invention relates to a self-adaption filtering method tracking environmental forces sudden change in dynamic positioning. The method includes steps of establishing a state space model of a ship system; using traceless Kalman filtering for estimating a state in the current moment; judging whether the environmental force in the current moment meets sudden change or not according to a judgment condition; performing self-adaption adjustment on posterior mean square error of the current moment if the judgment condition is met, which means environmental force sudden change occurs; judging positioning is completed or not; if the positioning is not finished, sending a thrust command of the next moment by a control system according to a position and the environmental force estimated by the self-adaption filtering so as to enable a ship to reach a positioning point and to hold at the positioning point; if the positioning is finished, finishing the whole cycle. According to the invention, environmental force sudden change can be judged and estimated value convergence to a real value can be realized quickly by modifying a posterior mean square error matrix, so that the controller can generate a thrust for compensating the environmental force and deviation between a real position and a set position can be reduced.

Description

The adaptive filter method of tracking environmental power sudden change in dynamic positioning

Technical field

The present invention relates to dynamic positioning of vessels, in particular to a kind of adaptive filter method of tracking environmental power sudden change in dynamic positioning.

Background technology

Equipped with the boats and ships of dynamic positioning system be the thrust utilizing propulsion system to send to keep self-position, there is good mobility, can promptly into and out mode of operation, there is significantly high safety.In power-positioning control system, the Main Function of filtering and state estimation is the high-frequency noise of elimination measured value, and measured value is separated into low frequency and high frequency two parts, makes propulsion system only offset slowly varying lower frequency external interference as far as possible.It addition, filtering can also estimate the parameters such as speed, the environmental forces that measurement system cannot provide.

Filtering method the most frequently used at present has Kalman filtering, EKF, Unscented kalman filtering, particle filter etc..But wherein Kalman filtering needs linear model boats and ships motion model to be nonlinear.EKF is by nonlinear model linearisation, then is filtered.Although expanded Kalman filtration algorithm is easy, but precision is low.Unscented kalman filtering is to utilize Unscented transform to produce a series of sigma points to be filtered, and does not need nonlinear system is converted into linear system, it is not required that calculate Jacobian matrix.Therefore amount of calculation is little, calculates speed fast, and precision is high, and current Unscented kalman filtering has been applied in a lot of field.Particle filter precision is high, and filtering performance is good, but has many key technologies to wait to solve, and algorithm is computationally intensive, so but without being widely used.

Although filtering algorithm is it is estimated that parameters such as position, speed, environmental forces, but the poor ability of tracking mode value mutation.If environmental forces is undergone mutation, filtering needs to grow very much one period of cycle and just can estimate the environmental forces of sudden change, thus causing that the position of boats and ships produces relatively large deviation.

Summary of the invention

The object of the invention is intended to overcome above-mentioned the deficiencies in the prior art to provide a kind of adaptive filter method of tracking environmental power sudden change in dynamic positioning.

Realizing the object of the invention and employed technical scheme comprise that a kind of adaptive filter method of tracking environmental power sudden change in dynamic positioning, the method comprises the following steps:

A () sets up the state-space model of marine system；

B () uses Unscented kalman filtering to estimate the state of current time；

C by Rule of judgment, () judges whether the environmental forces of current time suddenlys change；

If d () meets Rule of judgment, namely there is environmental forces sudden change, then the posteriority mean square deviation of current time is carried out self-adaptative adjustment；

E () judges whether location terminates；

If f () location is not terminated, position that control system is estimated according to adaptive-filtering and environmental forces send the thrust command of subsequent time, make boats and ships arrival anchor point and are held in position；If location is terminated, then terminate whole circulation.

In technique scheme, in described step (a), system state space model is as follows:

$\stackrel{\·}{x}=A\left(x\right)x+Bu+Ew$

Y=Hx+v

Wherein, A, B, E, H is coefficient matrix, and x is system mode vector, and y is observation vector, and u is control power and moment, and w is process noise, and v is observation noise.

In technique scheme, described step (b) was the state according to a upper moment, uses Unscented kalman filtering to estimate the state of current time.

In technique scheme, the Rule of judgment that in described step (c), whether environmental forces suddenlys change is:

(1) Rule of judgment is Δ=e_k ^T*(P_zk,zk)^-1*e_k, it is judged that the threshold values Δ of condition₀Determining according to error probability p, p represents the Δ > Δ when environmental forces is not undergone mutation₀Probability, p is more little, and the probability judged that means to make a mistake is more low, when Δ > Δ₀In time, is thought and meets Rule of judgment；

(2) a convergent cycle m is set according to the convergence rate of filtering algorithm, if current time suddenlys change the moment more than m cycle from last environmental forces, then it is assumed that meet Rule of judgment.

In technique scheme, when described step (d) meets Rule of judgment, posteriority mean square deviation is carried out following self-adaptative adjustment: when meeting after Rule of judgment (1) and Rule of judgment (2) judge that environmental forces is undergone mutation, according to preset value, the diagonal line value of environmental forces corresponding in posteriority mean square deviation being multiplied by the self adaptive pantographic factor, the self adaptive pantographic factor changes according to the change of current sea situation.

In technique scheme, described step (e) judging, whether current time location terminates, if location is terminated, terminating whole circulation, if not terminating, current time state being passed to control system.

In technique scheme, if location is not terminated in described step (f), position that control system is estimated according to adaptive-filtering and environmental forces send the thrust command of subsequent time, make boats and ships arrival anchor point and are held in position.

The present invention has the advantage that and has the benefit effect that

Under complicated oceanographic condition, environmental forces can be undergone mutation, and the poor ability of existing filtering algorithm tracking environmental power sudden change.The adaptive filter method adopting the tracking environmental power sudden change of present invention proposition can determine that environmental forces suddenlys change and is modified posteriority mean square deviation matrix makes estimated value converge to actual value faster, make controller produce thrust in time to offset environmental forces, reduce the deviation of actual position and setting position.

Accompanying drawing explanation

Fig. 1 is present invention flow chart of the adaptive filter method of tracking environmental power sudden change in dynamic positioning；

Fig. 2 is environmental forces sudden change and the adaptive filter method estimated value comparison diagram of UKF estimated value and the sudden change of tracking environmental power when not disappearing；

The adaptive filter method estimated value comparison diagram of UKF estimated value and the sudden change of tracking environmental power when Fig. 3 is environmental forces sudden change and disappearance；

Fig. 4 is environmental forces sudden change and the low frequency position of UKF, estimation position and observation position when not disappearing；

Fig. 5 be environmental forces sudden change and when not disappearing with the low frequency position of the adaptive filter method of tracking environmental power sudden change, estimate position and observation position；

The low frequency position of UKF, estimation position and observation position when Fig. 6 is environmental forces sudden change and disappearance；

With the low frequency position of the adaptive filter method of tracking environmental power sudden change when Fig. 7 is environmental forces sudden change and disappearance, estimate position and observation position；

Detailed description of the invention

Below in conjunction with the drawings and specific embodiments, the present invention is described in further detail.

The present embodiment is in conjunction with Unscented kalman filtering algorithm, it is proposed to tracking environmental power sudden change adaptive filter method.The adaptive filter method flow process suddenlyd change in conjunction with the tracking environmental power of Unscented kalman filtering is as it is shown in figure 1, to be embodied as step as follows:

S100, the system model setting up boats and ships are as follows:

$\stackrel{\·}{\mathrm{\ξ}}=A_w\mathrm{\ξ}+E_w{\mathrm{\ω}}_w$

$\stackrel{\·}{\mathrm{\η}}=R\left(\mathrm{\ψ}\right)v$

$M\stackrel{\·}{v}=-Dv+\mathrm{\τ}+R^T\left(\mathrm{\ψ}\right)b$

$\stackrel{\·}{b}=-T^{-1}b+E_b{\mathrm{\ω}}_b$

Y=η+η_ω+v_y

Wherein, ξ is boats and ships high frequency state vector, and η is boats and ships low frequency position vectors, and v is boats and ships low-frequency velocity vector, R (ψ) is coordinate conversion matrix, D is damping matrix, and M is inertial matrix, and τ is control power and moment, b represents environmental forces and moment, T is time constant matrix, and y represents that vessel position measures vector, η_ωFor high frequency position vector, ω_w, ω_b, v_yFor white Gaussian noise, E_w, E_bFor noise coefficient matrix.

The state-space model of boats and ships can be obtained by system model:

$\stackrel{\·}{x}=A\left(x\right)x+Bu+Ew$

Y=Hx+v

Wherein, x is system mode vector, and y is observation vector, and u is for controlling input, and v is observation noise,

$x=\left[\begin{array}{c}\mathrm{\ξ}\\ \mathrm{\η}\\ v\\ b\end{array}\right],A\left(x\right)=\left[\begin{array}{cccc}{A}_w& 0& 0& 0\\ 0& 0& R\left(\mathrm{\ψ}\right)& 0\\ 0& 0& -M^{-1}D& M^{-1}{R}^T\left(\mathrm{\ψ}\right)\\ 0& 0& 0& -T^{-1}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ M^{-1}\\ 0\end{array}\right]$

$H=\left[\begin{array}{cccc}{C}_w& I& 0& 0\end{array}\right],E=\left[\begin{array}{cccc}{E}_w& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& E_b\end{array}\right],\mathrm{\ω}=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{\ω}}\\ 0\\ 0\\ {\mathrm{\ω}}_{\mathrm{\β}}\end{array}\right].$

S200, use self adaptation Unscented kalman filtering carry out state estimation and obtain current time state, specifically comprise the following steps that

S201, initialization: given initial state value and posteriority mean square deviation matrix；

The weights of S202, calculating sigma point and correspondence:

Wherein n is the dimension of state vector x, λ=ξ²(n+ κ)-n is scale factor, and ξ is the span of sampled point, κ=0, η=2.

S203, time update:

Wherein f () represents non-linear process, Q_k∈R^n*nIt it is process noise covariance matrix.

S204, measurement updaue:

$\left\{\begin{array}{c}{Z}_{i,k|k-1}=H*X_{i,k|k-1}\\ {{\hat{Z}}_k}^{-}=\underset{i=0}{\overset{2n}{\Σ}}{w_i}^{\left(m\right)}{Z}_{i,k|k-1}\\ P_{z_k,z_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w_i}^{\left(c\right)}\[Z_{i,k|k-1}-{{\hat{Z}}_k}^{-}\]{\[Z_{i,k|k-1}-{{\hat{Z}}_k}^{-}\]}^T+R\\ P_{x_k,z_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w_i}^{\left(c\right)}\[X_{i,k|k-1}-{{\hat{x}}_k}^{-}\]{\[Z_{i,k|k-1}-{{\hat{Z}}_k}^{-}\]}^T\\ K_k=P_{xk,zk}*{P^{-1}}_{Zk,Zk}\\ {\hat{x}}_k={{\hat{x}}_k}^{-}+K_k(Z_k-{Z_k}^{-})\\ P_k={P_k}^{-}-K_k*P_{zk,zk}*{K_k}^T\end{array}\right.$

Wherein Z_kIt is the observation in k moment, R ∈ R^3*3It it is observation noise variance matrix.

S300, Rule of judgment one is set

When filtering estimated value and converging on actual value, the poor e of observation and priori estimates_kWith its covariance matrix P_zk,zkCan be more and more less.Wherein e_k=Z_k-Z_k ^-,Z_kFor the observation in k moment, Z_k ^-For the prior estimate in k moment, Z_i,k|k-1For the estimated value of i-th sigma point, w_iFor the weights that i-th sigma point is corresponding.But the two value can be made to increase when there being state value to undergo mutation, so set Rule of judgment as Δ=e_k ^T*(P_zk,zk)^-1*e_k, wherein e_k=Z_k-Z_k ^-Represent the difference of observation and priori estimates, P_zk,zkIt it is error co-variance matrix.The threshold values Δ of Rule of judgment₀Determining according to error probability p, p represents the Δ > Δ when environmental forces is not undergone mutation₀Probability, p is more little, and the probability judged that means to make a mistake is more low.When Δ > Δ₀In time, is thought and meets Rule of judgment one.

The threshold values Δ of Rule of judgment₀Obtaining value method: in order to reduce the number of times of false judgment as far as possible, error probability p=0.001 is set.Therefore take environmental forces constant time 1000 cycles in Δ₀Maximum as threshold values.This ship model takes Δ₀=8.3.

S400, Rule of judgment two is set

The setting of Rule of judgment two is mainly in view of the estimated value of observer needs certain cycle to converge on actual value, in order to prevent being process multiple times in the posteriority mean square deviation short time, convergence rate according to Unscented kalman filtering sets a convergent cycle m, if current time suddenlys change the moment more than m cycle from last environmental forces, then it is assumed that meet Rule of judgment two.

The present embodiment sets 100 cycles as convergent cycle, and within this cycle, estimated value is constantly close to actual value.The judgement whether environmental forces is suddenlyd change: think that when meeting Rule of judgment one and Rule of judgment two environmental forces produces sudden change.

S500, adaptive correction to posteriority mean square deviation

When meeting after Rule of judgment judges that environmental forces undergos mutation again, suitably adjust the value of corresponding environmental forces in posteriority mean square deviation and make the sudden change of the more effective tracking environmental power of observer.In this model, the value of corresponding environmental forces is three-dimensional cornerwise value after posteriority mean square deviation, and the self-adaptative adjustment factor is determined according to current sea situation, and this ship model is assumed to 20.

S600, judge location whether terminate

If location is terminated, then terminate whole circulation；If location is not terminated, position that control system is estimated according to adaptive-filtering and environmental forces send the thrust command of subsequent time, make boats and ships arrival anchor point and are held in position.

Control system used by the present invention is a kind of closed loop system, and it is by constantly detecting physical location of boats and ships and the deviation of target location, calculates, further according to the impact of the external disturbance power such as external wind, wave, stream, boats and ships of sening as an envoy to and returns to the size of target location required thrust.

Emulation experiment: consider that environmental forces sometimes can continue a very long time and sometimes only can continue the time one very short, two kinds of situations are designed: situation one is that environmental forces sports [5 when the 400th cycle from [0,0,0] at this,-5,5] and last till that emulation terminates；Situation two is that environmental forces sports [5 ,-5,5] and the 700th disappearance of periodicity when the 400th cycle from [0,0,0].Emulation carries out on MATLAB platform, and the initial position of boats and ships be [0,0,0] and in the 300th cycle arrival setting position [5,5,15 °].Time step is 0.5 second, emulation experiment with 1:20 ratio in the ship model of a platform supply vessel for object, the relevant principal dimensions data of ship model are as shown in table 1:

Table 1 ship model parameter

The inertial matrix of ship model and damping matrix be:

$M=\left[\begin{array}{ccc}748.7& 0& 0\\ 0& 189.1& 93.8\\ 0& 39.6& 660.4\end{array}\right],D=\left[\begin{array}{ccc}12.3& 0& 0\\ 0& 59.7& 3\\ 0& 3& 7.1\end{array}\right].$

Assume that process noise matrix and observation noise matrix are: Q=diag (0,0,0,0.01,0.01,0.0001,0,0,0,0,0,0,0.0001,0.0001,0.0001), R=diag (0.001,0.001,0.0001).Time constant matrix T=diag (1000,1000,500).It addition, leading wave frequency, relative damping ratio and intensity of wave are respectively as follows: ω₀₁=ω₀₂=ω₀₃=0.8, ζ₁=ζ₂=ζ₃=0.3, σ₁=σ₂=σ₃=1.PID controller is adopted to produce the control power in each cycle.

Fig. 2, Fig. 3 are the estimated value comparison diagram of the adaptive filter method (AUKF) in situation one and two times tracking environmental power sudden changes of situation and the environmental forces of Unscented kalman filtering (UKF).

Fig. 4, Fig. 5 be situation once with tracking environmental power sudden change adaptive filter method (AUKF) and with the ship observation position of Unscented kalman filtering (UKF), low frequency position and estimation position comparison diagram.

Fig. 6, Fig. 7 are at two times adaptive filter methods (AUKF) with the sudden change of tracking environmental power of situation and the comparison diagram with the ship observation position of Unscented kalman filtering (UKF), low frequency position and estimation position.

From simulation result it can be seen that the adaptive filter method of tracking environmental power sudden change can the sudden change of more effective tracking environmental power, so the deviation ratio Unscented kalman filtering of the ship model low frequency position estimated by the method obvious little a lot.

Claims (7)

1. the adaptive filter method of tracking environmental power sudden change in dynamic positioning, it is characterised in that comprise the following steps:

A () sets up the state-space model of marine system；

B () uses Unscented kalman filtering to estimate the state of current time；

C by Rule of judgment, () judges whether the environmental forces of current time suddenlys change；

If d () meets Rule of judgment, namely there is environmental forces sudden change, then the posteriority mean square deviation of current time is carried out self-adaptative adjustment；

E () judges whether location terminates；

If f () location is not terminated, position that control system is estimated according to adaptive-filtering and environmental forces send the thrust command of subsequent time, make boats and ships arrival anchor point and are held in position；If location is terminated, then terminate whole circulation.

2. the adaptive filter method that tracking environmental power is suddenlyd change in dynamic positioning according to claim 1, it is characterised in that in described step (a), system state space model is as follows:

$\stackrel{\·}{x}=A\left(x\right)x+Bu+Ew$

Y=Hx+v

Wherein, A, B, E, H is coefficient matrix, and x is system mode vector, and y is observation vector, and u is control power and moment, and w is process noise, and v is observation noise.

3. the adaptive filter method that tracking environmental power is suddenlyd change in dynamic positioning according to claim 1, it is characterised in that: described step (b) was the state according to a upper moment, and used Unscented kalman filtering to estimate the state of current time.

4. the adaptive filter method that tracking environmental power is suddenlyd change in dynamic positioning according to claim 1, it is characterised in that the Rule of judgment that in described step (c), whether environmental forces suddenlys change is:

(1) judge index is Δ=e_k ^T*(P_zk,zk)^-1*e_k, it is judged that the threshold values Δ of condition₀Determining according to error probability p, p represents the Δ > Δ when environmental forces is not undergone mutation₀Probability, p is more little, and the probability judged that means to make a mistake is more low, when Δ > Δ₀In time, is thought and meets Rule of judgment；

(2) a convergent cycle m is set according to the convergence rate of filtering algorithm, if current time suddenlys change the moment more than m cycle from last environmental forces, then it is assumed that meet Rule of judgment.

5. the adaptive filter method that tracking environmental power is suddenlyd change in dynamic positioning according to claim 4, it is characterized in that: when described step (d) meets Rule of judgment, posteriority mean square deviation is carried out following self-adaptative adjustment: when meeting after Rule of judgment (1) and Rule of judgment (2) judge that environmental forces is undergone mutation, according to preset value, the diagonal line value of environmental forces corresponding in posteriority mean square deviation being multiplied by the self adaptive pantographic factor, the self adaptive pantographic factor changes according to the change of current sea situation.

6. the adaptive filter method that tracking environmental power is suddenlyd change in dynamic positioning according to claim 1, it is characterized in that: described step (e) judging, whether current time location terminates, if location is terminated, terminating whole circulation, if not terminating, current time state being passed to control system.

7. the adaptive filter method that tracking environmental power is suddenlyd change in dynamic positioning according to claim 1, it is characterized in that: if location is not terminated in described step (f), position that control system is estimated according to adaptive-filtering and environmental forces send the thrust command of subsequent time, make boats and ships arrive anchor point and be held in position.