A path planning method for a water surface unmanned vehicle with autonomous obstacle avoidance function

Abstract

This invention discloses a path planning method for an unmanned surface vehicle (UAV) with autonomous obstacle avoidance. The method first constructs a state vector describing the UAV's navigation state, then uses the EKF algorithm to combine the state vector with the predicted vector to obtain a predicted vector. Simultaneously, it detects all obstacles near the UAV and the motion state of each obstacle, sets multiple feature parameters, and determines the value of each feature parameter based on the motion state of each obstacle. A command model is established, and control commands are derived by combining the predicted vector and feature parameter values. Constraints are established, and the final command to control the UAV's movement is obtained by combining these constraints with the control commands. An adaptive learning model is established to judge the constraint states; if the state is unfavorable, it is updated. This method ensures the UAV will not collide with obstacles through constraints, guaranteeing safety. The judgment is rapid and highly reliable, and the adaptive learning model helps ensure the UAV's operational efficiency.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and intelligent control technology, specifically relating to a path planning method for an unmanned surface vehicle with autonomous obstacle avoidance function. Background Technology

[0002] With the development of marine resources, various unmanned surface vehicles (USVs), including unmanned surface vessels (USVs), have been widely used in port inspection, environmental monitoring, and maritime security. For any USV, path planning is a core technology that determines its performance, and autonomous obstacle avoidance is a crucial part of path planning, directly related to the vehicle's safety. Existing path planning methods are mainly divided into two categories. One category is based on geometric models, such as the artificial potential field method and the velocity obstacle method. These methods define clear unsafe areas in the velocity or action space to achieve autonomous obstacle avoidance. For example, Chinese patent application No. 202510334728.6, entitled "A Path Search Method, Program, Device and Storage Medium for USVs Based on Random Sampling Algorithm," proposes a path planning method that uses a corner optimization strategy to filter random sampling points. During the filtering process, the strategy judges whether the path generated based on the current sampling point will collide with obstacles. This type of method has a clear principle and high reliability, but it is prone to oscillations in complex environments, especially when facing dynamic obstacles, resulting in long judgment times and difficulty in achieving efficient navigation.

[0003] Another type is data-driven methods, especially reinforcement learning methods that have developed rapidly in recent years with the rise of machine learning. For example, the Chinese patent application, titled "An Intelligent Perception and Navigation System and Method for Unmanned Surface Vessels," application number 202510408620.7, proposes a method that fuses obstacle locations measured by different sensors through a neural network and plans a path based on the obstacle's location. Using this type of method can improve the vehicle's decision-making ability over the long term, but the decision-making process lacks explicit safety guarantees, has low reliability in the initial stage, and struggles to make effective decisions when facing unknown risks. Furthermore, both of these methods suffer from ignoring various interference factors that can easily cause deviations between sensor observations and actual values, such as sensor noise and model errors, which can easily lead to safety hazards in highly dynamic scenarios.

[0004] Therefore, proposing a path planning method for surface unmanned vehicles (UAVs) that is highly reliable, efficient in decision-making, and can quantitatively incorporate the uncertainty of sensor observations into the decision-making process to achieve obstacle avoidance is of great significance for improving the operational efficiency of surface UAVs, ensuring their operational safety, and promoting the development of marine resources. Summary of the Invention

[0005] The purpose of this invention is to address the problems that obstacle avoidance methods based on geometric models are safe but not efficient for unmanned surface vessels, while machine learning-based methods are intelligent but lack safety guarantees and are easily affected by sensor errors. To this end, this invention proposes an instruction model based on reinforcement learning, as well as constraints to ensure that the vessel does not collide with obstacles, and proposes a path planning method for unmanned surface vessels with autonomous obstacle avoidance function that includes the above instruction model and constraints.

[0006] The technical solution adopted in this invention is as follows: A path planning method for an unmanned surface vehicle (USV) with autonomous obstacle avoidance capabilities, wherein the USV has an inertial measurement unit, a navigator, and a surface target detector, characterized by comprising the following steps: S1. Set a judgment period. During the navigation process, the vehicle obtains its current azimuth, attitude and speed through the inertial measurement unit (IMU) and navigator at each judgment period, and constructs a state vector accordingly. The prediction vector at the current moment is obtained by combining the state vector with the extended Kalman filter (EKF) algorithm.

[0007] S2. Set the detection range, which is the water surface area of ​​the water area where the vehicle is located. In each judgment period, the vehicle obtains all obstacles in the current detection range and the motion state of each obstacle through the water surface target detector. Set multiple feature parameters and determine the values ​​of all feature parameters of each obstacle according to its current motion state.

[0008] S3. Establish a command model for deriving control commands for the aircraft, and combine the predicted vector with the feature parameter values ​​corresponding to all obstacles to derive control commands.

[0009] S4. Establish constraints to constrain the motion state of the aircraft, and derive the final command for controlling the motion of the aircraft by combining the constraints with the control commands.

[0010] S5. Establish an adaptive learning model. After each constraint condition yields the final instruction, the state of the constraint condition is judged through the adaptive learning model. If the state is unsatisfactory, it is updated through the adaptive learning model.

[0011] To avoid the impact of measurement errors from the inertial measurement unit and navigator on the vehicle's path planning, this invention introduces an extended Kalman filter (EKF) to process the measurement results. To ensure the safety of the vehicle's navigation process, this invention sets constraints to judge the control commands obtained through the command model, ensuring that the vehicle's movement path does not intersect with the dynamic obstacles. The adaptive learning model is designed to avoid the vehicle from over-considering obstacle avoidance when planning its path, thereby affecting navigation efficiency.

[0012] Further optimization is needed; the specific process of step S1 is as follows: S1.1. Establish a Cartesian coordinate system covering the active range of the unmanned surface vessel (USV), and set a reference direction. Determine the x and y coordinates of the USV's location based on the coordinate system, and determine the azimuth angle of the USV based on the reference direction. Simultaneously, determine the USV's azimuth angle along its respective routes. x axis, y The velocity component in the axial direction.

[0013] S1.2. The state vector is represented as z t = [ x m t , y m t , ψ m t , u m t , v m t , r m t ] T ,in t For a moment, t = 1, 2, 3, …, x m t and y m t They are respectively t The horizontal and vertical coordinates of the aircraft's position are constantly measured by the navigation system. ψ m t for t The azimuth angle of the ship's bow, measured constantly by the inertial measurement unit, is the direction in which the ship's bow is pointing. u m t and v mt They are respectively t The distance of the aircraft along the navigation system at all times x axis, y The velocity component in the axial direction, r m t for t The angular velocity of the vehicle obtained at any time through the inertial measurement unit.

[0014] The S1.3.EKF algorithm is described by the following formula: (1) (2) in, x t|t-1 for t The prediction vector at time step, x t|t , x t-1|t-1 They are respectively t time, t - Estimated vector at time 1 T t-1 for t - Process noise at time 1 f d (·) is the prediction function. F and F T These are the update matrix and its transpose, respectively. Q It is a white noise matrix. P t|t-1 for t The prediction covariance matrix at time 1, P t|t , P t-1|t-1 They are respectively t time, t - Estimated covariance matrix at time 1 h (·) represents the observation function. H t and H t T They are respectively t The Jacobian matrix of the time-observation function and its transpose. R t for t The noise covariance matrix is ​​observed at all times. K t for t Kalman gain at time step I It is an identity matrix.

[0015] Based on the structural and functional characteristics of the inertial measurement unit and navigator, the settings are as follows: f d (·)and h (·), and set the initial estimation vector. x 0|0 Initial covariance matrix P 0|0 With initial process noise T 0, t = 1 when x 0|0 , P 0|0 and T Substituting 0 into formula (1), T t-1 and R t The structure and functional characteristics of the inertial measurement unit and the navigator, as well as t The water surface environment at any given time determines this.

[0016] Further optimization is needed, as the obstacles mentioned in step S2 include, but are not limited to, other surface vehicles. The specific process of step S2 is as follows: S2.1. Number all obstacles, and represent any obstacle as o. i,t , t To determine the moment the obstacle was detected, i Number the obstacles; the set feature parameters include d i,t , f i,t , v rel i,t , ψ rel i,t , l i,t , w i,t TTC i,t , d i,t for t Time Obstacles i Distance to the aircraft f i,t for t Time Obstacles i The azimuth angle of the direction the front is facing. v rel i,t for t Time Obstacles i Speed ​​magnitude, ψ rel i,t for t Time Obstaclesi velocity direction, l i,t and w i,t They are respectively t Time Obstacles i Feature dimensions in the front-back and left-right directions, TTC i,t for t Time Obstacles i The estimated time of collision with the spacecraft.

[0017] S2.2. Establish the relative position vector of each obstacle based on the location of the vehicle and the obstacles. p rel i,t And establish a system for calculating TTC i,t The predicted collision model is expressed by the following formula: (3) in, d safe i,t for t Time Obstacles i Safe distance from the aircraft p rel i,t T for p rel i,t Transpose of; d safe i,t Calculated using the following formula: (4) in, L USV The length of the aircraft L obs i,t for t Time Obstacles i Length, d margin For redundant distance, k v This is the speed safety factor.

[0018] For path planning safety, the most important parameter is the estimated time of collision (TTC) between the obstacle and the vehicle. i,t To calculate TTC i,t Required parameters L obs i,t According to l i,t and wi,t It is concluded that d margin According to d i,t , f i,t , v rel i,t , ψ rel i,t The four parameters are used to determine when the distance between the vehicle and the obstacle is greater than... d safe i,t Furthermore, a collision is only possible when the distance between the vehicle and the obstacle gradually decreases; otherwise, a collision is considered unlikely. (TTC) i,t The value is ∞. Considering that obstacles include not only other aircraft, but also irregularly shaped objects such as ship wreckage, tree branches, and ice blocks, the size of obstacles is described by characteristic dimensions in the front-back and left-right directions. For aircraft, the characteristic dimensions in the front-back and left-right directions are the length and width.

[0019] Further optimization involves establishing the instruction model based on reinforcement learning methods. The specific process of building the instruction model in step S3 is as follows: S3.1. Treat the aircraft as an intelligent agent and establish a strategy for controlling the aircraft's movement. i j,k Multiple initial strategies are set, each with a unique number. j The initial strategy number, k For the number of iterations, i j,k This indicates that the policy is the initial policy. j through k The result obtained after the second iteration of optimization, based on the strategy of the aircraft. i j,k The action taken is described as a velocity vector. a j,k = [ u j,k , v j,k ], u j,k , v j,k respectively along x axis, y The velocity component in the axial direction during the execution of the action a j,k The vehicle status is then updated, and the updated status is recorded as follows: s j,k .

[0020] S3.2. Construct a reward function, which is used to determine the reward based on the state. s j,k Computational strategy i j,k The reward.

[0021] S3.3. Establish a strategy optimization mechanism and set termination conditions. The iterative calculation specifically includes the following steps: S3.3.1. In the first k In the +1 calculation, the aircraft is based on each i j,k and corresponding status s j,k Take actions and execute them respectively; the state of the aircraft after the actions are executed. s j,k+1 The formula derived from the Fossen model is as follows: (5) in, M The inertial matrix is ​​whose element values ​​are determined by the vehicle's mass and drag characteristics. C (·) is the Coriolis centripetal matrix. D (·) represents the damping matrix. t For the ship's power, w d For the effect of wind and wave disturbance, v j,k To perform the action a j,k Speed ​​of the vehicle in a stable state after takeoff; execution of actions a j,k The status of the aircraft afterward depends on v j,k The system is updated by determining the current motion state of the vehicle based on the predicted vector, and this state is used as the initial state. s 0, in k = 0 When the vehicle is based on the initial strategy and s 0. Take action.

[0022] S3.3.2. Through the aforementioned reward function, combined with s j,k+1 The result is the k + 1 calculation in progress i j,k The reward is then used to optimize the strategy and combine it with the reward. i j,k Updated to i j,k+1Simultaneously, the calculated result is compared with the termination condition. If the result meets the termination condition, the iteration stops and the calculation is completed. If it does not meet the condition, the process described in steps S3.3.1-S3.3.2 is repeated for the next round of calculation.

[0023] S3.4. The strategy obtained after calculation is denoted as the final strategy, and the number of iterations in the entire calculation process is denoted as... N k The cumulative reward for each final strategy is calculated using the following formula: (6) in, R j For the final strategy j Cumulative rewards c For discount factors, 0 < c < 1, r j,k For strategy i j,k The reward value, E, represents the expected value; find the one with the largest value. R j And the corresponding final strategy, which is the action given by the vehicle based on its current motion state, which is the control command.

[0024] Further optimization is achieved by describing the reward function established in step S3.2 using the following formula: (7) in, r ψ j,k As a reward for consistent course, r d j,k As a reward for maintaining a safe distance, r s j,k For smooth reward, r p j,k For gradual rewards, r v j,k As a penalty for slow speed, r T j,k As a time penalty, w ψ For route consistency weighting, w d For safety distance weighting, w s For smoothness weights, w p For gradual weighting, w vFor low-speed weights, w T As time weights, all the above weight values ​​are within the interval (0, 1).

[0025] The rewards and penalties in formula (7) are calculated using the following formula: (8) in, ψ j,k , ψ ref j,k Execution actions a j,k The azimuth angle of the aft vehicle and the azimuth angle of the target, x g , y g These are the x and y coordinates of the target point, respectively. x j,k , y j,k Perform actions separately a j,k The horizontal and vertical coordinates of the location of the subsequent spacecraft d min The distance from the vehicle to the nearest obstacle. d safe For a safe distance, d min and d safe The value is derived from the characteristic parameters of all obstacles. d j,k-1 , d j,k Execution actions a j,k-1 , a j,k The distance from the rear vehicle to the target point, Δ t The time interval between two consecutive actions is denoted as .

[0026] Because path planning needs to consider both navigation efficiency and safety, and includes subsequent constraints to avoid collisions, the reward function primarily uses parameters to measure navigation efficiency, improving the feasibility and continuity of the resulting path, with only a safety distance reward. r d j,k Used to measure an aircraft's obstacle avoidance capabilities.

[0027] Further optimization is needed, with the constraint conditions based on the velocity obstacle method. The specific process of step S4 is as follows: S4.1. The control command is denoted as u RLt Establish an inertial coordinate system whose origin moves synchronously with the vehicle, and use the following formula to... u RL t Commands to convert to inertial coordinate system v RL t The conversion process can be described by the following formula: (9) in, J 2×2 It is a second-order transformation matrix. ψ For matrix independent variables, Obtained through the EKF algorithm t The azimuth angle in the time prediction vector.

[0028] S4.2. Establish the velocity obstacle space at the current moment. The velocity obstacle space is a continuous space composed of a feasible region and multiple obstacle regions. The feasible region and the obstacle regions do not overlap. The number of obstacle regions and obstacles is equal and corresponds one-to-one. The subspace is described by the following formula: (10) Among them, VO i,t for t Time Obstacles i The corresponding obstacle domain, R 2 It is a set of two-dimensional real vectors. Let velocity be the independent variable. v It is a second-order velocity vector. d safe i for t Time traveler and obstacles i The safe distance is determined based on the obstacle. i The characteristic parameters are derived from this.

[0029] S4.3. Based on the characteristics of the EKF algorithm, determine the upper and lower limits of the speed magnitude. The upper and lower limits of the speed magnitude are expressed by the following formulas: (11) in, v max t This is the upper limit of the speed. v min t This is the lower limit of the speed magnitude. v phys, max , v phys, min These represent the maximum and minimum speeds that the aircraft can reach during navigation, respectively, with max(·) and min(·) being functions for taking the larger and smaller speeds, respectively.α risk Let Δ be the collision risk coefficient, and diag(·) be the function for constructing the diagonal matrix. v t Let Δ be the velocity disturbance value. x t Here is the perturbation matrix. α unc To transform the loss matrix, α 1. α Both 2 are two-dimensional vectors. k σ This represents the disturbance coefficient.

[0030] S4.4. Combining the feasible region of the speed obstacle space and the upper and lower limits of the speed magnitude, the final command is derived, as shown in the following formula: (12) (13) Here, clip(·) and argmin(·) are both functions. The defined speed value. V safe t for t The feasible region of the velocity obstacle space at any given time. v s t For the final speed, v s t and v RL t Same direction J 2×2 T for J 2×2 transpose, u s t This is the final instruction.

[0031] Setting a speed obstacle space ensures that the resulting vehicle path does not intersect with obstacle paths. Setting a range of speeds prevents excessive speed, which increases collision risk and control difficulty, while also preventing excessively slow speeds that lead to prolonged travel time, thus achieving a balance between safety and efficiency. Converting control commands into commands in an inertial coordinate system allows for a vehicle-centric assessment of the feasibility of these commands, simplifying the calculation process and improving judgment efficiency.

[0032] Further optimization is achieved by establishing the adaptive learning model in step S5 as follows: S5.1. Set the sample time period and intervention threshold. If the results obtained at a certain time are... and v RL t If the constraint conditions are different, it is considered that the constraint conditions at that moment interfere with the final instruction. The sample period is a series of consecutive judgment periods including the current judgment period. After each final instruction is obtained, it is judged whether the constraint conditions interfere within the current judgment period. If the number of times the constraint conditions interfere within the sample period reaches the interference threshold, the constraint conditions are considered to be in poor condition.

[0033] S5.2. Setting Feature Factors s 1 and s 2, s 1 and s The value of 2 is derived from the following values: (14) in, d t for t Intervention intensity index at any given time N VO t for t The number of interventions within the sample time period corresponding to the given time point. k 1. k 2. k 3 are all coefficients; α 1. α 2 element values ​​and α risk All values ​​are based on the obtained s 1 and s 2. Update.

[0034] Because the reward function is biased towards path optimization, the control commands obtained through the command model can enable the aircraft to have high navigation efficiency. If the constraints are modified too much, the control commands may reduce navigation efficiency due to an overemphasis on safety considerations. Therefore, it is necessary to control the degree of intervention of the constraints.

[0035] The beneficial effects of the method of the present invention are as follows: 1. The path planning method proposed in this invention takes into account the uncertainty of sensor observations, introduces the EKF algorithm, and ensures safety by using constraints to prevent the vehicle from touching obstacles, thus exhibiting high reliability; 2. The constraints are established based on the velocity obstacle space, which is conducive to quickly judging the path safety. In addition, the method has an adaptive learning model to adjust the constraints, avoiding excessive consideration of safety that would reduce navigation efficiency, thus helping to ensure the operational efficiency of the vehicle. Attached Figure Description

[0036] Figure 1A schematic diagram of the overall process of the path planning method of this invention.

[0037] Figure 2 The curve showing the change in reward value during the training process of the instruction model. Detailed Implementation

[0038] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below through specific embodiments. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0039] Example 1: A path planning method for an unmanned surface vehicle with autonomous obstacle avoidance function. In this example, the unmanned surface vehicle is an unmanned surface vessel (USV). The USV weighs 100 kg, is 5 m long, and 2 m wide. The path planning method is based on the width direction of the USV. x Axial direction, height direction is y axial direction, length direction is z If an inertial coordinate system for the unmanned surface vessel is established along the axial direction, then the unmanned surface vessel will orbit... x axis, y axis, z The moments of inertia of the shafts are 30 kg·m. 2 20 kg·m 2 15 kg·m 2 The unmanned surface vessel (USV) is equipped with an inertial measurement unit (IMU), a GPS satellite navigator, and a lidar for detecting surface targets. The IMU has a noise standard deviation of [0.01, 0.01, 0.001] and a detection frequency of 50 Hz. The navigator has a noise standard deviation of [0.5, 0.5] m and a data update frequency of 10 Hz. The lidar has a detection radius of 50 m, an azimuth scanning range of 270°, and the scanning range is symmetrical about the forward and backward directions of the USV. The noise standard deviation is 0.1 m. The overall process of this method is as follows: Figure 1 As shown, the specific steps include: S1. Set a judgment period. During the navigation process, the unmanned surface vessel (USV) obtains its current azimuth, attitude, and speed through the inertial measurement unit and navigator at each judgment period, and constructs a state vector accordingly. The prediction vector for the current moment is then obtained by combining the state vector with the extended Kalman filter (EKF) algorithm. The specific process is as follows: S1.1. Establish a Cartesian coordinate system covering the unmanned surface vessel's (USV) activity range, and set a reference direction. Determine the horizontal and vertical coordinates of the USV's location based on the coordinate system, and determine the azimuth angle of the USV's orientation based on the reference direction. Simultaneously, determine the USV's orientation along the following directions... xaxis, y The velocity component in the axial direction.

[0040] S1.2. The state vector is represented as z t = [ x m t , y m t , ψ m t , u m t , v m t , r m t ] T ,in t For a moment, t = 1, 2, 3, …, x m t and y m t They are respectively t The horizontal and vertical coordinates of the unmanned surface vessel's position are constantly measured by the navigation system. ψ m t for t The azimuth angle of the unmanned surface vessel's bow, measured continuously by the inertial measurement unit, is the direction in which the vessel's bow is pointing. u m t and v m t They are respectively t The unmanned surface vessel's position is constantly monitored by the navigation system. x axis, y The velocity component in the axial direction, r m t for t The turning angular velocity of the unmanned surface vessel is obtained from the inertial measurement unit at all times.

[0041] The S1.3.EKF algorithm is described by formulas (1) and (2), and is set according to the structural and functional characteristics of the inertial measurement unit and the navigator. f d (·)and h (·), and set the initial estimation vector. x 0|0 Initial covariance matrix P 0|0With initial process noise T 0, t = 1 when x 0|0 , P 0|0 and T Substituting 0 into formula (1), T t-1 and R t The structure and functional characteristics of the inertial measurement unit and the navigator, as well as t The water surface environment at any given time determines this.

[0042] S2. Set the detection range, which is the water surface area of ​​the unmanned surface vessel (USV). Within each judgment period, the USV uses lidar to determine all obstacles within the current detection range, as well as the motion state of each obstacle. Set multiple feature parameters, and determine the values ​​of all feature parameters for each obstacle based on its current motion state. The obstacles include other surface vehicles, as well as irregularly shaped objects such as ship wreckage, tree branches, and ice blocks. The specific process is as follows: S2.1. Number all obstacles, and represent any obstacle as o. i,t , t To determine the moment the obstacle was detected, i Number the obstacles; the set feature parameters include d i,t , f i,t , v rel i,t , ψ rel i,t , l i,t , w i,t TTC i,t , d i,t for t Time Obstacles i Distance to the unmanned surface vessel f i,t for t Time Obstacles i The azimuth angle of the direction the front is facing. v rel i,t for t Time Obstacles i Speed ​​magnitude, ψ rel i,t for t Time Obstacles i velocity direction,l i,t and w i,t They are respectively t Time Obstacles i Feature dimensions in the front-back and left-right directions, TTC i,t for t Time Obstacles i The estimated time of collision with the unmanned surface vessel.

[0043] S2.2. Establish the relative position vector of each obstacle based on the location of the unmanned surface vessel and the obstacles. p rel i,t And establish a system for calculating TTC i,t The expected collision model is described by formulas (3) and (4).

[0044] S3. Establish a command model for deriving control commands for the unmanned surface vessel (USV). Combine the predicted vectors with the feature parameter values ​​corresponding to all obstacles to derive the control commands. The specific process for establishing the command model is as follows: S3.1. Treat the unmanned surface vessel as an intelligent agent and establish a strategy for controlling its movement. i j,k Multiple initial strategies are set, each with a unique number. j The initial strategy number, k For the number of iterations, i j,k This indicates that the policy is the initial policy. j through k The result obtained after the second iteration of optimization, based on the strategy of the unmanned surface vessel. i j,k The action taken is described as a velocity vector. a j,k = [ u j,k , v j,k ], u j,k , v j,k respectively along x axis, y The velocity component in the axial direction during the execution of the action a j,k The unmanned surface vessel's status was then updated, and the updated status was recorded as follows. s j,k .

[0045] S3.2. Construct a reward function, which is used to determine the reward based on the state. s j,k Computational strategy i j,k The reward is described by the reward function through formula (7), and each reward and penalty in formula (7) is calculated through formula (8).

[0046] S3.3. Establish a strategy optimization mechanism and set a termination condition. In this embodiment, the termination condition is that the number of iterations reaches 600. The iterative calculation specifically includes the following steps: S3.3.1. In the first k In +1 calculation, the unmanned surface vessel (USV) is based on each type of... i j,k and corresponding status s j,k Take actions and execute them respectively; the state of the unmanned surface vessel after the actions are executed. s j,k+1 The Fossen model is shown in formula (5). The drag characteristics of the unmanned surface vessel are represented by the added mass. In this embodiment, the added mass of the swell during the unmanned surface vessel's navigation is 115 kg, the added mass of the lateral sway is 120 kg, and the added mass of the yaw is 25 kg. The navigation damping of the unmanned surface vessel is 8.0, the lateral sway damping is 15.0, and the yaw damping is 2.0.

[0047] S3.3.2. Through the aforementioned reward function, combined with s j,k+1 The result is the k + 1 calculation in progress i j,k The reward is then used to optimize the strategy and combine it with the reward. i j,k Updated to i j,k+1 Simultaneously, the calculated result is compared with the termination condition. If the result meets the termination condition, the iteration stops and the calculation is completed. If it does not meet the condition, the process described in steps S3.3.1-S3.3.2 is repeated for the next round of calculation.

[0048] S3.4. The strategy obtained after calculation is denoted as the final strategy, and the number of iterations in the entire calculation process is denoted as... N k The cumulative reward for each final strategy is calculated using formula (6), and then the cumulative reward with the largest value and its corresponding final strategy are found. The final strategy is the action given by the unmanned surface vessel based on its current motion state, which is the control command. The change of the reward value with the number of iterations during the process of the control command is obtained as follows: Figure 2As shown in the figure, the horizontal axis represents the number of iterations and the vertical axis represents the reward value. It can be seen from the figure that after about 80 iterations, the obtained strategy begins to show a higher reward value, indicating that the obtained strategy begins to rapidly approach the expected goal. From the overall curve, most strategies correspond to higher reward values, and the obtained strategy and the expected goal have a high degree of overlap when training is completed.

[0049] S4. Establish constraints to control the motion state of the unmanned surface vessel (USV). Using these constraints and control commands, derive the final commands for controlling the USV's motion. The constraints are based on the velocity obstacle method, and the specific process is as follows: S4.1. The control command is denoted as u RL t By using formula (9) u RL t Commands converted to unmanned surface vessel inertial coordinate system v RL t .

[0050] S4.2. Establish the velocity obstacle space at the current moment. The velocity obstacle space is a continuous space composed of a feasible region and multiple obstacle regions. The feasible region and the obstacle regions do not overlap. The number of obstacle regions and obstacles is equal and corresponds one-to-one. The subspace is described by formula (10).

[0051] S4.3. Determine the upper and lower limits of the speed based on the characteristics of the EKF algorithm. The upper and lower limits of the speed are expressed by formula (11).

[0052] S4.4. The final command is derived by combining the feasible domain of the speed obstacle space and the upper and lower limits of the speed magnitude through formulas (12) and (13).

[0053] S5. Establish an adaptive learning model. After each constraint condition yields a final instruction, the state of the constraint condition is judged through the adaptive learning model. If the state is unsatisfactory, it is updated through the adaptive learning model. The specific process of establishing the adaptive learning model is as follows: S5.1. Set the sample time period and intervention threshold. If the result at a certain time... and v RL t If the constraint conditions are different, it is considered that the constraint conditions at that moment interfere with the final instruction. The sample period is a series of consecutive judgment periods including the current judgment period. After each final instruction is obtained, it is judged whether the constraint conditions interfere within the current judgment period. If the number of times the constraint conditions interfere within the sample period reaches the interference threshold, the constraint conditions are considered to be in poor condition.

[0054] S5.2. Setting Feature Factors s 1 and s 2, s 1 and s The value of 2 is obtained through formula (14).

Claims

1. A path planning method for a surface unmanned vehicle with autonomous obstacle avoidance function, wherein the surface unmanned vehicle has an inertial measurement unit, a navigator, and a surface target detector, characterized in that, Specifically, the following steps are included: S1. Set a judgment period. During the navigation process, the vehicle obtains its current azimuth, attitude and speed through the inertial measurement unit (IMU) and navigator at each judgment period, and constructs a state vector accordingly. The prediction vector at the current moment is obtained by combining the state vector with the extended Kalman filter (EKF) algorithm. S2. Set the detection range, which is the water surface area of ​​the water area where the vehicle is located. In each judgment period, the vehicle obtains all obstacles in the current detection range and the motion state of each obstacle through the water surface target detector. Set multiple feature parameters and determine the values ​​of all feature parameters of each obstacle according to its current motion state. S3. Establish a command model for deriving control commands for the aircraft, and combine the predicted vector with the feature parameter values ​​corresponding to all obstacles to derive control commands; S4. Establish constraints to constrain the motion state of the aircraft, and derive the final command to control the motion of the aircraft by combining the constraints with the control command. S5. Establish an adaptive learning model. After each constraint condition yields the final instruction, the state of the constraint condition is judged through the adaptive learning model. If the state is unsatisfactory, it is updated through the adaptive learning model.

2. The path planning method for a surface unmanned vehicle with autonomous obstacle avoidance function as described in claim 1, characterized in that, The specific process of step S1 is as follows: S1.

1. Establish a Cartesian coordinate system covering the active range of the unmanned surface vessel (USV), and set a reference direction. Determine the x and y coordinates of the USV's location based on the coordinate system, and determine the azimuth angle of the USV based on the reference direction. Simultaneously, determine the USV's azimuth angle along its respective routes. x axis, y Velocity components in the axial direction; S1.

2. The state vector is represented as follows: z t = [ x m t , y m t , ψ m t , u m t , v m t , r m t ] T ,in t For a moment, t = 1,2, 3, …, x m t and y m t They are respectively t The horizontal and vertical coordinates of the aircraft's position are constantly measured by the navigation system. ψ m t for t The azimuth angle of the ship's bow, measured constantly by the inertial measurement unit, is the direction in which the ship's bow is pointing. u m t and v m t They are respectively t The distance of the aircraft along the navigation system at all times x axis, y The velocity component in the axial direction, r m t for t The angular velocity of the vehicle obtained at all times through the inertial measurement unit; The S1.3.EKF algorithm is described by the following formula: (1) (2) in, x t|t-1 for t The prediction vector at time step, x t|t , x t-1|t-1 They are respectively t time, t - Estimated vector at time 1 T t-1 for t - Process noise at time 1 f d (·) is the prediction function. F and F T These are the update matrix and its transpose, respectively. Q It is a white noise matrix. P t|t-1 for t The prediction covariance matrix at time 1, P t|t , P t-1|t-1 They are respectively t time, t - Estimated covariance matrix at time 1 h (·) represents the observation function. H t and H t T They are respectively t The Jacobian matrix of the time-observation function and its transpose. R t for t The noise covariance matrix is ​​observed at all times. K t for t Kalman gain at time step I It is the identity matrix; Based on the structural and functional characteristics of the inertial measurement unit and navigator, the settings are as follows: f d (·)and h (·), and set the initial estimation vector. x 0|0 Initial covariance matrix P 0|0 With initial process noise T 0, t = 1 will x 0|0 , P 0|0 and T Substituting 0 into formula (1), T t-1 and R t The structure and functional characteristics of the inertial measurement unit and the navigator, as well as t The water surface environment at any given time determines this.

3. The path planning method for an unmanned surface vehicle with autonomous obstacle avoidance function as described in claim 1, characterized in that, The obstacles mentioned in step S2 include, but are not limited to, other surface vehicles. The specific process of step S2 is as follows: S2.

1. Number all obstacles, and represent any obstacle as o. i,t , t To determine the moment the obstacle was detected, i Number the obstacles; the set feature parameters include d i,t , φ i,t , v rel i,t , ψ rel i,t , l i,t , w i,t TTC i,t , d i,t for t Time Obstacles i Distance to the aircraft φ i,t for t Time Obstacles i The azimuth angle of the direction the front is facing. v rel i,t for t Time Obstacles i Speed ​​magnitude, ψ rel i,t for t Time Obstacles i velocity direction, l i,t and w i,t They are respectively t Time Obstacles i Feature dimensions in the front-to-back and left-to-right directions, TTC i,t for t Time Obstacles i The estimated time of collision with the aircraft; S2.

2. Establish the relative position vector of each obstacle based on the location of the vehicle and the obstacles. p rel i,t And establish a system for calculating TTC i,t The predicted collision model is expressed by the following formula: (3) in, d safe i,t for t Time Obstacles i Safe distance from the aircraft p rel i,t T for p rel i,t transpose; d safe i,t Calculated using the following formula: (4) in, L USV The length of the aircraft L obs i,t for t Time Obstacles i Length, d margin For redundant distance, κ v This is the speed safety factor.

4. The path planning method for a surface unmanned vehicle with autonomous obstacle avoidance function as described in claim 1, characterized in that, The instruction model is based on reinforcement learning. The process of establishing the instruction model in step S3 is as follows: S3.

1. Treat the aircraft as an intelligent agent and establish a strategy for controlling the aircraft's movement. θ j,k Multiple initial strategies are set, each with a unique number. j The initial strategy number, k For the number of iterations, θ j,k This indicates that the policy is the initial policy. j through k The result obtained after the second iteration of optimization, based on the strategy of the aircraft. θ j,k The action taken is described as a velocity vector. a j,k = [ u j,k , v j,k ], u j,k , v j,k respectively along x axis, y The velocity component in the axial direction during the execution of the action a j,k The vehicle status is then updated, and the updated status is recorded as follows: s j,k ; S3.

2. Construct a reward function, which is used to determine the reward based on the state. s j,k Computational strategy θ j,k The reward; S3.

3. Establish a strategy optimization mechanism and set termination conditions. The iterative calculation specifically includes the following steps: S3.3.

1. In the first k In the +1 calculation, the aircraft is based on each θ j,k and corresponding status s j,k Take actions and execute them respectively; the state of the aircraft after the actions are executed. s j,k+1 The formula derived from the Fossen model is as follows: (5) in, M The inertial matrix is ​​whose element values ​​are determined by the vehicle's mass and drag characteristics. C (·) is the Coriolis centripetal matrix. D (·) represents the damping matrix. τ For the ship's power, w d For the effect of wind and wave disturbance, v j,k To perform the action a j,k Speed ​​of the vehicle in a stable state after takeoff; execution of actions a j,k The status of the aircraft afterward depends on v j,k The system is updated by determining the current motion state of the vehicle based on the predicted vector, and this state is used as the initial state. s 0, in k = 0 When the vehicle is based on the initial strategy and s 0. Take action; S3.3.

2. Through the aforementioned reward function, combined with s j,k+1 The result is the k + 1 calculation in progress θ j,k The reward is then used to optimize the strategy and combine it with the reward. θ j,k Updated to θ j,k+1 Simultaneously, the calculated result is compared with the termination condition. If the result meets the termination condition, the iteration stops and the calculation is completed. If it does not meet the condition, the process described in steps S3.3.1-S3.3.2 is repeated for the next round of calculation. S3.

4. The strategy obtained after calculation is denoted as the final strategy, and the number of iterations in the entire calculation process is denoted as... N k The cumulative reward for each final strategy is calculated using the following formula: (6) in, R j For the final strategy j Cumulative rewards γ For discount factors, 0 < γ < 1, r j,k For strategy θ j,k The reward value, E, represents the expected value; find the one with the largest value. R j And the corresponding final strategy, which is the action given by the vehicle based on its current motion state, which is the control command.

5. A path planning method for a surface unmanned vehicle with autonomous obstacle avoidance function as described in claim 4, characterized in that, The reward function established in step S3.2 is described by the following formula: (7) in, r ψ j,k As a reward for consistent course, r d j,k As a reward for maintaining a safe distance, r s j,k For smooth reward, r p j,k For gradual rewards, r v j,k As a penalty for slow speed, r T j,k As a time penalty, w ψ For route consistency weighting, w d For safety distance weighting, w s For smoothness weights, w p For gradual weighting, w v For low-speed weights, w T As time weights, all the above weight values ​​are within the interval (0, 1); The rewards and penalties in formula (7) are calculated using the following formula: (8) in, ψ j,k , ψ ref j,k Execution actions a j,k The azimuth angle of the aft vehicle and the azimuth angle of the target, x g , y g These are the x and y coordinates of the target point, respectively. x j,k , y j,k Perform actions separately a j,k The horizontal and vertical coordinates of the location of the subsequent spacecraft d min The distance from the vehicle to the nearest obstacle. d safe For a safe distance, d min and d safe The value is derived from the characteristic parameters of all obstacles. d j,k-1 , d j,k Execution actions a j,k-1 , a j,k The distance from the rear vehicle to the target point, Δ t The time interval between two consecutive actions is denoted as .

6. The path planning method for a surface unmanned vehicle with autonomous obstacle avoidance function as described in claim 4, characterized in that, The constraints are based on the velocity obstacle method, and the specific process of step S4 is as follows: S4.

1. The control command is denoted as u RL t Establish an inertial coordinate system whose origin moves synchronously with the vehicle, and use the following formula to... u RL t Commands to convert to inertial coordinate system v RL t The conversion process can be described by the following formula: (9) in, J 2×2 It is a second-order transformation matrix. ψ For matrix independent variables, Obtained through the EKF algorithm t The azimuth angle in the time-predicting vector; S4.

2. Establish the velocity obstacle space at the current moment. The velocity obstacle space is a continuous space composed of a feasible region and multiple obstacle regions. The feasible region and the obstacle regions do not overlap. The number of obstacle regions and obstacles is equal and corresponds one-to-one. The subspace is described by the following formula: (10) Among them, VO i,t for t Time Obstacles i The corresponding obstacle domain, R 2 It is a set of two-dimensional real vectors. Let velocity be the independent variable. v It is a second-order velocity vector. d safe i for t Time traveler and obstacles i The safe distance is determined based on the obstacle. i The characteristic parameters are obtained; S4.

3. Based on the characteristics of the EKF algorithm, determine the upper and lower limits of the speed magnitude. The upper and lower limits of the speed magnitude are expressed by the following formulas: (11) in, v max t This is the upper limit of the speed. v min t This is the lower limit of the speed magnitude. v phys, max , v phys, min These represent the maximum and minimum speeds that the aircraft can reach during navigation, respectively, with max(·) and min(·) being functions for taking the larger and smaller speeds, respectively. α risk Let Δ be the collision risk coefficient, and diag(·) be the function for constructing the diagonal matrix. v t Let Δ be the velocity disturbance value. x t Here is the perturbation matrix. α unc To transform the loss matrix, α 1. α Both 2 are two-dimensional vectors. k σ The disturbance coefficient; S4.

4. Combining the feasible region of the speed obstacle space and the upper and lower limits of the speed magnitude, the final command is derived, as shown in the following formula: (12) (13) Here, clip(·) and argmin(·) are both functions. The defined speed value. V safe t for t The feasible region of the velocity obstacle space at any given time. v s t For the final speed, v s t and v RL t Same direction J 2×2 T for J 2×2 transpose, u s t This is the final instruction.

7. The path planning method for an unmanned surface vehicle with autonomous obstacle avoidance function as described in claim 1, characterized in that, The process of establishing the adaptive learning model in step S5 is as follows: S5.

1. Set the sample time period and intervention threshold. If the result at a certain time... and v RL t If the difference is that the constraint condition at that moment interferes with the final instruction, the sample period is a series of consecutive judgment periods including the current judgment period. After each final instruction is obtained, the constraint condition in the current judgment period is judged to determine whether it interferes. If the number of times the constraint condition interferes in the sample period reaches the interference threshold, the constraint condition is considered to be in a bad state. S5.

2. Setting Feature Factors σ 1 and σ 2, σ 1 and σ The value of 2 is derived from the following values: (14) in, δ t for t Intervention intensity index at any given time N VO t for t The number of interventions within the sample time period corresponding to the given time point. k 1. k 2. k 3 are all coefficients; α 1. α 2 element values ​​and α risk All values ​​are based on the obtained σ 1 and σ 2. Update.

Images