Control method and control device of underwater robot

Abstract

The invention provides a control method and a control device of an underwater robot. The control method of the underwater robot comprises the following steps: establishing an underwater robot system model; performing obstacle avoidance path planning by using a sampling algorithm to obtain a discrete geometric path point set connecting the starting point and the ending point; a full-state reference sequence matched with the control method underwater robot system model is generated based on the control method discrete geometric path point set; constructing a cost function for trajectory optimization based on the control method underwater robot system model and the control method full-state reference sequence; generating an initial control sequence and an initial state sequence according to the deviation between the current state of the underwater robot in the control method and the full-state reference sequence in the control method; and performing iterative optimization by taking the control method cost function as an optimization target and taking the initial control sequence and the initial state sequence of the control method as hot start initial values to obtain a control instruction sequence of the underwater robot of the control method.

Description

Technical Field

[0001] This disclosure relates to the field of underwater robot technology, specifically to a control method and control device for an underwater robot. Background Technology

[0002] In recent years, with the deepening of marine development, autonomous underwater vehicles (AUVs) have played an increasingly important role in tasks such as deep-sea resource exploration, long-term environmental monitoring, and maintenance of submarine pipelines and cables.

[0003] AUV motion planning methods are mainly divided into graph search methods, sampling-based planning methods, and optimization-based planning methods. Graph search methods (such as A* and its variants) find the optimal path by discretizing the workspace, but in large-scale three-dimensional space, their computational complexity increases exponentially with the increase of dimension, making it difficult to meet real-time requirements. Optimization-based methods (e.g., model predictive control) can handle dynamic constraints, but they are prone to getting trapped in local minima and are sensitive to initial conditions. Sampling-based algorithms (e.g., RRT) avoid explicit spatial discretization by randomly sampling in a continuous configuration space, and are therefore widely used in high-dimensional space planning. However, the traditional RRT* algorithm converges to a high-quality solution slowly in complex environments, and as the number of nodes increases, nearest neighbor search becomes the main computational bottleneck, severely limiting its planning efficiency in narrow passages or environments with dense obstacles.

[0004] In addition, existing collision detection methods are often designed for single shapes, making it difficult to efficiently and uniformly handle heterogeneous obstacles such as spheres, cuboids, and convex polyhedra commonly found in underwater environments.

[0005] At the motion control level, traditional control methods are usually decoupled from the global planning layer, making it difficult to take advantage of second-order trajectory optimization in the control loop. Iterative linear quadratic regulators (iLQR) are difficult to apply to underwater robots.

[0006] Existing single planning or control methods cannot simultaneously balance computational efficiency, geometric obstacle avoidance capability, and dynamic constraint handling capability. Summary of the Invention

[0007] One of the purposes of this disclosure is to provide a control method that enables efficient planning and precise, stable trajectory tracking of underwater robots in complex three-dimensional environments.

[0008] According to a first aspect of this disclosure, a control method for an underwater robot includes: establishing an underwater robot system model comprising a kinematic model, a dynamic model, and physical performance boundaries of actuators; performing obstacle avoidance path planning using a sampling algorithm based on a three-dimensional working space defined by the underwater robot system model to obtain a set of discrete geometric path points connecting the starting point and the ending point; generating a full-state reference sequence matching the underwater robot system model based on the set of discrete geometric path points; constructing a cost function for trajectory optimization based on the underwater robot system model and the full-state reference sequence; generating an initial control sequence and an initial state sequence based on the deviation between the current state of the underwater robot and the full-state reference sequence, provided that the physical performance boundaries are satisfied; and performing iterative optimization based on the underwater robot system model, using the cost function as the optimization objective and the initial control sequence and initial state sequence as initial values ​​for hot start, to obtain a control command sequence for the underwater robot.

[0009] According to embodiments of this disclosure, obstacle avoidance path planning using a sampling algorithm to obtain a discrete geometric path point set connecting the starting point and the ending point may include: dynamically reducing the effective sampling area in a three-dimensional work space based on the distribution of generated paths and the target position, and performing random sampling biased towards the target within the reduced effective sampling area to obtain a sample point to be tested; using the sample point to be tested as a retrieval target, performing a nearest neighbor search in a path point database composed of historical path points to determine the parent node closest to the sample point to be tested; establishing a connecting line segment between the sample point to be tested and the parent node, and using a collision detection algorithm to determine whether the connecting line segment meets the obstacle avoidance requirements; in response to the connecting line segment meeting the obstacle avoidance requirements, storing the sample point to be tested as a new historical path point in the path point database, and iteratively executing the above steps until a discrete geometric path point set connecting the starting point and the ending point is obtained.

[0010] According to embodiments of this disclosure, the pathpoint database adopts a KD-tree spatial index structure based on a dynamic maintenance mechanism. The process of storing test sampling points as new historical pathpoints in the pathpoint database includes: setting an incremental buffer for temporarily storing newly added historical pathpoints; real-time monitoring of the number of pathpoints in the incremental buffer and determining whether a preset scale threshold has been reached; in response to not reaching the preset scale threshold, adding the currently passed obstacle avoidance test sampling points to the incremental buffer; and in response to reaching the preset scale threshold, merging all pathpoints in the incremental buffer with existing nodes in the KD-tree, reconstructing the global KD-tree, and clearing the incremental buffer.

[0011] According to embodiments of this disclosure, determining whether a connecting line segment meets obstacle avoidance requirements using a collision detection algorithm may include: representing obstacles in a three-dimensional work space as a set of geometric objects composed of triangular facets; calculating the intersection points of the connecting line segment and the plane containing each triangular facet; determining whether the intersection point is located between the two endpoints of the connecting line segment; in response to the intersection point being located on the connecting line segment, determining whether the intersection point is located inside the corresponding triangular facet; in response to satisfying a first condition that the intersection point is located on the connecting line segment and inside the corresponding triangular facet, determining that the connecting line segment does not meet obstacle avoidance requirements; and in response to not satisfying the first condition, determining that the connecting line segment meets obstacle avoidance requirements.

[0012] According to embodiments of this disclosure, determining whether an intersection point is located inside a corresponding triangular facet may include: obtaining the coordinates of the three vertices of the corresponding triangular facet; calculating the centroid coordinate coefficient of the intersection point relative to the triangle based on the three vertex coordinates; determining that the intersection point is located inside the corresponding triangular facet in response to a second condition that all centroid coordinate coefficients are greater than or equal to zero and the sum of the centroid coordinate coefficients is equal to 1; and determining that the intersection point is located outside the corresponding triangular facet in response to a second condition that is not met.

[0013] According to embodiments of this disclosure, representing obstacles in a three-dimensional workspace as a set of geometric objects composed of triangular facets may include: for convex polyhedral obstacles, triangulating their surfaces into a first set of triangular facets based on their surface topology; for spherical obstacles, discretizing their surfaces into triangular facets according to a preset resolution to generate a second set of triangular facets; for cuboid obstacles, dividing their six rectangular faces into two triangles along their diagonals to form a third set of triangular facets; and merging the first set of triangular facets, the second set of triangular facets, and the third set of triangular facets to construct a set of geometric objects.

[0014] According to embodiments of this disclosure, generating a full-state reference sequence matching an underwater robot system model based on a discrete geometric path point set may include: performing curve fitting on the discrete geometric path point set and resampling at a preset time resolution to obtain reference positions at each time step; calculating linear velocity based on reference positions at adjacent time steps, and determining reference yaw angle and reference pitch angle according to the path tangent direction; performing numerical differentiation on the reference yaw angle and reference pitch angle respectively to obtain the corresponding angular velocity; and generating a full-state reference sequence based on the reference position, linear velocity, angular velocity, reference yaw angle, reference pitch angle, and a preset reference roll angle.

[0015] According to embodiments of this disclosure, under the premise of satisfying physical performance boundaries, an initial control sequence and an initial state sequence are generated based on the deviation between the current state of the underwater robot and the full-state reference sequence. This may include: comparing the current state of the underwater robot with the full-state reference sequence to obtain pose and velocity errors; performing multi-channel PID adjustment on the pose and velocity errors to obtain virtual control force and torque; constructing a thrust distribution model based on the actuator configuration, and using physical performance boundaries as constraints, mapping the virtual control force and torque to initial control commands for each actuator to obtain an initial control sequence; inputting the initial control sequence into the underwater robot system model, and obtaining the initial state sequence through forward integral prediction.

[0016] According to embodiments of this disclosure, based on an underwater robot system model, with a cost function as the optimization objective and initial control sequences and initial state sequences as initial values ​​for hot start, iterative optimization is performed to obtain a control command sequence for the underwater robot. This may include: introducing hard constraints from the physical performance boundary into the cost function to construct an augmented cost function containing Lagrange multipliers; using the initial control sequence and initial state sequence as initial values ​​for hot start, and under the constraints of the underwater robot system model, performing a descent direction search on the augmented cost function; updating the control sequence and performing forward extrapolation of the state trajectory based on the correction obtained from the search, and converging the augmented cost function to a local optimum through iterative iteration; and generating a control command sequence for driving the actuator based on the converged optimal control variables.

[0017] According to a second aspect of this disclosure, a control device for an underwater robot includes a memory and a processor. The memory stores programs or instructions, which, when executed by the processor, cause the processor to perform the aforementioned control method for the underwater robot.

[0018] The control method for an underwater robot according to embodiments of the present disclosure can improve the convergence speed of sampling algorithms in complex environments.

[0019] The control method for an underwater robot according to embodiments of the present disclosure enables efficient detection of mixed obstacles.

[0020] The control method for an underwater robot according to embodiments of the present disclosure can improve the convergence robustness and local optimization speed of the controller. Attached Figure Description

[0021] The above and other objects and features of exemplary embodiments of this disclosure will become clearer from the following description taken in conjunction with the accompanying drawings, which exemplarily illustrate the embodiments, wherein: Figure 1 This is a system architecture diagram illustrating the hierarchical motion planning and control of an underwater robot according to an embodiment of the present disclosure; Figure 2This is a flowchart of a control method for an underwater robot according to an embodiment of the present disclosure; Figure 3 This is a flowchart of an accelerated Informed RRT* path planning algorithm based on a K-dimensional (KD) tree index and lazy reconstruction strategy according to embodiments of this disclosure; Figure 4 This is a flowchart of a trajectory tracking control algorithm based on proportional-integral-derivative (PID) hot-start constrained augmented Lagrange iterative linear quadratic regulator (AL-iLQR) according to an embodiment of the present disclosure; Figure 5 This is a flowchart of the process for generating a full-state reference sequence according to an embodiment of the present disclosure; Figure 6 This is a comparison diagram of the path geometry generated by the KD-tree Informed RRT* algorithm and the baseline algorithm in a high-density obstacle environment according to embodiments of the present disclosure; Figure 7 This is a comparison chart of the trajectory tracking performance of the constrained AL-iLQR controller and the benchmark methods (PID and hot-start-free iLQR) in a complex obstacle environment according to embodiments of the present disclosure. Figure 8 It is a graph showing the change of control inputs (thrust, yaw rate and pitch rate) over time in a typical scenario according to embodiments of the present disclosure. Detailed Implementation

[0022] The embodiments of this disclosure will now be described in detail with reference to the accompanying drawings, examples of which are illustrated in the drawings, wherein the same reference numerals always refer to the same parts. The embodiments will now be described with reference to the accompanying drawings in order to explain this disclosure.

[0023] The following detailed description is provided to aid in obtaining a full understanding of the methods, apparatus, and / or systems described herein. However, the order of operations described herein is merely illustrative and is not limited to those orders set forth herein; equivalent substitutions or changes may be made, except for operations that must occur or be performed in a specific order. Furthermore, for clarity and conciseness, descriptions of content well-known in the art will be omitted or simplified.

[0024] Unless otherwise specified, the same reference numerals generally refer to the same elements (e.g., components, steps, and methods). Reference numerals described in previous embodiments that reappear in later embodiments may be omitted. Furthermore, technical features described in different or the same embodiments can be combined in any way, as long as the combined embodiment or technical solution is complete and can solve the technical problems of this application or achieve the technical effects described or not described in this disclosure but which can be determined based on the complete technical solution described above.

[0025] Unless otherwise defined, all terms used herein (including technical and scientific terms) shall have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains upon understanding this disclosure. Unless expressly defined herein, terms (such as those defined in a general dictionary) shall be interpreted as having a meaning consistent with their meaning in the context of the relevant field and in this disclosure, and shall not be interpreted in an idealized or overly formalistic manner.

[0026] The control method according to the embodiments of this disclosure adopts a hierarchical architecture, where the upper-level planner is responsible for generating geometrically optimal collision-free paths in complex obstacle environments, and the lower-level controller is responsible for accurately tracking the path while satisfying dynamic constraints and actuator physical limitations.

[0027] Figure 1 This is a system architecture diagram illustrating the hierarchical motion planning and control of an underwater robot according to an embodiment of the present disclosure; Figure 2 This is a flowchart of a control method for an underwater robot according to an embodiment of the present disclosure; Figure 3 This is a flowchart of an accelerated Informed RRT* path planning algorithm based on a K-dimensional (KD) tree index and lazy reconstruction strategy according to embodiments of this disclosure; Figure 4 This is a flowchart of a trajectory tracking control algorithm based on proportional-integral-derivative (PID) hot-start constrained augmented Lagrange iterative linear quadratic regulator (AL-iLQR) according to an embodiment of the present disclosure; Figure 5 This is a flowchart of the process for generating a full-state reference sequence according to an embodiment of the present disclosure; Figure 6 This is a comparison diagram of the path geometry generated by the KD-tree Informed RRT* algorithm and the baseline algorithm in a high-density obstacle environment according to embodiments of the present disclosure; Figure 7 This is a comparison chart of the trajectory tracking performance of the constrained AL-iLQR controller and the benchmark methods (PID and hot-start-free iLQR) in a complex obstacle environment according to embodiments of the present disclosure. Figure 8 It is a graph showing the change of control inputs (thrust, yaw rate and pitch rate) over time in a typical scenario according to embodiments of the present disclosure.

[0028] Reference Figure 1 , Figure 1 The overall architecture of hierarchical motion planning and control for an underwater robot according to an embodiment of the present disclosure is shown, which is mainly divided into two cooperative sub-tasks: global planning and constraint tracking.

[0029] As an example, the control method for an underwater robot according to embodiments of this disclosure can utilize an accelerated Informed RRT algorithm based on KD-tree spatial indexing. This algorithm significantly reduces the computational overhead of nearest neighbor search in three-dimensional space by introducing novel lazy reconstruction and tail compensation strategies, solving the problem of slow convergence of traditional sampling algorithms in complex environments. Specifically, the global planning layer in the control method according to embodiments of this disclosure is based on the Informed RRT* algorithm to generate a collision-free polyline path in a three-dimensional obstacle environment, and accelerates nearest neighbor search through KD-trees (combining lazy reconstruction and tail compensation).

[0030] As an example, the underwater robot control method according to the embodiments of this disclosure targets heterogeneous obstacles commonly found in underwater environments. By using a unified line segment-obstacle collision detection predicate and through triangulation and centroid coordinate inclusion testing, it can robustly handle various geometric shapes such as spheres, cuboids, and convex polyhedra, achieving efficient detection in mixed obstacle environments.

[0031] As an example, considering the dynamic characteristics and actuator limitations of underwater robots, the control method according to embodiments of this disclosure constructs a tracking controller with a feasible PID hot-start (e.g., an augmented Lagrange iterative linear quadratic regulator (AL-iLQR) tracking controller). This controller uses an initial trajectory that satisfies physical constraints generated by a three-channel PID controller to guide the optimization process, thereby improving the convergence robustness and local optimization speed of the controller while strictly satisfying hard constraints such as advance / retreat speed, yaw rate, and pitch rate.

[0032] For details, please refer to Figure 1 The system can perform equal-arc-length reparameterization on the path, outputting a discrete geometric waypoint sequence. The constraint tracking layer then transforms this sequence into a full-state reference trajectory. It first generates an initial control and state sequence that satisfies the actuator's physical boundaries (such as thrust and angular velocity limits) through a decoupled PID controller. This initial sequence is then used as the initial values ​​for a warm start, and trajectory optimization is performed using augmented Lagrangian iterative LQR (AL-iLQR). The optimal control command is output while ensuring dynamic feasibility. The overall architecture achieves an organic integration of efficient obstacle avoidance and high-precision constraint tracking. The following section will combine... Figures 2 to 8 The embodiments of this disclosure will be described in detail.

[0033] Reference Figure 2 The control method for an underwater robot according to embodiments of the present disclosure may include steps S100, S200, S300, S400, S500 and S600.

[0034] In step S100, an underwater robot system model is constructed.

[0035] Specifically, an underwater robot system model can be established that includes a kinematic model, a dynamic model, and the physical performance boundaries of the actuators.

[0036] An underwater robot system model describes the motion and dynamics of an underwater robot and is used for path planning, trajectory generation, and control optimization. An underwater robot model may include three parts: a kinematic model (describing the geometric relationship between pose and velocity), a dynamic model (describing the physical relationship between force / torque and acceleration, including parameters such as mass, damping, and added mass), and physical performance boundaries of the actuators (e.g., hardware constraints such as thrust amplitude and upper limit of angular velocity).

[0037] To accurately describe the motion of an underwater robot, an inertial coordinate system can be defined. and the body coordinate system fixed to the robot's center of mass Define the state vector. ,in For location, Euler angles, namely roll, pitch, and yaw angles; the velocity vector in the volume coordinate system can be defined. These are the linear velocities for forward and backward movement, lateral movement, and buoyancy, as well as the angular velocities for roll, pitch, and yaw, respectively.

[0038] Considering the symmetry and low-speed assumptions of underwater robots, a simplified six-degree-of-freedom kinematic and dynamic model can be established.

[0039] The kinematic equations describe the rate of change of velocity from the body coordinate system to the inertial coordinate system: (1) in, and Represent and .

[0040] The dynamic equations describe the change in velocity under the action of force and torque: (2) in, For inertial parameters that include added mass, The hydrodynamic damping coefficient is... These are the control inputs for forward and backward thrust, pitching moment, and yaw moment, respectively.

[0041] To facilitate the implementation of the digital controller, the above model can be discretized using the forward Euler method, with a sampling time of [missing information]. The discretized system state update equation is: (3) in, for The state at any given moment, To control the input vector (here, the advance / retreat speed, yaw rate, and pitch rate are regarded as directly controlled variables or implemented by the underlying thrust distribution to simplify the control hierarchy).

[0042] The physical performance boundaries of the actuators refer to the maximum / minimum thrust and its rate of change limits that the underwater robot's thrusters can provide. This disclosure models these as inequality constraints on the control inputs. Combining the underwater robot's symmetrical layout and low-speed assumptions, a thrust allocation matrix maps virtual forces / torques to each thruster, and boundary parameters are determined based on the thruster's rated power, motor characteristics, and measured response data. As an example, these boundary parameters can be used as components of the underwater robot system model. They can be used not only for instruction trimming during the thrust allocation phase but also further transformed into upper limits of the feasible domains for the body's forward and backward speeds, yaw rates, and pitch rates. These can be embedded as hard constraints in AL-iLQR optimization to ensure that the generated trajectory always meets hardware capability limitations.

[0043] The above-described underwater robot system model is merely an example. Further considerations could include: asymmetric mass distribution (such as device bias), variations in added mass, ocean current disturbances, actuator dynamic response delays and saturation characteristics, and strong attitude-position coupling terms. Furthermore, for complex tasks, a fully coupled model including roll degrees of freedom could be constructed. As an example, data-driven methods could also be introduced to compensate for unmodeled dynamics, thereby establishing a more accurate dynamic and kinematic model and improving control performance in scenarios with strong disturbances or high maneuverability.

[0044] In step S200, obstacle avoidance path planning is performed to generate a discrete geometric path point set.

[0045] Specifically, obstacle avoidance path planning can be performed based on the three-dimensional working space defined by the underwater robot system model using sampling algorithms (e.g., sampling algorithms with search acceleration and sampling area shrinkage mechanisms) to obtain a set of discrete geometric path points connecting the starting point and the ending point.

[0046] As an example, sampling algorithms with search acceleration and sampling region shrinkage mechanisms (e.g., InformedRRT*, BIT*, SST, etc.) can be used for obstacle avoidance path planning. However, this disclosure is not limited to this, as long as the sampling algorithm can generate a set of discrete geometric path points connecting the start and end points in the three-dimensional working space defined by the underwater robot system model.

[0047] As an example, within the 3D workspace, the effective sampling area can be dynamically reduced based on the distribution of generated paths and the target location. Within this reduced effective sampling area, random sampling biased towards the target is performed to obtain test sampling points. These test sampling points are then used as retrieval targets. A nearest neighbor search is performed in a path point database composed of historical path points to determine the nearest parent node. A connecting line segment is established between the test sampling point and its parent node, and a collision detection algorithm is used to determine if the connecting line segment meets obstacle avoidance requirements. If the connecting line segment meets the obstacle avoidance requirements, the test sampling point is stored as a new historical path point in the path point database, and the above steps are iteratively executed until a discrete geometric path point set connecting the start and end points is obtained. As an example, the search structure can be updated to maintain subsequent query efficiency. Furthermore, as the path extends towards the target, the sampling range can be continuously tightened and the retrieval of neighboring points accelerated until a discrete geometric path point set connecting the start and end points is successfully generated.

[0048] As an example, if a connecting line segment does not meet the obstacle avoidance requirements (i.e., the line segment is found to interfere with an obstacle during collision detection), the current sampled point to be tested is discarded, not added to the path point database, and no connection is established. Subsequently, the process can immediately return to the beginning stage, and the next random sampling can be performed again in the 3D work space to generate new sampled points to be tested, and the steps of nearest neighbor search, connection, and collision detection can be repeated.

[0049] Preferably, an improved Informed RRT* algorithm can be used for global path planning. As an example, an improved Informed RRT* algorithm based on KD-tree indexing can be used for global path planning.

[0050] Specifically, it can be done at a known starting point. and target point Three-dimensional bounded workspace In the middle, plan to avoid static obstacle sets The shortest path.

[0051] This disclosure improves upon the traditional RRT* algorithm. Firstly, an intelligent sampling strategy is introduced: once an initial feasible path is found, the sampling region can be restricted to a contracting ellipsoid centered on this path. Within this region, convergence is accelerated. The ellipsoidal region is defined as: (4) in, , representing the center of the ellipsoid. For rotation matrix, diagonal matrix Including half-shaft length: (5) in, This is the current optimal path length. This represents the Euclidean distance between the start and end points. The above method of shrinking the ellipsoid is merely an example; a suitable shrinkage strategy can be selected based on the characteristics of the task.

[0052] In addition, to address the issue of high computational cost in nearest neighbor search in three-dimensional space, a KD-tree (K-Dimensional Tree) can be used to store tree nodes. By employing a "lazy reconstruction" strategy (reconstructing the tree only when the number of nodes doubles) and a "tail compensation" strategy (using a linear list to store newly added nodes after reconstruction), the search complexity of a single iteration is reduced from linear to logarithmic, significantly improving planning efficiency.

[0053] As an example, the pathpoint database can adopt a KD-tree spatial index structure based on a dynamic maintenance mechanism. Storing the sampled points to be tested as new historical pathpoints into the pathpoint database can include: setting up an incremental buffer for temporarily storing newly added historical pathpoints; monitoring the number of pathpoints in the incremental buffer in real time and determining whether a preset size threshold has been reached; if the preset size threshold has not been reached, adding the sampled points to be tested that have passed obstacle avoidance verification to the incremental buffer; and if the preset size threshold has been reached, merging all pathpoints in the incremental buffer with existing nodes in the KD-tree, reconstructing the global KD-tree, and clearing the incremental buffer.

[0054] As an example, a unified line segment-obstacle collision detection interface can be designed for heterogeneous obstacles (spheres, cuboids, convex polyhedra) in underwater environments. For convex polyhedra, convex hull triangulation and centroid coordinate testing are used to determine whether a line segment intersects the obstacle surface or is located inside it, achieving robust geometric obstacle avoidance.

[0055] Specifically, using a collision detection algorithm to determine whether a connecting line segment meets the obstacle avoidance requirements may include: representing obstacles in the three-dimensional work space as a set of geometric objects composed of triangular facets; calculating the intersection point of the connecting line segment with the plane containing each triangular facet; determining whether the intersection point is located between the two endpoints of the connecting line segment; in response to the intersection point being located on the connecting line segment, determining whether the intersection point is located inside the corresponding triangular facet; in response to satisfying the first condition that the intersection point is located on the connecting line segment and inside the corresponding triangular facet, determining that the connecting line segment does not meet the obstacle avoidance requirements; otherwise, determining that the connecting line segment meets the obstacle avoidance requirements.

[0056] In specific decision-making, in addition to calculating the intersection points of line segments and surfaces, the centroid coordinates can be used to robustly determine whether a line segment intersects the obstacle surface or is completely inside the obstacle. Furthermore, a shrinking ellipsoid intelligent sampling strategy can be introduced, dynamically constraining the sampling area by using the current optimal path length. This reduces the number of invalid line segments requiring collision detection from the source, significantly improving planning efficiency in complex 3D environments.

[0057] As an example, for convex polyhedral obstacles, their surfaces can be triangulated into a first set of triangular facets based on their surface topology; for spherical obstacles, their surfaces can be discretized and triangulated according to a preset resolution to generate a second set of triangular facets; for cuboid obstacles, their six rectangular faces can be divided into two triangles along their diagonals to form a third set of triangular facets. As an example, the first, second, and third sets of triangular facets can be merged to construct a set of geometric objects.

[0058] As an example, a specific set of sampling points from the database described above can be used to form an optimal sequence or optimal path by connecting them according to the reconnection rules mentioned above.

[0059] Specifically, please refer to Figure 3 The steps for forming the optimal path may include steps S210, S220, S231, S232, S240, S250, S260, S270, S271, S272, S273, S274 and S275.

[0060] In step S210, the tree T and the KD tree are initialized. Tail c best =∞.

[0061] Specifically, tree T is used to store the planned geometric path nodes and edges; the KD-tree serves as a spatial index structure to accelerate retrieval in high-dimensional space; the tail... This is a linear list used to temporarily store newly generated nodes to implement a "tail compensation" strategy; simultaneously, the current optimal path length c is... best Setting it to infinity indicates that no feasible solution has been found yet.

[0062] In step S220, it is determined whether a feasible solution c exists. best <∞?

[0063] Specifically, it can monitor in real time whether a collision-free path connecting the starting point and the target point has been found. If no path has been found, global uniform sampling is performed; if a path has been found, it switches to a more efficient local search.

[0064] In step S231, x is obtained by uniform sampling throughout the entire space. rand .

[0065] Specifically, when in the initial path search phase, random and uniform sampling can be performed throughout the predefined three-dimensional workspace to cover as many unknown areas as possible.

[0066] In step S232, in the ellipsoidal region Internal sampling yields x rand .

[0067] Specifically, once an initial solution is found, a predetermined sampling strategy can be introduced to limit the sampling range to a point centered on the origin and destination, and with the current optimal path length c. best The search ellipsoid is within a contracting ellipsoid of axis length. This avoids invalid searches in regions where better solutions are impossible, thus accelerating convergence. However, as mentioned above, the search ellipsoid described is merely an example, and other geometries can also be used for the search.

[0068] In step S240, the nearest neighbor query x near (use ∪ Hybrid search).

[0069] Specifically, in order to quickly find the distance node x in three-dimensional space rand The nearest node can simultaneously retrieve the constructed KD-tree K and the temporarily stored linear list (tail). New nodes added in.

[0070] In step S250, x is expanded and generated. new And perform collision detection.

[0071] Specifically, it can be seen from x near To x rand Candidate nodes x are obtained by directional growth with a fixed step size. new Subsequently, a unified collision detection predicate is invoked, and geometric methods such as centroid coordinates are used to determine whether the newly added line segment passes through the obstacle surface or enters the obstacle interior.

[0072] In step S260, is it determined whether the path is collision-free? Specifically, logical branches are performed based on the detection result of step S250. If the line segment collides with an obstacle (determined as "No"), the sampling point is discarded and the process returns to step S220 to iterate again; if there is no collision (determined as "Yes"), the insertion operation continues.

[0073] As an example, the steps to determine whether an intersection point is located inside the corresponding triangular facet may include: obtaining the coordinates of the three vertices of the corresponding triangular facet; calculating the centroid coordinate coefficients of the intersection point relative to the triangle based on the three vertex coordinates; determining that the intersection point is located inside the corresponding triangular facet in response to a second condition that all centroid coordinate coefficients are greater than or equal to zero and the sum of the centroid coordinate coefficients is equal to 1; otherwise, determining that the intersection point is located outside the corresponding triangular facet.

[0074] As an example, before calculating the centroid coordinate coefficient, first calculate the line segment (x)near To x new The intersection point of the line segment with the plane containing the triangle. If the line segment does not intersect the plane at all, then there is no need to perform subsequent centroid coordinate testing.

[0075] In step S270, the optimal parent node is selected and inserted into T, and the tail is added. Reconnect.

[0076] Specifically, in x new Within the neighborhood of the node, the node that minimizes its cumulative path cost is selected as the parent node. After insertion, the node is synchronously stored in the linear list T, and the local tree structure is optimized according to the reconnection rules of RRT*.

[0077] In step S271, if the target is reached, update c. best and ellipsoid parameters.

[0078] Specifically, when a new node approaches or reaches the target area, the total length of the current path is calculated. If this length is less than the existing c... best If the optimal cost is updated, the new ellipsoidal rotation matrix and semi-axis length are calculated according to the formula to further narrow the sampling range.

[0079] In step S272, the tail portion is determined. Does the size exceed the threshold? Specifically, the number of nodes in the linear list can be monitored in real time to determine whether it has reached a preset scale threshold (e.g., the number of nodes has doubled).

[0080] In step S274, the KD-tree index K is rebuilt and the tail is cleared. .

[0081] Specifically, when the list reaches a threshold, in order to maintain logarithmic efficiency of the search, the algorithm merges all points in the tail list with the old KD tree, reconstructs a balanced global KD tree, and resets the list.

[0082] In step S273, determine whether the maximum number of iterations has been reached? Specifically, check if the current number of iterations exceeds the system's set limit. If not, return to step S220 to continue optimizing the path; if it has been reached, end the planning process.

[0083] In step S275, the optimal path is output.

[0084] Specifically, the lowest-cost collision-free trajectory in the current tree structure can be extracted and passed to the lower-level controller for path reparameterization and tracking.

[0085] Reference Figure 2In step S300, a full-state reference sequence matching the underwater robot system model can be generated based on the discrete geometric path point set.

[0086] The geometric path generated by the planner can consist of a series of discrete points. To provide a reference with consistent spatial resolution for the lower-level controller, the geometric path can be resampled into a sequence of waypoints of equal arc length. .

[0087] It can resample and complete the state dimension of discrete geometric path point sets to obtain a full-time domain reference state sequence. For example, referring to Figure 5 The steps for generating a full-state reference sequence that matches the underwater robot system model based on a discrete geometric path point set include steps S510, S520, S530, S540, and S550.

[0088] In step S510, the reference position at each moment is obtained.

[0089] Specifically, the discrete geometric path point set can be curve fitted and resampled according to a preset time resolution to obtain the reference position at each time.

[0090] As an example, third-order spline interpolation or Bézier curves can be used to perform continuous curve fitting on a discrete geometric path point set, eliminating geometric discontinuities at path inflection points. Subsequently, based on the sampling period of the lower-level controller, a preset time resolution can be set as the step size, and resampling can be performed on the fitted curve.

[0091] In step S520, the linear velocity is calculated.

[0092] Linear velocity can be calculated based on reference positions at adjacent time points. Specifically, the spatial displacement vector can be calculated based on the reference position coordinates of two adjacent sampling time points, and then divided by a preset time resolution to obtain the instantaneous linear velocity vector at that time point.

[0093] In step S530, the reference yaw angle and the reference pitch angle are determined.

[0094] As an example, the reference yaw angle and reference pitch angle can be determined based on the tangent direction of the path. For instance, the unit tangent vector of the path at the current moment can be calculated using the reference position coordinates, and then the reference yaw angle and reference pitch angle can be derived based on the projection relationship of this tangent vector on the horizontal and vertical planes, respectively.

[0095] In step S540, the angular velocity is calculated.

[0096] As an example, the reference yaw angle and reference pitch angle can be numerically differentiated to obtain the corresponding angular velocity.

[0097] In step S550, a full-state reference sequence is generated.

[0098] As an example, a full-state reference sequence can be generated based on reference position, linear velocity, angular velocity, reference yaw angle, reference pitch angle, and a preset reference roll angle. Specifically, a sequence describing the robot's six degrees of freedom motion state can be constructed based on the reference position (three-dimensional coordinates), linear velocity, angular velocity (yaw / pitch rate of change), reference yaw angle, and reference pitch angle at each sampling time, combined with a preset reference roll angle (usually set to zero).

[0099] The above-described method for obtaining the full-time domain reference state sequence is merely an example, and this disclosure is not limited thereto. Obtaining the full-time domain reference state sequence may also involve steps such as using spline interpolation to ensure the continuity of velocity and acceleration, dynamically adjusting the sampling interval according to the physical boundary to limit the peak velocity, and suppressing noise introduced by numerical differentiation through a smoothing algorithm.

[0100] In step S400, specifically, a cost function for trajectory optimization can be constructed based on the underwater robot system model and the full-state reference sequence. As an example, a cost function containing equality constraints and inequality penalty functions can be constructed using the system state update equation, physical performance boundaries, and the full-time domain reference state sequence.

[0101] The path tracking problem can be modeled as a finite-time optimal control problem, with the goal of minimizing tracking error and control cost while satisfying input constraints.

[0102] As an example, a stage cost function can be defined. and terminal cost function : (6) in, Current position This is the weight matrix. This is the terminal speed penalty coefficient. Additionally, in equation (6)... This represents the reference position at time k (i.e., the spatial coordinates obtained after resampling). This represents the coordinates of the endpoint of the planned path. The terminal cost function includes a position deviation penalty and a kinetic energy convergence penalty, where... v T Let be the linear velocity of the underwater robot at the final moment. w T ω is the angular velocity at the endpoint.

[0103] Considering the limitations of the physical actuator, inequality constraints can be introduced. : (7) The above constraints limit the forward and backward speeds. yaw rate r k and pitch angular velocity q k They respectively satisfy: .

[0104] As an example, the above constraints can be addressed using the Augmented Lagrangian Method to construct the augmented cost function. L σ : (8) in, For Lagrange multipliers, As a penalty factor, In other words, the penalty term does not apply when the robot is within the safe range. Once the robot violates the physical performance boundaries, the penalty term... This will incur a cost, forcing the optimization algorithm to adjust control commands to return to a safe zone.

[0105] In step S500, an initial control and state sequence that satisfies the physical performance boundaries is generated.

[0106] Specifically, under the premise of satisfying the physical performance boundaries, an initial control sequence and an initial state sequence are generated based on the deviation between the current state of the underwater robot and the full-state reference sequence.

[0107] As an example, the current state of the underwater robot can be compared with the full-state reference sequence to obtain the pose and velocity errors; the pose and velocity errors can be adjusted using multi-channel PID control to obtain virtual control force and torque; a thrust distribution model can be constructed based on the actuator configuration, and the virtual control force and torque can be mapped to the initial control commands of each actuator using physical performance boundaries as constraints to obtain the initial control sequence; the initial control sequence can be input into the underwater robot system model, and the initial state sequence can be obtained through forward integral prediction.

[0108] Specifically, since iLQR is sensitive to initial values, in order to prevent the algorithm from getting trapped in local minima or diverging, this disclosure uses a hot-start strategy based on a three-channel PID controller to generate an initial control sequence that satisfies physical constraints. and state sequence .

[0109] As an example, the 3D navigation task can be decoupled into three independent PID control channels: Forward / backward control: based on distance error .

[0110] Yaw control: based on yaw angle error .

[0111] Pitch control: based on pitch angle error .

[0112] The output of the PID controller, after saturation processing, can directly generate an initial control quantity that meets the constraints. (9) in, These represent the proportional, integral, and derivative coefficients of the longitudinal control channel, respectively. The control coefficient representing the yaw channel. These represent the control coefficients for the pitch channel. These coefficients can be obtained through offline tuning and are used to convert path tracking errors into preliminary actuator commands. There are no specific restrictions on which feedback control is sampled for each control channel. Additionally, a reference state sequence can be used as the tracking target, combined with coordinate standards and actuator distributed parameters to execute feedback control, thereby obtaining an initial control sequence and an initial state sequence that satisfy the physical performance boundaries.

[0113] The initial sequence in equation (9) can then be used to initiate the AL-iLQR optimization process.

[0114] Specific reference Figure 2 In step S600, the hot start performs iterative optimization and outputs a sequence of control commands.

[0115] Specifically, based on the underwater robot system model, the cost function can be used as the optimization objective, and the initial control sequence and initial state sequence can be used as initial values ​​for hot start to perform iterative optimization in order to obtain the control command sequence of the underwater robot.

[0116] For example, hard constraints in the physical performance boundary can be introduced into the cost function to construct an augmented cost function containing Lagrange multipliers. Using the initial control sequence and initial state sequence as initial values ​​for hot start, the augmented cost function is searched for in the descent direction under the constraints of the underwater robot system model. Based on the correction obtained from the search, the control sequence is updated and the state trajectory is forward extrapolated. Through iterative iteration, the augmented cost function is converged to a local optimum. Based on the converged optimal control variables, a control command sequence for driving the actuator is generated.

[0117] The convergence of iLQR can be determined by monitoring the decreasing slope of the augmented cost function (equation). If the cost difference between adjacent iterations is lower than the preset precision, the outer parameter update is initiated. In the outer loop, it can be detected in real time whether the current state trajectory meets the physical performance boundary constraints (i.e., the hard constraint violation is close to zero). If both the inner and outer layer determinations pass, the optimal control command sequence can be output. If the constraints are still violated, the penalty factor is increased and the inner layer optimization is restarted.

[0118] For details, please refer to Figure 4 The trajectory tracking control steps of the constrained augmented Lagrange iterative linear quadratic regulator (AL-iLQR) based on proportional-integral-derivative (PID) hot start may include steps S310, S320, S330, S340, S350, S360, S370, S380 and S390.

[0119] In step S310: PID hot start: Generate initial trajectory (x (0) ,u (0) ), and initialize λ and σ.

[0120] Specifically, based on position and attitude errors, the control quantities of each channel are calculated by equation (9). At the same time, the Lagrange multiplier λ can be initially set as a zero vector, and the penalty factor σ can be set as a small positive reference.

[0121] In step S320, the outer loop checks whether constraint convergence is satisfied, thereby verifying whether the current trajectory satisfies all physical performance boundary constraints. These physical performance boundary constraints may specifically include actuator thrust saturation constraints, servo motor deflection angle constraints, and velocity stability constraints of the robot under high-dynamic motion, etc., and can be determined using the positive part function operator. Real-time boundary checks are performed on the above constraints to ensure the final output control command sequence. It lies within the linear effective operating region of the propulsion system. This two-layer architecture ensures that the final solution is not only locally optimal but also strictly adheres to the physical limits of the actuator.

[0122] In step S330, the optimal control is output. .

[0123] When the outer loop determines that the trajectory has successfully converged and satisfies all hard constraints, the system determines the current control sequence as the optimal control command sequence. This sequence contains all control information from the start time to the end time.

[0124] In step S340, the inner loop: Determine if iLQR has converged? Specifically, the stability of the inner trajectory optimization can be determined under the Lagrange multiplier λ and the penalty factor σ. As an example, the decrease in the augmented cost function between adjacent iterations can be monitored. If the decrease in the cost function is less than the preset precision, it indicates that the optimal path has been found under the current constraint penalty level, and the result is judged as "yes," thus executing S350 to update the multiplier. If the cost function is still decreasing significantly, the result is judged as "no," and S370 is executed for dynamic linearization and quadratic optimization.

[0125] In step S350, the Lagrange multiplier λ is updated.

[0126] Once the inner layer optimization is complete, the Lagrange multipliers can be adjusted to a higher dimension based on the current constraint violation situation.

[0127] In step S360: If the constraint is violated and stagnation occurs, increase σ.

[0128] You can compare the constraint violation amount after the current inner loop with the result of the previous outer loop iteration. If the constraint violation amount decreases too slowly (i.e., stagnation occurs), it indicates that the current penalty factor strength is insufficient to constrain the nonlinear behavior of the system. In this case, the penalty factor value can be increased by a preset ratio (e.g., doubling).

[0129] Specifically, after the inner loop converges, the Lagrange multiplier λ is updated. k And the penalty factor σ: (10) If the degree of constraint violation does not decrease significantly, then increase Repeat the above process until the convergence condition is met.

[0130] Finally, the optimized control sequence It is applied to underwater robots to achieve accurate and stable tracking of reference paths.

[0131] In step S370, the dynamics are linearized and the cost function is quadraticized (including constraint terms).

[0132] Because the dynamics model of underwater robots is highly nonlinear, a linear model can be obtained by performing a first-order Taylor expansion near the current state point, and a second-order expansion can be performed on the augmented cost function, which includes reference position deviation, control energy, and constraint penalty terms. This transforms the complex nonlinear optimization problem into a locally linear quadratic (LQR) problem that is easier to solve.

[0133] Specifically, the dynamic model can be linearized near the current trajectory: (11) in, , A k and B k These represent the state transition matrix and the control input matrix, respectively. δ xk This represents the current state increment at time k.

[0134] Additionally, a second approximation can be made to the augmented cost function to calculate the Q-function matrix block ( (etc.), which includes the contribution of constraint terms: (12) In equation (12), C u Represents the constraint function vector Regarding the control input vector The Jacobian matrix, i.e. It reflects the gradient of the effect of small changes in the control input on the degree to which the constraints are satisfied.

[0135] In step S380, backward propagation: calculate the gain K. k k k .

[0136] The calculation can be performed recursively from the terminal moment, counterclockwise to the starting moment. The feedback gain matrix K is obtained by solving the Bellman equation. k and feedforward correction term k k K k Used to adjust the control direction in real time based on state deviation, while k k Pointing to the gradient direction where the cost value decreases the fastest.

[0137] As an example, the Riccati equation can be used for backpropagation to calculate the feedback gain matrix. and feedforward gain vector For details, please refer to equation (13) below.

[0138] (13) in, These represent the augmented Lagrangian function, including the constraint penalty term and Lagrange multipliers, with respect to the control input vector at the current operating point. The first-order gradient vector and the second-order Hessian matrix, where: , .

[0139] In step S390, forward pass: line search and trajectory update.

[0140] K can be calculated using S380 k and k k The deduction is re-enacted from the initial state. The control quantity is updated using the following formula (14).

[0141] (14) Where α is the line search parameter.

[0142] Reference Figure 6Compared to the messy, large-scale polyline paths generated by traditional RRT algorithms, the paths generated by KD-tree Informed RRT* are significantly smoother and more direct, indicating higher search efficiency and faster convergence speed within the Informed elliptical sampling region. In a high-density obstacle group composed of blue cubes, red spheres, and green polyhedra, the path generated by this disclosure (orange line) can accurately traverse within a compact search space, and its geometric length is significantly better than the benchmark algorithm.

[0143] Reference Figure 7 The constrained AL-iLQR controller disclosed herein has significant advantages over PID control and the hot-start-free iLQR method. Traditional PID control is prone to large tracking deviations and even collisions when facing high-density obstacles and dynamic nonlinear constraints; while the hot-start-free iLQR algorithm, lacking high-quality initial value guidance, converges slowly under complex physical boundary constraints, and the trajectory is prone to getting trapped in local extrema. In contrast, this disclosure uses a PID hot-start strategy to provide a physically feasible initial sequence for AL-iLQR, and combines augmented Lagrangian terms to accurately handle hard constraints such as thrust saturation, so that the final generated trajectory (red solid line) can fit the reference path with extremely high accuracy, while exhibiting better stability and dynamic consistency during obstacle avoidance. It effectively solves the technical problems of the benchmark method being prone to collisions and having poor robustness in complex and confined spaces.

[0144] Reference Figure 8 Furthermore, this disclosure also has significant advantages in terms of control stability and security. Figure 8 In (a), (b), and (c), the PID controller, lacking forward-looking optimization during path tracking, causes frequent triggering of physical limits in thrust and angular velocity, resulting in severe control oscillations that can easily damage the actuator. However, Figure 8 (d), (e), and (f) show that the hot-start-free optimization method completely deviates from the physical performance boundary shown by the dashed line at certain times, resulting in severe numerical divergence of control commands and algorithm failure. In contrast, the control input sequence of the method disclosed in this paper (red solid line) is the smoothest and most stable, with all commands strictly constrained within the preset physical boundary and without drastic jumps. This indicates that by combining PID hot-start with AL-iLQR two-layer iterative optimization, the risk of actuator saturation can be effectively avoided, and the motion trajectory of the underwater robot in complex environments can be ensured to be both accurate and physically feasible.

[0145] The above has been referred to Figures 1 to 8A control method for an underwater robot according to exemplary embodiments of the present disclosure is described. However, it should be understood that the devices, units, equipment, etc., shown in the drawings can be configured as software, hardware, firmware, or any combination thereof to perform specific functions. For example, these units and devices may correspond to dedicated integrated circuits, pure software code, or modules combining software and hardware. Furthermore, one or more functions implemented by these systems or devices may also be uniformly executed by components in a physical entity device (e.g., a processor, client, or server).

[0146] The instructions stored in the aforementioned computer-readable storage medium can be executed in environments deployed in computer devices such as clients, hosts, agent devices, and servers. It should be noted that the instructions can also be used to perform additional steps beyond those described above, or to perform more specific processing while executing the aforementioned steps. The details of these additional steps and further processing are already provided in the reference... Figures 1 to 8 As mentioned in the description of the relevant systems and methods, they will not be repeated here to avoid repetition.

[0147] It should be noted that the process generation method according to the exemplary embodiments of this disclosure can rely entirely on the operation of computer programs or instructions to achieve the corresponding functions. That is, each device corresponds to each step in the functional architecture of the computer program, so that the entire system is called through a special software package (e.g., a lib library) to achieve the corresponding functions.

[0148] This disclosure provides a control device for an underwater robot, which may include a memory and a processor. The memory stores programs or instructions, and when the programs or instructions are executed by the processor, they cause the processor to perform the aforementioned control method.

[0149] This disclosure provides a computer-readable storage medium that can store a program or instructions that, when executed by a processor, cause the processor to perform the control method described above.

[0150] The underwater robot control method according to the embodiments of this disclosure can achieve efficient planning and accurate and stable trajectory tracking of the underwater robot in complex three-dimensional environments.

[0151] The control method for underwater robots according to embodiments of this disclosure can effectively overcome the problem of nonlinear optimization algorithms being sensitive to initial values, significantly improve the convergence speed of the algorithm, and guide the system to avoid poor local minima.

[0152] The control method for underwater robots according to embodiments of the present disclosure can effectively avoid the problems of control command over-limit and oscillation that are prone to occur in complex environments by traditional PID control.

[0153] The underwater robot control method according to the embodiments of this disclosure can achieve accurate tracking of the reference path in complex obstacle environments. Compared with the benchmark method, its control trajectory is smoother, the tracking error is smaller, and it has stronger robustness in dealing with high dynamic motion constraints.

[0154] While specific terminology has been used to describe various embodiments of this disclosure, the specification and drawings are to be regarded as illustrative rather than restrictive in order to aid in understanding this disclosure. Various modifications and changes can be made by those skilled in the art (e.g., different features in different embodiments may be combined) without departing from the broader spirit and scope of this disclosure. Therefore, the scope of this disclosure is not limited by the specific embodiments and examples, but by the claims and their equivalents.

Claims

1. A control method for an underwater robot, characterized in that, include: Establish an underwater robot system model that includes kinematics, dynamics, and physical performance boundaries of actuators; Based on the three-dimensional working space defined by the underwater robot system model, an obstacle avoidance path planning is performed using a sampling algorithm to obtain a set of discrete geometric path points connecting the starting point and the ending point. Based on the discrete geometric path point set, a full-state reference sequence matching the underwater robot system model is generated; Based on the underwater robot system model and the full-state reference sequence, a cost function for trajectory optimization is constructed. Under the premise of satisfying the physical performance boundary, an initial control sequence and an initial state sequence are generated based on the deviation between the current state of the underwater robot and the full state reference sequence. Based on the underwater robot system model, with the cost function as the optimization objective, and the initial control sequence and initial state sequence as initial values ​​for hot start, iterative optimization is performed to obtain the control command sequence of the underwater robot.

2. The control method for an underwater robot according to claim 1, characterized in that, Obstacle avoidance path planning is performed using a sampling algorithm to obtain a set of discrete geometric path points connecting the start and end points, including: Within the three-dimensional work space, the effective sampling area is dynamically reduced based on the distribution of the generated paths and the target location, and random sampling biased towards the target is performed within the reduced effective sampling area to obtain the sampling points to be tested; Using the sample point to be tested as the retrieval target, a nearest neighbor search is performed in the path point database composed of historical path points to determine the parent node that is closest to the sample point to be tested. Establish a connecting line segment between the sampling point to be tested and the parent node, and use a collision detection algorithm to determine whether the connecting line segment meets the obstacle avoidance requirements; In response to the connection segment meeting the obstacle avoidance requirements, the sampling point to be tested is stored as a new historical path point in the path point database, and the above steps are iteratively executed until the discrete geometric path point set connecting the start and end points is obtained.

3. The control method for an underwater robot according to claim 2, characterized in that, The path point database adopts a KD-tree spatial index structure based on a dynamic maintenance mechanism. The sampled points to be tested are stored as new historical path points in the path point database, including: Set up an incremental buffer to temporarily store newly added historical path points; The number of path points in the incremental buffer is monitored in real time, and it is determined whether the preset scale threshold has been reached. In response to the failure to reach the preset scale threshold, the currently tested sampling points that have passed the obstacle avoidance verification are added to the incremental buffer. In response to reaching the preset size threshold, all path points in the incremental buffer are merged with existing nodes in the KD tree, the global KD tree is reconstructed, and the incremental buffer is cleared.

4. The control method for an underwater robot according to claim 2, characterized in that, Determining whether the connecting line segment meets obstacle avoidance requirements using a collision detection algorithm includes: The obstacles in the three-dimensional work space are represented as a set of geometric objects composed of triangular facets; Calculate the intersection points of the connecting line segment and the plane containing each triangular facet; Determine whether the intersection point is located between the two endpoints of the connecting line segment; In response to the fact that the intersection point is located on the connecting line segment, determine whether the intersection point is located inside the corresponding triangular facet; In response to the first condition that the intersection point is located on the connecting line segment and is located inside the corresponding triangular facet, it is determined that the connecting line segment does not meet the obstacle avoidance requirements; In response to the failure to meet the first condition, it is determined that the connecting line segment meets the obstacle avoidance requirements.

5. The control method for an underwater robot according to claim 4, characterized in that, Determining whether the intersection point is located inside the corresponding triangular facet includes: Obtain the coordinates of the three vertices of the corresponding triangular facet; Based on the coordinates of the three vertices, calculate the centroid coordinate coefficient of the intersection point relative to the triangle; In response to the second condition that all the centroid coordinate coefficients are greater than or equal to zero and the sum of the centroid coordinate coefficients is equal to 1, it is determined that the intersection point is located inside the corresponding triangular facet. In response to the failure to meet the second condition, it is determined that the intersection point is located outside the corresponding triangular facet.

6. The control method for an underwater robot according to claim 4, characterized in that, The obstacles in the three-dimensional work space are represented as a set of geometric objects composed of triangular facets, including: For the convex polyhedral obstacle among the obstacles, its surface is triangulated into a first set of triangular facets according to its surface topology. For the spherical obstacles among the obstacles, the spherical surface is discretized and triangulated according to a preset resolution to generate a second set of triangular facets; For the cuboid obstacle among the obstacles, its six rectangular faces are each divided into two triangles along the diagonal to form a third set of triangular facets; The first set of triangular facets, the second set of triangular facets, and the third set of triangular facets are merged to construct the set of geometric objects.

7. The control method for an underwater robot according to claim 1, characterized in that, Based on the discrete geometric path point set, a full-state reference sequence matching the underwater robot system model is generated, including: Curve fitting is performed on the discrete geometric path point set, and resampling is performed according to a preset time resolution to obtain the reference position at each time step. Based on the reference positions at adjacent moments, the linear velocity is calculated, and the reference yaw angle and reference pitch angle are determined according to the path tangent direction. The corresponding angular velocities are obtained by numerically differentiating the reference yaw angle and the reference pitch angle respectively. The full-state reference sequence is generated based on the reference position, the linear velocity, the angular velocity, the reference yaw angle, the reference pitch angle, and the preset reference roll angle.

8. The control method for an underwater robot according to claim 7, characterized in that, Under the premise of satisfying the physical performance boundaries, an initial control sequence and an initial state sequence are generated based on the deviation between the current state of the underwater robot and the full-state reference sequence, including: The current state of the underwater robot is compared with the full-state reference sequence to obtain the pose and velocity errors; The pose and velocity errors are adjusted using multi-channel PID control to obtain virtual control force and torque. A thrust allocation model is constructed based on the actuator configuration, and the physical performance boundary is used as a constraint condition to map the virtual control force and torque into the initial control commands of each actuator to obtain the initial control sequence. The initial control sequence is input into the underwater robot system model, and the initial state sequence is obtained through forward integral prediction.

9. The control method for an underwater robot according to claim 8, characterized in that, Based on the underwater robot system model, using the cost function as the optimization objective and the initial control sequence and initial state sequence as initial values ​​for hot start, iterative optimization is performed to obtain the control command sequence of the underwater robot, including: By incorporating the hard constraints in the physical performance boundary into the cost function, an augmented cost function containing Lagrange multiplier terms is constructed. Using the initial control sequence and the initial state sequence as initial values ​​for hot start, the augmented cost function is searched for a descent direction under the constraints of the underwater robot system model. Based on the correction amount obtained from the search, the control sequence is updated and the state trajectory is forward extrapolated. The augmented cost function is converged to a local optimum through iterative iteration. Based on the converged optimal control variables, the sequence of control instructions for driving the actuator is generated.

10. A control device for an underwater robot, characterized in that, The control device includes a memory and a processor. The memory stores programs or instructions that, when executed by the processor, cause the processor to perform the control method for the underwater robot according to any one of claims 1-9.

Images