A 3D path tracking method for underactuated autonomous underwater robots

Abstract

This invention discloses a three-dimensional path tracking method for underactuated autonomous underwater robots (AUVs). The method includes: initial path planning, path optimization, constructing a robot motion model, control law design, and real-time adjustment. This invention belongs to the field of underwater robot technology, specifically a three-dimensional path tracking method for underactuated AUVs. The path optimization in this scheme specifically represents the path as particle positions. During path optimization, the positions of producers, scavengers, waste collectors, and warning sparrows are updated, along with the speed and position of the sparrows, to obtain the optimal path value and improve robot tracking performance. The method defines the path tracking error, determines the control law target, designs the velocity and angular velocity control laws for virtual targets, calculates the parameter adaptive law, and adjusts the control law according to the state of the AUV to solve the problem of the AUV deviating from its curved trajectory.

Description

Technical Field

[0001] This invention belongs to the field of underwater robot technology, specifically a three-dimensional path tracking method for underactuated autonomous underwater robots. Background Technology

[0002] Unmanned underwater vehicles (UUVs) play a crucial role in seabed topographic mapping, resource exploration, environmental monitoring, and marine minesweeping, serving as essential equipment for ocean exploration and development. Autonomous underwater robots (AUVs) can perform tasks autonomously underwater without external cable connections, significantly improving the flexibility and scope of ocean exploration. Compared to fully driven AUVs, underactuated AUVs offer advantages such as simpler structure, lower cost, and lower energy consumption, but they introduce control complexity. Existing underactuated AUVs suffer from several technical problems: uncertain parameters and external disturbances create complex uncertainties, leading to poor tracking performance and robustness, making it difficult to find the optimal path; drift forces and lift from time-varying ocean currents cause the AUV to gradually deviate from its trajectory, resulting in tracking errors. Summary of the Invention

[0003] To address the above issues and overcome the shortcomings of existing technologies, this invention provides a three-dimensional path tracking method for underactuated autonomous underwater vehicles (AUVs). Addressing the technical problem of uncertain AUV parameters and external disturbances causing complex uncertainties that result in poor tracking performance and robustness, making it difficult to find the optimal path, this invention employs path optimization. Specifically, the path is represented as particle positions. During path optimization, the positions of producers, scavengers, waste collectors, and warning sparrows are updated, along with the sparrows' speed and position, to obtain the optimal path value. Furthermore, addressing the technical problem of drift forces and lift from time-varying ocean currents causing AUVs to gradually deviate from their curved trajectories and generate tracking errors, this invention defines the path tracking error, determines the control law objective, designs the velocity and angular velocity control laws for the virtual target, calculates the parameter adaptive law, and adjusts the control law according to the AUV's state.

[0004] The technical solution adopted by this invention is as follows: This invention provides a three-dimensional path tracking method for underactuated autonomous underwater robots, the method comprising the following steps:

[0005] Step S1: Initial path planning, specifically, determining the objective of the autonomous underwater robot's task, collecting environmental data, determining initial path points, and smoothing the path.

[0006] Step S2: Path optimization, specifically, the path is represented as the position of particles. During the path optimization process, the positions of producers, scavengers, and early warning sparrows are updated, and the speed and position of the sparrows are updated to obtain the optimal path value.

[0007] Step S3: Construct the robot motion model, specifically the kinematic and dynamic models of the autonomous underwater robot;

[0008] Step S4: Control law design, specifically defining the path tracking error, determining the control law target, designing the velocity control law and angular velocity control law for the virtual target, calculating the parameter adaptive law, and implementing the control law adjustment according to the state of the autonomous underwater robot;

[0009] Step S5: Real-time adjustment, specifically real-time monitoring, estimating ocean current speed and direction, and adding a compensation term to the control law to counteract drift effects and ensure control effectiveness.

[0010] Further, in step S1, the initial path planning includes the following steps:

[0011] Step S11: Determine the objective of the autonomous underwater robot's mission. For example, if it is a marine resource exploration mission, it is necessary to determine the scope of the exploration area and the key exploration targets; if it is a subsea pipeline inspection mission, it is necessary to clarify the pipeline's location, direction, and the content to be inspected.

[0012] Step S12: Environmental data collection, collecting relevant data on the working environment of the autonomous underwater robot, including ocean topography data, ocean current data, and marine life distribution data;

