A Method and System for Obstacle Avoidance Trajectory Planning of Marine Robots Considering Flow-Induced Position Deviation

Abstract

This invention relates to a method and system for obstacle avoidance trajectory planning for marine robots that considers current-induced positional deviations, belonging to the field of marine robot technology. It addresses the problem that existing marine robot path planning methods do not consider the impact of ocean currents on collision avoidance, leading to poor safety during navigation. The invention acquires the marine robot's current position, attitude, ground speed, water speed, and marine environmental information of the navigation area; obtains a set of speed limits for the marine robot; calculates a set of emergency obstacle avoidance actions using an obstacle evaluation function; obtains the final set of obstacle avoidance actions; combines this with the robot's velocity set for the next decision cycle to obtain the final action space window; and calculates a cluster of candidate trajectories in the current state based on velocity samples within the time window and current-induced drift angles, establishes an evaluation function, evaluates and optimizes the candidate trajectory clusters, and obtains the optimal obstacle avoidance trajectory. This invention is used for obstacle avoidance trajectory planning for marine robots.

Description

Technical Field

[0001] This invention belongs to the field of marine robot technology. Background Technology

[0002] Marine robots, as equipment for monitoring the marine environment, safeguarding maritime rights and interests, and performing combat missions, have broad application prospects. Currently, marine robots possess significant advantages in reconnaissance, surveillance, electronic warfare, and counter-reconnaissance operations in specific sea areas. Simultaneously, in the civilian sector, marine robots can perform tasks such as resource exploration, search and rescue, meteorological monitoring and forecasting, and communication relay stations. Therefore, in-depth research into marine robot technology is of great significance for enhancing my country's national defense capabilities and expanding its civilian applications.

[0003] Path planning is one of the core technologies in marine robot research. A good navigation route helps marine robots complete tasks efficiently and with high quality. In actual navigation, marine robots encounter unknown obstacles and other vessels not marked on maps; therefore, it is necessary to explore hazard avoidance methods for marine robots.

[0004] The paper "A constrained A* approach towards optimal path planning for an unmanned surface vehicle in a maritime environment containing dynamic obstacles and ocean currents" proposes a constrained A* method for optimal path planning of marine robots in restricted marine environments. Simulations in various environments including static obstacles, moving obstacles, and ocean currents of varying intensities validate the algorithm's ability to generate safe waypoints and maintain a safe distance from obstacles. However, this algorithm does not consider the kinematic impact of ocean currents on collision avoidance for the marine robot.

[0005] The paper "Path Planning for Autonomous Underwater Vehicles Under the Influence of Ocean Currents Based on a Fusion Heuristic Algorithm" proposes a heuristic fusion algorithm to solve the path planning problem for AUVs in complex underwater environments with dense obstacles, considering the influence of ocean currents. This algorithm not only accelerates convergence but also avoids getting trapped in local optima. However, this algorithm only considers the impact of ocean currents from the perspective of energy consumption and does not consider the impact of ocean currents on the safe navigation of marine robots.

[0006] However, most existing path planning methods do not consider the impact of strong gusts of ocean currents on the collision avoidance safety of marine robots. In practical engineering applications, marine robots are relatively small in size and are easily affected by gusts of ocean currents during collision avoidance. Summary of the Invention

[0007] This invention addresses the problem that existing path planning methods for marine robots do not consider the impact of ocean currents on collision avoidance, resulting in poor safety during navigation. It provides a method and system for planning obstacle avoidance trajectories for marine robots that considers current-induced positional deviations.

[0008] The obstacle avoidance trajectory planning method for marine robots that considers flow-induced position deviation, as described in this invention, includes:

[0009] Step 1: Obtain the current position, attitude, speed to land, speed to water, and marine environmental information of the navigation area of ​​the marine robot;

[0010] Step 2: Based on the marine robot's own motion characteristics and marine environment information, obtain the marine robot's navigation speed limit set, and use the obstacle evaluation function to calculate the marine robot's emergency obstacle avoidance action set under the current position, attitude, and water speed conditions; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set;

[0011] Step 3: Obtain the maximum acceleration of the marine robot, and then obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state. Find the intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle to obtain the final action space window.

[0012] Step 4: Based on the ocean robot's speed in water and speed on land obtained in Step 1, calculate the ocean current velocity on land and the current-induced drift angle β;

[0013] Step 5: Sample the motion velocity within the action space window. Using the motion velocity samples, the ocean current velocity to the ground, and the current-induced drift angle β, establish the state transition equation of the marine robot, calculate the candidate trajectory cluster in the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal obstacle avoidance trajectory.

[0014] Furthermore, in this invention, in step one, obtaining marine environmental information of the navigation area where the marine robot is located includes: information on static and dynamic obstacles around the marine robot and information on the speed of ocean currents.

[0015] Furthermore, in this invention, the method for obtaining the set of extreme speed ranges for the marine robot's navigation in step two is as follows:

[0016] First, calculate the set V of the marine robot's velocity range, determined by the maximum and minimum permissible linear and angular velocities. s :

[0017] V s ={(v w ,ω)∣v w ∈[v wmin ,v wmax ],ω∈[ω min ,ω max ]}

[0018] In the formula, v wmin ,v wmax ω represents the maximum and minimum permissible linear velocity of a marine robot. min ,ω max Indicates the maximum and minimum permissible angular velocities of a marine robot; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot.

[0019] Next, calculate the set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a :

[0020]

[0021] In the formula, dist(v w ,ω) is the obstacle distance evaluation function, used to evaluate the distance between the current trajectory and the nearest obstacle; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot. This represents the derivative of the ocean robot's velocity with respect to the waterline, i.e., the ocean robot's acceleration with respect to the waterline. The derivative of the angular velocity of a marine robot, i.e., the angular acceleration of the marine robot;

[0022] The set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a and the set of speed ranges for marine robots V s Find the intersection V s ∩V a Obtain the final set of obstacle avoidance speed ranges.

[0023] Furthermore, in this invention, in step three, the set of motion speed ranges that the marine robot can achieve within a decision cycle under the current state is obtained as follows:

[0024]

[0025] V dω represents the set of possible speeds that the marine robot can achieve within one decision cycle under the current state. a The angular velocity v represents the current state. wa Represents the linear velocity of the current state. This represents the maximum linear acceleration in the current state. T represents the current maximum angular acceleration. DWA Indicates the next decision-making cycle.

[0026] Furthermore, in this invention, in step three, the final action space window is:

[0027] V r =V s ∩V a ∩V d

[0028] Among them, V r This represents the final action space window.

[0029] Furthermore, in this invention, the process of calculating the flow-induced drift angle β in step four is as follows:

[0030] Through the formula:

[0031] v cux =v cx -v x

[0032] v cuy =v cy -v y

[0033] Calculate the ground velocity v of the ocean current in the X and Y directions. cux and v cuy , where v x and v y This represents the ground-based speed of the marine robot in the X and Y axes, v. cx and v cy This indicates the speed of the marine robot in the water along the X and Y axes;

[0034] Reuse formula:

[0035]

[0036] Calculate the drift angle β of the marine robot caused by ocean currents.

[0037] Furthermore, in this invention, the method for calculating the candidate trajectory cluster in the current state in step five is as follows:

[0038] Establish a kinematic model for the marine robot:

[0039]

[0040] In the formula, x(t) and y(t) are the Cartesian coordinates of the marine robot, u is the longitudinal linear velocity of the marine robot in the forward direction, v is the lateral linear velocity of the marine robot perpendicular to the forward direction, and ψ(t) is the heading angle of the marine robot. Let r(t) be the angular derivative of the heading angle of the marine robot. This represents the velocity of the marine robot along the x-axis. This indicates the velocity of the marine robot along the y-axis.

[0041] The motion velocity samples are added to the kinematic model of the marine robot to obtain the state transition equation of the marine robot. The candidate trajectory cluster of the previous state is obtained using the transition equation. The transition equation is:

[0042]

[0043] Where C(t) = θ(t) + β, v w (t)=v e (t)+v cu In the formula, t is the current time, Δt is the time window interval, θ(t) is the heading of the marine robot relative to the ocean current at time t, C(t) is the heading of the marine robot relative to the ground, and v w (t) represents the waterline velocity of the marine robot at time t, v e (t) is the ground speed of the ocean robot at time t, v cu α is the velocity of the ocean current relative to the land, a(t) is the linear acceleration of the marine robot at time t, α(t) is the angular acceleration of the marine robot at time t, and v x (t) and v y (t) represents the ground speed of the marine robot in the X and Y directions at time t.

[0044] Furthermore, in this invention, step five, the evaluation function, is:

[0045] G(v,ω)=σ(f·heading(v w ,ω)+g·dist(v w ,ω)+γ·velocity(v w ,ω))

[0046] In the formula, σ represents the normalization coefficient; heading(v w ,ω) is the azimuth evaluation function, f is the azimuth evaluation coefficient, dist(v w Velocity(v, ω) is the obstacle distance evaluation function, g is the obstacle distance evaluation coefficient, and velocity(v) is the distance evaluation coefficient. w ,ω) is the velocity evaluation function, and γ is the velocity evaluation coefficient.

[0047] A marine robot obstacle avoidance trajectory planning system considering flow-induced position deviation includes: an information acquisition module, a final obstacle avoidance action set generation module, an action space window generation module, and an optimal trajectory generation module;

[0048] The information acquisition module is used to acquire the current position, attitude, speed to ground, speed to ocean currents, and marine environmental information of the navigation area of ​​the marine robot;

[0049] The final obstacle avoidance action set generation module is used to obtain the speed limit set of the marine robot, calculate the current position of the marine robot and the emergency obstacle avoidance action set under the current position, attitude and speed of the marine robot in the water using the obstacle evaluation function; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set;

[0050] The action space window generation module is used to obtain the maximum acceleration of the marine robot, and then obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state. The intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle is calculated to obtain the final action space window.

[0051] The optimal trajectory generation module calculates the ocean current velocity and the current-induced drift angle β based on the ocean robot's speed relative to water and speed relative to land. It samples the motion velocity within the action space window, and uses the motion velocity samples, ocean current velocity relative to land, and current-induced drift angle β to establish the ocean robot's state transition equation. It then calculates the candidate trajectory clusters in the current state, establishes an evaluation function, evaluates and optimizes the candidate trajectory clusters, and obtains the optimal obstacle avoidance trajectory.

[0052] In practical engineering applications, marine robots face increasingly complex and dangerous collision avoidance scenarios. Due to their relatively small size, marine robots are easily affected by gusts of ocean currents during collision avoidance. This invention calculates the current-induced position deviation and current-induced drift angle under the influence of gusts of ocean currents, optimizes the DWA collision avoidance algorithm, and solves the problem of marine robots deviating from obstacles due to strong gusts of ocean currents during collision avoidance. It also selects safer and more effective hazard avoidance paths and ensures that marine robots can complete the obstacle avoidance process safely and at high speed, thereby improving the navigation safety and hazard avoidance efficiency of marine robots. Attached Figure Description

[0053] Figure 1 This is a flowchart of the agile obstacle avoidance trajectory planning method for marine robots that considers flow-induced position deviation, as described in this invention.

[0054] Figure 2 This is a diagram of a dynamic window;

[0055] Figure 3This is a graph showing the relationship between the speed of a marine robot in water and its speed on land.

[0056] Figure 4 This is a schematic diagram of the drift angle of a marine robot under the influence of ocean currents;

[0057] Figure 5 This is a diagram of a marine robot's motion model;

[0058] Figure 6 This is a cluster map of candidate trajectories for marine robots;

[0059] Figure 7 This is a schematic diagram for evaluating the azimuth angle of a marine robot;

[0060] Figure 8 This is a system schematic diagram of the present invention. Detailed Implementation

[0061] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and 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. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0062] Specific Implementation Method 1: Refer to Figure Figure 1 This embodiment specifically describes the obstacle avoidance trajectory planning method for marine robots that considers flow-induced position deviations, including:

[0063] Step 1: Obtain the current position, attitude, speed to land, speed to water, and marine environmental information of the navigation area of ​​the marine robot;

[0064] Step 2: Based on the marine robot's own motion characteristics and marine environment information, obtain the marine robot's navigation speed limit set, and use the obstacle evaluation function to calculate the marine robot's emergency obstacle avoidance action set under the current position, attitude, and water speed conditions; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set;

[0065] Step 3: Obtain the maximum acceleration of the marine robot, and then obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state. Find the intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle to obtain the final action space window.

[0066] Step 4: Based on the ocean robot's speed in water and speed on land obtained in Step 1, calculate the ocean current velocity on land and the current-induced drift angle β;

[0067] Step 5: Sample the motion velocity within the action space window. Using the motion velocity samples, the ocean current velocity to the ground, and the current-induced drift angle β, establish the state transition equation of the marine robot, calculate the candidate trajectory cluster in the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal obstacle avoidance trajectory.

[0068] Furthermore, in this invention, in step one, obtaining marine environmental information of the navigation area where the marine robot is located includes: information on static and dynamic obstacles around the marine robot and information on the speed of ocean currents.

[0069] Furthermore, in this invention, the method for obtaining the set of extreme speed ranges for the marine robot's navigation in step two is as follows:

[0070] First, calculate the set V of the marine robot's velocity range, determined by the maximum and minimum permissible linear and angular velocities. s :

[0071] V s ={(v w ,ω)∣v w ∈[v wmin ,v wmax ],ω∈[ω min ,ω max ]}

[0072] In the formula, v wmin ,v wmax ω represents the maximum and minimum permissible linear velocity of a marine robot. min ,ω max Indicates the maximum and minimum permissible angular velocities of a marine robot; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot.

[0073] Next, calculate the set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a :

[0074]

[0075] In the formula, dist(v w ,ω) is the obstacle distance evaluation function, used to evaluate the distance between the current trajectory and the nearest obstacle; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot. This represents the derivative of the ocean robot's velocity with respect to the waterline, i.e., the ocean robot's acceleration with respect to the waterline. The derivative of the angular velocity of a marine robot, i.e., the angular acceleration of the marine robot;

[0076] The set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a and the set of speed ranges for marine robots V s Find the intersection V s ∩V a Obtain the final set of obstacle avoidance speed ranges.

[0077] Furthermore, in this invention, in step three, the set of motion speed ranges that the marine robot can achieve within a decision cycle under the current state is obtained as follows:

[0078]

[0079] V d ω represents the set of possible speeds that the marine robot can achieve within one decision cycle under the current state. a The angular velocity v represents the current state. wa Represents the linear velocity of the current state. This represents the maximum linear acceleration in the current state. T represents the current maximum angular acceleration. DWA Indicates the next decision-making cycle.

[0080] Furthermore, in this invention, in step three, the final action space window is:

[0081] V r =V s ∩V a ∩V d

[0082] Among them, V r This represents the final action space window.

[0083] Furthermore, in this invention, the process of calculating the flow-induced drift angle β in step four is as follows:

[0084] Through the formula:

[0085] v cux =v cx -v x

[0086] v cuy =v cy -v y

[0087] Calculate the ground velocity v of the ocean current in the X and Y directions. cux and v cuy , where v x and v y This represents the ground-based speed of the marine robot in the X and Y axes, v.cx and v cy This indicates the speed of the marine robot in the water along the X and Y axes;

[0088] Reuse formula:

[0089]

[0090] Calculate the drift angle β of the marine robot caused by ocean currents.

[0091] Furthermore, in this invention, the method for calculating the candidate trajectory cluster in the current state in step five is as follows:

[0092] Establish a kinematic model for the marine robot:

[0093]

[0094] In the formula, x(t) and y(t) are the Cartesian coordinates of the marine robot, u is the longitudinal linear velocity of the marine robot in the forward direction, v is the lateral linear velocity of the marine robot perpendicular to the forward direction, and ψ(t) is the heading angle of the marine robot. Let r(t) be the angular derivative of the heading angle of the marine robot. This represents the velocity of the marine robot along the x-axis. This indicates the velocity of the marine robot along the y-axis.

[0095] The motion velocity samples are added to the kinematic model of the marine robot to obtain the state transition equation of the marine robot. The candidate trajectory cluster of the previous state is obtained using the transition equation. The transition equation is:

[0096]

[0097] Where C(t) = θ(t) + β, v w (t)=v e (t)+v cu In the formula, t is the current time, Δt is the time window interval, θ(t) is the heading of the marine robot relative to the ocean current at time t, C(t) is the heading of the marine robot relative to the ground, and v w (t) represents the waterline velocity of the marine robot at time t, v e (t) is the ground speed of the ocean robot at time t, v cu α is the velocity of the ocean current relative to the land, a(t) is the linear acceleration of the marine robot at time t, α(t) is the angular acceleration of the marine robot at time t, and v x (t) and v y (t) represents the ground speed of the marine robot in the X and Y directions at time t.

[0098] Furthermore, in this invention, step five, the evaluation function, is:

[0099] G(v,ω)=σ(f·heading(v w ,ω)+g·dist(v w ,ω)+γ·velocity(v w ,ω))

[0100] In the formula, σ represents the normalization coefficient; heading(v w ,ω) is the azimuth evaluation function, f is the azimuth evaluation coefficient, dist(v w Velocity(v, ω) is the obstacle distance evaluation function, g is the obstacle distance evaluation coefficient, and velocity(v) is the distance evaluation coefficient. w ,ω) is the velocity evaluation function, and γ is the velocity evaluation coefficient.

[0101] A marine robot obstacle avoidance trajectory planning system considering flow-induced position deviation includes: information acquisition module 1, final obstacle avoidance action set generation module 2, action space window generation module 3, and optimal trajectory generation module 4;

[0102] The information acquisition module 1 is used to acquire the current position, attitude, speed to ground, speed to ocean current, and marine environmental information of the navigation area of ​​the marine robot;

[0103] The final obstacle avoidance action set generation module 2 is used to obtain the speed limit set of the marine robot, calculate the current position of the marine robot and the emergency obstacle avoidance action set under the current position, attitude and speed of the marine robot in the water using the obstacle evaluation function; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set;

[0104] The action space window generation module 3 is used to obtain the maximum acceleration of the marine robot, and then obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state. The intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle is calculated to obtain the final action space window.

[0105] The optimal trajectory generation module 4 calculates the ocean current velocity and the current-induced drift angle β based on the ocean robot's water velocity and ground speed in step one; it samples the motion velocity within the action space window, and uses the motion velocity samples, ocean current velocity, and current-induced drift angle β to establish the ocean robot's state transition equation, calculates the candidate trajectory clusters in the current state, establishes an evaluation function, evaluates and optimizes the candidate trajectory clusters, and obtains the optimal obstacle avoidance trajectory.

[0106] The specific automatic obstacle avoidance method includes the following steps:

[0107] 1. Acquire marine environmental information of the navigation area of ​​the marine robot and its own position, attitude and speed information. The marine robot is equipped with lidar, binocular vision system, forward-looking sonar, multibeam sonar, electromagnetic current meter, Beidou integrated machine and integrated navigation system, which can obtain the marine robot's own position, attitude and speed information and surrounding marine environmental information in real time.

[0108] The aforementioned marine environmental information refers to information on static and dynamic obstacles around the marine robot, as well as the speed and direction of ocean currents.

[0109] 2. Obtain the set of speed limits for the marine robot and calculate the set of actions that enable the marine robot to avoid collisions with obstacles through emergency stopping;

[0110] V s This represents the set of speed limits for a marine robot, determined by its maximum and minimum permissible linear and angular velocities. The specific mathematical formula is as follows:

[0111] First, calculate the set V of the marine robot's velocity range, determined by the maximum and minimum permissible linear and angular velocities. s :

[0112] V s ={(v w ,ω)∣v w ∈[v wmin ,v wmax ],ω∈[ω min ,ω max ]}

[0113] In the formula, v wmin ,v wmax ω represents the maximum and minimum permissible linear velocity of a marine robot. min ,ω max Indicates the maximum and minimum permissible angular velocities of a marine robot; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot.

[0114] Next, calculate the set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a :

[0115]

[0116] In the formula, dist(v w ,ω) is the obstacle distance evaluation function, used to evaluate the distance between the current trajectory and the nearest obstacle; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot. This represents the derivative of the ocean robot's velocity with respect to the waterline, i.e., the ocean robot's acceleration with respect to the waterline. The derivative of the angular velocity of a marine robot, i.e., the angular acceleration of the marine robot;

[0117] The set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a and the set of speed ranges for marine robots V s Find the intersection V s ∩V a 3. Obtain the set of final obstacle avoidance motion speed ranges. Under the maximum acceleration constraint, obtain the set of speeds the marine robot can reach within one decision period T in the current state, and take the intersection to obtain the final action space window; such as... Figure 2 As shown;

[0118] V d This indicates that based on the current velocity v of the waterline... wa With angular velocity ω a At maximum linear acceleration With maximum angular acceleration Under constraints, the next decision period T DWA The set of speeds achievable by ocean-going robots. The specific mathematical formula is:

[0119]

[0120] Final dynamic action space window V r The calculation formula is:

[0121] V r =V s ∩V a ∩V d

[0122] 4. Based on the ocean robot's velocity relative to water and its speed relative to land, calculate the ocean current velocity relative to land and the current-induced drift angle β; the relationship between the ocean robot's velocity relative to water and its speed relative to land is shown in the figure below. Figure 3 As shown; the flow-induced drift angle β is as follows Figure 4 As shown;

[0123] v cux =v cx -v x

[0124] v cuy =v cy -v y

[0125] In the formula, v cux and v cuy This represents the velocity of the ocean current relative to the ground in the X and Y axes, v. x and vy This represents the ground-based speed of the marine robot in the X and Y axes, v. cx and v cy This indicates the induced current velocity in the X and Y axes, as measured by the electromagnetic current meter carried by the marine robot.

[0126]

[0127] In the formula, β represents the drift angle of the marine robot caused by ocean currents, and v cux and v cuy This represents the velocity of the ocean current relative to the ground in the X and Y axes, v. x and v y This indicates the ground speed of the marine robot in the X and Y axis directions.

[0128] 5. Sample the action space window in each decision cycle to obtain the candidate trajectory cluster in the current state, and design an evaluation function. Through repeated iterations in the candidate trajectory cluster, continuously search for the current optimal trajectory, determine the collision avoidance action that the marine robot should take, and finally reach the target point.

[0129] In a very short time, the trajectory of a marine robot can be approximated as uniform linear motion. XY is the geodetic coordinate system, x... robot -y robot Using the attached coordinate system, θ represents the heading angle of the marine robot. The kinematic model of the marine robot is as follows: Figure 5 As shown, the formula is:

[0130]

[0131] In the formula, x(t) and y(t) are the Cartesian coordinates of the marine robot, which can be expressed as follows: u represents the longitudinal linear velocity of the marine robot in the direction of travel, v represents the lateral linear velocity of the marine robot perpendicular to the direction of travel, and ψ(t) represents the heading angle of the marine robot. Let r(t) be the angular derivative of the marine robot's heading, i.e., its angular velocity; the velocity vector of the marine robot in Cartesian coordinates can be expressed as... This represents the velocity of the marine robot along the x-axis. Let u(t) represent the velocity along the y-axis of the marine robot. To represent dynamic constraints, the longitudinal velocity of the marine robot satisfies 0 ≤ u(t) ≤ u. max Angular velocity satisfies -r max ≤r(t)≤r max .

[0132] Let p(t) and v(t) represent the state of the marine robot at a certain moment. Then, by adding the two variables of sampling action (v, ω) from the DWA algorithm, the state can be obtained. Its mathematical expression is [x(t), y(t), θ(t), v, ω]. T The state transition equation for the marine robot can be obtained as follows:

[0133]

[0134] in, v w (t)=v e (t)+v c

[0135] In the formula, t is the current time, Δt is the time window interval, θ is the heading of the marine robot relative to the ocean current, C(t) is the heading of the marine robot relative to the ground, and v w (t) is the underwater speed of the marine robot, v e (t) represents the ground-based speed of the marine robot, a represents the linear acceleration of the marine robot, and α represents the angular acceleration of the marine robot. Candidate trajectory families for the marine robot are obtained through state transition equations, such as... Figure 6 As shown;

[0136] A schematic diagram of the azimuth evaluation of marine robots is shown below. Figure 7 As shown, the evaluation function formula for the DWA algorithm under the influence of dynamic coupling is:

[0137] G(v w ,ω)=σ(α·heading(v w ,ω)+β·dist(v w ,ω)+γ·velocity(v w ,ω))

[0138] In the formula, σ represents the normalization coefficient; heading(v w ω) is the azimuth evaluation function, used to evaluate the angular difference between the orientation of the marine robot and the target when it reaches the end of the simulated trajectory at the currently set sampling velocity, such as... Figure 7 As shown in the diagram, θ represents the deviation between the end-point heading of the marine robot's trajectory and the target heading. A smaller θ indicates a smaller deviation, but the evaluation function seeks the maximum value, so θ needs to be supplemented. α is the azimuth evaluation coefficient; a larger α indicates a trajectory closer to the target heading. dist(v w ω) is the obstacle distance evaluation function, used to evaluate the distance between the current trajectory and the nearest obstacle. β is the obstacle distance evaluation coefficient; the larger the β, the farther the target is from the obstacle. velocity(v) wω) is the velocity evaluation function, used to evaluate the magnitude of the current trajectory's velocity. γ is the velocity evaluation coefficient; the larger γ is, the greater the selected trajectory velocity, and the faster it can approach the target point.

[0139] The evaluation function is normalized using the following steps:

[0140]

[0141] In the formula, n is the number of all sampled trajectories, and i is the number of the current trajectory to be evaluated.

[0142] A marine robot obstacle avoidance trajectory planning system considering flow-induced position deviation, combined with Figure 8 As described above, the system includes: an information acquisition module 1, a final obstacle avoidance action set generation module 2, an action space window generation module 3, and an optimal trajectory generation module 4;

[0143] The information acquisition module 1 is used to acquire the current position, attitude, speed to ground, speed to ocean current, and marine environmental information of the navigation area of ​​the marine robot;

[0144] The final obstacle avoidance action set generation module 2 is used to obtain the marine robot's navigation speed limit set, calculate the marine robot's current position and the emergency obstacle avoidance action set under the conditions of the marine robot's current position, attitude, and ground speed using the obstacle evaluation function; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set;

[0145] The action space window generation module 3 is used to obtain the maximum acceleration of the marine robot, obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state, find the intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle, and obtain the final action space window.

[0146] The optimal trajectory generation module 4 is used to calculate the ocean current velocity relative to the ground and the current-induced drift angle β based on the ocean robot's information on ocean current velocity and positioning. Within the current decision period, the module samples the motion velocity within the action space window, uses the motion velocity samples, ocean current velocity relative to the ground and current-induced drift angle β to calculate the candidate trajectory cluster in the current state, establishes an evaluation function, evaluates and optimizes the candidate trajectory cluster, and obtains the optimal trajectory.

[0147] The system described in this invention continuously provides the latest and optimal hazard avoidance action plan based on updated marine environmental information and the real-time location of the marine robot.

[0148] The marine robot's environmental perception system acquires obstacle information about its surroundings using lidar, a binocular vision system, forward-looking sonar, and multibeam sonar. The binocular vision system can switch between day and night modes, using a visible light camera during the day and an infrared imaging camera at night. An electromagnetic current meter acquires information on the velocity and direction of ocean currents.

[0149] The marine robot positioning system obtains the marine robot's pose information through the Beidou integrated machine and combined navigation, transmits it to the information data processing module, and provides time-varying position information to the hazard avoidance module;

[0150] Marine robot control system: Upon receiving evasive maneuver commands, it executes the control mechanisms and navigates according to the prescribed instructions.

[0151] While the invention has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that different dependent claims and features described herein can be combined in ways different from those described in the original claims. It is also understood that features described in conjunction with individual embodiments can be used in other described embodiments.

Claims

1. A method for obstacle avoidance trajectory planning for marine robots considering flow-induced position deviation, characterized in that, include: Step 1: Obtain the current position, attitude, speed to land, speed to water, and marine environmental information of the navigation area of ​​the marine robot; Step 2: Based on the marine robot's own motion characteristics and marine environment information, obtain the marine robot's navigation speed limit set, and use the obstacle evaluation function to calculate the marine robot's emergency obstacle avoidance action set under the current position, attitude, and water speed conditions; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set; Step 3: Obtain the maximum acceleration of the marine robot, and then obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state. Find the intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle to obtain the final action space window. Step 4: Based on the ocean robot's speed in water and speed on land obtained in Step 1, calculate the ocean current velocity on land and the current-induced drift angle β; Step 5: Sample the motion velocity within the action space window. Using the motion velocity samples, the ocean current velocity to the ground, and the current-induced drift angle β, establish the state transition equation of the marine robot, calculate the candidate trajectory cluster in the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal obstacle avoidance trajectory. In step four, the process of calculating the flow-induced drift angle β is as follows: Through the formula: v cux =v cx -v x v cuy =v cy -v y Calculate the ground velocity v of the ocean current in the X and Y directions. cux and v cuy , where v x and v y This represents the ground-based speed of the marine robot in the X and Y axes, v. cx and v cy This indicates the speed of the marine robot in the water along the X and Y axes; Reuse formula: Calculate the flow-induced drift angle β of the marine robot caused by ocean currents; In step five, the method for calculating the candidate trajectory cluster in the current state is as follows: Establish a kinematic model for the marine robot: In the formula, x(t) and y(t) are the Cartesian coordinates of the marine robot, u is the longitudinal linear velocity of the marine robot in the forward direction, v is the lateral linear velocity of the marine robot perpendicular to the forward direction, and ψ(t) is the heading angle of the marine robot. Let r(t) be the angular derivative of the heading angle of the marine robot. This represents the velocity of the marine robot along the x-axis. This indicates the velocity of the marine robot along the y-axis. The motion velocity samples are added to the kinematic model of the marine robot to obtain the state transition equation of the marine robot. The candidate trajectory cluster of the previous state is obtained using the transition equation. The transition equation is: Where C(t) = θ(t) + β, v w (t)=v e (t)+v cu In the formula, t is the current time, Δt is the time window interval, θ(t) is the heading of the marine robot relative to the ocean current at time t, C(t) is the heading of the marine robot relative to the ground, and v w (t) represents the waterline velocity of the marine robot at time t, v e (t) is the ground speed of the ocean robot at time t, v cu α is the velocity of the ocean current relative to the land, a(t) is the linear acceleration of the marine robot at time t, α(t) is the angular acceleration of the marine robot at time t, and v x (t) and v y (t) represents the ground speed of the marine robot in the X and Y directions at time t.

2. The method for obstacle avoidance trajectory planning of marine robots considering flow-induced position deviation according to claim 1, characterized in that, In step one, the marine environment information of the navigation area where the marine robot is located includes: information on static and dynamic obstacles around the marine robot and the speed information of ocean currents.

3. The method for obstacle avoidance trajectory planning of marine robots considering flow-induced position deviation according to claim 1, characterized in that, In step two, the method for obtaining the set of extreme speed ranges for the ocean robot's navigation is as follows: First, calculate the set V of the marine robot's velocity range, determined by the maximum and minimum permissible linear and angular velocities. s : V s ={(v w ,ω)∣v w ∈[v wmin ,v wmax ],ω∈[ω min ,ω max ]} In the formula, v wmin ,v wmax ω represents the maximum and minimum permissible linear velocity of a marine robot. min ,ω max Indicates the maximum and minimum permissible angular velocities of a marine robot; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot. Next, calculate the set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a : In the formula, dist(v w ,ω) is the obstacle distance evaluation function, used to evaluate the distance between the current trajectory and the nearest obstacle; v w ω represents the vertical velocity of the marine robot relative to the waterline, and ω represents the angular velocity of the marine robot. This represents the derivative of the ocean robot's velocity with respect to the waterline, i.e., the ocean robot's acceleration with respect to the waterline. The derivative of the angular velocity of a marine robot, i.e., the angular acceleration of the marine robot; The set V of actions (v, ω) that enable the marine robot to avoid collisions with obstacles by making an emergency stop. a and the set of speed ranges for marine robots V s Find the intersection V s ∩V a Obtain the final set of obstacle avoidance speed ranges.

4. The obstacle avoidance trajectory planning method for marine robots considering flow-induced position deviation according to claim 1, characterized in that, In step three, the set of motion speed ranges that the marine robot can achieve within the current decision cycle is obtained as follows: V d ω represents the set of possible speeds that the marine robot can achieve within one decision cycle under the current state. a The angular velocity v represents the current state. wa Represents the linear velocity of the current state. This represents the maximum linear acceleration in the current state. T represents the current maximum angular acceleration. DWA Indicates the next decision-making cycle.

5. The obstacle avoidance trajectory planning method for marine robots considering flow-induced position deviation according to claim 4, characterized in that, In step three, the final action space window is: V r =V s ∩V a ∩V d Among them, V r This represents the final action space window.

6. The method for obstacle avoidance trajectory planning of marine robots considering flow-induced position deviation according to claim 1, characterized in that, Step 5, the evaluation function is: G(v,ω)=σ(f·heading(v w ,ω)+g·dist(v w ,ω)+γ·velocity(v w ,ω)) In the formula, σ represents the normalization coefficient; heading(v w ,ω) is the azimuth evaluation function, f is the azimuth evaluation coefficient, dist(v w Velocity(v, ω) is the obstacle distance evaluation function, g is the obstacle distance evaluation coefficient, and velocity(v) is the distance evaluation coefficient. w ,ω) is the velocity evaluation function, and γ is the velocity evaluation coefficient.

7. An agile obstacle avoidance trajectory planning system for marine robots considering flow-induced position deviation, characterized in that, include: Information acquisition module (1), final obstacle avoidance action set generation module (2), action space window generation module (3) and optimal trajectory generation module (4); The information acquisition module (1) is used to acquire the current position, attitude, speed to ground, speed to ocean current, and marine environmental information of the navigation area of ​​the marine robot; The final obstacle avoidance action set generation module (2) is used to obtain the marine robot's navigation speed limit set, calculate the marine robot's current position and the marine robot's current position, attitude, and water speed under emergency obstacle avoidance action set using the obstacle evaluation function; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set to obtain the final obstacle avoidance action set; The action space window generation module (3) is used to obtain the maximum acceleration of the marine robot, and then obtain the set of speeds that the marine robot can reach in one decision cycle under the current motion state, calculate the intersection of the final obstacle avoidance action set and the set of speeds that the marine robot can reach in the next decision cycle, and obtain the final action space window. The optimal trajectory generation module (4) calculates the ocean current velocity and the current-induced drift angle β based on the ocean robot's water velocity and ground speed; it samples the motion velocity within the action space window, and uses the motion velocity samples, ocean current velocity and current-induced drift angle β to establish the ocean robot's state transition equation, calculates the candidate trajectory clusters in the current state, establishes an evaluation function, evaluates and optimizes the candidate trajectory clusters, and obtains the optimal obstacle avoidance trajectory. The process of calculating the flow-induced drift angle β is as follows: Through the formula: v cux =v cx -v x v cuy =v cy -v y Calculate the ground velocity v of the ocean current in the X and Y directions. cux and v cuy , where v x and v y This represents the ground-based speed of the marine robot in the X and Y axes, v. cx and v cy This indicates the speed of the marine robot in the water along the X and Y axes; Reuse formula: Calculate the flow-induced drift angle β of the marine robot caused by ocean currents; The method for calculating the candidate trajectory cluster in the current state is as follows: Establish a kinematic model for the marine robot: In the formula, x(t) and y(t) are the Cartesian coordinates of the marine robot, u is the longitudinal linear velocity of the marine robot in the forward direction, v is the lateral linear velocity of the marine robot perpendicular to the forward direction, and ψ(t) is the heading angle of the marine robot. Let r(t) be the angular derivative of the heading angle of the marine robot. This represents the velocity of the marine robot along the x-axis. This indicates the velocity of the marine robot along the y-axis. The motion velocity samples are added to the kinematic model of the marine robot to obtain the state transition equation of the marine robot. The candidate trajectory cluster of the previous state is obtained using the transition equation. The transition equation is: Where C(t) = θ(t) + β, v w (t)=v e (t)+v cu In the formula, t is the current time, Δt is the time window interval, θ(t) is the heading of the marine robot relative to the ocean current at time t, C(t) is the heading of the marine robot relative to the ground, and v w (t) represents the waterline velocity of the marine robot at time t, v e (t) is the ground speed of the ocean robot at time t, v cu α is the velocity of the ocean current relative to the land, a(t) is the linear acceleration of the marine robot at time t, α(t) is the angular acceleration of the marine robot at time t, and v x (t) and v y (t) represents the ground speed of the marine robot in the X and Y directions at time t.

Images