Three-dimensional path tracking method for under-actuated autonomous underwater robot

Abstract

The invention discloses a three-dimensional path tracking method for an under-actuated autonomous underwater robot. The method comprises the steps of initial path planning, path optimization, robot motion model construction, control law design and real-time adjustment. The invention belongs to the technical field of underwater robots, and particularly relates to a three-dimensional path tracking method for an under-actuated autonomous underwater robot, and the method comprises the steps: representing a path as the position of a particle; in the path optimization process, the position of a producer, the position of a searcher, the position of a junkman and the position of an early warning sparrow are updated, the speed and the position of the sparrow are updated, the optimal value of the path is obtained, and the tracking performance of the robot is improved; a path tracking error is defined, a control law target is determined, a speed control law and an angular speed control law of a virtual target are designed, a parameter adaptive law is calculated, the control law is adjusted according to the state of the autonomous underwater robot, and the problem that the autonomous underwater robot deviates from a curve track is solved.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater robots, and in particular is a three-dimensional path tracking method for an underactuated autonomous underwater robot. Background Art

[0002] Unmanned underwater vehicles play an important role in seabed topography mapping, resource exploration, environmental monitoring, and marine minesweeping missions, and are important equipment for exploring and developing the ocean. Autonomous underwater robots can autonomously perform tasks underwater without the need for external cable connections, greatly improving the flexibility and range of ocean exploration. Compared with fully-driven autonomous underwater robots, underactuated autonomous underwater robots have the advantages of simple structure, low cost, and low energy consumption, but they bring complexity in control. Existing underactuated autonomous underwater robots have technical problems such as the uncertainty of autonomous underwater robot parameters and external disturbances, resulting in composite uncertain interference terms, resulting in poor tracking performance and robustness of autonomous underwater robots, and difficulty in finding the optimal path; there are drift forces and lift forces caused by time-varying ocean currents, which cause autonomous underwater robots to gradually deviate from the curved trajectory, thereby generating tracking errors. Summary of the invention

[0003] In view of the above situation, in order to overcome the defects of the prior art, the present invention provides a three-dimensional path tracking method for an under-actuated autonomous underwater robot. In view of the technical problem that the parameters of the autonomous underwater robot are uncertain and there are external disturbances, resulting in composite uncertain interference terms, leading to poor tracking performance and robust performance of the autonomous underwater robot and difficulty in finding the optimal path, path optimization is adopted, specifically, the path is represented as the position of a particle, and in the process of path optimization, the position of the producer, the position of the forager, the position of the scavenger and the position of the warning sparrow are updated, the speed and position of the sparrow are updated, and the optimal value of the path is obtained; in view of the technical problem that the drift force and lift brought by the time-varying ocean current cause the autonomous underwater robot to gradually deviate from the curved trajectory, thereby generating a tracking error, the path tracking error is defined, the control law target is determined, the speed control law and angular velocity control law of the virtual target are designed, the parameter adaptive law is calculated, and the control law is adjusted according to the state of the autonomous underwater robot.

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

[0005] Step S1: initial path planning, specifically determining the target of the autonomous underwater robot to perform the task, collecting environmental data, determining the initial path points, and smoothing the path;

[0006] Step S2: Path optimization, specifically, representing the path as the position of particles, updating the positions of producers, foragers, scavengers and warning sparrows during the path optimization process, updating the speed and position of the sparrows, and obtaining the optimal value of the path;

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

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

[0009] Step S5: real-time adjustment, specifically real-time monitoring, estimating the speed and direction of the current and adding compensation items to the control law to offset the drift effect to ensure control effectiveness.

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

[0011] Step S11: Determine the target of the autonomous underwater robot to perform the task. For example, if it is a marine resource exploration task, it is necessary to determine the scope of the exploration area and the key exploration target; if it is a submarine pipeline inspection task, it is necessary to clarify the pipeline location, direction and content to be inspected;

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

