A path planning method for unmanned boat for navigation mark inspection

Abstract

The present invention belongs to the technical field of inspection unmanned boats, and discloses a path planning method for navigation mark inspection unmanned boats, the specific steps of which are as follows: S1: Path optimization under dynamic sea conditions: by calculating the angle between navigation marks, dynamically decide whether to conduct a detour inspection clockwise or counterclockwise to avoid unnecessary detours. The present invention optimizes the path planning algorithm, combines advanced path generation and tracking strategies, and utilizes real-time error feedback and adjustment mechanisms during the unmanned boat inspection process to effectively optimize the inspection path, thereby improving inspection efficiency, reducing energy consumption, and providing the unmanned boat with a more efficient path tracking capability; through fine adjustment of the unmanned boat control system, combined with a high-precision lateral control strategy, the dynamic forward sight distance and MPC control are used to optimize the heading, thereby greatly improving the lateral control accuracy of the unmanned boat; enabling the unmanned boat to travel more accurately along the specified trajectory in an actual environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned inspection boats, and specifically relates to a path planning method for an unmanned boat for navigation mark inspection. Background Art

[0002] The path planning of the inspection unmanned boat mainly relies on: path planning technology: using general path planning methods such as the A* algorithm and the RRT algorithm, using a circular orbit with a fixed radius, and the orbiting direction is preset to clockwise or counterclockwise; motion control technology: using PID trajectory tracking control, the outer loop (position loop) outputs the desired speed, and the inner loop (speed loop) outputs the thruster command; traditional model predictive control (MPC), using 2-DOF (surge, bow roll) simplified dynamic model and fixed forward distance LOS guidance technology.

[0003] However, in the existing technology, the fixed detour direction leads to path redundancy (an average increase of 15%-20% in the journey), and the geometric relationship between consecutive buoys is not taken into account, which easily leads to "sharp turn" conditions. The traditional obstacle avoidance algorithm is not suitable for the labeling operation requirements of buoy inspection; the traditional PID control has a steady-state error of 0.5-1.2 meters in a dynamic water flow environment, and the traditional LOS guidance does not take into account the interference of lateral flow. The track deviation exceeds 2 meters and the control parameters are solidified, resulting in poor adaptability to complex working conditions; the parameters of the PID controller and the fixed forward-looking distance LOS guidance are calibrated based on calm sea conditions and are not dynamically adjusted with the environment, which easily leads to poor control stability in complex sea conditions, thereby causing the track to fluctuate in a serpentine manner. Summary of the Invention

[0004] The purpose of the present invention is to provide a path planning method for a navigation mark inspection unmanned boat to solve the problems raised in the above background technology.

[0005] In order to achieve the above-mentioned object, the present invention provides the following technical solution: a path planning method for a navigation mark inspection unmanned boat, the specific steps of which are as follows:

[0006] S1: Path optimization under dynamic sea conditions: By calculating the angle between navigation marks, we can dynamically decide whether to conduct a detour inspection clockwise or counterclockwise to avoid unnecessary detours.

[0007] Determine the exact position of each beacon in the inspection area. For each beacon position (x buoy ,y buoy ), set the inspection radius to radius, the total inspection angle to θ, and perform a circular inspection. The inspection path is as follows:

[0008] x path =x buoy +radius×cosθ

[0009] y path =ybuoy +radius×sinθ

[0010] After knowing the position of the navigation mark to be inspected, it is necessary to calculate the position of the navigation mark according to the current inspection position (x curbuoy ,y curbuoy ), the previous inspection beacon position (x prebuoy ,y prebuoy ) and the position of the next inspection beacon (x nextbuoy ,y nextbuoy ) calculate the angle θ diff :

[0011] θ start =π+atan2(y curbuoy -y prebuoy ,x curbuoy -x prebuoy )

[0012] θ next =atan2(y nextbuoy -y curbuoy ,x nextbuoy -x curbuoy )

[0013]

[0014] In calculating θ diff Then, compare it with π. If θ diff <π, the orbiting route is clockwise, otherwise it is counterclockwise;

[0015] S2: High-precision trajectory tracking under complex disturbances: Optimize tracking error through MPC-adaptive look-ahead distance LOS joint control;

[0016] S2.1: Design an MPC prediction model

[0017] The basic principle of trajectory tracking of patrol unmanned boat based on MPC:

[0018] In the system state measurement link, the real-time position, speed and heading angle of the patrol unmanned boat are obtained. The system model describes the state change law of the unmanned boat. In the state prediction, based on the current state and model, the future state trajectory is predicted. By solving the optimization problem, the prediction step length N is solved. p The optimal predictive control input sequence is then applied to the actuators of the patrol unmanned boat. The patrol unmanned boat adjusts its navigation according to the control input and feedbacks the status through sensors. This process is repeated continuously. Through this rolling optimization strategy, the input of each step is the optimal value calculated based on the current state.

[0019] Build a nonlinear state-space model:

[0020] In the path tracking process of the inspection unmanned boat, the sensor is first used to collect the motion state information of the current inspection unmanned boat. Then, the future predicted output trajectory sequence is calculated by combining the system's prediction model, the control input at the previous moment, and the unknown input sequence. Then, the predicted output trajectory sequence, the reference trajectory, and the unknown input sequence are incorporated into the objective function. Under the system constraints, the optimal input sequence is solved to minimize the objective function value. Finally, the first control input in the optimal input sequence is applied to the inspection unmanned boat and continues for one control cycle.

[0021] Define x = [x y ψu vr] T is the system state variable, y=[xy] T is the system output, and the motion equation of the inspection unmanned boat is:

[0022]

[0023] The function f is the nonlinear equation of the USV motion model, τ d is the environmental interference, v c is the velocity of the water flow in the attached coordinates,

[0024] System linearization and discretization:

[0025] In model predictive control, approximate linearization techniques are used. It is assumed that the reference system is already completely on the desired path. By obtaining the state and control variables at each moment on the path, and processing the deviation between the reference system and the current system, a model predictive controller is designed to track the desired path. That is, ignoring disturbances, the reference trajectory can be expressed as:

[0026]

[0027] At any reference point (x R ,u R ) and perform a first-order Taylor expansion of the function to obtain:

[0028]

[0029] Subtracting the above two equations yields a new state space model of the system:

[0030]

[0031] Where,

[0032] The Euler method is used to discretize the model, and we have:

[0033] A k =I+T·A,B k=T·B

[0034] Among them, A k and B k are the discretized system matrices, T is the discrete time step, and I is the unit matrix, so we can get:

[0035]

[0036] Where, represents the discrete system output, that is, This formula represents the nonlinear system at any reference point (x R ,u R ) after linearization of the system equations;

[0037] Set up constraints:

[0038] At a specific time k and prediction period N p The limits of the control quantity, control increment, and output constraints can be expressed as follows:

[0039] Δu min ≤Δu(k+i)≤Δu max ,i=0,1,2…,N c -1

[0040] u min ≤Δu(k+i)≤Δu max ,i=0,1,2,…,N c

[0041] y min ≤y(k+i)≤u max ,i=1,2,…,N p

[0042] Among them, N p is the prediction step length, N c To control the step size, Δu min , Δu max is the input increment constraint, u min 、u max is the input constraint, y min 、y max is the output constraint, and satisfies Δu(k+i)=u(k+i)-u(k+i-1). Generally speaking, N p ≥N c , and u(k+i)=u(k+N c -1),i=N c ,...,N p -1,

[0043] The discrete state space equation can be obtained by using the Euler method and a new state quantity can be constructed. After combining, a new spatial state model can be obtained:

[0044]

[0045] in, m is the system control dimension, n is the system state dimension, for the underactuated inspection unmanned boat motion model, m = 2, n = 6, the system is in the prediction time domain N p The prediction output expression is:

[0046]

[0047] in N is the time domain in the prediction p The predicted output within the time domain is ΔU(k), which is the control time domain N c The predicted input within, Ψ k and θ k is the system matrix, which is defined as follows:

[0048]

[0049]

[0050] To ensure that the inspection unmanned boat can quickly and smoothly track the desired trajectory, an objective function must be constructed based on the deviation of the system state quantity, the control quantity, and its increment. When designing the objective function, it is necessary to consider the speed of tracking the target and the energy consumption of the system input. Therefore, the objective function is defined as follows:

[0051]

[0052] In this formula, Q and R represent weight matrices, and their weights can be adjusted accordingly according to changes in control requirements. During the solution process, the objective function will be simplified as follows:

[0053]

[0054] in:

[0055]

[0056] Therefore, the ship trajectory tracking based on linear MPC can be described as the following optimal value problem:

[0057]

[0058] U min ≤MΔU(k)+U(k)≤U max ,

[0059] Y min ≤Y(k)+YR (k)≤Y max .

[0060] S2.2: Design of adaptive LOS

[0061] In order to solve the problem of oscillation and jitter in the tracking process caused by the fixed Δ, a design based on the lateral error y e The formula for adjusting the foresight distance Δ online and the designed time-varying foresight distance formula enable the inspection unmanned boat to complete path tracking more quickly and smoothly. The time-varying foresight distance formula is as follows:

[0062]

[0063] During the tracking process, the minimum and maximum values ​​of the forward sight distance (i.e. Δ min and Δ max ) are set to 2 times and 4 times the hull length respectively, γ is the convergence rate, which is set to 0.05. In order to obtain excellent path tracking effect, when the unmanned boat deviates far from the target path, the forward sight distance takes a smaller value to speed up the convergence speed; when the unmanned boat approaches the target path, the forward sight distance takes a larger value to improve the stability of the unmanned boat and effectively avoid oscillation during the path tracking process.

[0064] Since the inspection unmanned boat is disturbed by the environment and generates a drifting speed v, the actual total speed U is:

[0065]

[0066] This results in a small sideslip angle β between the actual movement direction of the inspection unmanned boat and the bow rolling angle direction:

[0067] β=atan2(v,u)

[0068] Therefore, the expected bow angle of the inspection unmanned boat considering the sideslip angle is ψ d In order to make the actual movement direction of the unmanned boat the desired course angle, that is, towards (x los ,y los )sports:

[0069]

[0070] As a preferred technical solution of the present invention, the positions of the navigation marks described in S1 are set to three evenly distributed points, and the distribution pattern and number of the navigation marks are allowed to be adjusted according to specific circumstances.

[0071] As a preferred technical solution of the present invention, the control performance of the MPC described in S2.1 is affected by the modeling accuracy, prediction step size and efficiency of solving the objective function.

[0072] As a preferred technical solution of the present invention, when establishing the nonlinear state space model as described in S2.1, due to the existence of external interference factors, a deviation may occur between the actual state of the ship and the predicted state.

[0073] As a preferred technical solution of the present invention, when establishing the nonlinear state space model as described in S2.1, a hydrodynamic model is used as the control basis for trajectory tracking.

[0074] As a preferred technical solution of the present invention, the hydrodynamic model does not include the driving models of the rudder and propeller and the influence of their interaction, and directly uses force and torque as input parameters of trajectory tracking control, and position as control output.

[0075] As a preferred technical solution of the present invention, the system linearization and discretization described in S2.1 requires first discretizing the system and then linearizing it, and finally solving the optimal control sequence using linear MPC.

[0076] As a preferred technical solution of the present invention, when setting the constraints described in S2.1, it is necessary to consider the limits of the control quantity and the control increment constraints, and it is also necessary to impose boundary restrictions on the motion path of the inspection unmanned boat.

[0077] As a preferred technical solution of the present invention, the foresight distance Δ described in S2.2 is a fixed value. In the initial stage of tracking, the lateral error y e If the distance Δ is too large, the inspection unmanned boat should converge to the LOS point quickly, but the fixed forward sight distance Δ makes it impossible for the inspection unmanned boat to adjust its course quickly, resulting in a slow convergence speed.

[0078] The beneficial effects of the present invention are as follows:

[0079] The present invention optimizes the path planning algorithm and combines it with advanced path generation and tracking strategies. During the inspection process of the unmanned boat, the inspection path is effectively optimized by using real-time error feedback and adjustment mechanisms, thereby improving inspection efficiency, reducing energy consumption, and providing the unmanned boat with a more efficient path tracking capability; through fine adjustment of the unmanned boat control system and combining it with a high-precision lateral control strategy, the heading is optimized using dynamic foresight distance and MPC control, which greatly improves the lateral control accuracy of the unmanned boat; enabling the unmanned boat to travel more accurately along the specified trajectory in the actual environment, thereby achieving more efficient path inspection and significantly improving the accuracy and reliability of task completion; by introducing an improved heading adjustment algorithm and a dynamic control strategy based on real-time error feedback, the unmanned boat can significantly improve the response speed when adjusting its heading, effectively reducing the turning time, improving the maneuverability and efficiency of the unmanned boat in complex environments, and providing the unmanned boat with higher flexibility and responsiveness when performing inspection tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 Flowchart of the present invention;

[0081] Figure 2 This is the basic principle diagram of MPC of the present invention;

[0082] Figure 3 This is a comparison diagram of the curve tracking path simulation of the present invention;

[0083] Figure 4 This is a comparison diagram of the lateral error comparison simulation of the present invention;

[0084] Figure 5 This is a simulation diagram of the circular tracking experiment of the present invention;

[0085] Figure 6 This is a comparison chart of the simulation results of the circular tracking experiment of the present invention;

[0086] Figure 7 This is a simulation diagram of the inspection around a navigation mark according to the present invention;

[0087] Figure 8 This is a simulation analysis diagram of the inspection around the navigation mark of the present invention. DETAILED DESCRIPTION

[0088] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0089] like Figures 1 to 8 As shown, an embodiment of the present invention provides a path planning method for a navigation mark inspection unmanned boat, and the specific steps are as follows:

[0090] S1: Path optimization under dynamic sea conditions: By calculating the angle between navigation marks, we can dynamically decide whether to conduct a detour inspection clockwise or counterclockwise to avoid unnecessary detours.

[0091] Determine the exact position of each beacon in the inspection area. For each beacon position (x buoy ,y buoy ), set the inspection radius to radius, the total inspection angle to θ, and perform a circular inspection. The inspection path is as follows:

[0092] x path =x buoy +radius×cosθ

[0093] y path =ybuoy +radius×sinθ

[0094] After knowing the position of the navigation mark to be inspected, it is necessary to calculate the position of the navigation mark according to the current inspection position (x curbuoy ,y curbuoy ), the previous inspection beacon position (x prebuoy ,y prebuoy ) and the position of the next inspection beacon (x nextbuoy ,y nextbuoy ) calculate the angle θ diff :

[0095] θ start =π+atan2(y curbuoy -y prebuoy ,x curbuoy -x prebuoy )

[0096] θ next =atan2(y nextbuoy -y curbuoy ,x nextbuoy -x curbuoy )

[0097]

[0098] In calculating θ diff Then, compare it with π. If θ diff <π, the orbiting route is clockwise, otherwise it is counterclockwise;

[0099] S2: High-precision trajectory tracking under complex disturbances: Optimize tracking error through MPC-adaptive look-ahead distance LOS joint control;

[0100] S2.1: Design an MPC prediction model

[0101] The basic principle of trajectory tracking of patrol unmanned boat based on MPC:

[0102] In the system state measurement link, the real-time position, speed and heading angle of the patrol unmanned boat are obtained. The system model describes the state change law of the unmanned boat. In the state prediction, based on the current state and model, the future state trajectory is predicted. By solving the optimization problem, the prediction step length N is solved. p The optimal predictive control input sequence is then applied to the actuators of the patrol unmanned boat. The patrol unmanned boat adjusts its navigation according to the control input and feedbacks the status through sensors. This process is repeated continuously. Through this rolling optimization strategy, the input of each step is the optimal value calculated based on the current state.

[0103] Build a nonlinear state-space model:

[0104] In the path tracking process of the inspection unmanned boat, the sensor is first used to collect the motion state information of the current inspection unmanned boat. Then, the future predicted output trajectory sequence is calculated by combining the system's prediction model, the control input at the previous moment, and the unknown input sequence. Then, the predicted output trajectory sequence, the reference trajectory, and the unknown input sequence are incorporated into the objective function. Under the system constraints, the optimal input sequence is solved to minimize the objective function value. Finally, the first control input in the optimal input sequence is applied to the inspection unmanned boat and continues for one control cycle.

[0105] Define x = [x y ψu vr] T is the system state variable, y=[xy] T is the system output, and the motion equation of the inspection unmanned boat is:

[0106]

[0107] y=P2x

[0108] The function f is the nonlinear equation of the USV motion model, τ d is the environmental interference, v c is the velocity of the water flow in the attached coordinates,

[0109] System linearization and discretization:

[0110] In model predictive control, approximate linearization techniques are used. It is assumed that the reference system is already completely on the desired path. By obtaining the state and control variables at each moment on the path, and processing the deviation between the reference system and the current system, a model predictive controller is designed to track the desired path. That is, ignoring disturbances, the reference trajectory can be expressed as:

[0111]

[0112] At any reference point (x R ,u R ) and perform a first-order Taylor expansion of the function to obtain:

[0113]

[0114] Subtracting the above two equations yields a new state space model of the system:

[0115]

[0116] Where,

[0117] The Euler method is used to discretize the model, and we have:

[0118] Ak =I+T·A,B k =T·B

[0119] Among them, A k and B k are the discretized system matrices, T is the discrete time step, and I is the unit matrix, so we can get:

[0120]

[0121] Where, represents the discrete system output, that is, This formula represents the nonlinear system at any reference point (x R ,u R ) after linearization of the system equations;

[0122] Set up constraints:

[0123] At a specific time k and prediction period N p The limits of the control quantity, control increment, and output constraints can be expressed as follows:

[0124] Δu min ≤Δu(k+i)≤Δu max ,i=0,1,2…,N c -1

[0125] u min ≤Δu(k+i)≤Δu max ,i=0,1,2,…,N c

[0126] y min ≤y(k+i)≤u max ,i=1,2,…,N p

[0127] Among them, N p is the prediction step length, N c To control the step size, Δu min , Δu max is the input increment constraint, u min 、u max is the input constraint, y min 、y max is the output constraint, and satisfies Δu(k+i)=u(k+i)-u(k+i-1). Generally speaking, N p ≥N c , and u(k+i)=u(k+N c -1),i=N c ,...,N p -1,

[0128] The discrete state space equation can be obtained by using the Euler method and a new state quantity can be constructed. After combining, a new spatial state model can be obtained:

[0129]

[0130] in, m is the system control dimension, n is the system state dimension, for the underactuated inspection unmanned boat motion model, m = 2, n = 6, the system is in the prediction time domain N p The prediction output expression is:

[0131]

[0132] in N is the time domain in the prediction p The predicted output within the time domain is ΔU(k), which is the control time domain N c The predicted input within, Ψ k and θ k is the system matrix, which is defined as follows:

[0133]

[0134]

[0135] To ensure that the inspection unmanned boat can quickly and smoothly track the desired trajectory, an objective function must be constructed based on the deviation of the system state quantity, the control quantity, and its increment. When designing the objective function, it is necessary to consider the speed of tracking the target and the energy consumption of the system input. Therefore, the objective function is defined as follows:

[0136]

[0137] In this formula, Q and R represent weight matrices, and their weights can be adjusted accordingly according to changes in control requirements. During the solution process, the objective function will be simplified as follows:

[0138]

[0139] in:

[0140]

[0141] Therefore, the ship trajectory tracking based on linear MPC can be described as the following optimal value problem:

[0142]

[0143] U min ≤MΔU(k)+U(k)≤U max ,

[0144] Y min ≤Y(k)+Y R (k)≤Y max .

[0145] S2.2: Design of adaptive LOS

[0146] In order to solve the problem of oscillation and jitter in the tracking process caused by the fixed Δ, a design based on the lateral error y e The formula for adjusting the foresight distance Δ online and the designed time-varying foresight distance formula enable the inspection unmanned boat to complete path tracking more quickly and smoothly. The time-varying foresight distance formula is as follows:

[0147]

[0148] During the tracking process, the minimum and maximum values ​​of the forward sight distance (i.e. Δ min and Δ max ) are set to 2 times and 4 times the hull length respectively, γ is the convergence rate, which is set to 0.05. In order to obtain excellent path tracking effect, when the unmanned boat deviates far from the target path, the forward sight distance takes a smaller value to speed up the convergence speed; when the unmanned boat approaches the target path, the forward sight distance takes a larger value to improve the stability of the unmanned boat and effectively avoid oscillation during the path tracking process.

[0149] Since the inspection unmanned boat is disturbed by the environment and generates a drifting speed v, the actual total speed U is:

[0150]

[0151] This results in a small sideslip angle β between the actual movement direction of the inspection unmanned boat and the bow rolling angle direction:

[0152] β=atan2(v,u)

[0153] Therefore, the expected bow angle of the inspection unmanned boat considering the sideslip angle is ψ d In order to make the actual movement direction of the unmanned boat the desired course angle, that is, towards (x los ,y los )sports:

[0154]

[0155] By optimizing the path planning algorithm, combining it with advanced path generation and tracking strategies and utilizing real-time error feedback and adjustment mechanisms, the inspection path was effectively optimized. By fine-tuning the UAV control system and combining it with a high-precision lateral control strategy, the UAV's lateral control accuracy was greatly improved by utilizing dynamic foresight distance and MPC control to optimize the heading. By introducing an improved heading adjustment algorithm and a dynamic control strategy based on real-time error feedback, the UAV's response speed can be significantly improved when making heading adjustments.

[0156] The positions of the navigation marks in S1 are set to three evenly distributed points, and the distribution pattern and number of the navigation marks can be adjusted according to the specific situation.

[0157] By setting the navigation mark positions to three evenly distributed points, research and calculation can be facilitated. By selecting the best inspection direction (clockwise or counterclockwise), the repeatability of the inspection path can be significantly reduced, thereby saving the time required for inspection.

[0158] Among them, the control performance of MPC in S2.1 is affected by modeling accuracy, prediction step size and efficiency of objective function solution.

[0159] In the actual control process, appropriate simplified models, control parameters and objective functions should be selected according to specific needs to meet the requirements of different control systems.

[0160] Among them, when establishing the nonlinear state space model in S2.1, due to the existence of external interference factors, there will be a deviation between the actual state of the ship and the predicted state.

[0161] In order to correct the deviation, it is necessary to re-acquire the actual motion state information of the ship through sensors, and use this as a basis for the next step of optimal input calculation.

[0162] Among them, when establishing the nonlinear state space model in S2.1, the hydrodynamic model is used as the control basis for trajectory tracking.

[0163] Trajectory tracking of an inspection unmanned boat means tracking the path of a moving target in real time. Therefore, high maneuverability is required of the inspection unmanned boat. Relying solely on heading control is not enough to meet the precise requirements of trajectory tracking. Therefore, a hydrodynamic model is needed as the control basis for trajectory tracking.

[0164] Among them, the hydrodynamic model does not include the driving models of the rudder and propeller and the influence of their interaction. The force and torque are directly used as the input parameters of trajectory tracking control, and the position is used as the control output.

[0165] By directly using force and torque as input parameters of trajectory tracking control and position as control output, the complexity of the model can be reduced and accurate trajectory tracking can be achieved. At the same time, considering the under-actuated nature of the inspection unmanned boat, the system input is composed of three-degree-of-freedom input τ = [f u f v t u ] becomes a two-degree-of-freedom input τ'=[f u t u ].

[0166] Among them, the system linearization and discretization in S2.1 require the system to be discretized first, then linearized, and finally the optimal control sequence is solved using linear MPC.

[0167] By discretizing and linearizing the system in succession and then using linear MPC to solve the optimal control sequence, the path tracking control problem of the nonlinear system of the inspection unmanned boat can be solved.

[0168] Among them, when setting the constraints in S2.1, it is necessary to consider the limit of the control quantity and the control increment constraints, and it is also necessary to impose boundary restrictions on the motion path of the inspection unmanned boat.

[0169] Due to the mechanical performance limitations of the propeller drive mechanism and servo of the patrol unmanned boat, the movement rate of the patrol unmanned boat has certain limitations. Therefore, the limit of the control quantity and the control increment constraint conditions must be considered. Imposing boundary restrictions on the movement path of the patrol unmanned boat can ensure navigation safety.

[0170] Among them, the foresight distance Δ in S2.2 is a fixed value. In the initial stage of tracking, the lateral error y e If the distance Δ is too large, the inspection unmanned boat should converge to the LOS point quickly, but the fixed forward sight distance Δ makes it impossible for the inspection unmanned boat to adjust its course quickly, resulting in a slow convergence speed.

[0171] As the UAV approaches the reference path, y e As the value of gradually decreases, the primary goal of the system shifts to smooth tracking, while a fixed Δ will cause oscillation and jitter in the tracking process, thus affecting path planning.

[0172] It should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus.

[0173] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method for a navigation mark inspection unmanned boat, characterized in that: The specific steps are as follows: S1: Path optimization under dynamic sea conditions: By calculating the angle between navigation marks, we can dynamically decide whether to conduct a detour inspection clockwise or counterclockwise to avoid unnecessary detours. Determine the exact position of each beacon in the inspection area. For each beacon position (x buoy ,y buoy ), set the inspection radius to radius, the total inspection angle to θ, and perform a circular inspection. The inspection path is as follows: x path =x buoy +radius×cosθ and path =and buoy +radius×sinθ After knowing the position of the navigation mark to be inspected, it is necessary to calculate the position of the navigation mark according to the current inspection position (x curbuoy ,y curbuoy ), the previous inspection beacon position (x prebuoy ,y prebuoy ) and the position of the next inspection beacon (x nextbuoy ,y nextbuoy ) calculate the angle θ diff : i start =π+atan2(y curbuoy -y prebuoy ,x curbuoy -x prebuoy ) θ next =atan2(y nextbuoy -y curbuoy ,x nextbuoy -x curbuoy ) In calculating θ diff Then, compare it with π. If θ diff <π, the orbiting route is clockwise, otherwise it is counterclockwise; S2: High-precision trajectory tracking under complex disturbances: Optimize tracking error through MPC-adaptive look-ahead distance LOS joint control; S2.1: Design an MPC prediction model The basic principle of trajectory tracking of patrol unmanned boat based on MPC: In the system state measurement link, the real-time position, speed and heading angle of the patrol unmanned boat are obtained. The system model describes the state change law of the unmanned boat. In the state prediction, based on the current state and model, the future state trajectory is predicted. By solving the optimization problem, the prediction step length N is solved. p The optimal predictive control input sequence is then applied to the actuators of the patrol unmanned boat. The patrol unmanned boat adjusts its navigation according to the control input and feedbacks the status through sensors. This process is repeated continuously. Through this rolling optimization strategy, the input of each step is the optimal value calculated based on the current state. Build a nonlinear state-space model: In the path tracking process of the inspection unmanned boat, the sensor is first used to collect the motion state information of the current inspection unmanned boat. Then, the future predicted output trajectory sequence is calculated by combining the system's prediction model, the control input at the previous moment, and the unknown input sequence. Then, the predicted output trajectory sequence, the reference trajectory, and the unknown input sequence are incorporated into the objective function. Under the system constraints, the optimal input sequence is solved to minimize the objective function value. Finally, the first control input in the optimal input sequence is applied to the inspection unmanned boat and continues for one control cycle. Define x = [x y ψu vr] T is the system state variable, y=[xy] T is the system output, and the motion equation of the inspection unmanned boat is: y=P2x The function f is the nonlinear equation of the USV motion model, τ d is the environmental interference, v c is the velocity of the water flow in the attached coordinates, System linearization and discretization: In model predictive control, approximate linearization techniques are used. It is assumed that the reference system is already completely on the desired path. By obtaining the state and control variables at each moment on the path, and processing the deviation between the reference system and the current system, a model predictive controller is designed to track the desired path. That is, ignoring disturbances, the reference trajectory can be expressed as: At any reference point (x R ,u R ) and perform a first-order Taylor expansion of the function to obtain: Subtracting the above two equations yields a new state space model of the system: Where, The Euler method is used to discretize the model, and we have: A k =I+T·A,B k =T·B Among them, A k and B k are the discretized system matrices, T is the discrete time step, and I is the unit matrix, so we can get: Where, represents the discrete system output, that is, This formula represents the nonlinear system at any reference point (x R ,u R ) after linearization of the system equations; Set up constraints: At a specific time k and prediction period N p The limits of the control quantity, control increment, and output constraints can be expressed as follows: Δu min ≤Δu(k+i)≤Δu max ,i=0,1,2…,N c -1 u min ≤Δu(k+i)≤Δu max ,i=0,1,2,…,N c y min ≤y(k+i)≤u max ,i=1,2,…,N p Among them, N p is the prediction step length, N c To control the step size, Δu min , Δu max is the input increment constraint, u min 、u max is the input constraint, y min 、y max is the output constraint, and satisfies Δu(k+i)=u(k+i)-u(k+i-1). Generally speaking, N p ≥N c , and u(k+i)=u(k+N c -1),i=N c ,...,N p -1, The discrete state space equation can be obtained by using the Euler method and a new state quantity can be constructed. After combining, a new spatial state model can be obtained: in, m is the system control dimension, n is the system state dimension, for the underactuated inspection unmanned boat motion model, m = 2, n = 6, the system is in the prediction time domain N p The prediction output expression is: in N is the time domain in the prediction p The predicted output within the time domain is ΔU(k), which is the control time domain N c The predicted input within, Ψ k and θ k is the system matrix, which is defined as follows: To ensure that the inspection unmanned boat can quickly and smoothly track the desired trajectory, an objective function must be constructed based on the deviation of the system state quantity, the control quantity, and its increment. When designing the objective function, it is necessary to consider the speed of tracking the target and the energy consumption of the system input. Therefore, the objective function is defined as follows: In this formula, Q and R represent weight matrices, and their weights can be adjusted accordingly according to changes in control requirements. During the solution process, the objective function will be simplified as follows: in: Therefore, the ship trajectory tracking based on linear MPC can be described as the following optimal value problem: argmin{J(ΔU(k))} st ΔU min ≤ΔU(k)≤ΔU max IN min ≤MΔU(k)+U(k)≤U max , AND min ≤Y(k)+Y R (k)≤Y max . S2.2: Design of adaptive LOS In order to solve the problem of oscillation and jitter in the tracking process caused by the fixed Δ, a design based on the lateral error y e The formula for adjusting the foresight distance Δ online and the designed time-varying foresight distance formula enable the inspection unmanned boat to complete path tracking more quickly and smoothly. The time-varying foresight distance formula is as follows: During the tracking process, the minimum and maximum values ​​of the forward sight distance (i.e. Δ min and Δ max ) are set to 2 times and 4 times the hull length respectively, γ is the convergence rate, which is set to 0.

05. In order to obtain excellent path tracking effect, when the unmanned boat deviates far from the target path, the forward sight distance takes a smaller value to speed up the convergence speed; when the unmanned boat approaches the target path, the forward sight distance takes a larger value to improve the stability of the unmanned boat and effectively avoid oscillation during the path tracking process. Since the inspection unmanned boat is disturbed by the environment and generates a drifting speed v, the actual total speed U is: This results in a small sideslip angle β between the actual movement direction of the inspection unmanned boat and the bow rolling angle direction: β=atan2(v,u) Therefore, the expected bow angle of the inspection unmanned boat considering the sideslip angle is ψ d In order to make the actual movement direction of the unmanned boat the desired course angle, that is, towards (x los ,y los )sports:

2. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: The positions of the navigation marks described in S1 are set to three evenly distributed points, and the distribution pattern and number of the navigation marks can be adjusted according to specific circumstances.

3. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: The control performance of MPC described in S2.1 is affected by the modeling accuracy, prediction step size, and efficiency of objective function solution.

4. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: When establishing the nonlinear state space model as described in S2.1, due to the existence of external interference factors, there will be a deviation between the actual state of the ship and the predicted state.

5. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: When establishing the nonlinear state space model as described in S2.1, the hydrodynamic model is used as the control basis for trajectory tracking.

6. The path planning method for a navigation mark inspection unmanned boat according to claim 5, characterized in that: The hydrodynamic model does not include the driving models of the rudder and propeller and the influence of their interaction. Force and torque are directly used as input parameters of trajectory tracking control, and position is used as control output.

7. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: The system linearization and discretization described in S2.1 require that the system be discretized first, then linearized, and finally the optimal control sequence be solved using linear MPC.

8. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: When setting the constraints described in S2.1, it is necessary to consider the limits of the control quantity and the control increment constraints, and it is also necessary to impose boundary restrictions on the motion path of the inspection unmanned boat.

9. The path planning method for a navigation mark inspection unmanned boat according to claim 1, characterized in that: The foresight distance Δ mentioned in S2.2 is a fixed value. In the initial stage of tracking, the lateral error y e If the distance Δ is too large, the inspection unmanned boat should converge to the LOS point quickly, but the fixed forward sight distance Δ makes it impossible for the inspection unmanned boat to adjust its course quickly, resulting in a slow convergence speed.

Images