CN104635773B - A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer - Google Patents

A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer Download PDF

Info

Publication number
CN104635773B
CN104635773B CN201510016643.XA CN201510016643A CN104635773B CN 104635773 B CN104635773 B CN 104635773B CN 201510016643 A CN201510016643 A CN 201510016643A CN 104635773 B CN104635773 B CN 104635773B
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
mover
mtd
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201510016643.XA
Other languages
Chinese (zh)
Other versions
CN104635773A (en
Inventor
王元慧
庹玉龙
付明玉
丁福光
王海英
赵强
张赞
杨云龙
迟岑
刘佳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201510016643.XA priority Critical patent/CN104635773B/en
Publication of CN104635773A publication Critical patent/CN104635773A/en
Application granted granted Critical
Publication of CN104635773B publication Critical patent/CN104635773B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Feedback Control In General (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a kind of based on the dynamic localization method for ship for improving Strong tracking filter state observer, comprise the following steps：Gather ship position and bow to；Vessel position and bow are filtered out to middle noise section using Strong tracking filter state observer is improved, and the vessel position and bow for being met required precision are sent to PID controller to estimate；PID controller is moved according to vessel position and bow to desired value and the information received, output control power and torque, control ship.Present invention tracking filter effect in the case where external interference is undergone mutation is more preferable, and the present invention can improve the stability of ship.

Description

A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer
Technical field
The invention belongs to dynamic positioning ship control field, more particularly to it is a kind of be directed under external environment catastrophe, A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer.
Background technology
Ocean is carried out fully to explore and study, carries out reasonable and effective resource exploitation, the development to human society Play immeasurable effect.As marine resources exploitation is constantly promoted to deep-sea, some present offshore work platforms, than Such as floating production storage unit, marine drilling platform, although can still complete oil-gas mining, pipe laying paving cable, Marine Geology exploration Deng, but it is undesirable to consider effect.In this context, Ship Dynamic Positioning Systems Based (Dynamic Positioning System, abbreviation DPS) come into being.In recent years, dynamic positioning system plays the dominant right for obtaining marine resources critical Effect.Ship Dynamic Positioning Systems Based be it is a kind of by propulsion system produce thrust with resist the external world environmental disturbances (wind, wave and Ocean current) so that dynamic control ship's fix is in certain definite position or the technology navigated by water according to certain default course line.
The ship at sea moved, can be disturbed be subject to wind, wave, stream and various noises.In these interference, wind, low frequency two Rank wave, stream can make the slow drift that ship occurs, and can compensate counteracting by the propeller of dynamic positioning；High frequency one Rank wave and noise jamming, will not cause the change of vessel position, but can cause hunting campaign, increase dynamic positioning The abrasion of the propeller of system, so as to reduce the service life of propeller.The state observer of design, exactly by high frequency single order Wave and noise filtering, respond interference of the propeller only to low frequency.Improvement Strong tracking filter employed in this patent Algorithm is for other filtering algorithms, and in the case where external interference is undergone mutation, tracking filter effect is more preferable.
The content of the invention
Stability of ship can be improved the object of the present invention is to provide a kind of, one kind is based on improvement Strong tracking filter state The dynamic localization method for ship of observer.
The present invention is achieved by the following technical solutions：
A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer, including following steps：
Step 1：Gather ship position and bow to；
Step 2：Vessel position and bow are filtered out to middle noise section using Strong tracking filter state observer is improved, and are obtained To the vessel position and bow for meeting required precision PID controller is sent to estimate；
Step 3：PID controller according to vessel position and bow to desired value and the information received, output control power and power Square, control ship movement.
The present invention is a kind of based on the dynamic localization method for ship for improving Strong tracking filter state observer, can also include：
1st, it is using the method for improving Strong tracking filter state observer and filtering out vessel position and bow to middle noise section：
Step 1：Establish the state space equation of ship motion mathematical model：
Z=h (x, v)
Wherein, f is the non-linear system status Jacobian matrix of marine system, and h is that nonlinear system measures function, x=[ηT νT]TIt is selected state vector, η=[x y ψ]TRepresent position and the attitude angle of the ship under east northeast coordinate system, ν=[u v r]TRepresent the linear velocity and angular speed of the ship under hull coordinate system, u=[X Y N]TIt is that input vector represents to sit in hull The power and torque suffered by ship under mark system, z are that output vector represents the ship for including noise section under east northeast coordinate system Position and bow are to w=wvAnd v=wzFor orthogonal zero mean Gaussian white noise,
The discretization of state space equation is expressed as：
Step 2：By the vessel position comprising noise section and bow to zkAs improvement Strong tracking filter state observer Input, be met required precision vessel position and bow to estimate
2nd, Strong tracking filter state observer is improved to realize by following design procedure：
A, state initial value is determined, error covariance initial value p0, forgetting factor ρ, reduction factor-beta and constant αi(i),i =1,2 ..., n；
B, residual error is calculated：
Wherein, HkThe obtained refined lattice of Taylor expansion are carried out than matrix for function h,
C, multiple suboptimum fading factor matrix and matrix are calculated
Determine residual covariance battle array Vk：
Wherein, ρ is forgetting factor set in advance；
Try to achieve multiple suboptimum fading factor matrix λk：
Wherein αiFor constant, coefficient
Wherein, QkAnd RkThe respectively variance of environmental disturbances high-frequency noise and measurement noise, β for reduction set in advance because Son, pkFor estimation error covariance, ΦkAnd ΓkRefined lattice obtained by Taylor expansion are carried out than matrix for function f,
Matrix is obtained according to multiple suboptimum fading factor Matrix Calculating
D, according to matrixCalculate state forecast error covariance pk|k-1：
E, according to state forecast error covariance pk|k-1Calculate filtering gain kk
F, state estimation is updated
G, more news autonomy covariance pk
Preserve filtering data resultWillInAs the vessel position and bow met the requirements to estimatePass Give PID controller, repeat step b to step h, until data processing terminates.
3rd, PID controller is：
τ=[τX τY τN]TRepresent marine propeller thrust output and torque, KP, KIAnd KDFor the ratio system in pid algorithm Number；ηeFor ship expectation target position and attitude ηdDeviation between physical location posture η, i.e. ηe=ηd- η, η=[x, y, ψ]TPoint The actual east northeast position of ship and bow are not corresponded to angle；ηd=[xd,yd,ψd]TCorresponding to ship respectively it is expected east northeast position and bow to angle. Beneficial effect：
Improvement strong tracking filter employed in this patent is for other filtering algorithms, in external interference Tracking filter effect is more preferable in the case of undergoing mutation.Extended Kalman filter (Extended Kalman Filter, referred to as EKF) when model is not known, poor robustness；And when filtering reaches stable state, the gain matrix K of EKFkFor minimum, if System is undergone mutation, and is allowed for the state that EKF is unable to tracking system, is lost the ability of tracking to mutation status.Strong tracking is filtered Ripple (Strong Tracking Filter, abbreviation STF) algorithm is based on orthogonality principle, and time-varying is introduced in prediction error conariance Fading factor λk, to weaken influence of the former appearance measurement to current filter estimate, there is very strong ability of tracking to mutation status.
When with the Strong tracking filter of multiple suboptimum fading factor, λkIt is a diagonal matrix, diagonal element λiIt is most In the case of be not completely equivalent, so λiIt is multiplied by that positively definite matrix is obtained to be not necessarily positively definite matrix, then final state is pre- It is not just positively definite matrix to report error covariance, and filtering divergence is likely to result under serious conditions.For this phenomenon, quote and improve Strong tracking filter (Improved Strong Track Filter, ISTF), decomposes with the trigonometric ratio of Cholesky and thinks Think, change the mode of action of the multiple time-varying fading factor in state forecast error covariance formula so that the present invention has more Good stability.
Brief description of the drawings
Fig. 1 is the method for the present invention flow chart；
Fig. 2 is present system block diagram.
Embodiment
The present invention is described in further details below in conjunction with attached drawing.
Attached drawing 1 is the motion control flow chart for using the ship based on the state observer for improving Strong tracking filter method, Describe the processing procedure of ship motion controller device.
The purpose of the present invention is realized as follows：
Step 1：In the case where there are environmental disturbances, the position of dynamic positioning ship and bow are gathered by sensing system To information.Dynamics and the Kinematic process that ship moves can be taken out to information by these positions and bow, wherein dynamic Power positioning ship kinematics model be：
Z=η+ωz
Wherein, η=[x y ψ]TRepresent position and the attitude angle of the ship under east northeast coordinate system；ν=[u v r]TRepresent The linear velocity and angular speed of ship under hull coordinate system；U=[X Y N]TIt is input vector, represents under hull coordinate system Ship suffered by power and torque；τ=[X Y N]TRepresent the power and torque suffered by the ship under hull coordinate system；J (η) is Coordinate conversion matrix, realizes the linear velocity of hull coordinate system and the position derivative and posture angular derivative of angular speed and east northeast coordinate system Mutually convert；M is system inertia matrix, and C (ν) is Coriolis centripetal force matrix, and D (ν) is damped coefficient matrix；ωvWith ωzIt is zero mean Gaussian white noise, represents measurement error, EνIt is ωvCorresponding coefficient matrix.
For convenience of the design of state observer in step 2, the canonical form of state space equation is turned to, such as following formula institute Show：
Z=h (x, v)
In above formula, f is the non-linear system status Jacobian matrix of system, and h is that nonlinear system measures function.X=[ηT νT]TIt is selected state vector, z is output vector, and the position of the corresponding ship under east northeast coordinate system and bow are to (including ring Noise section caused by border interference and measurement noise)；W and v is orthogonal zero mean Gaussian white noise, represents ring respectively Border is disturbed and error in measurement.
Sliding-model control is carried out to above-mentioned state space equation, following formula can be obtained：
xk=fk-1(xk-1,uk-1,wk-1)
zk=hk(xk,vk)
Wherein wkAnd vkStatistical property it is as follows：
Wherein, δklExpression formula is：
So, zkBe exactly the corresponding ship under east northeast coordinate system position and bow to (including environmental disturbances and measurement Noise section caused by noise), inputted as being filtered in step 2.
Step 2：Utilize the radio-frequency head filtered out based on the state observer for improving Strong tracking filter method in wave interference Divide the measurement noise produced with measurement sensor in measurement vessel position and bow during, meet required precision by what is estimated Ship position and bow be sent to state feedback controller to information.Based on the state observer for improving strong tracking filter Realized by following design procedure：
1. determine initial value
Determine state initial valueError covariance initial value p0, forgetting factor ρ, weakens factor-beta, and empirical αi(i) (i=1,2 ..., n).
2. calculate residual error
Residual computations formula is as follows
Wherein, HkThe obtained refined lattice of Taylor expansion are carried out than matrix for function h,And HkSpecific formula for calculation such as Under：
3. calculate multiple suboptimum fading factor matrix λkWith
1) residual covariance battle array V is determinedk, formula is as follows
Wherein, ρ is predetermined forgetting factor.
2) first calculateMkAnd Nk, so that design factor ck。
Wherein, Qk,RkFor environmental disturbances high-frequency noise and the variance of measurement noise, β is the predetermined reduction factor, pk For estimation error covariance, 7. specific formula is shown in；ΦkAnd ΓkBe that function f carries out refined lattice obtained by Taylor expansion than matrix, tool Body calculation formula is as follows：
3) according to predetermined empirical αi, try to achieve multiple suboptimum fading factor matrix λk。
4) according to λkObtain
4. calculate state forecast error covariance pk|k-1。
5. calculate filtering gain Kk。
6. update state estimation
7. more news autonomy covariance pk。
8. preserve filtering data resultBecause the state variable x=[η chosenT νT]T, soInIt is exactly me The required ship met the requirements movement position and bow to informationIt is fed back to state feedback controller.
Circulate above-mentioned steps 2. -7., constantly update state estimationWith estimation error covariance pk, it is possible to realization pair Noise in the position and attitude information of dynamic positioning vessel filters out.
Step 3：PID controller is designed, adjusts corresponding parameter, and draws corresponding controling power, controls the movement of ship.
Using very basic and very useful PID controller in the present invention, as shown in Figure 2.PID controller robust Property is very strong, less sensitive to the characteristic variations of controlled device.In PID control, proportional component, integral element and differentiation element Control is interactional, and the quality that its parameter is chosen directly affects the control effect of PID controller.The effect of each link is such as Under：
The adjustment of proportional component (Kp), the proportional generation control action of controller, reaches the effect for reducing deviation；
The adjustment of integral element (Ki), to eliminate static error, integral action cannot be adjusted too strong, can led main function Cause system is unstable；
The adjustment of differentiation element (Kd), influences the dynamic characteristic of system, introduces revise signal in advance in fact, accelerates system Regulate the speed.
Three links in PID controller reasonably choose the Different Effects of system and mutual influence The parameter of PID.
PID controller is represented by formula：
In formula, τ=[τX τY τN]TRepresent marine propeller thrust output and torque, KP, KIAnd KDFor in pid algorithm Proportionality coefficient；ηeFor ship expectation target position and attitude ηdDeviation between physical location posture η, i.e. ηe=ηd-η.Wherein, η =[x, y, ψ]T, the actual east northeast position of ship and bow are corresponded to respectively to angle；ηd=[xd,yd,ψd]T, ship is corresponded to respectively it is expected north Eastern position and bow are to angle.