[0013] Step S13: Initial path points are determined, and a strategy for covering the target area is formulated based on mission requirements and environmental data, 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 dimension of the problem to be solved. Fewer path points will make the path more curved, which is not conducive to the tracking of the path by the autonomous underwater robot. The basis function is 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 is more in line with the kinematics and dynamics requirements of the robot can be constructed independently of the complexity of the control points. A smooth path helps to reduce sharp turns, accelerations, and decelerations during the robot's movement, thereby reducing energy consumption and improving the stability of the movement, and improving the accuracy of the path tracking. The basis function is calculated using the following formula:

[0015] ;

[0016] In the formula, represents the basis function, represents the i-th node vector, represents the i+1th node vector, u represents the node variable, k represents the recursive series of the basis function, and different k values ​​correspond to the values ​​of the basis function under different calculation rules. represents the i+k-1th node vector, represents the i+kth node vector, represents the basis function of the i-th node at the k-1th level, represents the basis function of the i+1th node at the k-1th level.

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

[0018] Step S21: The path is represented as the position of a particle. Each particle represents a possible path solution. The particle updates its speed and position according to its own optimal position and the optimal position of the group. This update mechanism enables the particle to move in a more optimal direction and gradually converge to the optimal path solution.

[0019] Step S22: Update the position of the producer. During the path optimization process, the position of the producer must be updated in each iteration. The producer is regarded as a key role in leading the search direction in path optimization. As the number of iterations increases, the position of the producer changes continuously, thereby driving the entire search process to develop in the direction of a better path. The formula used is as follows:

[0020] ;

[0021] In the formula, t represents the iteration index, represents the position of the producer after the t+1th iteration, represents the producer position after the tth iteration. Q is a variable extracted from a normal distribution. The randomness of the value introduces a certain uncertainty to the update of the producer position. Indicates the alarm value, ranging from [0, 1]. ST indicates the safety threshold, ranging from [0.5, 1]. If the alarm value is less than or equal to the safety threshold, it is a safe situation. If the alarm value is greater than the safety threshold, it is a dangerous situation.

[0022] Step S23: Update the position of the searcher. The searcher plays an auxiliary role in the entire path optimization algorithm. The position update of the searcher 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, It represents the position of the searcher at the t+1th iteration, reflecting the dynamic changes of the searcher's position at different iteration stages. represents the position of the searcher at iteration t, The optimal position occupied by the producer, represents the worst position after the tth iteration, n represents the total number of scrapers, and L represents the adjustment amplitude of the scraper position update;

[0025] Step S24: Update the position of the scavenger. When the scavenger finds a better path, the position information is fed back to the entire system, thereby guiding the producers and searchers to adjust their search strategies, thereby balancing and supplementing the entire path optimization process. The formula used is as follows:

[0026] ;

[0027] ;

[0028] In the formula, represents the position of the scavenger in the t+1th iteration, represents the position of the scavenger at the tth iteration, A represents a matrix in which each element is randomly assigned to 1 or -1, T represents the transpose of the matrix, represents 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 position and the worst position. The formula used is as follows:

[0030] ;

[0031] In the formula, represents the position of the warning sparrow at the t+1 iteration, represents the position of the warning sparrow at the tth iteration, c represents the adaptive coefficient, which increases with the number of iterations. represents the best position of the warning sparrow in the search space at the tth iteration, Indicates the amplitude used to adjust the position update of the warning sparrow. represents the fitness function value of the individual warning sparrow, Represents the global fitness function value of the warning sparrow;

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

[0033] ;

[0034] ;

[0035] In the formula, It represents the speed of the particle in the d-dimensional space at the t+1th iteration, which determines the speed and direction of the particle in the next iteration. represents the speed of the particle at the tth iteration in the dth dimensional space, c1 represents the acceleration weight of the particle flying to its own historical optimal position, c2 represents the acceleration weight of the particle flying to the global optimal position, r1 and r2 represent random numbers uniformly distributed in the interval [0, 1]. represents the position of the particle at the t+1th iteration in the d-dimensional space, Represents the position of the particle at the tth iteration in the d-dimensional space.

[0036] Furthermore, in step S3, the construction of the robot motion model includes the following steps:

[0037] Step S31: construct a kinematic model of the autonomous underwater robot, using the following formula:

[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 robot. , θ, φ are the three Euler angles between the earth coordinate system and the body coordinate system, representing the posture of the autonomous underwater robot. is the transformation matrix from the body coordinate system to the earth coordinate system, represents the velocity vector of the underwater autonomous robot relative to the water flow, Indicates that the water flow itself is equivalent to the velocity vector of the geodetic coordinate system;

[0040] Step S32: construct a dynamic model of the autonomous underwater robot, using the following formula:

[0041] ;

[0042] Where M represents the inertia matrix, represents the centripetal force matrix, represents the damping matrix, represents the restoring force matrix due to gravity and buoyancy, represents the control input vector of the autonomous underwater robot, Represents the disturbance vector composed of model parameter uncertainty and unknown environmental interference.

[0043] Furthermore, 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 a parametric equation, determine the actual position of the robot, calculate the position tracking error along the x-axis and y-axis directions, and also calculate the error along the z-axis when tracking the spatial path, and combine the position, speed and direction tracking errors into a comprehensive path tracking error vector, which reflects the deviation between the actual motion state of the underwater robot and the reference path;

[0045] Step S42: determining the control law target, under the condition of a given desired forward speed, making the body coordinate origin of the autonomous underwater robot gradually converge to the virtual target point, adjusting the movement direction and speed of the robot according to the tracking error vector design, and moving along the space path, while correcting the robot's posture control input through the direction tracking error, and keeping the combined speed of the underwater autonomous robot aligned with the tangent vector of the path;

[0046] Step S43: Design the speed control law of the virtual target point. The formula used is as follows:

[0047] ;

[0048] In the formula, It represents the final control amount of the speed control law, which determines the comprehensive adjustment amount of the autonomous underwater robot in speed control. Represents a proportional coefficient greater than zero, which is used to adjust the contribution of the amount related to the relative water velocity and the angle amount related to the attitude error to the final control amount. Represents the quantity related to the relative water velocity of the autonomous underwater robot, represents the angle quantity related to the attitude error, Represents a proportional coefficient greater than zero, which is used to adjust the influence weight of position tracking error on the control amount. It represents the position tracking error, which is used to reflect 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, the formula used is as follows:

[0050] ;

[0051] In the formula, represents the control quantity in the virtual angular velocity control law, Represents a proportional coefficient greater than zero, which is used to adjust the influence weight of the desired attitude angle related quantity in the control law and adjust the response sensitivity. Indicates the posture angle that the autonomous robot expects to achieve;

[0052] Step S45: Calculate the parameter adaptation law, the formula used is as follows:

[0053] ;

[0054] In the formula, represents the adaptive parameter, which is used to adjust the control law in real time according to the state of the autonomous underwater robot. Represents the adaptive gain, which is used to adjust the speed and amplitude of the adaptation. represents the first foresight distance, Represents the status information of the autonomous underwater robot.

[0055] Furthermore, in step S5, the real-time adjustment is specifically to perform real-time monitoring during the operation of the autonomous underwater robot. If the path tracking error increases, the control law is adjusted to perform interference compensation. By estimating the speed and direction of the current, a compensation term is added to the control law to offset the drift effect of the current on the autonomous underwater robot. If the dynamic parameters of the autonomous underwater robot change, a parameter adaptive algorithm is used to adjust the control law. According to 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 scheme are as follows:

[0057] (1) In order to solve the technical problem that the autonomous underwater robot has poor tracking performance and robust performance due to the uncertainty of its parameters and external disturbances, and that it is difficult to find 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 the producer, the forager, the scavenger, and the warning sparrow are updated, and the speed and position of the sparrow are updated to obtain the optimal value of the path.

[0058] (2) In order to solve the technical problem that the drift force and lift caused by the time-varying ocean current cause the autonomous underwater robot to gradually deviate from the curved trajectory, thereby generating tracking errors, we define the path tracking error, determine the control law target, design the speed control law and angular velocity control law of the virtual target, calculate the parameter adaptation law, and adjust the control law according to the state of the autonomous underwater robot. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0060] Figure 2 is a schematic flow chart of step S1;

[0061] Figure 3 is a schematic flow chart of step S2;

[0062] Figure 4 It is a schematic diagram of the process of step S4.

[0063] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention. DETAILED DESCRIPTION

[0064] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments; based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0065] In the description of the present invention, it should be understood that terms such as “upper”, “lower”, “front”, “back”, “left”, “right”, “top”, “bottom”, “inside” and “outside” indicating directions or positional relationships are based on the directions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and therefore should not be understood as limiting the present invention.

[0066] Example 1, see Figure 1 The present 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 target of the autonomous underwater robot to perform the task, collecting environmental data, determining the initial path points, and smoothing the path;

[0068] Step S2: Path optimization, specifically, representing the path as the position of particles, updating the positions of producers, foragers, scavengers and warning sparrows during the path optimization process, updating the speed and position of the sparrows, and obtaining the optimal value of the path;

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

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

[0071] Step S5: Real-time adjustment, specifically real-time monitoring, estimating the speed and direction of the current and adding compensation items to the control law to offset the drift effect to 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 target of the autonomous underwater robot to perform the task. For example, if it is a marine resource exploration task, it is necessary to determine the scope of the exploration area and the key exploration target; if it is a submarine pipeline inspection task, it is necessary to clarify the pipeline location, direction and content to be inspected;

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

[0075] Step S13: Initial path points are determined, and a strategy for covering the target area is formulated based on mission requirements and environmental data, 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 dimension of the problem to be solved. Fewer path points will make the path more curved, which is not conducive to the tracking of the path by the autonomous underwater robot. The basis function is 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 is more in line with the kinematics and dynamics requirements of the robot can be constructed independently of the complexity of the control points. A smooth path helps to reduce sharp turns, accelerations, and decelerations during the robot's movement, thereby reducing energy consumption and improving the stability of the movement, and improving the accuracy of the path tracking. The basis function is calculated using the following formula:

[0077] ;

[0078] In the formula, represents the basis function, represents the i-th node vector, represents the i+1th node vector, u represents the node variable, k represents the recursive series of the basis function, and different k values ​​correspond to the values ​​of the basis function under different calculation rules. represents the i+k-1th node vector, represents the i+kth node vector, represents the basis function of the i-th node at the k-1th level, represents the basis function of the i+1th node at the k-1th level.

[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: The path is represented as the position of a particle. Each particle represents a possible path solution. The particle updates its speed and position according to its own optimal position and the optimal position of the group. This update mechanism enables the particle to move in a more optimal direction and gradually converge to the optimal path solution.

[0081] Step S22: Update the position of the producer. During the path optimization process, the position of the producer must be updated in each iteration. The producer is regarded as a key role in leading the search direction in path optimization. As the number of iterations increases, the position of the producer changes continuously, thereby driving the entire search process to develop in the direction of a better path. The formula used is as follows:

[0082] ;

[0083] In the formula, t represents the iteration index, represents the position of the producer after the t+1th iteration, represents the producer position after the tth iteration. Q is a variable extracted from a normal distribution. The randomness of the value introduces a certain uncertainty to the update of the producer position. Indicates the alarm value, ranging from [0, 1]. ST indicates the safety threshold, ranging from [0.5, 1]. If the alarm value is less than or equal to the safety threshold, it is a safe situation. If the alarm value is greater than the safety threshold, it is a dangerous situation.

[0084] Step S23: Update the position of the searcher. The searcher plays an auxiliary role in the entire path optimization algorithm. The position update of the searcher 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, It represents the position of the searcher at the t+1th iteration, reflecting the dynamic changes of the searcher's position at different iteration stages. represents the position of the searcher at iteration t, The optimal position occupied by the producer, represents the worst position after the tth iteration, n represents the total number of scrapers, and L represents the adjustment amplitude of the scraper position update;

[0087] Step S24: Update the position of the scavenger. When the scavenger finds a better path, the position information is fed back to the entire system, thereby guiding the producers and searchers to adjust their search strategies, thereby balancing and supplementing the entire path optimization process. The formula used is as follows:

[0088] ;

[0089] ;

[0090] In the formula, represents the position of the scavenger in the t+1th iteration, represents the position of the scavenger at the tth iteration, A represents a matrix in which each element is randomly assigned to 1 or -1, T represents the transpose of the matrix, represents 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 position and the worst position. The formula used is as follows:

[0092] ;

[0093] In the formula, represents the position of the warning sparrow at the t+1 iteration, represents the position of the warning sparrow at the tth iteration, c represents the adaptive coefficient, which increases with the number of iterations. represents the best position of the warning sparrow in the search space at the tth iteration, Indicates the amplitude used to adjust the position update of the warning sparrow. represents the fitness function value of the individual warning sparrow, Represents the global fitness function value of the warning sparrow;

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

[0095] ;

[0096] ;

[0097] In the formula, It represents the speed of the particle in the d-dimensional space at the t+1th iteration, which determines the speed and direction of the particle in the next iteration. represents the speed of the particle at the tth iteration in the dth dimensional space, c1 represents the acceleration weight of the particle flying to its own historical optimal position, c2 represents the acceleration weight of the particle flying to the global optimal position, r1 and r2 represent random numbers uniformly distributed in the interval [0, 1]. represents the position of the particle at the t+1th iteration in the d-dimensional space, Represents the position of the particle at the tth iteration in the d-dimensional space.

[0098] By executing the above operations, path optimization is adopted, specifically, the path is represented as the position of particles, and the positions of producers, foragers, scavengers and warning sparrows are updated during the path optimization process, and the speed and position of the sparrows are updated to obtain the optimal path value. This solves the technical problem that the parameters of the autonomous underwater robot are uncertain and external disturbances cause composite uncertain interference terms, resulting in poor tracking performance and robust performance of the autonomous underwater robot 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 robot motion model is constructed, including the following steps:

[0100] Step S31: construct a kinematic model of the autonomous underwater robot, using the following formula:

[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 robot. , θ, φ are the three Euler angles between the earth coordinate system and the body coordinate system, representing the posture of the autonomous underwater robot. is the transformation matrix from the body coordinate system to the earth coordinate system, represents the velocity vector of the underwater autonomous robot relative to the water flow, Indicates that the water flow itself is equivalent to the velocity vector of the geodetic coordinate system;

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

[0104] ;

[0105] Where M represents the inertia matrix, represents the centripetal force matrix, represents the damping matrix, represents the restoring force matrix due to gravity and buoyancy, represents the control input vector of the autonomous underwater robot, Represents the disturbance vector composed of model parameter uncertainty and unknown environmental interference.

[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 a parametric equation, determine the actual position of the robot, calculate the position tracking error along the x-axis and y-axis directions, and also calculate the error along the z-axis when tracking the spatial path, and combine the position, speed and direction tracking errors into a comprehensive path tracking error vector, which reflects the deviation between the actual motion state of the underwater robot and the reference path;

[0108] Step S42: determining the control law target, under the condition of a given desired forward speed, making the body coordinate origin of the autonomous underwater robot gradually converge to the virtual target point, adjusting the movement direction and speed of the robot according to the tracking error vector design, and moving along the space path, while correcting the robot's posture control input through the direction tracking error, and keeping the combined speed of the underwater autonomous robot aligned with the tangent vector of the path;

[0109] Step S43: Design the speed control law of the virtual target point. The formula used is as follows:

[0110] ;

[0111] In the formula, It represents the final control amount of the speed control law, which determines the comprehensive adjustment amount of the autonomous underwater robot in speed control. Represents a proportional coefficient greater than zero, which is used to adjust the contribution of the amount related to the relative water velocity and the angle amount related to the attitude error to the final control amount. Represents the quantity related to the relative water velocity of the autonomous underwater robot, represents the angle quantity related to the attitude error, Represents a proportional coefficient greater than zero, which is used to adjust the influence weight of position tracking error on the control amount. It represents the position tracking error, which is used to reflect 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, the formula used is as follows:

[0113] ;

[0114] In the formula, represents the control quantity in the virtual angular velocity control law, Represents a proportional coefficient greater than zero, which is used to adjust the influence weight of the desired attitude angle related quantity in the control law and adjust the response sensitivity. Indicates the posture angle that the autonomous robot expects to achieve;

[0115] Step S45: Calculate the parameter adaptation law, the formula used is as follows:

[0116] ;

[0117] In the formula, represents the adaptive parameter, which is used to adjust the control law in real time according to the state of the autonomous underwater robot. Represents the adaptive gain, which is used to adjust the speed and amplitude of the adaptation. represents the first foresight distance, Represents the status information of the autonomous underwater robot.

[0118] By performing the above operations, the path tracking error is defined, the control law target is determined, the speed control law and angular velocity control law of the virtual target are designed, the parameter adaptive law is calculated, and the control law is adjusted according to the state of the autonomous underwater robot. The technical problem that the drift force and lift brought by the time-varying ocean current cause the autonomous underwater robot to gradually deviate from the curved trajectory, thereby generating tracking errors is solved.

[0119] Example 6, see Figure 1 This embodiment is based on the above embodiment. In step S5, the real-time adjustment is specifically to perform real-time monitoring during the operation of the autonomous underwater robot. If the path tracking error increases, the control law is adjusted to perform interference compensation. By estimating the speed and direction of the current, a compensation term is added to the control law to offset the drift effect of the current on the autonomous underwater robot. If the dynamic parameters of the autonomous underwater robot change, a parameter adaptive algorithm is used to adjust the control law. According to 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 article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0121] While the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that many changes, modifications, substitutions and variations can be made to the embodiments without departing from the principles and spirit of the invention.

[0122] The present invention and its embodiments are described above, and such description is not restrictive. The drawings show only one embodiment of the present invention, and the actual structure is not limited thereto. In short, if ordinary technicians in the field are inspired by it, without departing from the purpose of the invention, they can design a structure and embodiment similar to the technical solution without creativity, which should belong to 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 comprises the following steps: Step S1: initial path planning, specifically determining the target of the autonomous underwater robot to perform the task, collecting environmental data, determining the initial path points, and smoothing the path; Step S2: Path optimization, specifically, representing the path as the position of particles, updating the positions of producers, foragers, scavengers and warning sparrows during the path optimization process, updating the speed and position of the sparrows, and obtaining the optimal value of the path; Step S3: constructing a robot motion model, specifically constructing a kinematic model and a dynamic model of the autonomous underwater robot; Step S4: control law design, specifically defining the path tracking error, determining the control law target, designing the speed control law and angular velocity control law of the virtual target, calculating the parameter adaptive law, and adjusting the control law according to the state of the autonomous underwater robot; Step S5: real-time adjustment.

2. A three-dimensional path tracking method for an underactuated autonomous underwater robot according to claim 1, characterized in that: In step S2, the path optimization includes the following steps: Step S21: representing the path as the position of the particle; Step S22: Update the position of the producer, the formula used is as follows: ； In the formula, t represents the iteration index, represents the position of the producer after the t+1th iteration, represents the producer position after the tth iteration, Q is a variable drawn from a normal distribution, Indicates the alarm value, ST indicates the safety threshold. If the alarm value is less than or equal to the safety threshold, it is a safe situation. If the alarm value is greater than the safety threshold, it is a dangerous situation. Step S23: Update the position of the searcher, the formula used is as follows: ； In the formula, represents the position of the searcher at iteration t+1, represents the position of the searcher at iteration t, The optimal position occupied by the producer, represents the worst position after the tth iteration, n represents the total number of scrapers, and L represents the adjustment amplitude of the scraper position update; Step S24: Update the position of the scavenger, the formula used is as follows: ； ； In the formula, represents the position of the scavenger in the t+1th iteration, represents the position of the scavenger at the tth iteration, A represents a matrix in which each element is randomly assigned to 1 and -1, T represents the transpose of the matrix, represents the scavenger update matrix; Step S25: Update the position of the warning sparrow, the formula used is as follows: ； In the formula, represents the position of the warning sparrow at the t+1 iteration, represents the position of the warning sparrow at the tth iteration, c represents the adaptive coefficient, which increases with the number of iterations. represents the best position of the warning sparrow in the search space at the tth iteration, Indicates the amplitude used to adjust the position update of the warning sparrow. represents the fitness function value of the individual warning sparrow, Represents the global fitness function value of the warning sparrow; Step S26: Update the speed and position of the sparrow to obtain the optimal path value. The formula used is as follows: ； ； In the formula, It represents the speed of the particle in the d-dimensional space at the t+1th iteration, which determines the speed and direction of the particle in the next iteration. represents the speed of the particle at the tth iteration in the dth dimensional space, c1 represents the acceleration weight of the particle flying to its own historical optimal position, c2 represents the acceleration weight of the particle flying to the global optimal position, r1 and r2 represent random numbers uniformly distributed in the interval [0, 1]. represents the position of the particle at the t+1th iteration in the d-dimensional space, Represents the position of the particle at the tth iteration in the d-dimensional space.

3. A 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: defining a path tracking error; Step S42: Determine the control law target; Step S43: Design the speed control law of the virtual target point. The formula used is as follows: ； In the formula, represents the final control quantity of the speed control law, Represents a proportional coefficient greater than zero, which is used to adjust the contribution of the amount related to the relative water velocity and the angle amount related to the attitude error to the final control amount. Represents the quantity related to the relative water velocity of the autonomous underwater robot, represents the angle quantity related to the attitude error, Represents a proportional coefficient greater than zero, which is used to adjust the influence weight of position tracking error on the control amount. It represents the position tracking error, which is used to reflect 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, the formula used is as follows: ； In the formula, represents the control quantity in the virtual angular velocity control law, Represents a proportional coefficient greater than zero, which is used to adjust the influence weight of the desired attitude angle related quantity in the control law and adjust the response sensitivity. Indicates the posture angle that the autonomous robot expects to achieve; Step S45: Calculate the parameter adaptation law, the formula used is as follows: ； In the formula, represents the adaptive parameter, which is used to adjust the control law in real time according to the state of the autonomous underwater robot. Represents the adaptive gain, which is used to adjust the speed and amplitude of the adaptation. represents the first foresight distance, Represents the status information of the autonomous underwater robot.

4. A 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: determining the target of the autonomous underwater robot to perform the task; Step S12: Environmental data collection, collecting relevant data of the working environment of the autonomous underwater robot; Step S13: Initial path points are determined, and a strategy for covering the target area is formulated based on mission requirements and environmental data; Step S14: path smoothing, using a smoothing algorithm to process the path, the basis function is used to construct a smooth path, and the basis function is calculated. The formula used is as follows: ； In the formula, represents the basis function, represents the i-th node vector, represents the i+1th node vector, u represents the node variable, k represents the recursive series of the basis function, and different k values ​​correspond to the values ​​of the basis function under different calculation rules. represents the i+k-1th node vector, represents the i+kth node vector, represents the basis function of the i-th node at the k-1th level, represents the basis function of the i+1th node at the k-1th level.

5. A three-dimensional path tracking method for an underactuated autonomous underwater robot according to claim 1, characterized in that: In step S3, the robot motion model is constructed, including the following steps: Step S31: construct a kinematic model of the autonomous underwater robot, using the following formula: ； 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 robot. , θ, φ are the three Euler angles between the earth coordinate system and the body coordinate system, representing the posture of the autonomous underwater robot. is the transformation matrix from the body coordinate system to the earth coordinate system, represents the velocity vector of the underwater autonomous robot relative to the water flow, Indicates that the water flow itself is equivalent to the velocity vector of the geodetic coordinate system; Step S32: construct a dynamic model of the autonomous underwater robot, using the following formula: ； Where M represents the inertia matrix, represents the centripetal force matrix, represents the damping matrix, represents the restoring force matrix due to gravity and buoyancy, represents the control input vector of the autonomous underwater robot, represents the perturbation vector.

6. 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 is specifically to perform real-time monitoring during the operation of the autonomous underwater robot, estimate the speed and direction of the current, add compensation terms to the control law to offset the drift effect of the current on the autonomous underwater robot, use a parameter adaptive algorithm to adjust the control law, and update the parameters in the dynamic model in real time according to state estimation and control effects.

Images