Path planning method for navigation mark inspection unmanned surface vehicle

Abstract

The invention belongs to the technical field of inspection unmanned surface vehicles, and discloses a navigation mark inspection unmanned surface vehicle path planning method comprising the following specific steps: S1, path optimization under a dynamic sea condition: calculating an included angle between navigation marks to dynamically determine clockwise or anticlockwise detour inspection so as to avoid unnecessary detour; by optimizing a path planning algorithm and combining an advanced path generation and tracking strategy, in the unmanned ship inspection process, the inspection path is effectively optimized by utilizing a real-time error feedback and adjustment mechanism, the inspection efficiency is improved, the energy consumption is reduced, and a more efficient path tracking capability is provided for the unmanned ship; through fine adjustment of an unmanned ship control system and combination of a high-precision transverse control strategy, the course is optimized by using a dynamic foresight distance and MPC control, and the transverse control precision of the unmanned ship is greatly improved; therefore, the unmanned ship can travel along the specified track more accurately in the actual environment.

Description

Technical Field

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

[0002] The path planning of inspection unmanned boats mainly relies on: path planning technology: adopting general path planning methods such as A* algorithm and RRT algorithm, using a circular detour trajectory with a fixed radius, and the detour direction is preset as clockwise or counterclockwise; motion control technology: adopting 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 a 2-DOF (surge, yaw) simplified dynamic model and fixed look-ahead distance LOS guidance technology.

[0003] However, in the prior art, the fixed detour direction leads to redundant paths (the journey increases by 15%-20% on average), the geometric relationship between consecutive buoys is not considered, which is prone to "sharp turn" working conditions, and the traditional obstacle avoidance algorithm is not applicable to the label pasting 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, the traditional LOS guidance does not consider the lateral flow interference, and the track deviation exceeds 2 meters. The control parameters are fixed, resulting in poor adaptability to complex working conditions; the parameters of the PID controller and the fixed look-ahead distance LOS guidance are calibrated based on calm sea conditions and are not adjusted dynamically with the environment, which is prone to poor control stability in complex sea conditions, resulting in snake-like fluctuations in the track. Summary of the Invention

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

[0005] To achieve the above purpose, the present invention provides the following technical solution: A path planning method for a navigation buoy inspection unmanned boat, and the specific steps are as follows:

[0006] S1: Path optimization under dynamic sea conditions: Dynamically determine whether to perform detour inspection clockwise or counterclockwise by calculating the angle between buoys, avoiding unnecessary detours,

[0007] Determine the exact positions of each buoy in the inspection area. For each buoy position (x buoy , y buoy ), set its inspection radius as radius, the total angle of inspection around as θ, and perform circular inspection. Its 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 angle θ based on the current position (x curbuoy , y curbuoy ) of the inspection navigation mark, the position (x prebuoy , y prebuoy ) of the previous inspection navigation mark, and the position (x nextbuoy , y nextbuoy ) of the next inspection navigation mark: 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] After calculating θ diff , compare it with π. If θ diff < π, the bypass route is clockwise, otherwise it is counterclockwise;

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

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

[0017] Basic principle of trajectory tracking of inspection unmanned boat based on MPC:

[0018] In the system state measurement link, obtain the real-time position, speed and heading angle of the inspection unmanned boat. The system model describes the state change law of the unmanned boat. In state prediction, based on the current state and the model, predict the future state trajectory. By solving the optimization problem, solve the optimal predicted control input sequence within the prediction step N p . Then apply the optimized control input to the actuator of the inspection unmanned boat. The inspection unmanned boat adjusts its navigation according to the control input and feeds back the state through the sensor, and then repeats this process continuously. Through this rolling optimization strategy, the input of each step is the optimal value calculated based on the current state;

[0019] Establish a nonlinear state space model:

[0020] During the path tracking of the inspection unmanned boat, first, sensors are used to collect the motion state information of the current inspection unmanned boat. Subsequently, combining the prediction model of the system, the control input at the previous moment, and the unknown input sequence, the future predicted output trajectory sequence is calculated. Then, the predicted output trajectory sequence, the reference trajectory, and the unknown input sequence are incorporated into the objective function, and 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 lasts for one control period.

[0021] Define \(x = [x\ y\ \psi\ u\ v\ r]\) T as the system state variables, and \(y = [x\ y]\) T as the system output. The motion equation of the inspection unmanned boat is:

[0022]

[0023] where the function \(f\) is the nonlinear equation of the USV motion model, \(\tau\) d is the environmental disturbance, and \(v\) c is the velocity of the water flow in the body-fixed coordinate system.

[0024] System linearization and discretization:

[0025] In model predictive control, an approximate linearization technique is adopted. Assuming that the reference system is already fully on the desired path, by obtaining the state variables and control variables at each moment on the path, based on the processing of the deviation between the reference system and the current system, a model predictive controller is designed to track the desired path. That is, in the case of ignoring disturbances, the reference trajectory can be expressed as:

[0026]

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

[0028]

[0029] Subtract the above two equations to obtain the new state-space model of the system:

[0030]

[0031] In the formula,

[0032] Adopt the Euler method to discretize the model, and we have:

[0033] \(A\) k = \(I + T\cdot A\), \(B\) k= T·B

[0034] Among them, A k and B k are respectively the discretized system matrices, T is the discrete time step, and I is the identity matrix. Therefore, we can obtain:

[0035]

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

[0037] Set the constraint conditions:

[0038] At a specific time k and prediction horizon N p within, the limits of the control quantity, control increment, and constraints of the output quantity 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, N c is the control step, Δu min , Δu max are the input increment constraints, u min , u max are the input constraints, y min , y max are the output constraints, and it 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] Using the forward Euler method, the discrete state-space equation can be obtained, and a new state variable is constructed A new spatial state model can be obtained after combination:

[0044]

[0045] Among them, m is the dimension of the system control quantity, n is the dimension of the system state quantity. For the underactuated inspection unmanned boat motion model, m = 2, n = 6. The system's predicted output expression within the prediction horizon N p is:

[0046]

[0047] Among them is the predicted output within the prediction horizon N p , ΔU(k) is the predicted input within the control horizon N c , Ψ k and θ k are system matrices, and their specific definitions are as follows:

[0048]

[0049]

[0050] To ensure that the inspection unmanned boat can quickly and smoothly track the desired trajectory, it is necessary to construct an objective function 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 both 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 weight sizes can be adjusted accordingly according to the changes in control requirements. During the solution process, the objective function will be simplified as follows:

[0053]

[0054] Among them:

[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 the adaptive forward-looking distance LOS

[0061] To solve the problem that a fixed Δ causes oscillations and jitters during the tracking process, design an online adjustment formula for the forward-looking distance Δ based on the lateral error y e The designed time-varying forward-looking distance formula enables the inspection unmanned boat to complete path tracking more quickly and smoothly. The time-varying forward-looking distance formula is as follows:

[0062]

[0063] During the tracking process, the minimum and maximum values of the forward-looking distance (i.e., Δ min and Δ max ) are set to 2 times and 4 times the hull length respectively. γ is the convergence rate, with a value of 0.05. To obtain excellent path tracking performance, when the unmanned boat deviates far from the target path, a smaller forward-looking distance is taken to accelerate the convergence speed; when the unmanned boat approaches the target path, a larger forward-looking distance is taken to improve the stability of the unmanned boat and effectively avoid oscillations during path tracking.

[0064] Due to the cross-drift velocity v generated by the environmental interference on the inspection unmanned boat, the actual resultant velocity U is:

[0065]

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

[0067] β = atan2(v, u)

[0068] Therefore, considering the sideslip angle, the desired bow swing angle of the inspection unmanned boat is ψ d so that the actual movement direction of the unmanned boat is the desired course angle, i.e., moving towards (x los , y los ):

[0069]

[0070] As a preferred technical solution of the present invention, the beacon positions described in S1 are set as three evenly distributed points, and the distribution pattern and quantity of the beacons allow parameter adjustment according to specific circumstances.

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

[0072] As a preferred technical solution of the present invention, when establishing the non-linear state space model in S2.1, due to the existence of external interference factors, there will be a deviation between the actual state and the predicted state of the ship.

[0073] As a preferred technical solution of the present invention, when establishing the non-linear state space model 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 drive models of the rudder and the propeller and their interaction effects are not included in the hydrodynamic model. The force and moment are directly used as the input parameters for trajectory tracking control, and the position is used as the control output.

[0075] As a preferred technical solution of the present invention, for the system linearization and discretization in S2.1, the system needs to be discretized first, then linearized, and finally the optimal control sequence is solved using linear MPC.

[0076] As a preferred technical solution of the present invention, when setting the constraint conditions in S2.1, the limits of the control quantity and the control increment constraint conditions need to be considered. At the same time, boundary restrictions need to be imposed on the movement path of the inspection unmanned boat.

[0077] As a preferred technical solution of the present invention, the forward viewing distance Δ in S2.2 is a fixed value. In the initial stage of path tracking, the lateral error y e is relatively large. The inspection unmanned boat should quickly converge to the LOS point. However, the fixed forward viewing distance Δ makes it impossible for the inspection unmanned boat to quickly adjust its heading, resulting in a slow convergence speed.

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

[0079] By optimizing the path planning algorithm and combining advanced path generation and tracking strategies, during the inspection of the unmanned boat, the real-time error feedback and adjustment mechanism are used to effectively optimize the inspection path, improve the inspection efficiency, reduce energy consumption, and provide the unmanned boat with more efficient path tracking ability; by finely adjusting the control system of the unmanned boat and combining high-precision lateral control strategies, the dynamic forward viewing distance and MPC control are used to optimize the heading, greatly improving the lateral control accuracy of the unmanned boat; enabling the unmanned boat to travel more precisely along the specified trajectory in the actual environment, thus 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 response speed of the unmanned boat can be significantly improved when adjusting the heading, effectively reducing the turning time, enhancing the mobility and efficiency of the unmanned boat in complex environments, and providing higher flexibility and response ability for the unmanned boat when performing inspection tasks. Description of the Drawings

[0080] Figure 1 This is the flow chart of the present invention;

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

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

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

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

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

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

[0087] Figure 8 This is the simulation analysis diagram of the inspection tour around the navigation mark of the present invention. Detailed Implementation Manner

[0088] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

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

[0090] S1: Path optimization under dynamic sea conditions: Dynamically determine whether to perform a clockwise or counterclockwise inspection tour by calculating the angle between navigation marks, and avoid unnecessary detours,

[0091] Determine the precise positions of each navigation mark within the inspection area. For each navigation mark with a position of (x buoy , y buoy ), set its inspection radius as radius, the total angle of the inspection tour as θ, and perform a circular inspection tour. Its inspection path is as follows:

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

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

[0094] After knowing the positions of the buoys to be inspected, it is necessary to calculate the angle θ based on the current position (x curbuoy ,y curbuoy ) of the inspected buoy, the position (x prebuoy ,y prebuoy ) of the previous inspected buoy, and the position (x nextbuoy ,y nextbuoy ) of the next inspected buoy: 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] After calculating θ diff , compare it with π. If θ diff < π, the bypass route is clockwise; otherwise, it is counterclockwise.

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

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

[0101] Basic principle of trajectory tracking of an inspection unmanned boat based on MPC:

[0102] In the system state measurement section, obtain the real-time position, speed, and heading angle of the inspection unmanned boat. The system model describes the state change law of the unmanned boat. In state prediction, based on the current state and the model, predict the future state trajectory. By solving the optimization problem, solve the optimal predicted control input sequence within the prediction step N p . Then apply the optimized control input to the actuator of the inspection unmanned boat. The inspection unmanned boat adjusts its navigation according to the control input and feeds back the state through the sensor, and then continuously repeats this process. Through this rolling optimization strategy, the input of each step is the optimal value calculated based on the current state;

[0103] Establish a nonlinear state space model:

[0104] During the path tracking of the inspection unmanned surface vehicle (USV), first, sensors are used to collect the motion state information of the current inspection USV. Subsequently, combining the system's prediction model, the control input at the previous moment, and the unknown input sequence, the future predicted output trajectory sequence is calculated. Then, the predicted output trajectory sequence, the reference trajectory, and the unknown input sequence are incorporated into the objective function, and 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 USV and lasts for one control period.

[0105] Define \(x = [x\ y\ \psi\ u\ v\ r]\) T as the system state variables, and \(y = [x\ y]\) T as the system output. The motion equation of the inspection USV is:

[0106]

[0107] \(y = P_2x\)

[0108] where the function \(f\) is the nonlinear equation of the USV motion model, \(\tau\) d is the environmental disturbance, and \(v\) c is the velocity of the water flow in the body coordinate system.

[0109] System linearization and discretization:

[0110] In model predictive control, an approximate linearization technique is adopted. Assuming that the reference system is already fully on the desired path, by obtaining the state variables and control variables at each moment on the path, based on the processing of the deviation between the reference system and the current system, a model predictive controller is designed to track the desired path. That is, in the case of ignoring the disturbance, the reference trajectory can be expressed as:

[0111]

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

[0113]

[0114] Subtract the above two equations to obtain the new state-space model of the system:

[0115]

[0116] In the formula,

[0117] Adopt the Euler method to discretize the model, and we have:

[0118] \(A\)k = I + T·A,B k = T·B

[0119] where A k and B k are the discretized system matrices respectively, T is the discrete time step, and I is the identity matrix. Thus, we can obtain:

[0120]

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

[0122] Set the constraint conditions:

[0123] At a specific time k and prediction horizon N p The limits of the control quantity, the control increment, and the constraints of the output quantity 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] where N p is the prediction step, N c is the control step, Δu min , Δu max are the input increment constraints, u min , u max are the input constraints, y min , y max are the output constraints, and Δ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 forward Euler method, and a new state variable is constructed. After combination, a new space - state model can be obtained:

[0129]

[0130] Among them, m is the dimension of the system control variable, n is the dimension of the system state variable. For the under - actuated inspection unmanned boat motion model, m = 2, n = 6. The system's predicted output expression within the prediction horizon N p is:

[0131]

[0132] Where is the predicted output within the prediction horizon N p ΔU(k) is the predicted input within the control horizon N c Ψ k and θ k are system matrices, and their specific definitions are as follows:

[0133]

[0134]

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

[0136]

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

[0138]

[0139] Among them:

[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 the adaptive forward-looking distance LOS

[0146] To solve the problem that a fixed Δ causes oscillations and jitters during the tracking process, design a formula for online adjusting the forward-looking distance Δ based on the lateral error y e The designed time-varying forward-looking distance formula enables the inspection unmanned boat to complete path tracking more quickly and smoothly. The time-varying forward-looking distance formula is as follows:

[0147]

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

[0149] Due to the cross drift velocity v of the inspection unmanned boat caused by environmental interference, the actual resultant velocity U is:

[0150]

[0151] This causes a small sideslip angle β between the actual motion direction of the inspection unmanned boat and the direction of the bow swing angle:

[0152] β = atan2(v, u)

[0153] Therefore, considering the sideslip angle, the desired bow swing angle of the inspection unmanned boat is ψ d to make the actual motion direction of the unmanned boat the desired course angle, that is, moving towards (x los , y los ):

[0154]

[0155] By optimizing the path planning algorithm, combining advanced path generation and tracking strategies, and utilizing real-time error feedback and adjustment mechanisms, the inspection path has been effectively optimized. By finely adjusting the unmanned boat control system, combining high-precision lateral control strategies, and using dynamic forward-looking distance and MPC control to optimize the heading, the lateral control accuracy of the unmanned boat has been greatly improved. By introducing an improved heading adjustment algorithm and a dynamic control strategy based on real-time error feedback, the response speed of the unmanned boat can be significantly enhanced when making heading adjustments.

[0156] Among them, the positions of the navigation marks in S1 are set as three evenly distributed points, and the distribution pattern and quantity of the navigation marks allow for parameter adjustment according to specific circumstances.

[0157] By setting the positions of the navigation marks as three evenly distributed points, it is convenient for research and calculation. By selecting the optimal inspection direction (clockwise or counterclockwise), the repeatability of the path during the inspection can be significantly reduced, thus saving the time required for the inspection.

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

[0159] In the actual control process, appropriate simplified models, control parameters, and objective functions should be selected according to specific requirements 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 disturbance factors, there will be a deviation between the actual state and the predicted state of the ship.

[0161] To correct the deviation, it is necessary to re-obtain the actual motion state information of the ship through sensors and perform the next optimal input calculation based on this.

[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] Performing trajectory tracking on the inspection unmanned boat means real-time tracking of the path of the moving target. Therefore, high requirements are placed on the maneuverability of the inspection unmanned boat. Relying solely on heading control is not sufficient to meet the precise requirements of trajectory tracking. Therefore, it is necessary to use the hydrodynamic model as the control basis for trajectory tracking.

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

[0165] By directly taking forces and torques as the input parameters for trajectory tracking control and the position as the control output, the complexity of the model can be reduced, and accurate trajectory tracking can be achieved. Considering the underactuation of the inspection unmanned boat, the system input changes from a three-degree-of-freedom input τ = [f u f v t u to a two-degree-of-freedom input τ' = [f u t u .

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

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

[0168] Among them, when setting the constraint conditions in S2.1, the limits of the control quantity, the control increment constraint conditions need to be considered, and at the same time, boundary restrictions need to be imposed on the movement path of the inspection unmanned boat.

[0169] Due to the mechanical performance limitations of the propeller drive mechanism and the steering gear of the inspection unmanned boat, there are certain limitations in the movement speed of the inspection unmanned boat. Therefore, the limits of the control quantity and the control increment constraint conditions must be considered, and imposing boundary restrictions on the movement path of the inspection unmanned boat can ensure navigation safety.

[0170] Among them, the forward-looking distance Δ in S2.2 is a fixed value. At the initial stage of path tracking, the lateral error y e is relatively large, and the inspection unmanned boat should quickly converge to the LOS point. However, the fixed forward-looking distance Δ makes it impossible for the inspection unmanned boat to quickly adjust its heading, resulting in a slow convergence speed.

[0171] As the unmanned boat approaches the reference path, the value of y e gradually decreases, and the primary goal of the system becomes stable tracking. The fixed Δ will cause oscillations and jitters in the path tracking process, thus affecting the path planning.

[0172] It should be noted that, in this document, relational terms such as first and second are only used 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 term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, such that a process, method, article or apparatus comprising a series of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article or apparatus.

[0173] Although the embodiments of the present invention have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method for an unmanned buoy inspection boat, characterized in that, The specific steps are as follows: S1: Path optimization under dynamic sea conditions: Dynamically determine whether to bypass the inspection clockwise or counterclockwise by calculating the angle between navigation buoys, avoiding unnecessary bypasses. Determine the precise positions of each navigation buoy within the inspection area. For each buoy with position (x buoy , y buoy ), set its inspection radius as radius and the total angle of the inspection loop as θ, and perform circular inspection. Its inspection path is as follows: x path = x buoy + radius × cosθ y path = y buoy + radius × sinθ After knowing the positions of the buoys to be inspected, it is necessary to calculate the angle θ curbuoy , y curbuoy ) based on the current position of the inspected buoy (x prebuoy , y prebuoy ), the position of the previous inspected buoy (x nextbuoy , y nextbuoy ) and the position of the next inspected buoy (x nextbuoy , y nextbuoy ) as follows: diff : θ start = π + atan2(y curbuoy -y prebuoy , x curbuoy -x prebuoy ) θ next = atan2(y nextbuoy - y curbuoy , x nextbuoy - x curbuoy ) After calculating θ diff compare it with π. If θ diff < π, the detour route is clockwise; otherwise, it is counterclockwise. S2: High-precision trajectory tracking under complex disturbances: Optimize the tracking error through the combined control of MPC and adaptive look-ahead distance LOS. S2.1: Design the MPC prediction model Basic principle of trajectory tracking of the inspection unmanned boat based on MPC: In the system state measurement phase, the real-time position, speed, and heading angle of the inspection unmanned boat are obtained. The system model describes the state change law of the unmanned boat. In state prediction, based on the current state and the model, the future state trajectory is predicted. By solving the optimization problem, the optimal predictive control input sequence within the prediction step length N p is solved, and then the optimized control input is applied to the actuator of the inspection unmanned boat. The inspection unmanned boat adjusts its navigation according to the control input, and feeds back the state through the sensor, and then continuously repeats this process. Through this rolling optimization strategy, the input of each step is the optimal value calculated based on the current state; Establish a nonlinear state-space model: During the path tracking of the inspection unmanned boat, first use sensors to collect the motion state information of the current inspection unmanned boat. Subsequently, combined with the system's prediction model, the control input at the previous moment, and the unknown input sequence, calculate the future predicted output trajectory sequence. Then, incorporate the predicted output trajectory sequence, the reference trajectory, and the unknown input sequence into the objective function, and under the system constraints, solve for the optimal input sequence to minimize the value of the objective function. Finally, apply the first control input in the optimal input sequence to the inspection unmanned boat and continue for one control period. Define \(x = [x\ y\ \psi\ u\ v\ r]^T\) T as the system state variables, and \(y = [x\ y]^T\) T as the system output. The motion equation of the inspection unmanned boat is as follows: y = P2x where the function f is the non - linear equation of the USV motion model, τ d is the environmental disturbance, v c is the velocity of the water flow in the body - fixed coordinate system, System linearization and discretization: In model predictive control, an approximate linearization technique is adopted. Assuming that the reference system is already fully on the desired path, by obtaining the state variables and control variables at each moment on the path, based on the processing of the deviation between the reference system and the current system, design a model predictive controller to track the desired path. That is, in the case of ignoring disturbances, the reference trajectory can be expressed as: At any reference point (x R , u R ), perform a first-order Taylor expansion of the function to obtain: Subtracting the above two equations gives the new state-space model of the system: In the formula, Discretize the model using the Euler method, and we have: A k = I + T·A,B k = T·B where A k and B k are respectively the discretized system matrices, T is the discrete time step, and I is the identity matrix. Therefore, we can obtain: wherein, represents the discrete system output, i.e., This equation represents the system equation after linearization of the non-linear system at any reference point (x R , u R ); Set the constraint conditions: At a specific moment k and prediction period N p within, the limit of the control quantity, the control increment, and the constraint of the output quantity 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 is the control step length, Δ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 it 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 forward Euler method, and a new state variable is constructed. After combination, a new space state model can be obtained: Among them, m is the dimension of the system control quantity, n is the dimension of the system state quantity. For the underactuated inspection unmanned surface vehicle motion model, m = 2, n = 6, and the system's predicted output expression within the prediction time domain N p is as follows: where is the predicted output within the prediction horizon N p , ΔU(k) is the predicted input within the control horizon N c , Ψ k and θ k are system matrices, and their specific definitions are as follows: To ensure that the inspection unmanned boat can quickly and smoothly track the desired trajectory, it is necessary to construct an objective function based on the deviation of the system state variables, the control variables, and their increments. When designing the objective function, it is necessary to consider both the speed of the tracking 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 weight sizes can be adjusted accordingly according to the changes in control requirements. During the solution process, the objective function will be simplified as follows: Where: Therefore, the ship trajectory tracking based on linear MPC can be described as the following optimal value problem: argmin{J(ΔU(k))} s.t. ΔU min ≤ΔU(k)≤ΔU max U min ≤MΔU(k)+U(k)≤U max , Y min ≤Y(k)+Y R (k)≤Y max . S2.2: Design the adaptive look-ahead distance LOS To solve the problem that a fixed Δ causes oscillations and jitters during the tracking process, a formula for online adjustment of the forward-looking distance Δ is designed based on the lateral error y e The designed time-varying forward-looking distance formula enables the inspection unmanned boat to complete path tracking more quickly and smoothly. The time-varying forward-looking distance formula is as follows: During the tracking process, the minimum and maximum values of the forward-looking distance (i.e., Δ min and Δ max ) are set to 2 times and 4 times the hull length respectively. γ is the convergence rate, with a value of 0.

05. To obtain excellent path tracking performance, when the unmanned boat deviates far from the target path, a smaller forward-looking distance is taken to accelerate the convergence speed; when the unmanned boat approaches the target path, a larger forward-looking distance is taken to improve the stability of the unmanned boat and effectively avoid oscillations during the path tracking process. Due to the cross drift velocity v generated by the inspection unmanned boat under environmental disturbances, the actual resultant velocity U is: This causes a small sideslip angle β between the actual motion direction of the inspection unmanned boat and the direction of the bow swing angle: β = atan2(v, u) Therefore, considering the sideslip angle, the desired yaw angle of the inspection unmanned boat is ψ d so that the actual movement direction of the unmanned boat is the desired course angle, that is, moving towards (x los , y los ):

2. The path planning method of an unmanned buoy inspection boat according to claim 1, characterized in that: The positions of the navigation buoys described in S1 are set as three evenly distributed points, and the distribution pattern and quantity of the navigation buoys allow for parameter adjustment according to specific circumstances.

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

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

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

6. The path planning method of an unmanned buoy inspection 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. Instead, the forces and moments are directly used as the input parameters for trajectory tracking control, and the position is used as the control output.

7. A path planning method for an unmanned buoy inspection boat according to claim 1, characterized in that: For the system linearization and discretization described in S2.1, the system needs to be discretized first, then linearized, and finally the optimal control sequence is solved using linear MPC.

8. The path planning method of an unmanned buoy inspection boat according to claim 1, characterized in that: When setting the constraint conditions in S2.1, the limits of the control variables, the control increment constraint conditions need to be considered. At the same time, boundary restrictions need to be imposed on the movement path of the inspection unmanned boat.

9. The path planning method of an unmanned buoy inspection boat according to claim 1, characterized in that: The forward viewing distance Δ described in S2.2 is a fixed value. In the initial stage of path tracking, the lateral error y e is relatively large. The inspection unmanned boat should quickly converge to the LOS point. However, the fixed forward viewing distance Δ makes it impossible for the inspection unmanned boat to quickly adjust its heading, resulting in a slow convergence speed.

Images