[0013] Step S13: Initial waypoint determination. Based on task requirements and environmental data, formulate a strategy to cover the target area, taking into account the safety and operability of the autonomous underwater robot.

[0014] Step S14: Path smoothing. A smoothing algorithm is used to process the path. The number of path points determines the dimensionality of the problem. Fewer path points will make the path more curved, which is not conducive to the autonomous underwater robot's path tracking. Basis functions are used to construct a smooth path. If the number of basis functions is not affected by the number of control points, a smooth path that better meets the requirements of robot kinematics and dynamics can be constructed independently of the complexity of control points. A smooth path helps to reduce abrupt turns, accelerations, and decelerations during robot movement, thereby reducing energy consumption and improving motion stability and path tracking accuracy. The basis functions are calculated using the following formula:

[0015] ;

[0016] In the formula, Denotes basis functions. This represents the vector of the i-th node. Let represent the vector of the (i+1)th node, u represent the node variable, and k represent the recursive series of the basis functions. Different values ​​of k correspond to the values ​​of the basis functions under different calculation rules. This represents the vector of the (i+k-1)th node. This represents the vector of the (i+k)th node. This represents the basis function at the i-th node in the (k-1)-th level. This represents the basis function at the (k-1)th level and the (i+1)th node.

[0017] Further, in step S2, the path optimization includes the following steps:

[0018] Step S21: Represent the path as the position of the particles. Each particle represents a possible path solution. The particles update their velocity and position based on their own optimal position and the optimal position of the group. This update mechanism enables the particles to move in a better direction and gradually converge to the optimal path solution.

[0019] Step S22: Update the producer's position. During path optimization, the producer's position must be updated in each iteration. The producer is considered a key player in guiding the search direction. As the number of iterations increases, the producer's position changes continuously, thereby driving the entire search process towards a better path. The formula used is as follows:

[0020] ;

[0021] In the formula, t represents the iteration number index. This indicates the position of the producer after the (t+1)th iteration. Let Q represent the producer's position after the t-th iteration. Q is a variable extracted from a normal distribution, and its randomness introduces some uncertainty into the update of the producer's position. The alarm value is represented by [0, 1], and ST represents the safety threshold, which is represented by [0.5, 1]. An alarm value less than or equal to the safety threshold indicates a safe situation, while an alarm value greater than the safety threshold indicates a dangerous situation.

[0022] Step S23: Update the location of the scraper. The scraper plays an auxiliary role in the entire path optimization algorithm. Updating the scraper's location will affect the coverage of the entire search space and the approximation of the optimal path. The formula used is as follows:

[0023] ;

[0024] In the formula, This indicates the position of the scavenger at iteration t+1, reflecting the dynamic change of the scavenger's position at different iteration stages. This indicates the position of the scavenger at iteration t. The optimal position occupied by the producer. Let represent the worst position after the t-th iteration, n represent the total number of looters, and L represent the adjustment range for looter position updates;

[0025] Step S24: Update the scavenger's location. When a scavenger discovers a better path, the location information is fed back to the entire system, thereby guiding producers and scavengers to adjust their search strategies. This plays a balancing and supplementing role in the entire path optimization process. The formula used is as follows:

[0026] ;

[0027] ;

[0028] In the formula, This indicates the position of the scavenger in the (t+1)th iteration. Let A represent the position of the scavenger in the t-th iteration, A represent a matrix where each element is randomly assigned a value of 1 or -1, and T represent the transpose of the matrix. This indicates the scavenger update matrix;

[0029] Step S25: Update the position of the warning sparrow. When the warning sparrow is in the current best position, we let it randomly jump to a random position between the best and worst positions. The formula used is as follows:

[0030] ;

[0031] In the formula, This indicates the position of the warning sparrow at iteration t+1. Let represent the position of the warning sparrow at iteration t, and c represent the adaptive coefficient, which increases with the number of iterations. This indicates the optimal position of the sparrow in the search space at the t-th iteration. This indicates the magnitude used to adjust the update of the sparrow's location in the early warning system. This represents the fitness function value of the early warning sparrow. This represents the global fitness function value of the early warning sparrow;

[0032] Step S26: Execute the PSO algorithm to update the sparrow's speed and position, and obtain the optimal path value. The formula used is as follows:

[0033] ;

[0034] ;

[0035] In the formula, This represents the velocity of the particle in the (t+1)th iteration in the d-th space, which determines the speed and direction of the particle's movement in the next iteration. Let represent the velocity of the particle in the t-th iteration of the d-th space, c1 represent the acceleration weight for the particle to fly towards its own historical best position, c2 represent the acceleration weight for the particle to fly towards the global best position, and r1 and r2 represent random numbers uniformly distributed in the interval [0, 1]. This represents the position of the particle in the (t+1)th iteration in the d-th dimension. This represents the position of the particle in the t-th iteration of the d-th dimension.

[0036] Further, in step S3, constructing the robot motion model includes the following steps:

[0037] Step S31: Construct the kinematic model of the autonomous underwater robot, using the following formulas:

[0038] ;

[0039] In the formula, Where x, y, and z are the coordinates of the body coordinate system in the geodetic coordinate system, representing the global position of the autonomous underwater vehicle. θ and φ are three Euler angles between the geodetic coordinate system and the body coordinate system, representing the attitude of the autonomous underwater vehicle. This is the transformation matrix from the body coordinate system to the geodetic coordinate system. This represents the velocity vector of the underwater autonomous robot relative to the water flow. This represents the velocity vector of the water flow itself, which is equivalent to the Earth's coordinate system.

[0040] Step S32: Construct the dynamic model of the autonomous underwater robot, using the following formulas:

[0041] ;

[0042] In the formula, M represents the inertia matrix. Represents the centripetal force matrix. Represents the damping matrix. The matrix representing the restoring forces generated by gravity and buoyancy. This represents the control input vector of an autonomous underwater vehicle. It represents the disturbance vector composed of uncertain model parameters and unknown environmental disturbances.

[0043] Further, in step S4, the control law design includes the following steps:

[0044] Step S41: Define the path tracking error, clarify the reference path of the underwater robot and express it with parametric equations, determine the actual position of the robot, calculate the position tracking error along the x-axis and y-axis, and when tracking the spatial path, it is also necessary to calculate the error along the z-axis. Combine the position, velocity and direction tracking errors into a comprehensive path tracking error vector. The error vector reflects the deviation between the actual motion state of the underwater robot and the reference path.

[0045] Step S42: Determine the control law target. Given the desired forward velocity, gradually converge the origin of the autonomous underwater robot's body coordinates to the virtual target point. Design and adjust the robot's motion direction and velocity according to the tracking error vector, and move along the spatial path. At the same time, correct the robot's attitude control input through the direction tracking error to keep the resultant velocity of the underwater autonomous robot aligned with the tangent vector of the path.

[0046] Step S43: Design the velocity control law for the virtual target point, using the following formula:

[0047] ;

[0048] In the formula, The final control variable, representing the velocity control law, determines the overall adjustment amount for the autonomous underwater robot in terms of velocity control. This represents a proportional gain greater than zero, used to adjust the contribution of quantities related to relative water flow velocity and angular quantities related to attitude error to the final control quantity. This represents a quantity related to the relative water flow velocity of an autonomous underwater vehicle. This represents an angular quantity related to attitude error. This represents a proportional gain greater than zero, used to adjust the weight of the position tracking error on the control quantity. This represents the position tracking error, which reflects the deviation between the actual position of the autonomous underwater robot and the expected position on the reference path.

[0049] Step S44: Calculate the angular velocity control law, using the following formula:

[0050] ;

[0051] In the formula, This represents the control quantity in the virtual angular velocity control law. This represents a proportional gain greater than zero, used to adjust the weight of the desired attitude angle related quantities in the control law, and to adjust the response sensitivity. This indicates the desired pose angle that the autonomous robot expects to achieve.

[0052] Step S45: Calculate the adaptive law of parameters, using the following formula:

[0053] ;

[0054] In the formula, These represent adaptive parameters, used to adjust the control law in real time based on the state of the autonomous underwater robot. This represents the adaptive gain, used to adjust the speed and magnitude of the adaptive process. Indicates the first forward sight distance. This indicates the status information of the autonomous underwater vehicle.

[0055] Furthermore, in step S5, the real-time adjustment specifically involves real-time monitoring during the operation of the autonomous underwater vehicle (AUV). If the path tracking error increases, the control law is adjusted to compensate for the disturbance. By estimating the ocean current speed and direction, a compensation term is added to the control law to counteract the drift effect of the ocean current on the AUV. If the dynamic parameters of the AUV change, a parameter adaptive algorithm is used to adjust the control law. Based on the state estimation and control effect, the parameters in the dynamic model are updated in real time to ensure the effectiveness of the control.

[0056] The beneficial results achieved by the present invention using the above solution are as follows:

[0057] (1) To address the technical problem that autonomous underwater robots have poor tracking performance and robustness due to uncertain parameters and external disturbances, resulting in complex uncertain interference terms and difficulty in finding the optimal path, path optimization is adopted. Specifically, the path is represented as the position of particles. During the path optimization process, the positions of producers, scavengers, scavengers and warning sparrows are updated, and the speed and position of the sparrows are updated to obtain the optimal path value.

[0058] (2) To address the technical problem that the autonomous underwater vehicle gradually deviates from the curved trajectory due to the drift force and lift caused by the time-varying ocean current, resulting in tracking error, the following approach is adopted: define the path tracking error, determine the control law target, design the velocity control law and angular velocity control law of the virtual target, calculate the parameter adaptive law, and implement the adjustment control law according to the state of the autonomous underwater vehicle. Attached Figure Description

[0059] Figure 1 A flowchart illustrating a three-dimensional path tracking method for an underactuated autonomous underwater robot provided by the present invention;

[0060] Figure 2 This is a flowchart illustrating step S1;

[0061] Figure 3 This is a flowchart illustrating step S2;

[0062] Figure 4 This is a flowchart illustrating step S4.

[0063] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof. Detailed Implementation

[0064] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0065] In the description of this invention, it should be understood that the terms "upper", "lower", "front", "rear", "left", "right", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0066] Example 1, see Figure 1 This invention provides a three-dimensional path tracking method for an underactuated autonomous underwater robot, the method comprising the following steps:

[0067] Step S1: Initial path planning, specifically, determining the objective of the autonomous underwater robot's task, collecting environmental data, determining initial path points, and smoothing the path.

[0068] Step S2: Path optimization, specifically, the path is represented as the position of particles. During the path optimization process, the positions of producers, scavengers, and early warning sparrows are updated, and the speed and position of the sparrows are updated to obtain the optimal path value.

[0069] Step S3: Construct the robot motion model, specifically the kinematic and dynamic models of the autonomous underwater robot;

[0070] Step S4: Control law design, specifically defining the path tracking error, determining the control law target, designing the velocity control law and angular velocity control law for the virtual target, calculating the parameter adaptive law, and implementing the control law adjustment according to the state of the autonomous underwater robot;

[0071] Step S5: Real-time adjustment, specifically real-time monitoring, estimating ocean current speed and direction, and adding a compensation term to the control law to counteract drift effects and ensure control effectiveness.

[0072] Example 2, see Figure 1 and Figure 2 This embodiment is based on the above embodiment. In step S1, the initial path planning includes the following steps:

[0073] Step S11: Determine the objective of the autonomous underwater robot's mission. For example, if it is a marine resource exploration mission, it is necessary to determine the scope of the exploration area and the key exploration targets; if it is a subsea pipeline inspection mission, it is necessary to clarify the pipeline's location, direction, and the content to be inspected.

[0074] Step S12: Environmental data collection, collecting relevant data on the working environment of the autonomous underwater robot, including ocean topography data, ocean current data, and marine life distribution data;

[0075] Step S13: Initial waypoint determination. Based on task requirements and environmental data, formulate a strategy to cover the target area, taking into account the safety and operability of the autonomous underwater robot.

[0076] Step S14: Path smoothing. A smoothing algorithm is used to process the path. The number of path points determines the dimensionality of the problem. Fewer path points will make the path more curved, which is not conducive to the autonomous underwater robot's path tracking. Basis functions are used to construct a smooth path. If the number of basis functions is not affected by the number of control points, a smooth path that better meets the requirements of robot kinematics and dynamics can be constructed independently of the complexity of control points. A smooth path helps to reduce abrupt turns, accelerations, and decelerations during robot motion, thereby reducing energy consumption and improving motion stability and path tracking accuracy. The basis functions are calculated using the following formula:

[0077] ;

[0078] In the formula, Denotes basis functions. This represents the vector of the i-th node. Let represent the vector of the (i+1)th node, u represent the node variable, and k represent the recursive series of the basis functions. Different values ​​of k correspond to the values ​​of the basis functions under different calculation rules. This represents the vector of the (i+k-1)th node. This represents the vector of the (i+k)th node. This represents the basis function at the i-th node in the (k-1)-th level. This represents the basis function at the (k-1)th level and the (i+1)th node.

[0079] Example 3, see Figure 1 and Figure 3 This embodiment is based on the above embodiment. In step S2, the path optimization includes the following steps:

[0080] Step S21: Represent the path as the position of the particles. Each particle represents a possible path solution. The particles update their velocity and position based on their own optimal position and the optimal position of the group. This update mechanism enables the particles to move in a better direction and gradually converge to the optimal path solution.

[0081] Step S22: Update the producer's position. During path optimization, the producer's position must be updated in each iteration. The producer is considered a key player in guiding the search direction. As the number of iterations increases, the producer's position changes continuously, thereby driving the entire search process towards a better path. The formula used is as follows:

[0082] ;

[0083] In the formula, t represents the iteration number index. This indicates the position of the producer after the (t+1)th iteration. Let Q represent the producer's position after the t-th iteration. Q is a variable extracted from a normal distribution, and its randomness introduces some uncertainty into the update of the producer's position. The alarm value is represented by [0, 1], and ST represents the safety threshold, which is represented by [0.5, 1]. An alarm value less than or equal to the safety threshold indicates a safe situation, while an alarm value greater than the safety threshold indicates a dangerous situation.

[0084] Step S23: Update the location of the scraper. The scraper plays an auxiliary role in the entire path optimization algorithm. Updating the scraper's location will affect the coverage of the entire search space and the approximation of the optimal path. The formula used is as follows:

[0085] ;

[0086] In the formula, This indicates the position of the scavenger at iteration t+1, reflecting the dynamic change of the scavenger's position at different iteration stages. This indicates the position of the scavenger at iteration t. The optimal position occupied by the producer. Let represent the worst position after the t-th iteration, n represent the total number of looters, and L represent the adjustment range for looter position updates;

[0087] Step S24: Update the scavenger's location. When a scavenger discovers a better path, the location information is fed back to the entire system, thereby guiding producers and scavengers to adjust their search strategies. This plays a balancing and supplementing role in the entire path optimization process. The formula used is as follows:

[0088] ;

[0089] ;

[0090] In the formula, This indicates the position of the scavenger in the (t+1)th iteration. Let A represent the position of the scavenger in the t-th iteration, A represent a matrix where each element is randomly assigned a value of 1 or -1, and T represent the transpose of the matrix. This indicates the scavenger update matrix;

[0091] Step S25: Update the position of the warning sparrow. When the warning sparrow is in the current best position, we let it randomly jump to a random position between the best and worst positions. The formula used is as follows:

[0092] ;

[0093] In the formula, This indicates the position of the warning sparrow at iteration t+1. Let represent the position of the warning sparrow at iteration t, and c represent the adaptive coefficient, which increases with the number of iterations. This indicates the optimal position of the sparrow in the search space at the t-th iteration. This indicates the magnitude used to adjust the update of the sparrow's location in the early warning system. This represents the fitness function value of the early warning sparrow. This represents the global fitness function value of the early warning sparrow;

[0094] Step S26: Execute the PSO algorithm to update the sparrow's speed and position, and obtain the optimal path value. The formula used is as follows:

[0095] ;

[0096] ;

[0097] In the formula, This represents the velocity of the particle in the (t+1)th iteration in the d-th space, which determines the speed and direction of the particle's movement in the next iteration. Let represent the velocity of the particle in the t-th iteration of the d-th space, c1 represent the acceleration weight for the particle to fly towards its own historical best position, c2 represent the acceleration weight for the particle to fly towards the global best position, and r1 and r2 represent random numbers uniformly distributed in the interval [0, 1]. This represents the position of the particle in the (t+1)th iteration in the d-th dimension. This represents the position of the particle in the t-th iteration of the d-th dimension.

[0098] By performing the above operations and using path optimization, specifically by representing the path as the position of particles, the positions of producers, scavengers, waste collectors, and early warning sparrows are updated during the path optimization process. The speed and position of the sparrows are also updated to obtain the optimal path value. This solves the technical problem that the uncertainty of the parameters of autonomous underwater robots and external disturbances cause complex uncertainties, resulting in poor tracking performance and robustness of autonomous underwater robots and difficulty in finding the optimal path.

[0099] Example 4, see Figure 1 This embodiment is based on the above embodiment. In step S3, the construction of the robot motion model includes the following steps:

[0100] Step S31: Construct the kinematic model of the autonomous underwater robot, using the following formulas:

[0101] ;

[0102] In the formula, Where x, y, and z are the coordinates of the body coordinate system in the geodetic coordinate system, representing the global position of the autonomous underwater vehicle. θ and φ are three Euler angles between the geodetic coordinate system and the body coordinate system, representing the attitude of the autonomous underwater vehicle. This is the transformation matrix from the body coordinate system to the geodetic coordinate system. This represents the velocity vector of the underwater autonomous robot relative to the water flow. This represents the velocity vector of the water flow itself, which is equivalent to the Earth's coordinate system.

[0103] Step S32: Construct a dynamic model of the autonomous underwater robot, using the following formulas:

[0104] ;

[0105] In the formula, M represents the inertia matrix. Represents the centripetal force matrix. Represents the damping matrix. The matrix representing the restoring forces generated by gravity and buoyancy. This represents the control input vector of an autonomous underwater vehicle. It represents the disturbance vector composed of uncertain model parameters and unknown environmental disturbances.

[0106] Example 5, see Figure 1 and Figure 4 This embodiment is based on the above embodiment. In step S4, the control law design includes the following steps:

[0107] Step S41: Define the path tracking error, clarify the reference path of the underwater robot and express it with parametric equations, determine the actual position of the robot, calculate the position tracking error along the x-axis and y-axis, and when tracking the spatial path, it is also necessary to calculate the error along the z-axis. Combine the position, velocity and direction tracking errors into a comprehensive path tracking error vector. The error vector reflects the deviation between the actual motion state of the underwater robot and the reference path.

[0108] Step S42: Determine the control law target. Given the desired forward velocity, gradually converge the origin of the autonomous underwater robot's body coordinates to the virtual target point. Design and adjust the robot's motion direction and velocity according to the tracking error vector, and move along the spatial path. At the same time, correct the robot's attitude control input through the direction tracking error to keep the resultant velocity of the underwater autonomous robot aligned with the tangent vector of the path.

[0109] Step S43: Design the velocity control law for the virtual target point, using the following formula:

[0110] ;

[0111] In the formula, The final control variable, representing the velocity control law, determines the overall adjustment amount for the autonomous underwater robot in terms of velocity control. This represents a proportional gain greater than zero, used to adjust the contribution of quantities related to relative water flow velocity and angular quantities related to attitude error to the final control quantity. This represents a quantity related to the relative water flow velocity of an autonomous underwater vehicle. This represents an angular quantity related to attitude error. This represents a proportional gain greater than zero, used to adjust the weight of the position tracking error on the control quantity. This represents the position tracking error, which reflects the deviation between the actual position of the autonomous underwater robot and the expected position on the reference path.

[0112] Step S44: Calculate the angular velocity control law, using the following formula:

[0113] ;

[0114] In the formula, This represents the control quantity in the virtual angular velocity control law. This represents a proportional gain greater than zero, used to adjust the weight of the desired attitude angle related quantities in the control law, and to adjust the response sensitivity. This indicates the desired pose angle that the autonomous robot expects to achieve.

[0115] Step S45: Calculate the adaptive law of parameters, using the following formula:

[0116] ;

[0117] In the formula, These represent adaptive parameters, used to adjust the control law in real time based on the state of the autonomous underwater robot. This represents the adaptive gain, used to adjust the speed and magnitude of the adaptive process. Indicates the first forward sight distance. This indicates the status information of the autonomous underwater vehicle.

[0118] By performing the above operations, defining the path tracking error, determining the control law target, designing the velocity and angular velocity control laws for the virtual target, calculating the parameter adaptive law, and implementing the control law adjustment according to the state of the autonomous underwater vehicle, the technical problem of drift force and lift caused by time-varying ocean currents, which cause the autonomous underwater vehicle to gradually deviate from the curved trajectory and thus generate tracking error, is solved.

