CN-121995937-A - Control method and control device for underwater robot

CN121995937ACN 121995937 ACN121995937 ACN 121995937ACN-121995937-A

Abstract

The disclosure provides a control method and a control device for an underwater robot. The control method of the underwater robot comprises the steps of establishing an underwater robot system model, conducting obstacle avoidance path planning through a sampling algorithm to obtain a discrete geometric path point set of a connection starting point and an end point, generating a full-state reference sequence matched with the control method underwater robot system model based on the control method discrete geometric path point set, constructing a cost function for track 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 deviation of a current state of the control method underwater robot and the control method full-state reference sequence, and conducting iterative optimization by taking the control method cost function as an optimization target and taking the control method initial control sequence and the initial state sequence as a hot start initial value to obtain a control instruction sequence of the control method underwater robot.

Inventors

WANG JIAN
WU ZHENGXING
ZHANG ZHENTAO
Luo Yelin
ZHOU CHAO
TAN MIN

Assignees

中国科学院自动化研究所

Dates

Publication Date: 20260508
Application Date: 20260204

Claims (10)

1. A control method of an underwater robot, comprising: Establishing an underwater robot system model comprising a kinematic model, a dynamic model and physical performance boundaries of an actuator; Based on a three-dimensional operation space defined by the underwater robot system model, carrying out obstacle avoidance path planning by utilizing a sampling algorithm to obtain a discrete geometric path point set for connecting a starting point and an end point; Generating a full-state reference sequence matched with the underwater robot system model based on the discrete geometric path point set; Constructing a cost function for track optimization based on the underwater robot system model and the full-state reference sequence; On the premise of meeting the physical performance boundary, generating an initial control sequence and an initial state sequence according to the deviation of the current state of the underwater robot and the full-state reference sequence; And based on the underwater robot system model, taking the cost function as an optimization target, and taking the initial control sequence and the initial state sequence as a hot start initial value to perform iterative optimization so as to obtain a control instruction sequence of the underwater robot.
2. The method of claim 1, wherein performing obstacle avoidance path planning using a sampling algorithm to obtain a set of discrete geometric path points connecting a start point and an end point comprises: dynamically shrinking an effective sampling area in the three-dimensional operation space according to the distribution of the generated paths and the target position, and executing random sampling of a deflection target in the shrunk effective sampling area to obtain a sampling point to be detected; taking the sampling point to be detected as a retrieval target, and executing nearest neighbor search in a path point database formed by historical path points to determine a father node closest to the sampling point to be detected; establishing a connecting line segment between the sampling point to be detected and the father node, and judging whether the connecting line segment meets the obstacle avoidance requirement by utilizing a collision detection algorithm; and in response to the connection line segment meeting the obstacle avoidance requirement, storing the sampling point to be detected as a new historical path point into the path point database, and iteratively executing the steps until a discrete geometric path point set of a connection starting point and a connection end point is obtained.
3. The method for controlling an underwater robot according to claim 2, wherein, The path point database adopts a KD tree space index structure based on a dynamic maintenance mechanism, and the sampling point to be detected is used as a new historical path point to be stored in the path point database, and the method comprises the following steps: setting an increment buffer zone for temporarily storing newly added history path points; Monitoring the number of path points in the increment buffer zone in real time, and judging whether a preset scale threshold is reached or not; in response to the preset scale threshold being not reached, adding the current sampling point to be tested passing the obstacle avoidance check to the increment buffer zone; And in response to the preset scale threshold, merging all path points in the increment buffer area with the existing nodes in the KD tree, reconstructing a global KD tree and emptying the increment buffer area.
4. The control method of an underwater robot according to claim 2, wherein determining whether the connecting line segment meets an obstacle avoidance requirement by using a collision detection algorithm comprises: representing an obstacle in the three-dimensional working space as a set of geometric objects composed of triangular patches; calculating the intersection point of the connecting line segment and the plane where each triangular patch is located; Judging whether the intersection point is positioned between two endpoints of the connecting line segment; Judging whether the intersection point is positioned in the corresponding triangular patch or not according to the fact that the intersection point is positioned on the connecting line segment; Responding to a first condition that the intersection point is positioned on the connecting line segment and positioned in the corresponding triangular patch, and determining that the connecting line segment does not meet the obstacle avoidance requirement; And in response to the first condition being not met, determining that the connecting line segment meets the obstacle avoidance requirement.
5. The method of claim 4, wherein determining whether the intersection point is located inside the corresponding triangular patch comprises: acquiring three vertex coordinates of a corresponding triangular patch; Calculating the barycentric 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 patch in response to a second condition that the barycentric coordinate coefficients are all greater than or equal to zero and the sum of the barycentric coordinate coefficients is equal to 1 being satisfied; In response to not satisfying the second condition, it is determined that the intersection point is located outside of the corresponding triangular patch.
6. The method of controlling an underwater robot according to claim 4, wherein representing the obstacle in the three-dimensional work space as a set of geometric objects constituted by triangular patches, comprises: Aiming at the convex polyhedron obstacle in the obstacle, triangulating the surface of the obstacle into a first triangular patch set according to the surface topological structure of the obstacle; Aiming at a sphere obstacle in the obstacle, discretizing triangulation is carried out on the sphere of the sphere obstacle according to a preset resolution, and a second triangular patch set is generated; for cuboid barriers in the barriers, dividing six rectangular faces of the cuboid barriers into two triangles along diagonal lines respectively to form a third triangular patch set; And merging the first triangular patch set, the second triangular patch set and the third triangular patch set to construct the geometric object set.
7. The method of claim 1, wherein generating a full state reference sequence that matches the model of the underwater robot system based on the set of discrete geometric path points comprises: Curve fitting is carried out on the discrete geometric path point set, resampling is carried out according to the preset time resolution, and the reference position of each moment is obtained; calculating linear speed based on the reference positions at adjacent moments, and determining a reference yaw angle and a reference pitch angle according to the tangential direction of the path; Respectively carrying out numerical differentiation on the reference yaw angle and the reference pitch angle to obtain corresponding angular speeds; And generating the full-state reference sequence based on the reference position, the linear speed, the angular speed, the reference yaw angle, the reference pitch angle and a preset reference roll angle.
8. The method according to claim 7, wherein generating an initial control sequence and an initial state sequence based on a deviation of a current state of the underwater robot from the full state reference sequence on the premise that the physical property boundary is satisfied, comprises: Comparing the current state of the underwater robot with the full-state reference sequence to obtain pose and speed errors; performing multi-channel PID (proportion integration differentiation) adjustment on the pose and the speed error to obtain virtual control force and moment; Constructing a thrust distribution model based on the configuration of the executors, and mapping the virtual control force and the moment into initial control instructions of the executors by taking the physical performance boundary as a constraint condition so as to obtain the initial control sequence; And inputting the initial control sequence into the underwater robot system model, and obtaining the initial state sequence through forward integral prediction.
9. The method according to claim 8, wherein performing iterative optimization with the initial control sequence and the initial state sequence as initial values for hot start based on the model of the underwater robot system to obtain a control instruction sequence of the underwater robot includes: Introducing hard constraints in the physical performance boundary into the cost function, and constructing an augmented cost function containing Lagrangian multiplier sub-terms; Taking the initial control sequence and the initial state sequence as a hot start initial value, and under the constraint of the underwater robot system model, searching the descending direction of the augmented cost function; According to the correction amount obtained by searching, updating a control sequence and forward deduction of a state track are carried out, and the augmented cost function is converged to local optimum through loop iteration; and generating the control instruction sequence for driving the actuator based on the converged optimal control variable.
10. A control device of an underwater robot, characterized in that the control device comprises a memory and a processor, the memory storing a program or instructions which, when executed by the processor, cause the processor to perform the control method of an underwater robot according to any of claims 1-9.

Description

Control method and control device for underwater robot Technical Field The disclosure relates to the technical field of underwater robots, in particular to a control method and a control device of an underwater robot. Background In recent years, with the deep development of the ocean, autonomous underwater vehicles (Autonomous Underwater Vehicle, AUV) play an increasingly important role in the tasks of deep sea resource exploration, environment durable monitoring, submarine pipeline cable maintenance and the like. AUV motion planning methods are mainly divided into graph search methods, sampling-based planning methods and optimization-based planning methods. The graph search method (such as a and its variants) searches the optimal path through the discretized working space, but in the large-scale three-dimensional space, as the dimension increases, the computation complexity increases exponentially, and it is difficult to meet the real-time requirement. Optimization-based methods (e.g., model predictive control), while capable of handling kinetic constraints, are prone to trapping local minima and are sensitive to initial values. Sampling-based algorithms (e.g., RRT) avoid explicit spatial discretization by randomly sampling in a continuous configuration space and are thus widely used in high-dimensional space planning. However, the conventional RRT algorithm converges to a high quality solution in a complex environment at a slow speed, and as the number of nodes increases, nearest neighbor search (Nearest Neighbor Search) becomes a main computational bottleneck, which severely limits its planning efficiency in a narrow channel or obstacle-dense environment. In addition, the existing collision detection method is often designed aiming at a single shape, and is difficult to efficiently and uniformly treat common heterogeneous obstacles such as spheres, cuboids and convex polyhedrons in an underwater environment. At the motion control level, the conventional control method is usually decoupled from the global planning layer, the advantage of second-order trajectory optimization is difficult to use in a control loop, and the Iterative Linear Quadratic Regulator (iLQR) is difficult to apply to the underwater robot. The existing single planning or control method is difficult to simultaneously consider the calculation efficiency, the geometric obstacle avoidance capability and the dynamic constraint processing capability. Disclosure of Invention One of the purposes of the present disclosure is to provide a control method capable of realizing efficient planning and accurate and stable trajectory tracking of an underwater robot in a complex three-dimensional environment. According to the first aspect of the disclosure, the control method of the underwater robot comprises the steps of establishing an underwater robot system model comprising a kinematic model, a dynamic model and a physical performance boundary of an actuator, carrying out obstacle avoidance path planning by utilizing a sampling algorithm based on a three-dimensional operation space defined by the underwater robot system model to obtain a discrete geometric path point set of a connection starting point and an end point, generating a full-state reference sequence matched with the underwater robot system model based on the discrete geometric path point set, constructing a cost function for track optimization based on the underwater robot system model and the full-state reference sequence, generating an initial control sequence and an initial state sequence according to the deviation of the current state of the underwater robot and the full-state reference sequence on the premise of meeting the physical performance boundary, and carrying out iterative optimization based on the underwater robot system model by taking the initial control sequence and the initial state sequence as a hot start initial value to obtain a control instruction sequence of the underwater robot. According to the embodiment of the disclosure, a sampling algorithm is utilized to conduct obstacle avoidance path planning to obtain a discrete geometric path point set of a connection starting point and an end point, and the method can comprise the steps of dynamically shrinking an effective sampling area in a three-dimensional working space according to distribution and target positions of generated paths, executing random sampling of a deflection target in the shrunk effective sampling area to obtain sampling points to be detected, taking the sampling points to be detected as retrieval targets, executing nearest neighbor searching in a path point database formed by historical path points to determine father nodes closest to the sampling points to be detected, establishing a connection line segment between the sampling points to be detected and the father nodes, judging whether the connection line segment meets obstacle avoidance requirements by utilizing a collision detection algorithm