CN108415423B - A high noise immunity adaptive path following method and system - Google Patents

Abstract

The present invention relates to a high anti-interference adaptive path following method and system. Given the desired path point, input the desired path point and the real-time position information of the underwater navigation equipment into the guidance module, and calculate the desired heading state ψ _d through the high disturbance immunity adaptive path following method _; The actual heading state information ψ of the underwater navigation equipment obtained and filtered by the filter module obtains the absolute value e(k) of the heading state deviation, and inputs it to the CFDL_MFAC controller module, and outputs the desired command u(k) to the control mechanism module; the control mechanism The module receives and executes the desired command u(k) (such as the desired rudder angle) to make the underwater navigation equipment continuously approach the desired heading ψ _d . The present invention does not need to rely 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 vehicle to track the expectations path.

Description

A high noise immunity adaptive path following method and system

technical field

The present invention relates to a path following method and system, in particular to a high disturbance immunity adaptive path following method and system.

Background technique

Path following means that the ship sails according to a desired path. In the engineering application of ships, many practical engineering problems can be abstracted as path following problems. For example: ships entering and leaving ports and waterways, submarine pipeline laying, nautical charting, marine hydrology measurement, biochemical pollutant monitoring, etc. Therefore, studying the path following problem of UAV has important theoretical and engineering value. However, ships are vulnerable to the uncertain influence of the external environment and the interference of water currents in the marine environment, which will reduce the accuracy of the ship's tracking of the desired path, and even lead to mission failure.

λ是与船长有关的正参数，使得该制导算法适用于曲线的路径跟踪，且视线角ψ^* _los能根据跟踪误差自适应调整，从而达到加快跟踪误差收敛速度的效果。但是上述方法没有解决水流干扰问题。At present, for the ship path following methods, the more similar ones are: the publication date is August 19, 2015, the publication number is CN104850122A, and the name of the invention is "Linear Path Tracking Method for Crosswind Resistant Unmanned Surface Vehicle Based on Variable Ship Length Ratio". Patent application, this method is a path following method for unmanned boats that resists crosswinds. According to the difference of different captains affected by wind, combined with fuzzy control, the length ratio of the ship is adjusted, and then the heading angle is adjusted, so as to achieve straight line path tracking. "Underactuated USV Path Tracking Control for Asymmetric Models", Line-of-sight angle in a straight-line path tracking method for crosswind-resistant unmanned surface vehicles based on variable length ratio

λ is a positive parameter related to the length of the ship, which makes the guidance algorithm suitable for the path tracking of the curve, and the line of sight angle ψ ^* _los can be adjusted adaptively according to the tracking error, so as to achieve the effect of accelerating the convergence speed of the tracking error. However, the above method does not solve the problem of water flow interference.

SUMMARY OF THE INVENTION

Aiming at the above-mentioned prior art, the technical problem solved by the present invention is to provide a robust, adaptive and anti-water flow interference adaptive path following method and system with high anti-disturbance.

In order to solve the above-mentioned technical problems, a high-disturbance adaptive path following method of the present invention includes the following steps:

Step 1: Input the desired path command, the desired path consists of N desired path points, the desired path point P=(P ₁ , P ₂ , P ₃ . . . P _n )N≥2, where the nth desired path point P _n = (x _n , y _n ), 1≤n<N, the initialization is set to n=1, the threshold a of the initialization safety distance, a is a constant greater than 0.

Step 2: Obtain the path point P _n =(x _n , y _n ) and P _n+1 =(x _n+1 , y _n+1 ) two-point straight line path and obtain the direction angle of the straight line path ψ _pn , ψ _pn represents the angle between the straight line path and the positive direction of the X axis, which satisfies:

ψ _pn =a tan 2(y _n+1 -y _n ,x _n+1 -x _n ),ψ _pn ∈[-π,π]

Step 3: Obtain the tracking error Z _e according to the position (x _t , y _t ) of the underwater navigation equipment, the coordinates (x _n , y _n ) of the path point P _n and the path direction angle ψ _pn measured in real time by the sensor, and _Ze satisfies:

Z _e =-(x _t -x _n )sin(ψ _pn )+(y _t -y _n )cos(ψ _pn )

Step 4: Design the radius of convergence circle R _n , where R _n satisfies:

β为正常数，用于调节跟对过程的动态行为，且β＞1；in,

β is a normal number, used to adjust the dynamic behavior of the tracking process, and β>1;

Step 5: _{Calculate the lead distance Δ from Ze and R n ,} and Δ satisfies:

Step 6: Calculate the line of sight angle ψ ^* _los , the line of sight angle ψ ^* _los satisfies:

ψ ^* _los = arctan(-K _P *Ze _i -K _i *ξ(Ze _i )), i=1, 2, ...

K_i为积分项的可调参数，ξ(Ze_i)满足：Among them, Ze _i is the tracking error of the system running for the ith time,

K _i is an adjustable parameter of the integral term, and ξ(Ze _i ) satisfies:

Among them, Δt represents the time step of the system operation, and Ze _max is a parameter related to the length of the underwater navigation equipment;

Step 7: Obtain the desired heading ψ _d , where ψ _d satisfies:

ψ _d = ψ ^* _los +ψ _pn

和角速度r的信息，令

其中k₁为与舰船动力学特性有关的参数，取适当的k₁值，计算ψ_d与ψ之差得e(k)，k表示系统运行第k次。Step 8: Obtain the actual heading state ψ and real-time position of the underwater navigation equipment, ψ includes the heading of the ship

and information on the angular velocity r, let

Among them, k ₁ is a parameter related to the dynamic characteristics of the ship. Take an appropriate value of k ₁ and calculate the difference between ψ _d and ψ to obtain e(k), where k represents the kth operation of the system.

Step 9: Use e(k) as the input of the CFDL-MFAC (Redefined Compact Format Dynamic Linearization Model-Free Adaptive Controller) heading controller, solve the desired command u(k), and execute the steering gear or speed differential mechanism The expected instruction u(k), using the CFDL-MFAC (Redefinition Compact Format Dynamic Linearization Model-Free Adaptive Control Algorithm) algorithm solution process satisfies:

为方法运行k次伪偏导数估计值，

为方法运行k-1时伪偏导数估计值，Δy(k)为方法运行k次时的航向系统输出量与方法运行k-1次时的航向系统输出量的差值，u(k)为方法运行k次时航向系统期望输入，u(k-1)为方法运行k-1次时航向系统期望输入，Δu(k-1)为方法运行k-1次时航向系统期望输入与方法运行k-2次时航向系统期望输入之差，ε为一个充分小的正常，ε∈(0,0.001]。Among them, ρ∈(0,1] is the step factor, η∈(0,1] is the step factor, μ＞0 is the weight coefficient, λ＞0 is the constant variable, φ(k) is the pseudo partial derivative,

Run k pseudo-partial derivative estimates for the method,

is the estimated value of the pseudo-partial derivative when the method runs k-1, Δy(k) is the difference between the output of the heading system when the method runs k times and the output of the heading system when the method runs k-1 times, u(k) is The expected input of the heading system when the method runs k times, u(k-1) is the expected input of the heading system when the method runs k-1 times, Δu(k-1) is the expected input of the heading system when the method runs k-1 times and the method runs The difference between the expected inputs of the heading system at times k-2, ε is a sufficiently small normal, ε∈(0,0.001].

Step ten: according to the real-time position of the underwater navigation equipment, calculate the distance PL of the underwater navigation equipment from the desired path point P _n+1 , when PL<a, go to step eleven; when PL≥a, go to step three;

Step eleven: when n+1=N, end; when n+1<N, let n=n+1, and execute step two.

A path following system based on the high anti-disturbance adaptive path following method of the present invention, inputting a desired path point to generate a desired path; inputting the desired path point information and the position information of underwater navigation equipment obtained through a position sensor module into a guidance module, Calculate the desired heading state ψ _d through the high disturbance immunity adaptive path following method; make the difference between the desired heading state ψ _d and the actual heading state ψ of the underwater navigation equipment obtained by the heading sensor module and filtered by the filter module, Obtain the heading state deviation e(k) and input it to the CFDL-MFAC controller, and output the desired command u(k) to the manipulation mechanism module; the manipulation mechanism module receives and executes the desired command u(k), and inputs the execution result to the underwater navigation The equipment module makes the underwater navigation equipment continuously approach the desired heading ψ _d .

Beneficial effects of the present invention: the present invention only needs to input the desired path point under the disturbance of water flow, and calculates the desired heading angle of the ship according to the time-to-time position of the underwater navigation equipment and the improved line-of-sight method, and combines CFDL-MFAC (compact form dynamic dynamic) Linearization model free adaptive control) heading control algorithm can quickly reduce the tracking error of the ship and drive the ship to continuously converge to the desired path. The proposed adaptive path-following method for ships does not need to rely on the system model, and can effectively resist current interference, is insensitive to uncertain influences such as model perturbation and noise, has good robustness and adaptability, and can Fast-drive unmanned aerial vehicle tracking on the desired path. The tracking error is introduced into the calculation of the radius of the convergence circle and the line of sight angle, and combined with the MFAC heading control algorithm, the adverse effects of uncertain factors such as water flow interference and model perturbation on the ship can be effectively eliminated.

Description of drawings

Fig. 1 is a flow chart of the high disturbance immunity adaptive path following method;

Figure 2 is a block diagram of a high noise immunity adaptive path following system;

Detailed ways

Ships in the present invention refer to various underwater navigation equipment in a broad sense, such as underwater robots, underwater submarines, unmanned surface ships, surface ships, etc. The above-mentioned various underwater navigation equipment are all within the scope of application of the present invention. The present invention will be described in more detail below with reference to the accompanying drawings.

其中，Z_e为跟踪误差，

β为正参数，用于调节跟综过程的动态行为，且β＞1。(5)由Z_e和R_n可以求出超前距离Δ。(6)考虑水流干扰的影响，将视线角ψ^* _los的求解改进为ψ^* _los＝arctan(-K_P*-K_i*ξ(Ze_i)),i＝1、2、...，

K_i为积分项的可调参数，Δ为超前距离。(7)计算出的视线角ψ^* _los与ψ_pn之和作为期望航向ψ_d。(8)根据ψ_d和舰船的实际航向状态ψ,计算ψ_d与ψ之差得e(k)，k表示系统运行第k次，ψ包括舰船的航向

和角速度r的信，令

其中k为与舰船动力学特性有关的参数，考虑一种极端情况当舰船的艏向增大到180度时，下一时刻变为-180度，通过理论推导当

时，其中T_S表示重定义输出式CFDL-MFAC算法执行一次的时间，Δu(k)为重定义输出式CFDL-MFAC算法执行第k次时舰船的期望舵角与重定义输出式MFAC算法执行第k-1次时舰船的期望舵角之差，K，T为舰船的操纵性系数。取适当的k₁值，从而使得舰船的航向系统满足CFDL-MFAC算法对受控系统“拟线性”假设条件的要求。(9)将e(k)作为CFDL-MFAC航向控制器的输入，解算期望指令u(k)(如期望舵角)，舵机或速度差动机构执行期望指令u(k)然后执行步骤(8)。(10)根据舰船的时时位置p_t＝(x_t,y_t)和路经点位置P_n＝(x_n,y_n)，计算舰船与路径点的距离PL，当PL＜a时，执行(11)，当PL≥a，执行(3)。(11)当n+1＝N时，结束；当n+1＜N，令n＝n+1，执行(2)。The invention is mainly aimed at the tracking of the straight line path of the ship under the influence of the uncertain current. The main steps include: (1) Input the desired path command, the desired path consists of N desired path points, the desired path point P=(P ₁ , P ₂ , P ₃ . . . P _n )N≥2, where P _n =(x _n , y _n ), representing the coordinates of the nth desired path point, 1≤n<N, the initialization is set to n=1, the threshold a of the initialization safety distance, a is a constant greater than 0, and the unit is m. (2) According to the path points P _n =(x _n , y _n ) and P _n+1 =(x _n+1 , y _n+1 ), a straight line path connecting two points is obtained, and the direction angle ψ of the straight line path is obtained _pn . (3) According to the real-time measured ship position (x _t , y _t ), and at the same time combined with the coordinates of the nth path point P _n = (x _n , y _n ) and the path direction angle ψ _pn The information is calculated to obtain the tracking Error _Ze . (4) Set the convergence circle radius R _n as

Among them, Z _e is the tracking error,

β is a positive parameter used to adjust the dynamic behavior of the follow-up process, and β>1. (5) The lead distance Δ can be obtained from _Ze and _Rn . (6) Considering the influence of water flow disturbance, the solution of sight angle ψ ^* _los is improved to ψ ^* _los =arctan(-K _P *-K _i *ξ(Ze _i )), i=1, 2,...,

K _i is an adjustable parameter of the integral term, and Δ is the lead distance. (7) The sum of the calculated line-of-sight angles ψ ^* _los and ψ _pn is taken as the desired heading ψ _d . (8) According to ψ _d and the actual heading state ψ of the ship, calculate the difference between ψ _d and ψ to obtain e(k), where k represents the kth operation of the system, and ψ includes the heading of the ship

and the angular velocity r, let

Among them, k is a parameter related to the dynamic characteristics of the ship. Considering an extreme case, when the heading of the ship increases to 180 degrees, the next moment becomes -180 degrees.

, where T _S represents the execution time of the redefinition output CFDL-MFAC algorithm once, and Δu(k) is the expected rudder angle of the ship when the redefinition output CFDL-MFAC algorithm executes the kth time and the redefinition output MFAC algorithm The difference between the expected rudder angles of the ship when executing the k-1th time, K and T are the maneuverability coefficients of the ship. An appropriate value of k ₁ is taken so that the ship's heading system meets the requirements of the CFDL-MFAC algorithm for the "quasi-linear" assumption of the controlled system. (9) Take e(k) as the input of the CFDL-MFAC heading controller, solve the desired command u(k) (such as the desired rudder angle), and execute the desired command u(k) by the steering gear or speed differential mechanism and then execute the steps (8). (10) Calculate the distance PL between the ship and the waypoint according to the time-to-time position p _t =(x _t , y _t ) and the position of the waypoint P _n =(x _n , y _n ), when PL<a , execute (11), when PL≥a, execute (3). (11) When n+1=N, end; when n+1<N, let n=n+1, execute (2).

With reference to Figure 1, under the condition of water flow disturbance, the method includes the following steps:

Step 1: Input the desired path command, the desired path consists of N desired path points, the desired path point P=(P ₁ , P ₂ , P ₃ ... P _n )N≥2, where the nth desired path point coordinate P _n = (x _n , y _n ), 1≤n<N, the initialization is set to n=1, the threshold value of the initialization safety distance is a, a is a constant greater than 0, and the unit is m.

Step 2: Obtain the path point P _n =(x _n , y _n ) and P _n+1 =(x _n+1 , y _n+1 ) two-point straight line path and obtain the direction angle of the straight line path ψ _pn , ψ _pn represents the angle between the straight line path and the positive direction of the X axis, which satisfies:

ψ _pn =a tan 2(y _n+1 -y _n ,x _n+1 -x _n ),ψ _pn ∈[-π,π]

Step 3: Obtain the tracking error Z _e according to the position (x _t , y _t ) of the underwater navigation equipment, the coordinates (x _n , y _n ) of the path point P _n and the path direction angle ψ _pn measured in real time by the sensor, and _Ze satisfies:

Z _e =-(x _t -x _n )sin(ψ _pn )+(y _t -y _n )cos(ψ _pn )

Step 4: Design the radius of convergence circle R _n , where R _n satisfies:

β为正参数常量，用于调节跟对过程的动态行为，且β＞1；in,

β is a positive parameter constant used to adjust the dynamic behavior of the tracking process, and β>1;

Step 5: _{Calculate the lead distance Δ from Ze and R n ,} and Δ satisfies:

Step 6: Calculate the line of sight angle ψ ^* _los , the line of sight angle ψ ^* _los satisfies:

ψ ^* _los = arctan(-K _P *Ze _i -K _i *ξ(Ze _i )), i=1, 2, ...

K_i为积分项的可调参数，ξ(Ze_i)满足：in,

K _i is an adjustable parameter of the integral term, and ξ(Ze _i ) satisfies:

Wherein, Ze _max is a parameter related to the length of the underwater navigation equipment, and the present invention takes Ze _max as 100-200 times the length of the underwater navigation equipment.

Step 7: Obtain the desired heading ψ _d , where ψ _d satisfies:

ψ _d = ψ ^* _los +ψ _pn

Step 8: According to ψ _d and the actual heading state ψ of the ship, calculate the difference between ψ _d and ψ to obtain e(k);

Step 9: Take e(k) as the input of the CFDL_MFAC heading controller, solve the desired command u(k), execute the desired command u(k) by the steering gear or the speed differential mechanism, and execute step 8. The CFDL_MFAC heading control algorithm is as follows, The desired input u(k) can be found from e(k) using the following equation:

为方法运行k次伪偏导数估计值，

为方法运行k-1时伪偏导数估计值，Δy(k)为方法运行k次时的航向系统输出量与方法运行k-1次时的航向系统输出量的差值，u(k)为方法运行k次时航向系统期望输入，u(k-1)为方法运行k-1次时航向系统期望输入，Δu(k-1)为方法运行k-1次时航向系统期望输入与方法运行k-2次时航向系统期望输入之差，ε为一个充分小的正常数ε∈(0,0.001]。Among them, ρ∈(0,1] is the step factor, η∈(0,1] is the step factor, μ＞0 is the weight coefficient, λ＞0 is the constant variable, φ(k) is the pseudo partial derivative,

Run k pseudo-partial derivative estimates for the method,

is the estimated value of the pseudo-partial derivative when the method runs k-1, Δy(k) is the difference between the output of the heading system when the method runs k times and the output of the heading system when the method runs k-1 times, u(k) is The expected input of the heading system when the method runs k times, u(k-1) is the expected input of the heading system when the method runs k-1 times, Δu(k-1) is the expected input of the heading system when the method runs k-1 times and the method runs The difference between the expected inputs of the heading system at times k-2, ε is a sufficiently small positive constant ε∈(0,0.001].

Step 10: Calculate the distance PL between the ship and the path point according to the time-to-time position of the ship pt = (x _t , y _{t )} , and the position of the path point P _n = (x _n , y _n ), when PL<a When , go to step eleven; when PL≥a, go to step three;

Step eleven: when n+1=N, end; when n+1<N, let n=n+1, and execute step two.

With reference to Figure 2, under the condition of water flow disturbance, the tracking control system includes the following steps:

Step 1: Enter the desired path point command to generate the desired path.

Step 2: According to the desired path and the current position information of the ship, the desired heading ψ _d is calculated through the guidance algorithm of FIG. 1 ;

和角速度r的信息，令

其中k为与舰船动力学特性有关的参数，取适当的k值，从而使得舰船的航向系统满足CFDL-MFAC算法对受控系统“拟线性”假设条件的要求，将e(k)作为CFDL_MFAC航向控制器的输入，解算期望指令u(k)。Step 3: The heading control system adopts the CFDL-MFAC control method. According to the actual heading state information ψ, calculate the difference between ψ _d and ψ to obtain e(k), where k represents the kth system operation, and ψ includes the heading of the ship

and information on the angular velocity r, let

Among them, k is a parameter related to the dynamic characteristics of the ship, and an appropriate value of k is taken, so that the heading system of the ship meets the requirements of the CFDL-MFAC algorithm for the "quasi-linear" assumption of the controlled system, and e(k) is taken as The input of the CFDL_MFAC heading controller, which solves the desired command u(k).

Step 4: The steering gear or speed differential mechanism receives and executes the desired command u(k), thereby driving the ship to continuously approach the desired heading.

Step 5: The position of the ship is constantly updated, and the heading is constantly changing. The attitude of the aircraft is observed in real time through sensors such as magnetic compass, and is fed back to the heading control system. The real-time position information is fed back to the guidance system. Repeat step 2 until the ship Track the desired path and implement path following.

A high disturbance immunity adaptive path following method, comprising the following steps:

(1) Input the desired path command, the desired path consists of N desired path points, the desired path point P=(P ₁ , P ₂ , P ₃ . . . P _n )N≥2, where the nth desired path point P _n =(x _n , y _n ), 1≤n<N, the initialization is set to n=1, and the threshold value a of the initialization safety distance is a, a is a constant greater than 0, and the unit is m.

(2) According to the path points P _n =(x _n , y _n ) and P _n+1 =(x _n+1 , y _n+1 ), a straight line path connecting two points is obtained, and the direction angle ψ of the straight line path is obtained _pn .

(3) According to the real-time measured ship position (x _t , y _t ), and at the same time combining the coordinates of the nth path point P _n = (x _n , y _n ) and the information of the path direction angle ψ _pn to calculate the tracking Error _Ze .

其中，Z_e为跟踪误差，

n为正参数，用于调节跟综过程的动态行为，且β＞1。(4) Set the convergence circle radius R _n as

Among them, Z _e is the tracking error,

n is a positive parameter used to adjust the dynamic behavior of the follow-up process, and β>1.

(5) The lead distance Δ can be obtained from _Ze and _Rn .

其中K_i为积分项的可调参数，Δ为超前距离。(6) Now consider the influence of water flow disturbance, and improve the solution of sight angle ψ ^* _los to ψ ^* _los =arctan(-K _P *-K _i *ξ(Ze _i )), i=1, 2,... ,

Among them, K _i is the adjustable parameter of the integral term, and Δ is the lead distance.

(7) The sum of the calculated line-of-sight angles ψ ^* _los and ψ _pn is taken as the desired heading ψ _d .

和角速度r的信，令

其中k为与舰船动力学特性有关的参数，考虑一种极端情况当舰船的艏向增大到180度时，下一时刻变为-180度，通过理论推导当

时，其中T_S表示重定义输出式CFDL-MFAC算法执行一次的时间，Δu(k)为重定义输出式CFDL-MFAC算法执行第k次时舰船的舵角与重定义输出式MFAC算法执行第k-1次时舰船的舵角之差，K，T为舰船的操纵性系数，取适当的k值从而使得舰船的航向系统满足CFDL_MFAC算法对受控系统“拟线性”假设条件的要求。(8) According to ψ _d and the actual heading state ψ of the ship, calculate the difference between ψ _d and ψ to obtain e(k), where k represents the kth operation of the system, and ψ includes the heading of the ship

and the angular velocity r, let

Among them, k is a parameter related to the dynamic characteristics of the ship. Considering an extreme case, when the heading of the ship increases to 180 degrees, the next moment becomes -180 degrees.