[0119] Example 6, see Figure 1 This embodiment is based on the above embodiment. In step S5, the real-time adjustment specifically involves real-time monitoring during the operation of the autonomous underwater vehicle (AUV). If the path tracking error increases, the control law is adjusted to compensate for the interference. By estimating the ocean current speed and direction, a compensation term is added to the control law to counteract the drift effect of the ocean current on the AUV. If the dynamic parameters of the AUV change, a parameter adaptive algorithm is used to adjust the control law. Based on the state estimation and control effect, the parameters in the dynamic model are updated in real time to ensure the effectiveness of the control.

[0120] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0121] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention.

[0122] The present invention and its embodiments have been described above. This description is not restrictive, and the accompanying drawings are only one embodiment of the present invention; the actual structure is not limited thereto. In conclusion, if those skilled in the art are inspired by this description and design similar structures and embodiments without departing from the spirit of the invention, such designs should fall within the protection scope of the present invention.

Claims

1. A three-dimensional path tracking method for an underactuated autonomous underwater robot, characterized in that: The method includes the following steps: Step S1: Initial path planning, specifically, determining the objective of the autonomous underwater robot's task, collecting environmental data, determining initial path points, and smoothing the path. Step S2: Path optimization, specifically, the path is represented as the position of particles. During the path optimization process, the positions of producers, scavengers, and early warning sparrows are updated, and the speed and position of the sparrows are updated to obtain the optimal path value. Step S3: Construct the robot motion model, specifically the kinematic and dynamic models of the autonomous underwater robot; Step S4: Control law design, specifically defining the path tracking error, determining the control law target, designing the velocity control law and angular velocity control law for the virtual target, calculating the parameter adaptive law, and implementing the control law adjustment according to the state of the autonomous underwater robot; Step S5: Real-time adjustment; The path optimization includes the following steps: Step S21: Represent the path as the position of the particle; Step S22: Update the producer's position using the following formula: ； In the formula, t represents the iteration number index. This indicates the position of the producer after the (t+1)th iteration. Let Q represent the producer position after the t-th iteration. Q is a variable extracted from a normal distribution, and the randomness of its value introduces a certain degree of uncertainty into the update of the producer position. This indicates the alarm value, and ST indicates the safety threshold. An alarm value less than or equal to the safety threshold indicates a safe situation, while an alarm value greater than the safety threshold indicates a dangerous situation. Step S23: Update the looter's location using the following formula: ； In the formula, This indicates the position of the scavenger at iteration t+1, reflecting the dynamic change of the scavenger's position at different iteration stages. This indicates the position of the scavenger at iteration t. The optimal position occupied by the producer. Let represent the worst position after the t-th iteration, n represent the total number of looters, and L represent the adjustment range for looter position updates; Step S24: Update the scavenger's location using the following formula: ； ； In the formula, This indicates the position of the scavenger in the (t+1)th iteration. Let A represent the position of the scavenger in the t-th iteration, A represent a matrix where each element is randomly assigned 1 and -1, and T represent the transpose of the matrix. This indicates the scavenger update matrix; Step S25: Update the location of the warning sparrow, using the following formula: ； In the formula, This indicates the position of the warning sparrow at iteration t+1. Let represent the position of the warning sparrow at iteration t, and c represent the adaptive coefficient, which increases with the number of iterations. This indicates the optimal position of the sparrow in the search space at the t-th iteration. This indicates the magnitude used to adjust the update of the sparrow's location in the early warning system. This represents the fitness function value of the early warning sparrow. This represents the global fitness function value of the early warning sparrow; Step S26: Update the sparrow's speed and position to obtain the optimal path value, using the following formula: ； ； In the formula, This represents the velocity of the particle in the (t+1)th iteration in the d-th space, which determines the speed and direction of the particle's movement in the next iteration. Let represent the velocity of the particle in the t-th iteration of the d-th space, c1 represent the acceleration weight for the particle to fly towards its own historical best position, c2 represent the acceleration weight for the particle to fly towards the global best position, and r1 and r2 represent random numbers uniformly distributed in the interval [0, 1]. This represents the position of the particle in the (t+1)th iteration in the d-th dimension. This represents the position of the particle in the t-th iteration of the d-th dimension.

2. The three-dimensional path tracking method for an underactuated autonomous underwater robot according to claim 1, characterized in that: In step S4, the control law design includes the following steps: Step S41: Define the path tracking error; Step S42: Determine the control law objective; Step S43: Design the velocity control law for the virtual target point, using the following formula: ； In the formula, This represents the final control variable of the speed control law. This represents a proportional gain greater than zero, used to adjust the contribution of quantities related to relative water flow velocity and angular quantities related to attitude error to the final control quantity. This represents a quantity related to the relative water flow velocity of an autonomous underwater vehicle. This represents an angular quantity related to attitude error. This represents a proportional gain greater than zero, used to adjust the weight of the position tracking error on the control quantity. This represents the position tracking error, which reflects the deviation between the actual position of the autonomous underwater robot and the expected position on the reference path. Step S44: Calculate the angular velocity control law, using the following formula: ； In the formula, This represents the control quantity in the virtual angular velocity control law. This represents a proportional gain greater than zero, used to adjust the weight of the desired attitude angle related quantities in the control law, and to adjust the response sensitivity. This indicates the desired pose angle that the autonomous robot expects to achieve. Step S45: Calculate the adaptive law of parameters, using the following formula: ； In the formula, These represent adaptive parameters, used to adjust the control law in real time based on the state of the autonomous underwater robot. This represents the adaptive gain, used to adjust the speed and magnitude of the adaptive process. Indicates the first forward sight distance. This indicates the status information of the autonomous underwater vehicle.

3. The three-dimensional path tracking method for an underactuated autonomous underwater robot according to claim 1, characterized in that: In step S1, the initial path planning includes the following steps: Step S11: Determine the objective of the autonomous underwater robot's mission; Step S12: Environmental data collection, collecting relevant data on the working environment of the autonomous underwater robot; Step S13: Determine the initial waypoints and formulate a strategy to cover the target area based on task requirements and environmental data; Step S14: Path smoothing. A smoothing algorithm is used to process the path. Basis functions are used to construct smooth paths. The formulas used to calculate the basis functions are as follows: ； In the formula, Denotes basis functions. This represents the vector of the i-th node. Let represent the vector of the (i+1)th node, u represent the node variable, and k represent the recursive series of the basis functions. Different values ​​of k correspond to the values ​​of the basis functions under different calculation rules. This represents the vector of the (i+k-1)th node. This represents the vector of the (i+k)th node. This represents the basis function at the i-th node in the (k-1)-th level. This represents the basis function at the (k-1)th level and the (i+1)th node.

4. A three-dimensional path tracking method for an underactuated autonomous underwater robot according to claim 1, characterized in that: In step S3, constructing the robot motion model includes the following steps: Step S31: Construct the kinematic model of the autonomous underwater robot, using the following formulas: ； In the formula, Where x, y, and z are the coordinates of the body coordinate system in the geodetic coordinate system, representing the global position of the autonomous underwater vehicle. θ and φ are three Euler angles between the geodetic coordinate system and the body coordinate system, representing the attitude of the autonomous underwater vehicle. This is the transformation matrix from the body coordinate system to the geodetic coordinate system. This represents the velocity vector of the underwater autonomous robot relative to the water flow. This represents the velocity vector of the water flow itself, which is equivalent to the Earth's coordinate system. Step S32: Construct the dynamic model of the autonomous underwater robot, using the following formulas: ； In the formula, M represents the inertia matrix. Represents the centripetal force matrix. Represents the damping matrix. The matrix representing the restoring forces generated by gravity and buoyancy. This represents the control input vector of an autonomous underwater vehicle. This represents the perturbation vector.

5. A three-dimensional path tracking method for an underactuated autonomous underwater robot according to claim 1, characterized in that: In step S5, the real-time adjustment specifically involves real-time monitoring during the operation of the autonomous underwater vehicle (AUV). By estimating the ocean current speed and direction, a compensation term is added to the control law to counteract the drift effect of the ocean current on the AUV. A parameter adaptive algorithm is used to adjust the control law, and the parameters in the dynamic model are updated in real time based on the state estimation and control effect.

Images