Ocean robot obstacle avoidance trajectory planning method and system considering flow-induced position deviation

Abstract

The invention discloses an ocean robot obstacle avoidance trajectory planning method and system considering flow-induced position deviation, and belongs to the technical field of ocean robots. The problem that an existing ocean robot path planning method does not consider the influence of ocean current on collision avoidance of the ocean robot, and consequently the safety is poor in the navigation process is solved. The method comprises the following steps: acquiring the current position, posture, ground navigational speed, water navigational speed and marine environment information of a navigation area of the marine robot; acquiring a navigation speed limit set of the ocean robot, and calculating an action set of emergency obstacle avoidance of the ocean robot by using the obstacle evaluation function; obtaining a final obstacle avoidance action set; a final action space window is obtained by combining a speed set of the ocean robot in the next decision period; and on the basis of the internal speed sample in the time window and the flow-induced drift angle, calculating a candidate trajectory cluster in the current state, establishing an evaluation function, evaluating and optimizing the candidate trajectory cluster, and obtaining an optimal obstacle avoidance trajectory. The method is used for planning the obstacle avoidance trajectory of the ocean robot.

Description

Technical Field

[0001] The invention belongs to the technical field of marine robots. Background Art

[0002] Marine robots, as equipment for monitoring the marine environment, safeguarding marine rights and interests, and performing combat missions, have broad application prospects. At present, marine robots have great advantages in reconnaissance, monitoring, electronic warfare, and counter-reconnaissance operations in specific sea areas; at the same time, marine robots can perform tasks such as resource detection, search and rescue, meteorological monitoring and forecasting, and communication relay stations in the civilian field. Therefore, in-depth research on marine robot technology is of great significance to enhancing my country's national defense strength and broadening civilian use scenarios.

[0003] Path planning is one of the core technologies in marine robot research. A good navigation route helps marine robots complete their tasks efficiently and with high quality. Marine robots will encounter unknown obstacles and other ships that are not marked on the map during actual navigation, so it is necessary to explore the danger avoidance methods of 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 a constrained marine environment. Through simulation evaluation in various environments including static obstacles, moving obstacles and sea surface currents of varying strengths, the goal of generating safer waypoints and maintaining a safe distance from obstacles was verified. However, the algorithm does not consider the impact of ocean currents on marine robot collision avoidance from a kinematic perspective.

[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 AUV path planning problem in a complex obstacle-dense underwater environment considering the influence of ocean currents. This algorithm can not only accelerate convergence, but also avoid falling into local optimality. However, this algorithm only considers the influence 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 of the existing path planning methods do not consider the impact of strong gusty ocean currents on the collision avoidance safety of marine robots. In actual engineering applications, the scale of marine robots is relatively small and they are easily affected by gusty ocean currents during the collision avoidance process. Summary of the invention

[0007] The present invention aims to solve the problem that the existing marine robot path planning methods do not consider the influence of ocean currents on the marine robot's collision avoidance, resulting in poor safety during navigation. A marine robot obstacle avoidance trajectory planning method and system that considers flow-induced position deviation is provided.

[0008] The method for obstacle avoidance trajectory planning of a marine robot taking into account flow-induced position deviations of the present invention comprises:

[0009] Step 1: Obtain the current position, posture, ground speed, water speed and marine environment information of the marine robot in the navigation area;

[0010] Step 2: According to the motion characteristics of the marine robot and the marine environment information, the marine robot's navigation speed limit set is obtained, and the action set of emergency obstacle avoidance under the conditions of the marine robot's current position, posture, and water speed is calculated using the obstacle evaluation function; the intersection of the speed limit set and the emergency obstacle avoidance action set is obtained to obtain the final obstacle avoidance action set;

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

[0012] Step 4: Based on the speed of the ocean robot against the water and the speed against the ground in step 1, calculate the speed of the ocean current against the ground and the drift angle β caused by the flow;

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

[0014] Furthermore, in the present invention, in step 1, obtaining the ocean environment information of the navigation area where the marine robot is located includes: static and dynamic obstacle information around the marine robot and speed information of the ocean current.

[0015] Furthermore, in the present invention, in step 2, the method for obtaining the set of extreme motion speed ranges of the marine robot is:

[0016] First, calculate the speed range set V of the marine robot determined by the maximum and minimum allowable linear speed and angular speed of the marine robot S :

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

[0018] In the formula, v wmin ,v wmax Indicates the maximum and minimum permissible linear speed of the marine robot; ω min ,ω max represents the maximum and minimum permissible angular velocity of the marine robot; v w represents the speed of the marine robot through water, ω represents the angular velocity of the marine robot;

[0019] Then calculate the set V of actions (v,ω) that enables the marine robot to avoid collision with obstacles by emergency stopping a :

[0020]

[0021] In the formula, dist(v w ,ω) is the obstacle distance evaluation function, which is used to evaluate the distance between the current trajectory and the nearest obstacle; v w represents the waterline velocity of the marine robot, ω represents the angular velocity of the marine robot; It represents the derivative of the marine robot's waterline velocity, that is, the marine robot's waterline acceleration; represents the derivative of the angular velocity of the marine robot, that is, the angular acceleration of the marine robot;

[0022] The set V of actions (v, ω) that enables the marine robot to avoid collision with obstacles by emergency stop a and the marine robot speed range set V S Find the intersection V S ∩V A ,Get the final obstacle avoidance motion speed range set.

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

[0024]

[0025] V drepresents the range of motion speeds that the marine robot can achieve within a decision cycle in the current state, ω a Indicates the angular velocity of the current state, v wa Indicates the linear speed of the current state, Indicates the maximum acceleration of the current state, Indicates the current maximum angular acceleration, T DWA Indicates the next decision cycle.

[0026] Furthermore, in the present invention, in step 3, the final action space window is:

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

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

[0029] Furthermore, in the present invention, in step 4, the process of calculating the flow-induced drift angle β is:

[0030] By formula:

[0031] v cux =v cx -v x

[0032] v cuy =v cy -v y

[0033] Calculate the velocity v of the ocean current over the ground in the X-axis and Y-axis directions cux and v cuy , where v x and v y represents the speed of the marine robot over the ground in the X-axis and Y-axis directions, v cx and v cy Indicates the speed of the marine robot in the water in the X-axis and Y-axis directions;

[0034] Reuse the formula:

[0035]

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

[0037] Furthermore, in the present invention, in step 5, the method for calculating the candidate trajectory cluster in the current state is:

[0038] Establish the kinematic model of the marine robot:

[0039]

[0040] In the formula, x(t), 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 transverse linear velocity of the marine robot perpendicular to the forward direction; ψ(t) is the heading angle of the marine robot, is the heading angle derivative of the marine robot, that is, the angular velocity r(t); represents the speed of the marine robot in the x-axis direction, Indicates the speed of the marine robot in the y-axis direction;

[0041] The motion speed sample is added into the kinematic model of the marine robot to obtain the state conversion equation of the marine robot, and the candidate trajectory cluster under the previous state is obtained by using the conversion equation:

[0042]

[0043] Where C(t) = θ(t) + β, v w (t) = v e (t)+v cu , where t is the current time, Δt is the time interval of the time window, θ(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) is the velocity of the marine robot relative to the waterline at time t, v e (t) is the speed of the marine robot relative to the ground at time t, v cu is the velocity of the ocean current against the ground, 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 speed of the marine robot relative to the ground in the X-axis and Y-axis directions at time t.

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

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

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

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

[0048] The information acquisition module is used to obtain the current position, posture, speed relative to the ground, speed relative to the sea current and the marine environment information of the navigation area of ​​the marine robot;

[0049] The final obstacle avoidance action set generation module is used to obtain the marine robot's navigation speed limit set, and use the obstacle evaluation function to calculate the marine robot's current position and the marine robot's current position, posture, and emergency obstacle avoidance action set under the water speed condition; 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 speed set that the marine robot can reach in the previous decision cycle under the current motion state, and find the intersection of the final obstacle avoidance action set and the speed set that the marine robot can reach in the next decision cycle to obtain the final action space window;

[0051] The optimal trajectory generation module solves the current velocity over the ground and the flow-induced drift angle β based on the marine robot's water speed and ground speed; samples the movement speed in the action space window, and uses the movement speed samples and the current velocity over the ground and the flow-induced drift angle β to establish the marine robot state transition equation, calculate the candidate trajectory cluster under the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal obstacle avoidance trajectory.

[0052] In actual engineering applications of the present invention, marine robots face more complex and dangerous collision avoidance scenarios. The scale of marine robots is relatively small, and they are easily affected by gusty currents during the collision avoidance process. The present invention solves the flow-induced position deviation and flow-induced drift angle under the influence of gusty currents, optimizes the DWA collision avoidance algorithm, solves the problem of marine robots being deflected toward obstacles during the collision avoidance process due to the influence of strong gusty currents, selects safer and more effective danger avoidance paths, ensures that the marine robots complete the obstacle avoidance process safely and at high speed, and improves the navigation safety and danger avoidance efficiency of the marine robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 This is a flow chart of the method for agile obstacle avoidance trajectory planning of a marine robot considering flow-induced position deviation according to the present invention;

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

[0055] Figure 3This is the relationship diagram between the ocean robot’s speed relative to water and speed relative to the ground;

[0056] Figure 4 is a schematic diagram of the flow-induced drift angle of the marine robot under the influence of the ocean current;

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

[0058] Figure 6 is the candidate trajectory cluster graph of the marine robot;

[0059] Figure 7 It is a schematic diagram of the azimuth evaluation of the marine robot;

[0060] Figure 8 It is a system principle diagram of the present invention. DETAILED DESCRIPTION

[0061] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0062] Specific implementation method 1: Referring to FIG. Figure 1 Specifically describing this embodiment, the marine robot obstacle avoidance trajectory planning method considering flow-induced position deviation described in this embodiment includes:

[0063] Step 1: Obtain the current position, posture, ground speed, water speed and marine environment information of the marine robot in the navigation area;

[0064] Step 2: According to the motion characteristics of the marine robot and the marine environment information, the marine robot's navigation speed limit set is obtained, and the action set of emergency obstacle avoidance under the conditions of the marine robot's current position, posture, and water speed is calculated using the obstacle evaluation function; the intersection of the speed limit set and the emergency obstacle avoidance action set is obtained to obtain the final obstacle avoidance action set;

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

[0066] Step 4: Based on the speed of the ocean robot against the water and the speed against the ground in step 1, calculate the speed of the ocean current against the ground and the drift angle β caused by the flow;

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

[0068] Furthermore, in the present invention, in step 1, obtaining the ocean environment information of the navigation area where the marine robot is located includes: static and dynamic obstacle information around the marine robot and speed information of the ocean current.

[0069] Furthermore, in the present invention, in step 2, the method for obtaining the set of extreme motion speed ranges of the marine robot is:

[0070] First, calculate the speed range set V of the marine robot determined by the maximum and minimum allowable linear speed and angular speed of the marine robot S :

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

[0072] In the formula, v wmin ,v wmax Indicates the maximum and minimum permissible linear speed of the marine robot; ω min ,ω max represents the maximum and minimum permissible angular velocity of the marine robot; v w represents the speed of the marine robot against water, ω represents the angular velocity of the marine robot;

[0073] Then calculate the set V of actions (v,ω) that enables the marine robot to avoid collision with obstacles by emergency stopping a :

[0074]

[0075] In the formula, dist(v w ,ω) is the obstacle distance evaluation function, which is used to evaluate the distance between the current trajectory and the nearest obstacle; v w represents the waterline velocity of the marine robot, ω represents the angular velocity of the marine robot; It represents the derivative of the marine robot's waterline velocity, that is, the marine robot's waterline acceleration; represents the derivative of the angular velocity of the marine robot, that is, the angular acceleration of the marine robot;

[0076] The set V of actions (v, ω) that enables the marine robot to avoid collision with obstacles by emergency stop a and the marine robot speed range set V S Find the intersection V S ∩V A ,Get the final obstacle avoidance motion speed range set.

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

[0078]

[0079] V d represents the range of motion speeds that the marine robot can achieve within a decision cycle in the current state, ω a Indicates the angular velocity of the current state, v wa Indicates the linear speed of the current state, Indicates the maximum acceleration of the current state, Indicates the current maximum angular acceleration, T DWA Indicates the next decision cycle.

[0080] Furthermore, in the present invention, in step 3, the final action space window is:

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

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

[0083] Furthermore, in the present invention, in step 4, the process of calculating the flow-induced drift angle β is:

[0084] By formula:

[0085] v cux =v cx -v x

[0086] v cuy =v cy -v y

[0087] Calculate the velocity v of the ocean current over the ground in the X-axis and Y-axis directions cux and v cuy , where v x and v y represents the speed of the marine robot over the ground in the X-axis and Y-axis directions, vcx and v cy Indicates the speed of the marine robot in the water in the X-axis and Y-axis directions;

[0088] Reuse the formula:

[0089]

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

[0091] Furthermore, in the present invention, in step 5, the method for calculating the candidate trajectory cluster in the current state is:

[0092] Establish the kinematic model of the marine robot:

[0093]

[0094] In the formula, x(t), 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 transverse linear velocity of the marine robot perpendicular to the forward direction; ψ(t) is the heading angle of the marine robot, is the heading angle derivative of the marine robot, that is, the angular velocity r(t); represents the speed of the marine robot in the x-axis direction, Indicates the speed of the marine robot in the y-axis direction;

[0095] The motion speed sample is added into the kinematic model of the marine robot to obtain the state conversion equation of the marine robot, and the candidate trajectory cluster under the previous state is obtained by using the conversion equation:

[0096]

[0097] Where C(t) = θ(t) + β, v w (t) = v e (t)+v cu , where t is the current time, Δt is the time interval of the time window, θ(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) is the velocity of the marine robot relative to the waterline at time t, v e (t) is the speed of the marine robot relative to the ground at time t, v cu is the velocity of the ocean current against the ground, 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 speed of the marine robot relative to the ground in the X-axis and Y-axis directions at time t.

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

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

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

[0101] The marine robot obstacle avoidance trajectory planning system considering flow-induced position deviation comprises: 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;

[0102] The information acquisition module 1 is used to obtain the current position, posture, speed relative to the ground, speed relative to the sea current and the marine environment information of the navigation area of ​​the marine robot;

[0103] The final obstacle avoidance action set generation module 2 is used to obtain the marine robot navigation speed limit set, and use the obstacle evaluation function to calculate the marine robot's current position and the marine robot's current position, posture, and emergency obstacle avoidance action set under the water speed condition; 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 speed set that the marine robot can reach in the previous decision cycle under the current motion state, and find the intersection of the final obstacle avoidance action set and the speed set that the marine robot can reach in the next decision cycle to obtain the final action space window;

[0105] The optimal trajectory generation module 4 calculates the sea current velocity over the ground and the flow-induced drift angle β based on the marine robot's water speed and ground speed in step 1; samples the motion speed in the action space window, and uses the motion speed samples and the sea current velocity over the ground and the flow-induced drift angle β to establish the marine robot state transition equation, calculate the candidate trajectory cluster under the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal obstacle avoidance trajectory.

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

[0107] 1. Obtain the marine environment information of the marine robot's navigation area and its own position, posture, and speed information. The marine robot is equipped with a laser radar, a binocular vision system, a forward-looking sonar, a multi-beam sonar, an electromagnetic current meter, a Beidou integrated machine, and an integrated navigation system, which can obtain the marine robot's own position, posture, speed information and surrounding marine environment information in real time;

[0108] The above-mentioned ocean environment information refers to the static and dynamic obstacle information around the ocean robot, and the speed and direction information of the ocean current.

[0109] 2. Obtain the speed limit set of the marine robot and calculate the action set that enables the marine robot to avoid collision with obstacles through emergency parking;

[0110] V S It represents the speed limit set of the marine robot determined by the maximum and minimum allowable linear speed and angular speed of the marine robot. The specific mathematical formula is:

[0111] V S ={(v w ,ω)|v w ∈[v wmin ,v wmax ],ω∈[ω min ,ω max ]}

[0112] In the formula, v wmin ,v wmax Indicates the maximum and minimum permissible linear speed of the marine robot; ω min ,ω max represents the maximum and minimum permissible angular velocity of the marine robot; v w represents the speed of the marine robot through water, ω represents the angular velocity of the marine robot.

[0113] V a represents any set of actions (v, ω) that enable the marine robot to avoid collision with obstacles by emergency parking. The specific mathematical formula is:

[0114]

[0115] In the formula, dist(v w ,ω) is the obstacle distance evaluation function, which is used to evaluate the distance between the current trajectory and the nearest obstacle; v w represents the waterline velocity of the marine robot, ω represents the angular velocity of the marine robot; It represents the derivative of the marine robot's waterline velocity, that is, the marine robot's waterline acceleration; It represents the derivative of the angular velocity of the marine robot, that is, the angular acceleration of the marine robot.

[0116] 3. Obtain the set of velocities that the marine robot can reach within a decision cycle T in the current state under the maximum acceleration constraint, and take the intersection to obtain the final action space window; Figure 2 As shown;

[0117] V d Indicates the current waterline speed v wa With angular velocity ω a , at maximum acceleration With maximum angular acceleration Under the constraint, the next decision cycle T DWA The speed set that the inner ocean robot can reach. The specific mathematical formula is:

[0118]

[0119] The final dynamic action space window V r The calculation formula is:

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

[0121] 4. Based on the ocean robot's speed over water and speed over the ground, calculate the ocean current speed over the ground and the flow-induced drift angle β; the relationship between the ocean robot's speed over water and speed over the ground is shown in the figure below: Figure 3 As shown; the flow-induced drift angle β is as Figure 4 As shown;

[0122] v cux =v cx -v x

[0123] v cuy =v cy -v y

[0124] In the formula, v cux and v cuy represents the velocity of the ocean current over the ground in the X-axis and Y-axis directions, v x and v y represents the speed of the marine robot over the ground in the X-axis and Y-axis directions, v cx and v cy It indicates the induced flow velocity of the ocean current in the X-axis and Y-axis directions measured by the electromagnetic flow meter carried by the marine robot.

[0125]

[0126] Where β represents the drift angle of the marine robot caused by the influence of ocean current, v cux and vcuy represents the velocity of the ocean current over the ground in the X-axis and Y-axis directions, v x and v y Indicates the speed of the marine robot over the ground in the X-axis and Y-axis directions.

[0127] 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 to search for the current optimal trajectory through repeated iterations in the candidate trajectory cluster to determine the collision avoidance action that the marine robot should take and finally reach the target point;

[0128] In a very short time, the trajectory of the marine robot can be approximately regarded as uniform linear motion. XY is the earth coordinate system, x robot -y robot is the attached coordinate system, θ represents the heading angle of the marine robot. At this time, the kinematic model of the marine robot is as follows: Figure 5 As shown, the formula is:

[0129]

[0130] Where x(t), y(t) are the Cartesian coordinates of the marine robot, and its coordinates can be expressed as u is the longitudinal linear velocity of the marine robot in the forward direction, v is the transverse linear velocity of the marine robot perpendicular to the forward direction; ψ(t) is the heading angle of the marine robot, is the heading angle derivative of the marine robot, that is, the angular velocity r(t); the velocity vector of the marine robot in the Cartesian coordinate system can be expressed as Marine Robot x The speed in the axis direction, represents the velocity of the marine robot in the y-axis direction. To represent the dynamic constraints, the longitudinal velocity of the marine robot satisfies 0≤u(t)≤u max , the angular velocity satisfies -r max ≤r(t)≤r max .

[0131] The state of the marine robot at a certain moment is represented by p(t) and v(t). Then the two variables of the sampling action (v, ω) in the DWA algorithm are added to obtain the state. Its mathematical expression is [x(t), y(t), θ(t), v, ω] T . The state transition equation of the marine robot can be obtained:

[0132]

[0133] in, v w (t) = v e (t)+vc

[0134] Where t is the current time, Δt is the time interval of the time window, θ 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 speed of the marine robot through water, v e (t) is the speed of the marine robot over the ground, a is the linear acceleration of the marine robot, α is the angular acceleration of the marine robot. The candidate trajectory cluster of the marine robot is obtained through the state transition equation, such as Figure 6 As shown;

[0135] Schematic diagram of azimuth evaluation of marine robot Figure 7 As shown in the figure, the evaluation function formula of the DWA algorithm affected by dynamic coupling is:

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

[0137] In the formula, σ Represents the normalization coefficient; heading(v w ,ω) is the azimuth evaluation function, which is used to evaluate the angle difference between the orientation and the target when the marine robot reaches the end of the simulation trajectory at the currently set sampling speed, such as Figure 7 θ is shown in the figure. θ is the deviation between the heading of the end of the marine robot trajectory and the target heading. The smaller θ is, the smaller the deviation is. However, the evaluation function is to find the maximum value, so θ needs to be processed as a supplementary angle. α is the azimuth evaluation coefficient, α The larger the value, the closer the selected trajectory is to the target heading. w ,ω) is the obstacle distance evaluation function, which is used to evaluate the distance between the current trajectory and the nearest obstacle. β is the obstacle distance evaluation coefficient. The larger β is, the farther the target from the obstacle is selected. velocity(v w ,ω) is the speed evaluation function, which is used to evaluate the speed of the current trajectory. γ is the speed evaluation coefficient. The larger the γ, the faster the selected trajectory speed is, and the faster it can approach the target point.

[0138] Normalize the evaluation function. The specific steps are as follows:

[0139]

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

[0141] Marine robot obstacle avoidance trajectory planning system considering flow-induced position deviation, combined with Figure 8 To illustrate, 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;

[0142] The information acquisition module 1 is used to obtain the current position, posture, speed relative to the ground, speed relative to the sea current and the marine environment information of the navigation area of ​​the marine robot;

[0143] The final obstacle avoidance action set generation module 2 is used to obtain the marine robot navigation speed limit set, and use the obstacle evaluation function to calculate the marine robot's current position and the marine robot's current position, posture, and emergency obstacle avoidance action set under the ground 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;

[0144] The action space window generation module 3 is used to obtain the maximum acceleration of the marine robot, obtain the speed set that the marine robot can reach in the previous decision cycle under the current motion state, calculate the intersection of the final obstacle avoidance action set and the speed set that the marine robot can reach in the next decision cycle, and obtain the final action space window;

[0145] The optimal trajectory generation module 4 is used to solve the sea current velocity over the ground and the flow-induced drift angle β based on the ocean robot's ocean current velocity and positioning information; sample the motion speed in the action space window in the current decision cycle, use the motion speed samples and the sea current velocity over the ground and the flow-induced drift angle β to calculate the candidate trajectory cluster in the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal trajectory.

[0146] The system of the present invention continuously provides the latest optimal danger avoidance action plan based on updated marine environment information and the real-time position of the marine robot.

[0147] Marine robot environmental perception system: Obtain obstacle information around the current position through laser radar, binocular vision system, forward-looking sonar and multi-beam sonar; binocular vision system can realize day and night conversion function, using visible light camera during the day and infrared imaging camera at night. Obtain the velocity and direction of the ocean current through electromagnetic current meter.

[0148] Marine robot positioning exercises: obtain the marine robot's position information through Beidou integrated machine and integrated navigation, transmit it to the information data processing module, and provide time-varying position information to the danger avoidance module;

[0149] Marine robot control system: receives avoidance action instructions, executes control mechanisms, and navigates according to specified instructions.

[0150] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.

Claims

1. A marine robot obstacle avoidance trajectory planning method considering flow-induced position deviation, characterized in that: include: Step 1: Obtain the current position, posture, ground speed, water speed and marine environment information of the marine robot in the navigation area; Step 2: According to the motion characteristics of the marine robot and the marine environment information, the marine robot's navigation speed limit set is obtained, and the action set of emergency obstacle avoidance under the conditions of the marine robot's current position, posture, and water speed is calculated using the obstacle evaluation function; the intersection of the speed limit set and the emergency obstacle avoidance action set is obtained to obtain the final obstacle avoidance action set; Step 3: Obtain the maximum acceleration of the marine robot, and then obtain the speed set that the marine robot can reach in the previous decision cycle under the current motion state, calculate the intersection of the final obstacle avoidance action set and the speed set that the marine robot can reach in the next decision cycle, and obtain the final action space window; Step 4: Based on the speed of the ocean robot against the water and the speed against the ground in step 1, calculate the speed of the ocean current against the ground and the drift angle β caused by the flow; Step 5: Sample the motion speed in the action space window, use the motion speed samples and the current velocity over the ground and the flow-induced drift angle β to establish the state transition equation of the marine robot, calculate the candidate trajectory cluster under the current state, establish an evaluation function, evaluate and optimize the candidate trajectory cluster, and obtain the optimal obstacle avoidance trajectory.

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

3. The marine robot obstacle avoidance trajectory planning method considering flow-induced position deviation according to claim 1, characterized in that: In step 2, the method for obtaining the set of extreme motion speed ranges of the marine robot is: First, calculate the speed range set V of the marine robot determined by the maximum and minimum allowable linear speed and angular speed of the marine robot S : V S ={(v w ,ω)∣v w ∈[v wmin ,v wmax ],ω∈[ω min ,ω max ]} In the formula, v wmin ,v wmax Indicates the maximum and minimum permissible linear speed of the marine robot; ω min ,ω max represents the maximum and minimum permissible angular velocity of the marine robot; v w represents the speed of the marine robot through water, ω represents the angular velocity of the marine robot; Then calculate the set V of actions (v,ω) that enables the marine robot to avoid collision with obstacles by emergency stopping a : In the formula, dist(v w ,ω) is the obstacle distance evaluation function, which is used to evaluate the distance between the current trajectory and the nearest obstacle; v w represents the waterline velocity of the marine robot, ω represents the angular velocity of the marine robot; It represents the derivative of the marine robot's waterline velocity, that is, the marine robot's waterline acceleration; represents the derivative of the angular velocity of the marine robot, that is, the angular acceleration of the marine robot; The set V of actions (v, ω) that enables the marine robot to avoid collision with obstacles by emergency stop a and the marine robot speed range set V S Find the intersection V S ∩V A , get the final obstacle avoidance motion speed range set.

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

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

6. The marine robot obstacle avoidance trajectory planning method considering flow-induced position deviation according to claim 1, characterized in that: In step 4, the process of solving the flow-induced drift angle β is: By formula: v=vv cux cx x v cuy =v cy -v y Calculate the ground velocity v of the ocean current in the X-axis and Y-axis directions cux and v cuy , where v x and v y represents the speed of the marine robot over the ground in the X-axis and Y-axis directions, v cx and v cy Indicates the speed of the marine robot in the water in the X-axis and Y-axis directions; Reuse the formula: Calculate the drift angle β of the marine robot caused by the influence of ocean current.

7. The marine robot obstacle avoidance trajectory planning method considering flow-induced position deviation according to claim 1, characterized in that: In step 5, the method for calculating the candidate trajectory cluster in the current state is: Establish the kinematic model of the marine robot: In the formula, x(t), 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 transverse linear velocity of the marine robot perpendicular to the forward direction; ψ(t) is the heading angle of the marine robot, is the heading angle derivative of the marine robot, that is, the angular velocity r(t); represents the speed of the marine robot in the x-axis direction, Indicates the speed of the marine robot in the y-axis direction; The motion speed sample is added into the kinematic model of the marine robot to obtain the state conversion equation of the marine robot, and the candidate trajectory cluster under the previous state is obtained by using the conversion equation: Where C(t) = θ(t) + β, v w (t) = v e (t)+v cu , where t is the current time, Δt is the time interval of the time window, θ(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) is the velocity of the marine robot relative to the waterline at time t, v e (t) is the speed of the marine robot relative to the ground at time t, v cu is the velocity of the ocean current against the ground, 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 speed of the marine robot relative to the ground in the X-axis and Y-axis directions at time t.

8. The marine robot obstacle avoidance trajectory planning method 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 ,ω)) Where, σ represents the normalization coefficient; heading(v w ,ω) is the azimuth evaluation function, f is the azimuth evaluation coefficient,,dist(v w ,ω) is the obstacle distance evaluation function, g is the obstacle distance evaluation coefficient, velocity(v w ,ω) is the speed evaluation function, and γ is the speed evaluation coefficient.

9. An agile obstacle avoidance trajectory planning system for marine robots considering flow-induced position deviations, 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 obtain the current position, posture, speed relative to the ground, speed relative to the sea current and the marine environment 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, posture, and emergency obstacle avoidance action set under the water speed condition by using the obstacle evaluation function; obtain the intersection of the speed limit set and the emergency obstacle avoidance action set, and 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 speed set that the marine robot can reach in the previous decision cycle under the current motion state, and calculate the intersection of the final obstacle avoidance action set and the speed set that the marine robot can reach in the next decision cycle to obtain the final action space window; The optimal trajectory generation module (4) calculates the velocity of the ocean current over the ground and the drift angle β caused by the flow based on the speed of the ocean robot over water and the speed over the ground; samples the motion speed in the action space window, uses the motion speed samples and the velocity of the ocean current over the ground and the drift angle β caused by the flow to establish the state conversion equation of the ocean robot, calculates the candidate trajectory cluster under the current state, establishes an evaluation function, evaluates and optimizes the candidate trajectory cluster, and obtains the optimal obstacle avoidance trajectory.

Images