Claims (2)

  1. It is 1. a kind of based on the dynamic localization method for ship for improving Strong tracking filter state observer, it is characterised in that including following Several steps：
    Step 1：Gather ship position and bow to；
    Step 2：Vessel position and bow are filtered out to middle noise section using Strong tracking filter state observer is improved, and are expired The vessel position and bow of sufficient required precision are sent to PID controller to estimate；
    Step 3：PID controller according to vessel position and bow to desired value and the information received, output control power and torque, Control ship movement；
    The utilization, which improves Strong tracking filter state observer and filters out the method for vessel position and from bow to middle noise section, is：
    Step 1：Establish the state space equation of ship motion mathematical model：
    <mrow> <mover> <mi>x</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>u</mi> <mo>,</mo> <mi>w</mi> <mo>)</mo> </mrow> </mrow>
    Z=h (x, v)
    Wherein, f is the non-linear system status Jacobian matrix of marine system, and h is that nonlinear system measures function, x=[ηT ν'T ]TIt is selected state vector, η=[ηx ηy ηψ]TRepresent position and the attitude angle of the ship under east northeast coordinate system, ν '=[U V R]TRepresent the linear velocity and angular speed of the ship under hull coordinate system, u=[X Y N]TIt is that input vector is represented in hull The power and torque suffered by ship under coordinate system, z are that output vector represents the ship for including noise section under east northeast coordinate system Berth is put with bow to w=wvAnd v=wzFor orthogonal zero mean Gaussian white noise,
    The discretization of state space equation is expressed as：
    <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>f</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>u</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>h</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mrow> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>v</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>
    Step 2：By the vessel position comprising noise section and bow to zkAs improve Strong tracking filter state observer input, Be met required precision vessel position and bow to estimate
    The improvement Strong tracking filter state observer is realized by following design procedure：
    A, state initial value is determinedError covariance initial value p0, forgetting factor ρ, reduction factor-beta and constant αi(i), i=1, 2,…,n；
    B, residual error is calculated：
    <mrow> <msub> <mi>S</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
    Wherein, HkThe obtained refined lattice of Taylor expansion are carried out than matrix for function h,
    <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>u</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>h</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>v</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </msub> <mo>;</mo> </mrow>
    C, multiple suboptimum fading factor matrix and matrix are calculated
    Determine residual covariance battle array Vk：
    <mrow> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;epsiv;</mi> <mn>1</mn> </msub> <msubsup> <mi>&amp;epsiv;</mi> <mn>1</mn> <mi>T</mi> </msubsup> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mrow> <msub> <mi>&amp;rho;V</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;epsiv;</mi> <mi>k</mi> </msub> <msubsup> <mi>&amp;epsiv;</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mi>&amp;rho;</mi> </mrow> </mfrac> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>k</mi> <mo>&amp;GreaterEqual;</mo> <mn>2</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
    Wherein, ρ is forgetting factor set in advance；
    Try to achieve multiple suboptimum fading factor matrix λk：
    <mrow> <msubsup> <mi>&amp;lambda;</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;alpha;</mi> <mi>i</mi> </msub> <msub> <mi>c</mi> <mi>k</mi> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;alpha;</mi> <mi>i</mi> </msub> <msub> <mi>c</mi> <mi>k</mi> </msub> <mo>&amp;GreaterEqual;</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>1</mn> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;alpha;</mi> <mi>i</mi> </msub> <msub> <mi>c</mi> <mi>k</mi> </msub> <mo>&lt;</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
    Wherein αiFor constant, coefficient
    <mrow> <mover> <mi>A</mi> <mo>&amp;OverBar;</mo> </mover> <mo>=</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mo>&amp;lsqb;</mo> <msqrt> <msub> <mi>&amp;alpha;</mi> <mn>1</mn> </msub> </msqrt> <mo>,</mo> <msqrt> <msub> <mi>&amp;alpha;</mi> <mn>2</mn> </msub> </msqrt> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msqrt> <msub> <mi>&amp;alpha;</mi> <mi>n</mi> </msub> </msqrt> <mo>&amp;rsqb;</mo> <mo>,</mo> </mrow>
    <mrow> <msub> <mi>N</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&amp;Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&amp;Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&amp;beta;R</mi> <mi>k</mi> </msub> </mrow>
    <mrow> <msub> <mi>M</mi> <mi>k</mi> </msub> <mo>=</mo> <mover> <mi>A</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow>
    Wherein, QkAnd RkThe respectively variance of environmental disturbances high-frequency noise and measurement noise, β are the reduction factor set in advance, pk For estimation error covariance, ΦkAnd ΓkRefined lattice obtained by Taylor expansion are carried out than matrix for function f,
    <mrow> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>f</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>u</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </msub> </mrow>
    <mrow> <msub> <mi>&amp;Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>f</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>u</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </msub> <mo>,</mo> </mrow>
    Matrix is obtained according to multiple suboptimum fading factor Matrix Calculating
    <mrow> <msub> <mover> <mi>&amp;Lambda;</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mo>&amp;lsqb;</mo> <msqrt> <msubsup> <mi>&amp;lambda;</mi> <mi>k</mi> <mn>1</mn> </msubsup> </msqrt> <mo>,</mo> <msqrt> <msubsup> <mi>&amp;lambda;</mi> <mi>k</mi> <mn>2</mn> </msubsup> </msqrt> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msqrt> <msubsup> <mi>&amp;lambda;</mi> <mi>k</mi> <mi>n</mi> </msubsup> </msqrt> <mo>&amp;rsqb;</mo> <mo>;</mo> </mrow>
    D, according to matrixCalculate state forecast error covariance pkk-1：
    <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>&amp;Lambda;</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <msubsup> <mover> <mi>&amp;Lambda;</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>&amp;Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&amp;Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>;</mo> </mrow>
    E, according to state forecast error covariance pkk-1Calculate filtering gain kk
    <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>;</mo> </mrow>
    F, state estimation is updated
    <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
    G, more news autonomy covariance pk
    <mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>R</mi> <mi>k</mi> </msub> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>;</mo> </mrow>
    H, filtering data result is preservedWillInAs the vessel position and bow met the requirements to estimateSend to PID controller, repeat step b to step h, until data processing terminates.
  2. 2. a kind of dynamic localization method for ship based on improvement Strong tracking filter state observer according to claim 1, It is characterized in that：The PID controller is：
    <mrow> <mi>&amp;tau;</mi> <mo>=</mo> <msub> <mi>K</mi> <mi>P</mi> </msub> <msub> <mi>&amp;eta;</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>I</mi> </msub> <mo>&amp;Integral;</mo> <msub> <mi>&amp;eta;</mi> <mi>e</mi> </msub> <mi>d</mi> <mi>t</mi> <mo>+</mo> <msub> <mi>K</mi> <mi>D</mi> </msub> <mfrac> <mrow> <msub> <mi>d&amp;eta;</mi> <mi>e</mi> </msub> </mrow> <mrow> <mi>d</mi> <mi>t</mi> </mrow> </mfrac> </mrow>
    τ=[τX τY τN]TRepresent marine propeller thrust output and torque, KP, KIAnd KDFor the proportionality coefficient in pid algorithm；ηe For ship expectation target position and attitude ηdDeviation between physical location posture η, i.e. ηe=ηd- η, η=[ηX,ηY,ηψ]TRespectively The position of ship under corresponding northeast coordinate system and attitude angle；ηd=[ηXd,ηYd,ηψd]TShip is corresponded to respectively it is expected east northeast position And attitude angle.
CN201510016643.XA 2015-01-13 2015-01-13 A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer Active CN104635773B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510016643.XA CN104635773B (en) 2015-01-13 2015-01-13 A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510016643.XA CN104635773B (en) 2015-01-13 2015-01-13 A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer

Publications (2)

Publication Number Publication Date
CN104635773A CN104635773A (en) 2015-05-20
CN104635773B true CN104635773B (en) 2018-04-17

Family

ID=53214639

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510016643.XA Active CN104635773B (en) 2015-01-13 2015-01-13 A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer

Country Status (1)

Country Link
CN (1) CN104635773B (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106338919B (en) * 2016-11-02 2019-04-19 哈尔滨工程大学 Unmanned boat Track In Track control method based on enhancing learning type intellectual algorithm
CN108195400A (en) * 2017-12-22 2018-06-22 清华大学 The moving alignment method of strapdown micro electro mechanical inertia navigation system
CN108388738A (en) * 2018-03-01 2018-08-10 青岛科技大学 A kind of Ship Dynamic Positioning Systems Based noise and state real-time estimation adaptive filter method
CN109143850A (en) * 2018-07-05 2019-01-04 江苏理工学院 Rail vehicle suspension system method for parameter estimation based on strong tracking filter
CN108983774B (en) * 2018-07-16 2021-06-01 哈尔滨工程大学 Single-jet-pump-propelled unmanned surface vehicle self-adaptive course control method based on fuzzy state observer
CN110456631B (en) * 2019-08-19 2022-12-09 哈尔滨工业大学 Planet detection capture brake and device separation physical tracking device and method
CN111469999A (en) * 2020-04-29 2020-07-31 上海交通大学 Dynamic positioning model test control method and system based on wave feedforward
CN113296396B (en) * 2021-05-26 2022-06-03 广东电网有限责任公司 Automatic tracking system and method for high-frequency noise power gain
CN116753961B (en) * 2023-08-16 2023-10-31 中国船舶集团有限公司第七〇七研究所 Dynamic positioning ship high-speed tracking navigation method based on state observation

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102411657A (en) * 2011-10-31 2012-04-11 江苏科技大学 Designing method for filter for locating dynamics of towing suction dredger in free sailing state
CN103605886A (en) * 2013-11-12 2014-02-26 中交天津航道局有限公司 Multi-model self-adaptive fusion filtering method of ship dynamic positioning system
KR20140056887A (en) * 2012-11-02 2014-05-12 대우조선해양 주식회사 Dynamic positioning system considering draft variation and dynamic positioning method of the same
CN103970021A (en) * 2014-05-21 2014-08-06 哈尔滨工程大学 Relaxation power positioning control system based on model prediction control
CN104199464A (en) * 2014-08-19 2014-12-10 哈尔滨工程大学 Real-time environment optimum heading ship dynamic positioning control method based on environment estimation

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102411657A (en) * 2011-10-31 2012-04-11 江苏科技大学 Designing method for filter for locating dynamics of towing suction dredger in free sailing state
KR20140056887A (en) * 2012-11-02 2014-05-12 대우조선해양 주식회사 Dynamic positioning system considering draft variation and dynamic positioning method of the same
CN103605886A (en) * 2013-11-12 2014-02-26 中交天津航道局有限公司 Multi-model self-adaptive fusion filtering method of ship dynamic positioning system
CN103970021A (en) * 2014-05-21 2014-08-06 哈尔滨工程大学 Relaxation power positioning control system based on model prediction control
CN104199464A (en) * 2014-08-19 2014-12-10 哈尔滨工程大学 Real-time environment optimum heading ship dynamic positioning control method based on environment estimation

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
船舶动力定位***多模型自适应融合滤波;杨金节 等;《舰船科学技术》;20140831;第36卷(第8期);第39页第2节第1段、第41页第3节末段 *

Also Published As

Publication number Publication date
CN104635773A (en) 2015-05-20

Similar Documents

Publication Publication Date Title
CN104635773B (en) A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer
CN107168312B (en) Space trajectory tracking control method for compensating UUV kinematic and dynamic interference
Fossen et al. Kalman filtering for positioning and heading control of ships and offshore rigs
Do Global robust and adaptive output feedback dynamic positioning of surface ships
CN103955218B (en) A kind of unmanned boat Trajectory Tracking Control device and method based on Non-Linear Control Theory
CN109032136B (en) Under-actuated multi-unmanned ship formation tracking method based on master-slave distributed model predictive control
Zhang et al. NNFFC-adaptive output feedback trajectory tracking control for a surface ship at high speed
Li et al. Robust adaptive motion control for underwater remotely operated vehicles with velocity constraints
CN101833338B (en) Autonomous underwater vehicle vertical plane under-actuated motion control method
CN112965371B (en) Water surface unmanned ship track rapid tracking control method based on fixed time observer
CN102385316A (en) Deepening controlling method of underactuated automatic underwater vehicle based on neural network back stepping method
CN109189103B (en) Under-actuated AUV trajectory tracking control method with transient performance constraint
CN107065569B (en) Ship dynamic positioning sliding mode control system and method based on RBF neural network compensation
JP2007317165A (en) Method, apparatus, and program for planning operation of autonomous mobile robot, method for controlling autonomous mobile robot using method, recording medium thereof, and program for controlling autonomous mobile robot
CN104181815B (en) Ship movement compensation control method based on environmental estimation
CN114442640B (en) Track tracking control method for unmanned surface vehicle
EP3164741A1 (en) Method and system for dynamic positioning of instrumented cable towed in water
Dukan et al. Sea floor geometry approximation and altitude control of ROVs
CN110083057A (en) PID control method based on hydrofoil athletic posture
CN107315348B (en) Position and posture control method of unmanned underwater vehicle based on punishment type wavelet network
CN109885096A (en) A kind of autonomous underwater robot path trace closed loop control method based on Lyapunov-MPC technology
CN114967714A (en) Anti-interference motion control method and system for autonomous underwater robot
CN113110512B (en) Benthonic AUV self-adaptive trajectory tracking control method for weakening unknown interference and buffeting influence
de Cerqueira Gava et al. AUV cruising auto pilot for a long straight confined underwater tunnel
Garcia-Garcia et al. Wave filtering for heading control of an AUV based on passive observer

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant