CN108415423B - High-interference-rejection self-adaptive path following method and system - Google Patents
High-interference-rejection self-adaptive path following method and system Download PDFInfo
- Publication number
- CN108415423B CN108415423B CN201810106099.1A CN201810106099A CN108415423B CN 108415423 B CN108415423 B CN 108415423B CN 201810106099 A CN201810106099 A CN 201810106099A CN 108415423 B CN108415423 B CN 108415423B
- Authority
- CN
- China
- Prior art keywords
- expected
- psi
- path
- course
- navigation equipment
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 230000003044 adaptive effect Effects 0.000 claims description 11
- 230000001105 regulatory effect Effects 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 230000036039 immunity Effects 0.000 claims description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 abstract description 13
- 238000010586 diagram Methods 0.000 description 3
- 229920000535 Tan II Polymers 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 description 2
- 230000011664 signaling Effects 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 239000003344 environmental pollutant Substances 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 231100000719 pollutant Toxicity 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
The invention relates to a high-interference-rejection self-adaptive path following method and a high-interference-rejection self-adaptive path following system. Giving an expected path point, inputting the expected path point and real-time position information of the underwater navigation equipment into a guidance module, and solving an expected heading state psi by a high-immunity self-adaptive path following methodd(ii) a The desired heading state psidThe actual course state information psi of the underwater navigation equipment, which is obtained by the course sensor module and filtered by the filter module, obtains a course state deviation absolute value e (k), inputs the absolute value e (k) into the CFDL _ MFAC controller module, and outputs an expected instruction u (k) to the control mechanism module; the steering mechanism module receives and executes the desired command u (k) (e.g., desired rudder angle) so that the underwater navigation equipment is constantly approaching the desired heading ψd. The method does not need to depend on a system model, can effectively resist water flow interference, is insensitive to uncertain influences such as model perturbation and noise, has good robustness and adaptability, and can quickly drive the unmanned aircraft to track an upper expected path.
Description
Technical Field
The invention relates to a path following method and a system, in particular to a high-noise-rejection self-adaptive path following method and a system.
Background
Path following refers to a ship sailing according to a certain expected path, and in the engineering application of the ship, many actual engineering problems can be abstracted as path following problems. For example: ship entry and exit ports and channels, submarine pipeline laying, sea chart mapping, marine hydrological measurements, biochemical pollutant monitoring, and the like. Therefore, the research on the unmanned aircraft path following problem has important theoretical and engineering values. However, in marine environments, ships are susceptible to uncertain external environments and water flow interference, which may reduce the accuracy of tracking a desired path of the ship and even cause task failure.
At present, aiming at a ship path following method, the following method is similar: the disclosure date is 2015, 8, 19 and the publication number is CN104850122A, the invention name is a patent application of a variable-ship-length-ratio-based crosswind-resistant unmanned surface vessel straight-line path tracking method, the method is a crosswind-resistant unmanned surface vessel path tracking method, aiming at different ship lengths affected by wind, the method combines fuzzy control to adjust the ship length ratio and further adjust the course angle so as to realize straight-line path tracking, and the document 'asymmetric-model-based under-actuated USV path tracking control' indicates the line-of-sight angle in the variable-ship-length-ratio-based crosswind-resistant unmanned surface vessel straight-line path tracking method
λ is a positive parameter related to the captain of the ship, so that the guidance algorithm is suitable for path tracking of curves, and the viewing angle ψ* losThe tracking error convergence speed can be accelerated by self-adaptive adjustment according to the tracking error. The above method does not solve the problem of water flow disturbance.
Disclosure of Invention
Aiming at the prior art, the technical problem to be solved by the invention is to provide a robust, adaptive and high-disturbance-rejection adaptive path following method and system with water flow disturbance resistance.
In order to solve the technical problem, the invention provides a high-noise-immunity self-adaptive path following method, which comprises the following steps:
the method comprises the following steps: inputting a desired path command, wherein the desired path is composed of N desired path points, and the desired path point P is (P)1,P2,P3…Pn) N ≧ 2, where the nth desired path point Pn＝(xn,yn) And N is more than or equal to 1 and less than N, initializing N to be 1, and initializing a threshold value a of the safety distance, wherein a is a constant greater than 0.
Step two: get the path point Pn＝(xn,yn) And Pn+1＝(xn+1,yn+1) Connecting the two points to form a straight line and obtaining the direction angle psi of the straight linepn，ψpnThe included angle between the straight line path and the positive direction of the X axis is shown, and the following conditions are satisfied:
ψpn＝a tan 2(yn+1-yn,xn+1-xn),ψpn∈[-π,π]
step three: position (x) of underwater navigation equipment measured in real time by sensort,yt) Route point PnCoordinate (x)n,yn) And path azimuth angle psipnObtaining a tracking error Ze，ZeSatisfies the following conditions:
Ze＝-(xt-xn)sin(ψpn)+(yt-yn)cos(ψpn)
step four: design of convergent circle radius Rn，RnSatisfies the following conditions:
wherein,
beta is a normal number used for regulating the dynamic behavior of the tracking process, and beta is more than 1;
step five: from ZeAnd RnThe advance distance delta is found out,Δ satisfies:
step six: calculating the viewing angle psi* losAngle of sight psi* losSatisfies the following conditions:
ψ* los＝arctan(-KP*Zei-Ki*ξ(Zei)),i＝1、2、...
wherein, ZeiFor the tracking error of the ith run of the system,
Kibeing an adjustable parameter of the integral term, ξ (Ze)i) Satisfies the following conditions:
where Δ t represents the time step in which the system operates, ZemaxIs a parameter related to the length of the underwater navigation equipment;
step seven: obtaining a desired heading ψd，ψdSatisfies the following conditions:
ψd＝ψ* los+ψpn
step eight: obtaining an actual heading state psi and a real-time position of the underwater navigation equipment, the psi including a heading of the ship
And information of angular velocity r, order
Wherein k is1For parameters related to ship dynamics, take the appropriate k1Value, calculating psidThe difference with psi is e (k), k represents the k-th time the system is operated.
Step nine: taking e (k) as an input of a CFDL-MFAC (redefined compact format dynamic linearization model-free adaptive controller) heading controller, resolving an expected instruction u (k), executing the expected instruction u (k) by a steering engine or a speed differential mechanism, and solving the following conditions by using a CFDL-MFAC (redefined compact format dynamic linearization model-free adaptive control algorithm) algorithm:
where ρ ∈ (0, 1)]Is the step size factor, η ∈ (0, 1)]Is a step factor, mu > 0 is a weight coefficient, lambda > 0 is a constant variable, phi (k) is a pseudo-partial derivative,
to run the method k times the pseudo partial derivative estimate,
the method comprises the steps of (a) a pseudo partial derivative estimation value when the method operates k-1, delta y (k) is the difference value of the output quantity of a course system when the method operates k times and the output quantity of the course system when the method operates k-1 times, u (k) is the expected input of the course system when the method operates k times, u (k-1) is the expected input of the course system when the method operates k-1 times, delta u (k-1) is the difference value of the expected input of the course system when the method operates k-1 times and the expected input of the course system when the method operates k-2 times, and the difference value is a small enough normal, and belongs to (0, 0.001)]。
Step ten: calculating the distance between the underwater navigation equipment and the expected path point P according to the real-time position of the underwater navigation equipmentn+1When PL < a, executing step eleven; when PL is more than or equal to a, executing a third step;
step eleven: when N +1 is equal to N, ending; and when N +1 is less than N, enabling N to be N +1, and executing the step two.
Base of a fuel cellIn the path following system of the high-immunity self-adaptive path following method, an expected path point is input to generate an expected path; inputting expected path point information and underwater navigation equipment real-time position information obtained by the position sensor module into the guidance module, and calculating an expected course state psi by the high-immunity self-adaptive path following methodd(ii) a The desired heading state psidAnd the actual course state psi of the underwater navigation equipment obtained by the course sensor module and filtered by the filter module is subjected to subtraction to obtain course state deviation e (k), the course state deviation e (k) is input into the CFDL-MFAC controller, and an expected instruction u (k) is output to the control mechanism module; the control mechanism module receives and executes the expected command u (k), and inputs the execution result to the underwater navigation equipment module to enable the underwater navigation equipment to approach the expected heading psi continuouslyd。
The invention has the beneficial effects that: the invention only needs to input an expected path point under the water flow interference, calculates the expected course angle of the ship according to the real-time position of the underwater navigation equipment and an improved line-of-sight method, and simultaneously combines a CFDL-MFAC (compact form dynamic navigation model free adaptive control) course control algorithm to enable the ship to quickly reduce the tracking error and drive the ship to continuously converge on the expected path. The self-adaptive path following method of the ship does not need to depend on a system model, can effectively resist water flow interference, is insensitive to uncertain influences such as model perturbation and noise, has good robustness and self-adaptability, and can quickly drive the unmanned aircraft to track the upper expected path. The tracking error is introduced into the calculation of the radius of the convergence circle and the line-of-sight angle, and meanwhile, the adverse effect of uncertain factors such as water flow interference and model perturbation on the ship can be effectively eliminated by combining an MFAC course control algorithm.
Drawings
FIG. 1 is a block diagram of a flow diagram of a high immunity adaptive path following method;
FIG. 2 is a block diagram of a high immunity adaptive path following system;
Detailed Description
The ship in the invention refers to various underwater navigation equipment in a broad sense, such as an underwater robot, an underwater submarine, an unmanned surface vessel, a surface ship and the like, and the various underwater navigation equipment is in the application range of the invention. The invention is described in more detail below by way of example with reference to the accompanying drawings.
The invention is mainly directed to the tracking of a line path by a ship under the influence of uncertain water flow. The method mainly comprises the following steps: (1) inputting a desired path command, wherein the desired path is composed of N desired path points, and the desired path point P is (P)1,P2,P3…Pn) N is more than or equal to 2, wherein Pn＝(xn,yn) The coordinates of the nth desired path point are expressed, 1 is not less than N < N, the initialization is that N is 1, the threshold value a of the safety distance is initialized, and a is a constant which is greater than 0 and has a unit of m. (2) According to the path point Pn＝(xn,yn) And Pn+1＝(xn+1,yn+1) Obtaining a straight line path of the two points, and obtaining a direction angle psi of the straight line pathpn. (3) From the ship position (x) measured in real timet,yt) Simultaneously combining the coordinates P of the nth path pointn＝(xn,yn) And path azimuth angle psipnCalculating a tracking error Z from the information ofe. (4) Will converge to a circular radius RnIs arranged as
Wherein Z iseIn order to track the error, the tracking error is,
beta is a positive parameter for regulating the dynamic behavior of the tracking process, and beta is greater than 1. (5) From ZeAnd RnThe lead distance Δ can be found. (6) Considering the influence of water flow interference, the viewing angle phi* losIs improved to psi* los＝arctan(-KP*-Ki*ξ(Zei)),i＝1、2、...，
KiAnd delta is an adjustable parameter of the integral term and is a leading distance. (7) Calculated viewing angle psi* losAnd psipnSum ofAs the desired heading ψd. (8) According to psidCalculating psi according to the actual course state psi of the shipdThe difference e (k) from phi, k representing the kth time the system is operated, phi including the ship heading
And signalling of angular velocity r
Wherein k is a parameter related to ship dynamics, and considering an extreme case that when the heading of a ship is increased to 180 degrees, the next time is changed to-180 degrees, the ship dynamic characteristic is deduced through theory
In which T isSRepresents the time when the redefined output CFDL-MFAC algorithm is executed once, Deltau (K) is the difference between the expected rudder angle of the ship at the K time when the redefined output CFDL-MFAC algorithm is executed and the expected rudder angle of the ship at the K-1 time when the redefined output MFAC algorithm is executed, and K and T are the maneuverability coefficients of the ship. Take the appropriate k1And the ship course system meets the requirement of the CFDL-MFAC algorithm on the assumption condition of quasi-linearity of the controlled system. (9) And e (k) is used as an input of a CFDL-MFAC course controller, a desired command u (k) (such as a desired rudder angle) is calculated, and the steering engine or the speed differential mechanism executes the desired command u (k) and then executes the step (8). (10) According to the real-time position p of the shipt＝(xt,yt) Position P of the sum-path pointn＝(xn,yn) And (3) calculating the distance PL between the ship and the path point, executing (11) when PL is less than a, and executing (3) when PL is more than or equal to a. (11) When N +1 is equal to N, ending; and when N +1 is less than N, enabling N to be N +1, and executing (2).
With reference to fig. 1, in a water flow disturbance condition, the method comprises the following steps:
the method comprises the following steps: inputting a desired path command, wherein the desired path is composed of N desired path points, and the desired path point P is (P)1,P2,P3…Pn) N ≧ 2, where the nth desired path point coordinate Pn＝(xn,yn)，1≤N is less than N, the initialization is that N is 1, the threshold value of the initialization safe distance is a, a is a constant larger than 0, and the unit is m.
Step two: get the path point Pn＝(xn,yn) And Pn+1＝(xn+1,yn+1) Connecting the two points to form a straight line and obtaining the direction angle psi of the straight linepn，ψpnThe included angle between the straight line path and the positive direction of the X axis is shown, and the following conditions are satisfied:
ψpn＝a tan 2(yn+1-yn,xn+1-xn),ψpn∈[-π,π]
step three: position (x) of underwater navigation equipment measured in real time by sensort,yt) Route point PnCoordinate (x)n,yn) And path azimuth angle psipnObtaining a tracking error Ze，ZeSatisfies the following conditions:
Ze＝-(xt-xn)sin(ψpn)+(yt-yn)cos(ψpn)
step four: design of convergent circle radius Rn，RnSatisfies the following conditions:
wherein,
beta is a positive parameter constant used for adjusting the dynamic behavior of the tracking process, and beta is more than 1;
step five: from ZeAnd RnFinding the advance distance Δ, Δ satisfies:
step six: calculating the viewing angle psi* losAngle of sight psi* losSatisfies the following conditions:
ψ* los＝arctan(-KP*Zei-Ki*ξ(Zei)),i＝1、2、...
wherein,
Kibeing an adjustable parameter of the integral term, ξ (Ze)i) Satisfies the following conditions:
wherein, ZemaxIs a parameter related to the length of underwater navigation equipment, and the Ze is taken by the inventionmaxThe length of the underwater navigation equipment is 100-200 times.
Step seven: obtaining a desired heading ψd，ψdSatisfies the following conditions:
ψd＝ψ* los+ψpn
step eight: according to psidCalculating psi according to the actual course state psi of the shipdThe difference with psi is e (k);
step nine: taking e (k) as the input of the CFDL _ MFAC heading controller, calculating an expected command u (k), executing the expected command u (k) by a steering engine or a speed differential mechanism, and executing the step eight, wherein the CFDL _ MFAC heading control algorithm is as follows, and the expected input u (k) can be obtained by e (k) according to the following formula:
where ρ ∈ (0, 1)]Is the step size factor, η ∈ (0, 1)]Is a step factor, mu > 0 is a weight coefficient, lambda > 0 is a constant variable, phi (k) is a pseudo-partial derivative,
to run the method k times the pseudo partial derivative estimate,
a pseudo partial derivative estimated value when the method operates k-1, delta y (k) is the difference value of the output quantity of the course system when the method operates k times and the output quantity of the course system when the method operates k-1 times, u (k) is the expected input of the course system when the method operates k times, u (k-1) is the expected input of the course system when the method operates k-1 times, delta u (k-1) is the difference value of the expected input of the course system when the method operates k-1 times and the expected input of the course system when the method operates k-2 times, and is a small enough normal number epsilon (0, 0.001)]。
Step ten: according to the real-time position p of the shipt＝(xt,yt) And the position P of the road-passing pointn＝(xn,yn) Calculating the distance PL between the ship and the path point, and executing the eleventh step when PL is less than a; when PL is more than or equal to a, executing a third step;
step eleven: when N +1 is equal to N, ending; and when N +1 is less than N, enabling N to be N +1, and executing the step two.
With reference to fig. 2, in a water flow disturbance condition, the tracking control system includes the following steps:
the method comprises the following steps: and inputting a desired path point command to generate a desired path.
Step two: according to the expected path and the current position information of the ship, the expected heading psi is calculated by a guidance algorithm shown in figure 1d；
Step three: the course control system adopts a CFDL-MFAC control method to calculate psi according to the actual course state information psidThe difference e (k) from phi, k representing the kth time the system is operated, phi including the ship heading
And information of angular velocity r, order
Where k is a parameter related to ship dynamics,and taking a proper k value so that the ship heading system meets the requirement of the CFDL-MFAC algorithm on the assumption condition of quasi linearity of the controlled system, and taking e (k) as the input of a CFDL-MFAC heading controller to solve the expected command u (k).
Step four: and the steering engine or the speed differential mechanism receives and executes the expected command u (k), so that the ship is driven to approach the expected heading continuously.
Step five: and (4) continuously updating the position of the ship, continuously changing the course, observing the attitude of the aircraft in real time through sensors such as a magnetic compass and the like, feeding back the attitude to a course control system, feeding back real-time position information to a guidance system, and repeating the step two until the ship tracks the expected path to realize path following.
A high-noise-immunity self-adaptive path following method comprises the following steps:
(1) inputting a desired path command, wherein the desired path is composed of N desired path points, and the desired path point P is (P)1,P2,P3…Pn) N ≧ 2, where the nth desired path point Pn＝(xn,yn) N is more than or equal to 1 and less than N, initializing N to 1, and initializing a threshold value a of the safety distance, wherein a is a constant which is more than 0 and has a unit of m.
(2) According to the path point Pn＝(xn,yn) And Pn+1＝(xn+1,yn+1) Obtaining a straight line path of the two points, and obtaining a direction angle psi of the straight line pathpn。
(3) From the ship position (x) measured in real timet,yt) Simultaneously combining the nth path point Pn＝(xn,yn) Coordinate of (2) and path direction angle psipnCalculating a tracking error Z from the information ofe。
(4) Will converge to a circular radius RnIs arranged as
Wherein Z iseIn order to track the error, the tracking error is,
n is a positive parameter, and n is a positive parameter,used for regulating the dynamic behavior of the heald following process, and beta is more than 1.
(5) From ZeAnd RnThe lead distance Δ can be found.
(6) Now consider the effect of water flow disturbance, the viewing angle psi* losIs improved to psi* los＝arctan(-KP*-Ki*ξ(Zei)),i＝1、2、...，
Wherein KiAnd delta is an adjustable parameter of the integral term and is a leading distance.
(7) Calculated viewing angle psi* losAnd psipnThe sum as the desired heading psid。
(8) According to psidCalculating psi according to the actual course state psi of the shipdThe difference e (k) from phi, k representing the kth time the system is operated, phi including the ship heading
And signalling of angular velocity r
Wherein k is a parameter related to ship dynamics, and considering an extreme case that when the heading of a ship is increased to 180 degrees, the next time is changed to-180 degrees, the ship dynamic characteristic is deduced through theory
In which T isSAnd expressing the time of executing the redefined output CFDL-MFAC algorithm once, delta u (K) is the difference between the rudder angle of the ship at the K time of executing the redefined output CFDL-MFAC algorithm and the rudder angle of the ship at the K-1 time of executing the redefined output MFAC algorithm, K and T are maneuverability coefficients of the ship, and the proper K value is taken so that the ship heading system meets the requirement of the CFDL-MFAC algorithm on the assumption of quasi-linearity of the controlled system.
(9) And e (k) is used as an input of a CFDL-MFAC course controller, an expected command u (k) (such as an expected rudder angle) is calculated, the steering engine or a speed differential mechanism executes the expected command u (k), and then the step (8) is executed.
(10) According to the real-time position p of the shipt＝(xt,yt) Position P of the sum-path pointn＝(xn,yn) And (3) calculating the distance PL between the ship and the path point, executing (11) when PL is less than a, and executing (3) when PL is more than or equal to a.
(11) When N +1 is equal to N, ending; and when N +1 is less than N, enabling N to be N +1, and executing (2).
An adaptive path following method for a ship further comprises:
(1) taking into account the radius of the convergent circle RkWhen the lateral error Z is taken into accounteIs greater than RnIt will cause the above guidance algorithm to fail, and therefore it is necessary to change RnAdapted to varying ZeAnd let Z beeFast convergence to zero can be expressed as:
wherein,
n is a positive parameter for adjusting the dynamic behavior of the following process, and β > 1; while the above formula ensures RnGreater than the transverse error ZeThe conditions of (1).
(2) Taking into account the disturbance of the water flow, #* losIs improved to be* los＝arctan(-KP*Zei-Ki*ξ(Zei)),i＝1、2、...，
KiAdjustable parameters being integral terms, ZeiFor the tracking error of the ith time of system operation, delta is the lead distance, xi (Ze)i) The specific expression form of (A) is as follows:
where Δ t, represents the time step in which the system operates, ZemaxIs a parameter related to the length of a ship, and the Ze is takenmaxThe length of the underwater navigation equipment is 100-200 times.
Claims (2)
1. A high-noise-immunity self-adaptive path following method is characterized by comprising the following steps: the method comprises the following steps:
the method comprises the following steps: inputting a desired path command, wherein the desired path is composed of N desired path points, and the desired path point P is (P)1,P2,P3…Pn) N ≧ 2, where the nth desired path point Pn＝(xn,yn) N is more than or equal to 1 and less than N, initializing N to be 1, initializing a threshold value a of a safety distance, and a is a constant greater than 0;
step two: get the desired path point Pn＝(xn,yn) And Pn+1＝(xn+1,yn+1) Connecting the two points to form a straight line and obtaining the direction angle psi of the straight linepn，ψpnThe included angle between the straight line path and the positive direction of the X axis is shown, and the following conditions are satisfied:
ψpn＝atan2(yn+1-yn,xn+1-xn),ψpn∈[-π,π]
step three: position (x) of underwater navigation equipment measured in real time by sensort,yt) Desired path point PnCoordinate (x)n,yn) And straight path azimuth angle psipnObtaining a tracking error Ze，ZeSatisfies the following conditions:
Ze＝-(xt-xn)sin(ψpn)+(yt-yn)cos(ψpn)
step four: design of convergent circle radius Rn，RnSatisfies the following conditions:
wherein,
beta is a normal number used for regulating the dynamic behavior of the tracking process, and beta is more than 1;
step five: from ZeAnd RnFinding the advance distance Δ, Δ satisfies:
step six: calculating the viewing angle psi* losAngle of sight psi* losSatisfies the following conditions:
ψ* los＝arctan(-KP*Zei-Ki*ξ(Zei)),i＝1、2、...
wherein, ZeiFor the tracking error of the ith run of the system,
Kibeing an adjustable parameter of the integral term, ξ (Ze)i) Satisfies the following conditions:
where Δ t represents the time step in which the system operates, ZemaxIs a parameter related to the length of the underwater navigation equipment;
step seven: obtaining a desired heading ψd，ψdSatisfies the following conditions:
ψd＝ψ* los+ψpn
step eight: obtaining an actual heading state psi and a real-time position of the underwater navigation equipment, the psi including a heading of the ship
And information of angular velocity r, order
Wherein k is1For parameters related to ship dynamics, take the appropriate k1Value, calculating psidThe difference with psi is e (k), k represents the k-th time of system operation;
step nine: taking e (k) as an input of a CFDL-MFAC (redefined compact format dynamic linearization model-free adaptive controller) heading controller, resolving an expected instruction u (k), executing the expected instruction u (k) by a steering engine or a speed differential mechanism, and solving the following conditions by using a CFDL-MFAC (redefined compact format dynamic linearization model-free adaptive control algorithm) algorithm:
Where ρ ∈ (0, 1)]Is the step size factor, η ∈ (0, 1)]Is a step factor, mu > 0 is a weight coefficient, lambda > 0 is a constant variable, phi (k) is a pseudo-partial derivative,
to run the method k times the pseudo partial derivative estimate,
pseudo partial derivative estimate for method run k-1, Δ y (k) course for method run k timesThe difference value of the system output quantity and the course system output quantity when the method operates for k-1 times, u (k) is the course system expected input when the method operates for k times, u (k-1) is the course system expected input when the method operates for k-1 times, delta u (k-1) is the difference value between the course system expected input when the method operates for k-1 times and the course system expected input when the method operates for k-2 times, and is a sufficiently small normal element, 0.001]；
Step ten: calculating the distance between the underwater navigation equipment and the expected path point P according to the real-time position of the underwater navigation equipmentn+1When PL < a, executing step eleven; when PL is more than or equal to a, executing a third step;
step eleven: when N +1 is equal to N, ending; and when N +1 is less than N, enabling N to be N +1, and executing the step two.
2. A path following system based on the high noise immunity adaptive path following method of claim 1, characterized in that: inputting expected path points to generate an expected path; inputting expected path point information and underwater navigation equipment real-time position information obtained by the position sensor module into the guidance module, and calculating an expected course state psi by the high-immunity self-adaptive path following methodd(ii) a The desired heading state psidAnd the actual course state psi of the underwater navigation equipment obtained by the course sensor module and filtered by the filter module is subjected to subtraction to obtain course state deviation e (k), the course state deviation e (k) is input into the CFDL-MFAC controller, and an expected instruction u (k) is output to the control mechanism module; the control mechanism module receives and executes the expected command u (k), and inputs the execution result to the underwater navigation equipment module to enable the underwater navigation equipment to approach the expected heading psi continuouslyd。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810106099.1A CN108415423B (en) | 2018-02-02 | 2018-02-02 | High-interference-rejection self-adaptive path following method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810106099.1A CN108415423B (en) | 2018-02-02 | 2018-02-02 | High-interference-rejection self-adaptive path following method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108415423A CN108415423A (en) | 2018-08-17 |
| CN108415423B true CN108415423B (en) | 2020-12-04 |
Family
ID=63127420
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810106099.1A Active CN108415423B (en) | 2018-02-02 | 2018-02-02 | High-interference-rejection self-adaptive path following method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108415423B (en) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109144066B (en) * | 2018-09-05 | 2021-07-06 | 哈尔滨工程大学 | Integral separation type PI type compact format model-free self-adaptive course control algorithm for ships |
| CN109254585A (en) * | 2018-10-08 | 2019-01-22 | 哈尔滨工程大学 | A kind of improvement model-free adaption Heading control algorithm that naval vessel inputoutput data merges |
| CN109460041A (en) * | 2018-12-28 | 2019-03-12 | 珠海市汉图达科技有限公司 | A kind of unmanned boat auto-pilot control system and method |
| CN109814386B (en) * | 2019-01-24 | 2021-08-31 | 天津大学 | Robot trajectory tracking active disturbance rejection control method based on model-free outer loop compensation |
| CN109656142B (en) * | 2019-02-15 | 2021-09-10 | 哈尔滨工程大学 | Cascade structure model-free self-adaptive guidance method for unmanned ship |
| CN109828570B (en) * | 2019-02-18 | 2021-10-01 | 哈尔滨工程大学 | Control and guide method for self-adaptive boundary layer water surface unmanned ship |
| CN110673598B (en) * | 2019-09-29 | 2022-10-25 | 哈尔滨工程大学 | Intelligent path tracking control method for unmanned surface vehicle |
| CN111198502B (en) * | 2020-02-28 | 2021-04-09 | 浙江大学 | Unmanned ship track tracking control method based on interference observer and fuzzy system |
| CN111798701B (en) * | 2020-07-07 | 2022-07-26 | 中国船舶工业系统工程研究院 | Unmanned ship path tracking control method, system, storage medium and terminal |
| CN112346465B (en) * | 2020-11-27 | 2022-09-02 | 哈尔滨工程大学 | IALOS guide law-based adaptive fuzzy control method for under-actuated unmanned ship |
| CN113581388B (en) * | 2021-08-30 | 2022-05-20 | 广东工业大学 | Unmanned ship control method for environmental protection operation and unmanned ship system applying same |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102722177A (en) * | 2012-06-27 | 2012-10-10 | 哈尔滨工程大学 | Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain |
| CN103955218A (en) * | 2014-04-28 | 2014-07-30 | 上海大学 | Unmanned surface vehicle trajectory tracking control device and method based on nonlinear control theory |
| CN104765370A (en) * | 2015-04-15 | 2015-07-08 | 哈尔滨工业大学 | UUV trajectory sight guiding method with sideslip angle considered under condition of environmental disturbance |
| CN105955268A (en) * | 2016-05-12 | 2016-09-21 | 哈尔滨工程大学 | Local obstacle avoidance considering UUV moving object sliding mode tracking control method |
| CN106950974A (en) * | 2017-04-19 | 2017-07-14 | 哈尔滨工程大学 | Drive lacking Autonomous Underwater Vehicle is understood three-dimensional path and tracking and controlling method |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20060058931A1 (en) * | 2004-09-15 | 2006-03-16 | Honeywell International Inc. | Collision avoidance involving radar feedback |
| IL196276A (en) * | 2008-12-30 | 2013-04-30 | Elbit Systems Ltd | Autonomous navigation system and method for a maneuverable platform |
-
2018
- 2018-02-02 CN CN201810106099.1A patent/CN108415423B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102722177A (en) * | 2012-06-27 | 2012-10-10 | 哈尔滨工程大学 | Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain |
| CN103955218A (en) * | 2014-04-28 | 2014-07-30 | 上海大学 | Unmanned surface vehicle trajectory tracking control device and method based on nonlinear control theory |
| CN104765370A (en) * | 2015-04-15 | 2015-07-08 | 哈尔滨工业大学 | UUV trajectory sight guiding method with sideslip angle considered under condition of environmental disturbance |
| CN105955268A (en) * | 2016-05-12 | 2016-09-21 | 哈尔滨工程大学 | Local obstacle avoidance considering UUV moving object sliding mode tracking control method |
| CN106950974A (en) * | 2017-04-19 | 2017-07-14 | 哈尔滨工程大学 | Drive lacking Autonomous Underwater Vehicle is understood three-dimensional path and tracking and controlling method |
Non-Patent Citations (5)
| Title |
|---|
| 《Heading control method and experiments for an unmanned wave glider》;LIAO Yu-lei.ect;《中南大学学报：英文版》;20171231(第11期);第2504-2512页 * |
| 《Serret−Frenet frame based on path following control for underactuated unmanned surface vehicles with dynamic uncertainties》;LIAO Yu-lei.ect;《Journal of Central South University of Technology》;20151231(第22期);第214-223页 * |
| 《Unmanned Surface Vehicle Target Tracking Based on Marine Radar》;Jiayuan Zhuang.ect;《International Conference on Computer Science and Service System》;20111231;第1872-1875页 * |
| 《基于非对称模型的欠驱动USV路径跟踪控制》;董早鹏等;《中国造船》;20160331;第57卷(第1期);第116-126页 * |
| 《欠驱动船舶直线航迹的滑模控制方法》;廖煜雷等;《应用科技》;20111130;第38卷(第11期);第13-17页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108415423A (en) | 2018-08-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108415423B (en) | High-interference-rejection self-adaptive path following method and system | |
| CN109828570B (en) | Control and guide method for self-adaptive boundary layer water surface unmanned ship | |
| Fossen et al. | Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces | |
| CN106950974B (en) | Three-dimensional path understanding and tracking control method for under-actuated autonomous underwater vehicle | |
| CN108845576B (en) | Thrust distribution method based on combination of particle swarm optimization and sequence quadratic programming | |
| CN108319140B (en) | Redefined output type model-free self-adaptive course control method and system | |
| CN110618686B (en) | Unmanned ship track control method based on explicit model predictive control | |
| CN111487966A (en) | Self-adaptive path tracking control method for unmanned surface vehicle based on waypoints | |
| CN104950882A (en) | Global consistent asymptotic type path tracking guiding control method | |
| CN105549611B (en) | The flight path precise tracking method of the marine environment self-identifying of autonomous underwater robot | |
| CN111562742B (en) | On-line modeling and self-adaptive control method of under-actuated ship | |
| CN108563113A (en) | Naval vessel is with multiloop model-free adaption bow to control method | |
| CN109656142B (en) | Cascade structure model-free self-adaptive guidance method for unmanned ship | |
| CN113467231A (en) | Unmanned ship path tracking method based on sideslip compensation ILOS guidance law | |
| CN115113524B (en) | ASV multiport event trigger path tracking control method based on intervention LVS guidance | |
| CN106444359A (en) | Human-simulated intelligent control method for autonomous region keeping of water-jet propulsion unmanned ship | |
| Liu et al. | Model predictive controller design with disturbance observer for path following of unmanned surface vessel | |
| CN116185024A (en) | Unmanned ship heading anti-interference control method | |
| Liu et al. | ADRC‐SMC‐based disturbance rejection depth‐tracking control of underactuated AUV | |
| Gao et al. | Saturated exponential super‐twisting sliding mode control for bottom‐following of biomimetic underwater vehicles with unmeasured velocities | |
| Yuan et al. | Combined depth and heading control and experiment of ROV under the influence of residual buoyancy, current disturbance, and control dead zone | |
| CN114035567B (en) | Unmanned surface vehicle navigation control system | |
| Bibuli et al. | Discrete-time sliding mode control for guidance of an unmanned surface vehicle | |
| Han et al. | A Ship Path Tracking Control Method Using a Fuzzy Control Integrated Line-of-Sight Guidance Law | |
| CN114879703A (en) | Underwater robot path tracking control method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |