Obstacle avoidance control method based on intention understanding in multi-underwater-robot scene

Abstract

The invention relates to the technical field of underwater robot control, solves the technical problem that the multi-underwater robot cooperative obstacle avoidance is difficult to adapt to a complex and changeable underwater environment, and particularly relates to an intention understanding-based obstacle avoidance control method in a multi-underwater robot scene, and the method comprises the steps: obtaining local environment information; processing the point cloud information to generate a three-dimensional bounding box of the dynamic target; modeling an intention understanding model to calculate and deduce intention probability distribution of the current dynamic target including four behavior intentions; performing fusion intention and trajectory prediction to obtain a fusion intention prediction trajectory; autonomous obstacle avoidance trajectory planning of the underwater robots is carried out, and cooperation and efficient obstacle avoidance control of the multiple underwater robots in a complex dynamic environment are achieved. According to the invention, cooperative operation and autonomous obstacle avoidance among multiple underwater robots can be realized under the condition of ensuring underwater obstacle avoidance safety, and the robot adapts to a complex and changeable underwater environment, so that the autonomous operation capability of the multiple underwater robots in the complex underwater environment is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robot control, and in particular to an obstacle avoidance control method based on intention understanding in a multi-underwater robot scenario. Background Art

[0002] As the strategic position of the ocean becomes increasingly prominent, activities such as underwater resource development and marine scientific research are becoming increasingly frequent, and the demand for underwater operations is also increasing. As an important underwater operation platform, the Autonomous Underwater Vehicle (AUV) plays an increasingly important role in marine resource exploration, seabed environmental monitoring, underwater facility maintenance and other fields with its autonomy, flexibility and adaptability. When the underwater mission is complex, it is difficult for a single AUV to complete the task, and multiple AUVs need to work together. However, the complex underwater environment poses huge challenges to the autonomous operation of multiple AUVs, mainly including: difficulties in environmental perception, dynamic collaborative obstacle avoidance problems, and distributed computing resource constraints. Each AUV needs to make real-time decisions within its own limited computing resources, and may also need to exchange information and conduct collaborative computing with other AUVs. This makes it difficult for complex behavior prediction and obstacle avoidance algorithms to operate effectively in a resource-constrained environment.

[0003] Existing research on multi-AUV collaborative obstacle avoidance focuses primarily on the following areas: rule-based, potential field-based, and machine learning-based approaches. However, these approaches all have limitations. They typically require extensive training data and have limited generalization capabilities, making them difficult to adapt to complex and changing underwater environments, and even more so to the new and unknown scenarios that emerge during multi-AUV collaborative operations. Furthermore, the interactions between multiple AUVs complicate data collection and model training. Summary of the Invention

[0004] In response to the shortcomings of the existing technology, the present invention provides an obstacle avoidance control method based on intention understanding in a multi-underwater robot scenario, which solves the technical problem that the collaborative obstacle avoidance of multiple underwater robots is difficult to adapt to the complex and changeable underwater environment.

[0005] To solve the above technical problems, the present invention provides the following technical solution: an obstacle avoidance control method based on intention understanding in a multi-underwater robot scenario, the method comprising the following steps:

[0006] Acquisition of local environment information: The multi-beam sonar integrated on the underwater robot scans the local environment around it and returns 3D point cloud information;

[0007] Point cloud information processing: downsampling the 3D point cloud information and storing it in a dynamic hash table. A density-based clustering algorithm is then used to cluster the filtered 3D point cloud information to generate a 3D bounding box for the dynamic target. The 3D bounding box includes historical trajectory information such as the dynamic target's position, velocity, acceleration, and 3D bounding box size.

[0008] Intent understanding modeling: By continuously collecting the three-dimensional bounding boxes of dynamic targets and performing Markov decision process behavior modeling to obtain the intent understanding model, the state transition probability matrix is ​​dynamically updated based on the historical trajectory information of every N frames to calculate and infer the intent probability distribution of the current dynamic target, including the four behavioral intentions;

[0009] Fusion of intent and trajectory predictions. A motion model is established to predict the future trajectory of a dynamic target under each intent based on its current state. The fused intent prediction trajectory is obtained by weightedly averaging the probability of each intent and the corresponding predicted trajectory. The uncertainty region based on the variance of the intent probability is then calculated.

[0010] The autonomous obstacle avoidance trajectory planning of underwater robots first synchronizes the underwater robot trajectory with the dynamic target predicted trajectory. Then, the fused intention predicted trajectory is used as the dynamic target constraint for the trajectory optimization and obstacle avoidance algorithm. At the same time, lateral obstacle avoidance constraints are introduced to achieve collaboration and efficient obstacle avoidance control of multiple underwater robots in complex dynamic environments.

[0011] By means of the above technical solution, the present invention provides an obstacle avoidance control method based on intention understanding in a multi-underwater robot scenario, which has at least the following beneficial effects:

[0012] 1. The present invention can realize collaborative operation and autonomous obstacle avoidance among multiple underwater robots while ensuring underwater obstacle avoidance safety, and adapt to complex and changeable underwater environments, thereby improving the autonomous operation capabilities of multiple underwater robots in complex underwater environments.

[0013] 2. This paper proposes an intention understanding modeling method based on a Markov decision process. This method is used to understand the behavioral intentions of other underwater robots or dynamic obstacles and integrate them into the decision-making process of the autonomous underwater robot. By introducing a temporal smoothing factor to smooth the intention probability and incorporating historical trajectory information, it also reduces the impact of noise and improves the stability of intention recognition. Furthermore, the state transition probability matrix is ​​dynamically adjusted based on the relative relationship and relative motion state between the dynamic target and the autonomous underwater robot, enabling the model to adapt to different scenarios and interaction modes.

[0014] 3. This invention generates multiple trajectory predictions for each dynamic target's behavior patterns under different behavioral intentions. To fully reflect the uncertainty of the target's future behavioral intentions, the predicted trajectories corresponding to different behavioral intentions are weighted and fused based on the intention probabilities of each behavioral intention calculated from the state transition probability matrix.

[0015] 4. The present invention establishes the uncertainty region of the trajectory by introducing the weighted variance between the predicted trajectory of each intention and the fused predicted trajectory during the fusion process. When the trajectories between different intentions diverge significantly and the variance is large, the fusion result will automatically expand its uncertainty region, thereby improving the safety and robustness of the predicted trajectory.

[0016] 5. This invention takes into account the collision volume between the underwater robot and a dynamic object, as well as the uncertainty of the dynamic object's motion. Therefore, the obstacle avoidance constraints are strengthened to ensure that the robot and the dynamic object maintain a sufficient safe distance. Within the underwater robot's safety zone, the robot is kept in a straight line as much as possible, improving motion efficiency and reducing energy consumption.

[0017] 6. Aiming at the uncertainty of underwater dynamic object motion, the present invention designs a special lateral obstacle avoidance constraint to ensure that the underwater robot can operate safely and stably. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0019] Figure 1 This is a principle framework diagram of the autonomous obstacle avoidance control method for multiple underwater robots in the present invention;

[0020] Figure 2 This is a flow chart of processing point cloud information based on multi-beam sonar in the present invention;

[0021] Figure 3 A flowchart of the Markov decision process-based intention understanding modeling process in the present invention;

[0022] Figure 4 This is a flowchart of the underwater robot's autonomous obstacle avoidance that integrates the intention trajectory in the present invention. DETAILED DESCRIPTION

[0023] To make the above-mentioned objectives, features, and advantages of the present invention more clearly understood, the present invention is further described below in detail with reference to the accompanying drawings and specific embodiments. This will enable a full understanding of how this application uses technical means to solve technical problems and achieve technical effects, and to implement the invention accordingly.

[0024] When faced with complex underwater missions, a single autonomous underwater vehicle (AUV) cannot complete the task, requiring multiple AUVs to work together. However, the complex underwater environment poses significant challenges to the autonomous operation of multiple AUVs, including:

[0025] Difficulty in environmental perception: Underwater environments have poor lighting conditions and low visibility, which limits the application of traditional optical sensors. While underwater acoustic sensors can detect targets at long distances, the complexity of the underwater acoustic channel often results in noise and uncertainty in the information they obtain.

[0026] Dynamic collaborative obstacle avoidance challenges: A multi-AUV system must not only avoid obstacles it may encounter, but also consider the motion states of other AUVs and potential collaborative needs. Other AUVs and marine life in the dynamic underwater environment constitute a complex swarm of dynamic obstacles, each with a high degree of uncertainty in its trajectory. If individual AUVs cannot accurately predict the trajectory of their companions or external obstacles, collisions are highly likely to occur, leading not only to mission failure but also to equipment damage, seriously threatening the overall safety of the multi-AUV system.

[0027] Distributed computing resource constraints: Unlike centralized systems, multi-AUV systems typically employ a distributed computing architecture. Each AUV must make real-time decisions within its own limited computing resources and may also need to exchange information and collaborate with other AUVs. This makes complex behavior prediction and obstacle avoidance algorithms difficult to implement effectively in resource-constrained environments.

[0028] For the research on multi-AUV collaborative obstacle avoidance, existing technologies mainly focus on the following aspects, but all of them have certain limitations:

[0029] Rule-based obstacle avoidance: This approach predefines a set of rules and makes independent decisions based on environmental information acquired by each AUV's sensors. This approach is simple and easy to implement, but due to the limitations of the rules, it struggles to effectively cope with the complex, changing, and uncertain underwater dynamics, and even more so, achieve coordinated obstacle avoidance among multiple AUVs.

[0030] Potential field-based obstacle avoidance: This method treats obstacles as repulsive fields, guiding the AUV to avoid them. It can also introduce attractive fields to guide the AUV toward the target. However, potential field design in a multi-AUV collaborative environment is more complex and more prone to falling into local minima, preventing the AUV from reaching the target. It can even lead to multiple AUVs "passing the buck," severely impacting collaborative efficiency.

[0031] Machine learning-based obstacle avoidance methods: This method learns an environmental model, predicts the trajectory of obstacles, and then performs obstacle avoidance planning. Common machine learning methods include reinforcement learning and deep learning. However, these methods typically require large amounts of training data and have limited generalization capabilities, making them difficult to adapt to complex and changing underwater environments, and even more so to the new and unknown scenarios that emerge during multi-AUV collaborative operations. Furthermore, the interactions between multiple AUVs complicate data collection and model training.

[0032] This embodiment provides a new solution for the collaborative operation and autonomous obstacle avoidance of multiple AUVs underwater while ensuring safety. It also proposes an obstacle avoidance control method based on intention understanding in a multi-underwater robot scenario, thereby improving the autonomous operation capability of multiple AUVs in complex underwater environments. Figure 1 As shown, the method includes the following steps:

[0033] To acquire local environmental information, the multibeam sonar integrated into the underwater robot scans the local environment around it and returns 3D point cloud information. This embodiment integrates three multibeam sonars on the underwater robot, one located directly in front and on the left and right sides, to achieve wide-field environmental perception, thereby acquiring 3D point cloud information of the local environment around the underwater robot.

[0034] The imaging principle of the multi-beam sonar used in this embodiment is as follows: Multi-beam Echosounder (MBES) transmits and receives sound waves through an array antenna, and "scans and samples" multiple areas in front or below with multiple receiving beams. For each beam, a distance-intensity (A-plot) echo signal sequence is obtained. Each beam defines a "ray", which can be regarded as a detection direction along a specified angle. These rays interact with the surface of objects in the environment or scene (point scattering model). The emission angle direction of each beam in the sonar coordinate system is known (θ, Typically, strip beams have a fixed distribution, with each range bin corresponding to a specific point in space. For each bin with a high intensity (exceeding a threshold) on the A-plot curve, the 3D coordinates of the strong echo are inferred. In other words, the 3D coordinates (x, y, z) of each bin can be calculated based on its direction θ and distance r. High-intensity "echo points" from all beams are aggregated to form a 3D point cloud.

[0035] Point cloud processing uses a voxel filter to downsample the 3D point cloud information and then stores it in a dynamic hash table. A density-based clustering algorithm is then used to cluster the filtered 3D point cloud information to generate a 3D bounding box for the dynamic target. The 3D bounding box includes historical trajectory information such as the dynamic target's position, velocity, acceleration, and 3D bounding box size.

[0036] The specific implementation process of point cloud information processing in this embodiment includes:

[0037] Downsampling and dynamic hash storage. Divide the 3D space into cube grids (voxels) with a side length of res units. Only one point is retained in each cube grid, which is the first point to fall into the grid. For each point p(x p ,y p ,z p ), whose voxel coordinates are calculated as:

[0038]

[0039] Then use the hash table to save the voxel in the form of "x_y_z" string as the key. Each time you save a new voxel, you only need to check whether the same key value is generated. If the same key value is generated, it means that the voxel coordinates have already been recorded and there is no need to save it again.

[0040] DBSCAN clustering. The DBSCAN algorithm first defines the radius parameter ∈ and the minimum number of cluster points minpts. The radius parameter ∈ defines the range of the cluster neighborhood. The minimum number of cluster points minpts is the number of points contained in the neighborhood of a point that is considered dense. Let the many points stored in the above hash table be a set D. For each point p in this set, its neighborhood is calculated, that is:

[0041] N ∈ (p)={q∈D|dist(p,q)≤∈} (44)

[0042] Where q is any point in the neighborhood except point p; dist is the Euclidean distance between two points. If point p is a core point (i.e. |N ∈ (p)|≥minpts considers point p to be a core point), then a new cluster is created and the neighborhood N ∈ All densities in (p) can be reached (for points p, q, if q∈N ∈ (p) and point p is a core point, then point q is said to be densely accessible from point p, and if there is a sample sequence p1,…,p from point p to point q n So that p1=p, p n =q, and p n+1 From the sample sequence p n If the density is directly reachable, then point q is added to the cluster and marked as visited. Otherwise, point p is marked as noise. Then, all points that are density reachable are added to the cluster.

[0043] The capture of dynamic target motion information in this embodiment is to obtain the three-dimensional point cloud information of dynamic targets in the marine environment through the multi-beam sonar integrated by the autonomous underwater robot, and use the voxel filter to downsample the collected three-dimensional point cloud information to reduce the amount of data. Due to underwater noise interference and the uncertainty of the motion of dynamic objects, a dynamic hash table is used to manage voxels. The dynamic hash table does not need to pre-set the spatial range. It can dynamically create and manage voxels according to the distribution of actual point cloud data. Then, DBSCAN is used to cluster the filtered point cloud to obtain multiple clusters, and each cluster represents an underwater dynamic target. In the present invention, each cluster is represented by a 3D bounding box, and the historical trajectory data of the dynamic target three-dimensional bounding box is continuously extracted for use by subsequent algorithms. The specific workflow is as follows. Figure 2 shown.

[0044] Intent understanding model is built by continuously collecting the three-dimensional bounding box of the dynamic target and performing Markov decision process behavior modeling to obtain the intention understanding model. The state transition probability matrix is ​​dynamically updated based on the historical trajectory information of every ten frames to calculate and infer the intention probability distribution of the current dynamic target, including four behavioral intentions.

[0045] The specific implementation process of the intent understanding model in this embodiment includes:

[0046] Define the state space, action space, and state transition probability matrix of the Markov decision process (MDP). In this invention, the state space is defined as four state parameters: the motion change trend θ of the dynamic target; the speed r, which reflects the speed of the dynamic target; the relative motion angle difference Robotdifangel between the robot and the dynamic target; and the distance RobotDist between the dynamic target and the robot. Therefore, the state space can be defined as: S t =[θ t ,r t ,Robotdifangel t ,RobotDist t ].

[0047] The action space is defined by four behavioral intentions: APPROACHING, where a dynamic target approaches the underwater robot; AVOIDING, where a dynamic target moves away from the underwater robot; COOPERATIVE, where a dynamic target and the underwater robot move in the same direction; and STOPPING, where a dynamic target stops within the underwater robot's detection range within a certain timeframe or moves at a speed below a set threshold. Therefore, the action space is defined as: A = APPROACHING, AVOIDING, COOPERATIVE, STOPPING.

[0048] In the present invention, the state transition probability P(s t+1 |s t ,a t ) indicates that in the current state s t Next take action a t Then, transfer to the next state s t+1 The probability of state transition is calculated as follows:

[0049]

[0050] Among them, the formulas of P (APPROACHING), P (AVOIDING) and P (COOPERATIVE) are mainly determined by the Gaussian distribution formula and Robotdifangel. It is stipulated that when Robotdifangel is 0, the underwater robot and the dynamic target keep moving in the same direction. At this time, the probability of COOPERATIVE intention is the highest. Due to the water flow interference and sensor error in the underwater environment, an interference coefficient r needs to be subtracted when calculating COOPERATIVE. distub . The probability of APPROACHING is defined as the maximum when Robotdifangel is closer to -90 degrees (-1.57 in radians), and the same applies to AVOIDING. STOPPING is defined as the maximum probability when the speed r approaches 0. scale is the weight adjustment factor of the corresponding intention, which changes continuously with the probability matrix. scale is the weight adjustment factor of the corresponding intention, which changes continuously with the probability matrix; approach_sigma, avoiding_sigma, cooper_sigma and sigma represent the speed of the probability change trend as Robotdifangell increases or decreases. The above probability calculation is normalized, that is:

[0051]

[0052] Among them, P(A i ) is the i-th behavioral intention A i The state transition probability.

[0053] Calculate the corresponding intention probability. For each dynamic target within the detection range of the underwater robot sensor, initialize an intention probability vector P, and define the state transition probabilities of all behavioral intentions under the initial conditions to be equal. For the historical trajectory information of each dynamic target, starting from the third-to-last frame and ending at the second frame, the method of cutting off the head and tail can ensure that the probability calculation will not be affected by the inaccurate collected trajectory data and the error caused by the unstable motion state. Extract information such as position, speed and size, calculate the speed r, the relative motion angle Robotdifangel and the relative distance RobotDist. According to formulas (5)-(8), calculate the intention probability vector P of each behavioral intention in each frame and put them into the state transition probability matrix in turn. Use the state transition probability matrix to update the intention probability vector P. Here, the historical state transition probability matrix is ​​quoted to intervene in the calculation of the current frame intention probability vector P, so that it takes into account the influence of historical trajectory and historical intention, while avoiding drastic changes in probability. The formula is as follows:

[0054] P=alph a*newP+(1-alph a)*prevP (10)

[0055] alpha is the time scaling factor, which determines the degree of consideration of historical trajectory information. A larger value indicates greater trust in the intent probability vector P calculated at the current moment, while a smaller value indicates greater trust in the intent probability vector P calculated at previous moments. newP is the intent probability vector calculated at the current moment, and prevP is the intent probability vector calculated iteratively several frames ago.

[0056] This embodiment proposes an intention understanding modeling method based on Markov decision process to understand the behavioral intentions of other underwater robots or dynamic obstacle targets and integrate them into the decision-making process of autonomous underwater robots. The specific workflow is as follows: Figure 3 The key step of this method is intention probability reasoning. The details are as follows.

[0057] Intention probability reasoning: This embodiment uses the Markov decision process (MDP) to model the behavioral intention of underwater dynamic targets. The state space includes the position, velocity, acceleration, and relative distance and angle of the dynamic target to the autonomous underwater robot. The action space corresponds to the possible behavioral intentions of the dynamic target, including approaching (APPROACHING), avoiding (AVOIDING), cooperating (COOPERATIVE) and stopping (STOPPING). The state transition probability is described by the intention transfer matrix, which is dynamically adjusted according to the relative relationship and motion state between the target and the autonomous underwater robot. The intention probability vector of each frame of the dynamic target motion is obtained by continuous iterative calculations in the state space, and is sequentially placed into the state transition probability matrix and the intention probability vector is updated to obtain the maximum behavior intention probability, thereby inferring the target's motion intention.

[0058] To improve the accuracy and robustness of intent inference, the present invention adopts the following strategies:

[0059] Smoothing: A time smoothing factor is introduced to smooth the intent probability, adding the intervention of historical trajectory information, while also reducing the impact of noise and improving the stability of intent recognition.

[0060] Dynamic adjustment of the state transition probability matrix: The state transition probability matrix in the present invention is dynamically adjusted according to the relative relationship and relative motion state between the dynamic target and the autonomous underwater robot, so that the model can adapt to different scenarios and interaction modes.

[0061] Intention and trajectory prediction are integrated to establish a motion model that predicts the future trajectory of the dynamic target under each intention based on the current state. The fused intention prediction trajectory is obtained by weighted averaging the probability of each intention and the corresponding predicted trajectory, and the uncertainty region based on the variance of the intention probability is calculated.

[0062] The specific implementation process of this embodiment for integrating intention and trajectory prediction includes:

[0063] The motion models of four intentions are established. For the cooperative intention model, it is assumed that the dynamic target and the robot move in parallel, that is, the movement direction of the dynamic target and the robot is consistent, but the speed fluctuates within a certain range. The speed range is set to v∈[0.8v0,1.2v0], where v0 is the current speed of the underwater robot, and the angle range is set to θ∈[θ robot -frontAngel,θ robot +frontAngel],θ robot is the current motion direction of the underwater robot, frontAngel is the allowed relative motion direction deviation, a uniform linear motion model is established, and propagation is made in the form of uniform linear motion in the motion direction. For the away intention model, the angle range is set to θ∈[θ robot -π,θ robot ], that is, the fan-shaped area away from the direction of the underwater robot, the speed range is v∈[0,min(1.2v0,v max )],v max is the maximum forward speed of the underwater robot. For the approach intention model, the angle range is set to θ∈[θ robot ,θ robot +π], that is, the fan-shaped area facing the direction of the underwater robot, the speed range is v∈[0,min(1.2v0,v max )]. For the stopping intention model, the position of the dynamic target in all prediction moments is set to be equal to the current position of the dynamic target.

[0064] To ensure that high intent probabilities have a greater impact on the final trajectory, the intent probability vector is weighted and fused into the trajectory prediction. First, each intent is traversed to obtain its probability weight, ignoring intents with extremely low probabilities (less than 5%). For each time step, the position and size are accumulated weighted by the intent probability. The position of the fused predicted trajectory is the weighted average of the positions of the trajectories for each intent:

[0065]

[0066] Among them, P i (t) is the predicted position of intention i at time t, ω i is the probability of the intention. By continuously accumulating, we can eventually get a trajectory that integrates the probability of the intention.

[0067] Consider trajectory uncertainty. Calculate the weighted variance of the trajectories for different intentions and the fused predicted trajectory. A larger weighted variance indicates greater prediction differences between different intentions and higher uncertainty. Increase the size of the prediction region based on the standard deviation and multiply by the variance coefficient to obtain a confidence interval of approximately 95%. Thus, when the differences between different predicted intentions are large, the prediction region will automatically expand, reflecting higher trajectory uncertainty. Uncertainty is calculated as follows:

[0068]

[0069] Expand the prediction region based on variance:

[0070]

[0071] Among them, Size fused (t) is the expanded prediction area; Var(t) is the weighted variance of different intention trajectories and fusion trajectories; Size avg (t) is the original prediction area size; Z score It is usually taken as 1.96, corresponding to a 95% confidence level.

[0072] This embodiment generates multiple corresponding trajectory prediction results for the behavior patterns of dynamic targets under different behavior intentions (such as approach, cooperate, stop, and move away). In order to fully reflect the uncertainty of the target's future behavior intentions, the predicted trajectories corresponding to different behavior intentions are weightedly fused based on the intention probability of each behavior intention calculated based on the above-mentioned state transition probability matrix. Specifically, the trajectory prediction sequence under each behavior intention is first obtained, and then these trajectories are weighted averaged according to the intention probability of each behavior intention to achieve effective integration of the probability trajectories of each behavior intention, and obtain the final fused prediction trajectory reflecting all possible behaviors. In addition, the present invention also introduces the weighted variance between the predicted trajectory of each intention and the fused predicted trajectory in the fusion process, and establishes the uncertainty region of the trajectory. When the trajectories between different intentions diverge significantly and the variance is large, the fusion result will automatically expand its uncertainty region to improve the safety and robustness of the predicted trajectory.

[0073] The autonomous obstacle avoidance trajectory planning of underwater robots first synchronizes the underwater robot trajectory with the dynamic target predicted trajectory, and uses the fusion intention predicted trajectory as the dynamic target constraint of the trajectory optimization and obstacle avoidance algorithm. At the same time, lateral obstacle avoidance constraints are introduced to achieve collaboration and efficient obstacle avoidance control of multiple underwater robots in complex dynamic environments.

[0074] The specific implementation process of autonomous obstacle avoidance trajectory planning for the underwater robot in this embodiment includes:

[0075] Obtain the predicted trajectory of the fusion intent. Record the underwater robot's current position, velocity, angular velocity, and attitude, and set the target point. Each time a new target point is received, clear the old trajectory and replan it.

[0076] Trajectory obstacle avoidance constraint function design. According to the uncertainty of underwater dynamic target behavior intention, enhanced obstacle avoidance constraint, lateral obstacle avoidance constraint and linear regression constraint are designed. For obstacle avoidance constraint, when a static target is detected, for static target obstacle avoidance, each static target position O j =(O jx ,O jy ,O jz ), the distance d between the trajectory point and the obstacle is:

[0077]

[0078] Considering the radius r of the underwater robot robot and safety margin (distance) r margin , effective distance d eff for:

[0079] d eff =dr robot -r margin (15)

[0080] The repulsive force F of the obstacle avoidance algorithm in TEB path planning obs Set to:

[0081]

[0082] Among them, w obs is the obstacle avoidance weight; d min The minimum safe distance.

[0083] For dynamic target obstacle avoidance, considering the predicted position P pred and size S pred , set the obstacle radius r obs =max(s pred,x ,s pred,y ,s pred,z ) / 2, effective distance d eff =dr robot -r obs , the repulsive force F obs The settings are the same as above, but can be adjusted dynamically according to actual conditions.

[0084] For lateral obstacle avoidance constraints, calculate the current robot position (x i ,y i ,z i )Towards the goal x ,goal y ,goal z )’s target direction unit vector u goal ,Right now:

[0085]

[0086] Then the lateral vector u lat Defined as (in the horizontal plane, 90 degrees rotation):

[0087] u lat =(-u goal,y ,u goal,x ,0) (18)

[0088] Among them, u goal,y Represents the component of the target direction unit vector on the y-axis; u goal,x Similarly, its component on the x-axis.

[0089] For a dynamic target point O approaching from the side, x ,O y ,O z ) relative to the trajectory point x i =(x i ,y i ,zi ) is the displacement vector d obs =(x i -O x ,y i -O y ,z i -O z ), its projection distance d lateral Defined as:

[0090] d lateral =d obs *u lat (19)

[0091] This value represents how much the dynamic target tends to approach laterally. A projection value of 0 means the dynamic target is directly in front of or behind the trajectory point; a projection value greater than 0 means the dynamic target is on the left side of the trajectory point; a projection value less than 0 means the dynamic target is on the right side. A lateral obstacle avoidance threshold is also set. If the projection distance is greater than the obstacle avoidance threshold, it means that the dynamic target is far away from the underwater robot and the impact of its movement does not need to be considered. If it is less than the threshold, a lateral constraint force F is set. lat Size:

[0092]

[0093] Among them, w lat is the lateral obstacle avoidance weight; d is the actual distance from the dynamic target to the underwater robot; ∈ is a small constant to prevent the denominator from being 0. The direction of the lateral constraint force for:

[0094]

[0095] Among them, avoiding_direction=+1 means that the dynamic target is on the right and avoids obstacles to the left; avoiding_direction=-1 means that the dynamic target is on the left and avoids obstacles to the right.

[0096] For the linear regression constraint, calculate each trajectory point x i and the ideal current position to the target point x i,straight The deviation Δx from the straight line i ,Right now:

[0097] Δx i =x i -x i,straight (twenty two)

[0098] Set Threat Level I th reat for:

[0099]

[0100] Among them, d min is the distance from the trajectory point to the nearest dynamic target; d safe For a safe distance.

[0101] Set adaptive regression weight α straight for:

[0102]

[0103] Linear regression constraint F straight for:

[0104] F straight =-α straight Δx i (25)

[0105] Compare the current trajectory and predicted trajectory at the same moment to perform trajectory avoidance. The underwater robot's current TEB trajectory point and the fused predicted trajectory point within the same time step are extracted and compared to see if their distance satisfies the currently set trajectory obstacle avoidance constraint function. If so, the planned trajectory is feasible. If not, the trajectory is replanned until the constraint is satisfied, achieving autonomous obstacle avoidance.

[0106] Before adding the above-mentioned trajectory prediction based on fusion intention to the robot path planning algorithm, the present invention particularly emphasizes the importance of time synchronization. Specifically, it is necessary to ensure that the predicted trajectory of the dynamic object and the planned path of the underwater robot are strictly aligned on the time axis before making an obstacle avoidance decision. The present invention eliminates the time deviation between the two through the time synchronization mechanism, thereby ensuring that the future state of the dynamic object is accurately evaluated at the same time. Therefore, considering that the TEB path planning algorithm can avoid dynamic obstacles in real time by adjusting the time distribution and shape of the trajectory, and the algorithm can generate a smooth trajectory by optimizing the curvature and time distribution of the trajectory, thereby reducing the energy consumption of the underwater robot, the present invention uses TEB as the basic robot path planning algorithm, and adds the dynamic object trajectory prediction with fusion intention as one of the constraints in the algorithm to realize the collaborative obstacle avoidance of the underwater robot and realize the workflow such as Figure 4 In addition, in order to enhance the safety of underwater robot obstacle avoidance, the present invention also introduces the following constraints:

[0107] Enhanced obstacle avoidance constraints: Considering the collision volume between the underwater robot and the dynamic object and the uncertainty of the dynamic object's motion, the strength of the obstacle avoidance constraints is increased to ensure that the robot and the dynamic object maintain a sufficient safe distance.

[0108] Adaptive linear regression constraint: within the underwater robot's safety zone, the robot is kept in linear motion as much as possible, improving motion efficiency and reducing energy consumption.

[0109] Lateral obstacle avoidance constraints: In response to the uncertainty of underwater dynamic object motion, special lateral obstacle avoidance constraints are designed to ensure that underwater robots can operate safely and stably.

[0110] In order to overcome the above-mentioned obstacle avoidance problem of multiple underwater robots, the present invention first integrates three multi-beam sonars on the underwater robot. The sonars are respectively located in front and on the left and right sides to achieve wide-field environmental perception, obtain three-dimensional point cloud information, and combine the density-based spatial clustering algorithm (Density-Based Spatial Clustering of Applications with Noise, DBSCAN) to process the point cloud to obtain a 3D bounding box of the dynamic target. Then, by continuously collecting the historical trajectory data (position, speed, acceleration, size, etc.) of the 3D bounding box of the dynamic target, Markov decision process behavior modeling is performed to generate an intention understanding model. Finally, the modeled intention is weighted and fused into the trajectory prediction, and the trajectory prediction result is added to the underwater robot trajectory planning constraint, thereby improving the autonomous operation capability of multiple underwater robots in complex underwater environments.

[0111] Those skilled in the art will appreciate that all or part of the steps in the above-mentioned embodiment methods can be accomplished by instructing the relevant hardware through a program. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0112] Each embodiment in this specification is described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the same or similar parts between the embodiments. For the above embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For relevant parts, refer to the partial description of the method embodiments.

[0113] The above embodiments provide a detailed introduction to the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core ideas. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the contents of this specification should not be understood as limiting the present invention.

Claims

1. An obstacle avoidance control method based on intention understanding in a multi-underwater robot scenario, characterized in that: The method comprises the following steps: Acquisition of local environment information: The multi-beam sonar integrated on the underwater robot scans the local environment around it and returns 3D point cloud information; Point cloud information processing: downsampling the 3D point cloud information and storing it in a dynamic hash table. A density-based clustering algorithm is then used to cluster the filtered 3D point cloud information to generate a 3D bounding box for the dynamic target. The 3D bounding box includes historical trajectory information such as the dynamic target's position, velocity, acceleration, and 3D bounding box size. Intent understanding modeling: By continuously collecting the three-dimensional bounding boxes of dynamic targets and performing Markov decision process behavior modeling to obtain the intent understanding model, the state transition probability matrix is ​​dynamically updated based on the historical trajectory information of every N frames to calculate and infer the intent probability distribution of the current dynamic target, including the four behavioral intentions; Fusion of intent and trajectory predictions. A motion model is established to predict the future trajectory of a dynamic target under each intent based on its current state. The fused intent prediction trajectory is obtained by weightedly averaging the probability of each intent and the corresponding predicted trajectory. The uncertainty region based on the variance of the intent probability is then calculated. The autonomous obstacle avoidance trajectory planning of underwater robots first synchronizes the underwater robot trajectory with the dynamic target predicted trajectory. Then, the fused intention predicted trajectory is used as the dynamic target constraint for the trajectory optimization and obstacle avoidance algorithm. At the same time, lateral obstacle avoidance constraints are introduced to achieve collaboration and efficient obstacle avoidance control of multiple underwater robots in complex dynamic environments.

2. The obstacle avoidance control method according to claim 1, characterized in that: The process of point cloud information processing includes: Downsampling and dynamic hash storage: Divide the 3D space into a cubic grid with a side length of res units. Only the first point that falls into each cubic grid is retained. For each point p(x p ,y p ,z p ) is calculated as: Then use the hash table to save it in the form of "x_y_z" string as the key; DBSCAN clustering: Set the many points stored in the above hash table as a set D, and calculate the neighborhood of each point p in the set, that is: N ∈ (p)={q∈D|dist(p,q)≤∈} Where q is any point in the neighborhood except point p; dist is the Euclidean distance between two points; If point p is a core point, create a new cluster and group the neighborhood N ∈ All density-reachable points within (p) are added to the cluster and marked as visited, otherwise point p is marked as noise; By analogy, all density-reachable points are added to the current cluster to obtain the three-dimensional bounding box of any dynamic target.

3. The obstacle avoidance control method according to claim 1, characterized in that: The process of building the intent understanding model includes: Define the state space, action space, and state transition probability matrix of the Markov decision process, namely: The state space includes the position, velocity, acceleration, and relative distance and angle of the dynamic target to the autonomous underwater vehicle. The state space is defined as: S t =[θ t ,r t ,RobotDefense t ,RobotDist t ] Among them, θ is the motion change trend of the dynamic target; r is the speed, which reflects the speed of the dynamic target; Robotdifangel is the relative motion angle difference between the robot and the dynamic target; RobotDist is the distance between the dynamic target and the robot; The action space corresponds to the possible behavioral intentions of the dynamic target, including approaching, moving away, cooperating, and stopping. The action space is defined as: a=[APPROACHING,AVOIDING,COOPERATIVE,STOPPING] Among them, APPROACHING is defined as the dynamic target gradually approaching the underwater robot; AVOIDING is defined as the dynamic target gradually moving away from the underwater robot; COOPERATIVE is defined as the dynamic target and the underwater robot moving in the same direction; STOPPING is defined as the dynamic target stopping within the detection range of the underwater robot within a certain time range or the movement speed is less than the set speed threshold; The state transition probability matrix is ​​composed of the state transition probability P(s t+1 |s t ,a t ) is a matrix composed of, which means that in the current state s t Next take action a t Then, transfer to the next state s t+1 probability; The state transition probability is described by the intention transfer matrix and is dynamically adjusted according to the relative relationship and motion state between the dynamic target and the autonomous underwater robot; Calculate the corresponding intention probability: Continuously iterate the state space to obtain the intention probability vector of the dynamic target motion in each frame, put it into the state transition probability matrix in turn and update the intention probability vector to obtain the maximum behavior intention probability, and then infer the target's behavior intention.

4. The obstacle avoidance control method according to claim 3, characterized in that: The calculation formula for state transition probability is as follows: The probability of APPROACHING is defined as the maximum when the Robotdifangel is closer to -90 degrees, and the same applies to AVOIDING. The probability of COOPERATIVE is defined as the maximum probability when the underwater robot and the dynamic target keep moving in the same direction when the Robotdifangel is 0. The probability of STOPPING is defined as the maximum probability when the speed r approaches 0. Among them, scale is the weight adjustment factor of the corresponding intention, which changes continuously with the state transition probability matrix; approach_sigma, avoiding_sigma, cooper_sigma and sigma represent the speed of the state transition probability change trend as Robotdifangel increases or decreases.

5. The obstacle avoidance control method according to claim 3, characterized in that: The calculation of the corresponding intention probability includes: Initialize the intention probability vector P of each dynamic target within the detection range of the underwater robot sensor, and define the state transition probability of all behavioral intentions to be equal under the initial conditions; For each dynamic target's historical trajectory information, starting from the third-to-last frame to the end of the second frame, extract the dynamic target's position, speed, and size information, and calculate the dynamic target's speed r, relative motion angle Robotdifangel, and relative distance RobotDist; According to the calculation formula of state transition probability, the intention probability vector P of each behavior intention in each frame is calculated, and the intention probability vector P is updated in sequence by adding it to the state transition probability matrix to obtain the maximum behavior intention probability, and then the behavior intention of the dynamic target is inferred. The calculation formula of the intention probability vector P of the current frame is: P=alph a*newP+(1-alph a)*prevP Among them, alpha is the time scaling factor, which determines the degree of consideration of historical trajectory information. If it is larger, it means more trust in the intention probability vector P calculated at the current moment; if it is smaller, it means more trust in the intention probability vector P calculated at the historical moment; newP is the intention probability vector calculated at the current moment, and prevP is the intention probability vector calculated iteratively in the previous frames.

6. The obstacle avoidance control method according to claim 1, characterized in that: The fusion intention and trajectory prediction includes: Establish four intention motion models: including cooperative intention model, away intention model, approach intention model and stop intention model; The intention probability vector is weighted and fused into the trajectory prediction of the motion model: First, each intention is traversed to obtain the probability weight of the intention. For each time step, the position and size are accumulated according to the weighted intention probability. The position of the fused prediction trajectory is the weighted average of the positions of the intention trajectories, that is: Among them, P fused (t) is the fused prediction trajectory that incorporates the intention probability; P i (t) is the predicted position of intention i at time t; ω i is the probability of the intention; Considering trajectory uncertainty: Calculate the weighted variance of the trajectories of different intentions and the fused predicted trajectory. The larger the weighted variance, the greater the difference in predictions for different intentions and the higher the uncertainty. When the prediction differences between different intentions are large, the prediction area will automatically expand to reflect the higher trajectory uncertainty. The expression for expanding the prediction area based on the variance is: Among them, Size fused (t) is the expanded prediction area; Var(t) is the weighted variance of different intention trajectories and fusion trajectories; Size avg (t) is the original prediction area size; Z score Corresponding to 95% confidence level.

7. The obstacle avoidance control method according to claim 6, characterized in that: The motion model includes: Collaborative intention model: Assume that the dynamic target and the robot move in parallel, that is, the dynamic target and the robot move in the same direction, but the speed fluctuates within a certain range. The speed range is set to v∈[0.8v0,1.2v0], where v0 is the current speed of the underwater robot, and the angle range is set to θ∈[θ robot -frontAngel,θ robot +frontAngel],θ robot is the current motion direction of the underwater robot, frontAngel is the allowable relative motion direction deviation, a uniform linear motion model is established, and the uniform linear motion is propagated in the motion direction; Away intention model: set the angle range to θ∈[θ robot -π,θ robot ], that is, the fan-shaped area away from the direction of the underwater robot, the speed range is v∈[0,min(1.2v0,v max )],v max is the maximum forward speed of the underwater robot; Approach intention model: Set the angle range to θ∈[θ robot ,θ robot +π], that is, the fan-shaped area facing the direction of the underwater robot, the speed range is v∈[0,min(1.2v0,v max )]; Stop intention model: Set the position of the dynamic target in all prediction moments to be equal to the current position of the dynamic target.

8. The obstacle avoidance control method according to claim 1, characterized in that: The autonomous obstacle avoidance trajectory planning of the underwater robot includes: Obtain the predicted trajectory of the fusion intention: record the current position, speed, angular velocity and posture of the underwater robot, and set the target point; Trajectory obstacle avoidance constraint function design: Based on the uncertainty of underwater dynamic target behavior intentions, enhanced obstacle avoidance constraints, lateral obstacle avoidance constraints, and linear regression constraints are designed; Compare the current trajectory and predicted trajectory at the same time to perform trajectory obstacle avoidance: extract the current TEB trajectory point of the underwater robot and the trajectory point of the fused predicted trajectory in the same time step, and compare their distance to see if they meet the currently set trajectory obstacle avoidance constraint function. If so, it means that the planned trajectory is feasible. If not, the trajectory is replanned until the trajectory constraint is met to achieve autonomous obstacle avoidance.

9. The obstacle avoidance control method according to claim 8, characterized in that: The trajectory obstacle avoidance constraint function design includes: Obstacle avoidance constraints include static target obstacle avoidance constraints and dynamic target obstacle avoidance constraints, namely: Static target obstacle avoidance constraint: For each static target position O j =(O jx ,O jy ,O jz ), the distance d between the trajectory point and the obstacle is: According to the radius r of the underwater robot robot and safety margin r margin , effective distance d eff for: d eff =d-r robot -r margin The repulsive force F of the obstacle avoidance algorithm in TEB path planning obs Set to: Among them, w obs is the obstacle avoidance weight; d min is the minimum safe distance; Dynamic target obstacle avoidance constraint: According to the predicted position P of the dynamic target pred and size S pred , set the obstacle radius r obs =max(s pred,x ,s pred,y ,s pred,z ) / 2, effective distance d eff =dr robot -r obs , the repulsive force F obs The settings are the same as above; Lateral obstacle avoidance constraints: Calculate the current robot position (x i ,y i ,z i )Towards the goal x ,goal y ,goal z )’s target direction unit vector u goal ,Right now: According to the target direction unit vector u goal Define the lateral vector u lat for: in lat =(-u goal,y ,in goal,x ,0) Among them, u goal,y Represents the target direction unit vector u goal The component on the y-axis; u goal,x Represents the target direction unit vector u goal The component on the x-axis; For a dynamic target point O approaching from the side, x ,O y ,O z ) relative to the trajectory point x i =(x i ,y i ,z i ) is the displacement vector d obs =(x i -O x ,y i -O y ,z i -O z ), its projection distance d lateral Defined as: d lateral =d obs *u lat Set a lateral obstacle avoidance threshold, if the projection distance d lateral If the value is greater than the obstacle avoidance threshold, it means that the dynamic target is far away from the underwater robot and the impact of its movement does not need to be considered; if the value is less than the threshold, a lateral constraint force F is set. lat Size: Among them, w lat is the lateral obstacle avoidance weight; d is the actual distance from the dynamic target to the underwater robot; ∈ is a small constant to prevent the denominator from being zero; The direction of the lateral restraint force for: Among them, avoiding_direction = +1, indicating that the dynamic target is on the right and avoids obstacles to the left; avpodomg_dorectopm = -1, indicating that the dynamic target is on the left and avoids obstacles to the right; Linear regression constraints: Calculate each trajectory point x i and the ideal current position to the target point x i,straight The deviation Δx from the straight line i ,Right now: Δx i =x i -x i,straigh t Set Threat Level I th reat for: Among them, d min is the distance from the trajectory point to the nearest dynamic target; d safe For safe distance; Set adaptive regression weight α straigh t for: The linear regression constraints are: F straigh t =-a straigh t *Dx i Among them, F straigh t is the linear regression constraint.

Images