, where T _S represents the execution time of the redefine output CFDL-MFAC algorithm once, Δu(k) is the rudder angle of the ship when the redefine output CFDL-MFAC algorithm executes the kth time and the redefine output MFAC algorithm executes The difference between the rudder angles of the ship at the k-1th time, K and T are the maneuverability coefficients of the ship, and an appropriate value of k is taken so that the heading system of the ship satisfies the "quasi-linear" assumption of the controlled system by the CFDL_MFAC algorithm requirements.

(9) Take e(k) as the input of the CFDL-MFAC heading controller, solve the desired command u(k) (such as the desired rudder angle), and execute the desired command u(k) by the steering gear or speed differential mechanism, and then execute Step (8).

(10) Calculate the distance PL between the ship and the waypoint according to the time-to-time position p _t =(x _t , y _t ) and the position of the waypoint P _n =(x _n , y _n ), when PL<a , execute (11), when PL≥a, execute (3).

(11) When n+1=N, end; when n+1<N, let n=n+1, execute (2).

An adaptive path following method for ships further comprising:

(1) Considering the value of the convergence circle radius R _k , when the absolute value of the lateral error _{Ze exceeds R n ,} the above guidance algorithm will fail, so it is necessary to change R _n to adapt to the changing _Ze , and make _Ze fast converges to zero and can be expressed as:

n为正参数，用于调节跟随过程的动态行为，且β＞1；同时上式确保R_n大于横向误差Z_e的条件。in,

n is a positive parameter, which is used to adjust the dynamic behavior of the following process, and β>1; at the same time, the above formula ensures that R _n is greater than the condition of the lateral error _Ze .

K_i为积分项的可调参数，Ze_i为系统运行第i次的跟踪误差，Δ为超前距离，ξ(Ze_i)的具体表达形式如下：(2) Considering the water flow disturbance, the solution of ψ ^* _los is improved as, ψ ^* _los =arctan(-K _P *Ze _i -K _i *ξ(Ze _i )), i=1, 2,...,

K _i is the adjustable parameter of the integral term, Ze _i is the tracking error of the system running for the ith time, Δ is the lead distance, and the specific expression of ξ(Ze _i ) is as follows:

Among them, Δt represents the time step of system operation, Ze _max is a parameter related to the length of the ship, and the present invention takes Ze _max as 100-200 times the length of the underwater navigation equipment.

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)₁,P₂,P₃…P_n) N ≧ 2, where the nth desired path point P_n＝(x_n,y_n) 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 P_n＝(x_n,y_n) And P_n+1＝(x_n+1,y_n+1) Connecting the two points to form a straight line and obtaining the direction angle psi of the straight line_pn，ψ_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(y_n+1-y_n,x_n+1-x_n),ψ_pn∈[-π,π]

step three: position (x) of underwater navigation equipment measured in real time by sensor_t,y_t) Desired path point P_nCoordinate (x)_n,y_n) And straight path azimuth angle psi_pnObtaining a tracking error Z_e，Z_eSatisfies the following conditions:

Z_e＝-(x_t-x_n)sin(ψ_pn)+(y_t-y_n)cos(ψ_pn)

step four: design of convergent circle radius R_n，R_nSatisfies 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 Z_eAnd R_nFinding the advance distance Δ, Δ satisfies:

step six: calculating the viewing angle psi^* _losAngle of sight psi^* _losSatisfies the following conditions:

ψ^* _los＝arctan(-K_P*Ze_i-K_i*ξ(Ze_i)),i＝1、2、...

wherein, Ze_iFor the tracking error of the ith run of the system,

K_ibeing an adjustable parameter of the integral term, ξ (Ze)_i) Satisfies the following conditions:

where Δ t represents the time step in which the system operates, Ze_maxIs 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 is₁For parameters related to ship dynamics, take the appropriate k₁Value, calculating psi_dThe 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:

when | delta u (k-1) | is less than or equal to

Or

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 equipment_n+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 method_d(ii) a The desired heading state psi_dAnd 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 continuously_d。

Images