A collision avoidance path tracking control method, system, device and medium

Abstract

The present application relates to the field of unmanned ship control, and provides a collision avoidance path tracking control method, system, device and medium, comprising calculating a forward distance, calculating a heading error according to the forward distance, and performing heading correction; calculating a measurement step length according to a curvature change rate, calculating a predicted path error using the measurement step length, and obtaining optimal parameters according to the predicted path error to obtain optimized navigation parameters; calculating a collision avoidance radius according to a dangerous radius, constructing an unmanned ship obstacle avoidance constraint according to the collision avoidance radius, and obtaining an obstacle avoidance route according to the unmanned ship obstacle avoidance constraint; constructing an optimization vector using artificial bee colony algorithm parameters, optimizing the optimization vector using an optimization objective function, obtaining optimized algorithm parameters, and constructing an optimized artificial bee colony algorithm; calculating a dynamic weight coefficient and an adaptive scaling factor, solving the optimal control instruction in the optimized artificial bee colony algorithm, and finally manipulating the unmanned ship through the optimal control instruction and the heading correction data.

Description

Technical Field

[0001] This invention relates to the field of unmanned vessel control technology, and in particular to a collision avoidance path tracking control method, system, device and medium. Background Technology

[0002] The use of unmanned platforms in marine surveying and mapping operations can ensure personnel safety while successfully acquiring high-precision seabed topographic data in areas previously inaccessible. Therefore, unmanned platforms demonstrate broad application prospects in the field of marine surveying and mapping.

[0003] As a typical underactuated system, the path tracking accuracy of unmanned surface platforms directly affects the reliability of marine mapping data. In actual marine operations, unmanned platforms often face multiple time-varying disturbances such as wind, waves, and currents, posing a continuous challenge to their control stability. Traditional control methods, such as proportional-derivative-integral (PDI), sliding mode control, backstepping control, and model predictive control, perform reasonably well in calm waters or simple paths, but when applied to curved or continuously turning mapping paths, especially under variable sea conditions, they often struggle to balance accuracy and robustness. For example, while commonly used sliding mode control has strong disturbance rejection capabilities, it is prone to high-frequency chattering, affecting sensor data quality. The PDI method, after parameter tuning, has weak adaptability to dynamic environments and often requires online adjustments. Model predictive control, due to its ability to systematically handle multivariate constraints and multi-objective optimization, has received considerable attention in unmanned platform path tracking in recent years. This method, through rolling optimization and feedback correction, is theoretically more adaptable to complex environments. However, if the model parameters fail to be updated dynamically with the environment, the algorithm is prone to getting trapped in local optima and will exhibit tracking lag in sudden winds or surges.

[0004] Chinese invention patent CN121028814A, entitled "Path tracking method, device, medium and product based on dynamic prediction step length", provides a method for heading tracking, but does not provide a method for real-time obstacle avoidance, and it cannot dynamically adjust the prediction step length according to the curvature of the flight path and the rate of change of curvature when the measured step length is obtained. Summary of the Invention

[0005] This invention aims to at least solve one of the technical problems existing in related technologies. To this end, this invention provides a collision avoidance path tracking control method, system, device, and medium, which realizes the planning of the unmanned surface vessel's (USV) route and the generation of corresponding control commands, enabling the USV to accurately track the route and avoid obstacles.

[0006] This invention provides a collision avoidance path tracking control method, comprising:

[0007] S1: Obtain the unmanned surface vessel's navigation parameters, calculate the azimuth error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the azimuth error, calculate the heading error based on the forward sight distance, and obtain the heading correction data based on the heading error;

[0008] S2: Calculate the rate of change of curvature, calculate the measurement step size based on the rate of change of curvature, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters;

[0009] S3: Determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the unmanned surface vessel (USV), construct the USV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the USV, and the obstacle radius, and obtain the obstacle avoidance route based on the USV obstacle avoidance constraint;

[0010] S4: Obtain the parameters of the artificial bee colony algorithm, construct an optimization vector using the artificial bee colony algorithm parameters, construct an optimization objective function, use the optimization objective function to optimize the optimization vector, obtain the optimization algorithm parameters, and use the optimization algorithm parameters to construct an optimized artificial bee colony algorithm;

[0011] S5: Determine the number of iterations, calculate the dynamic weight coefficient and adaptive scaling factor based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficient and adaptive scaling factor, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the unmanned surface vessel through the optimal control command and heading correction data.

[0012] According to the collision avoidance path tracking control method provided by the present invention, step S1 further includes:

[0013] S11: Obtain the unmanned surface vessel's (USV) navigation parameters, including its current speed and current position, and use its current position to calculate the azimuth error, including lateral and longitudinal errors.

[0014] S12: Determine the length of the unmanned surface vessel (USV), calculate the forward sight distance using the USV length, the bearing error, and the current speed of the USV, calculate the sliding mode virtual input based on the bearing error, calculate the heading error using the sliding mode virtual input and the forward sight distance, and obtain heading correction data based on the heading error.

[0015] According to the collision avoidance path tracking control method provided by the present invention, step S2 further includes:

[0016] S21: Determine the path curvature, calculate the rate of change of curvature based on the path curvature, and calculate the measurement step size based on the rate of change of curvature;

[0017] S22: Construct a discrete displacement expression, and predict the path error using the discrete displacement expression and the measurement step size to obtain the predicted path error;

[0018] S23: Construct a path error optimization function, substitute the predicted path error into the path error optimization function, and solve the path error optimization function to obtain the optimized navigation parameters.

[0019] According to the collision avoidance path tracking control method provided by the present invention, in step S2, the measurement step size The calculation method is as follows:

[0020]

[0021] in, For the minimum step size, For the maximum step size, For path curvature, This represents the maximum path curvature. To find the differential, let t be time. For longitudinal error, For lateral error, The first adjustment factor is... This is the second adjustment factor. The coefficient of curvature variation. This is the tracking error coefficient.

[0022] According to the collision avoidance path tracking control method provided by the present invention, step S3 further includes:

[0023] S31: Select a target obstacle, measure the danger radius of the target obstacle, determine the minimum turning radius of the unmanned surface vessel (USV), calculate the collision avoidance radius based on the minimum turning radius and the danger radius, and construct the first obstacle avoidance constraint of the USV based on the collision avoidance radius;

[0024] S32: Measure the radius of the obstacle, calculate the predicted position of the unmanned surface vessel (USV) including the predicted position coordinates of the lateral direction and the predicted position coordinates of the longitudinal direction, and construct the second obstacle avoidance constraint of the USV using the predicted position of the USV and the radius of the obstacle. The obstacle avoidance constraint of the USV includes the first obstacle avoidance constraint of the USV and the second obstacle avoidance constraint of the USV.

[0025] S33: Adjust the predicted route to obtain a predicted route that conforms to the obstacle avoidance constraints of the unmanned surface vessel as the obstacle avoidance route.

[0026] According to the collision avoidance path tracking control method provided by the present invention, step S4 further includes:

[0027] S41: Select an artificial bee colony algorithm, and select artificial bee colony algorithm parameters including a cost function weighting matrix, a control input weighting matrix, a first curvature sensitive parameter, and a second curvature sensitive parameter from the artificial bee colony algorithm, and construct an optimization vector through the artificial bee colony algorithm parameters;

[0028] S42: Determine the optimization regular vector, construct the optimization objective function through the optimization regular vector, substitute the optimization vector into the optimization objective function, and use the optimization objective function to optimize the optimization vector to obtain the optimization algorithm parameters;

[0029] S43: Substitute the optimization algorithm parameters into the artificial bee colony algorithm to complete the construction of the optimized artificial bee colony algorithm.

[0030] According to a collision avoidance path tracking control method provided by the present invention, in step S5, when the optimized artificial bee colony algorithm solves for the optimal control command through the optimized navigation parameters and obstacle avoidance route, it determines a transition probability threshold such that the state transition probability of the optimized artificial bee colony algorithm is higher than the transition probability threshold.

[0031] The present invention also provides a collision avoidance path tracking control system, comprising:

[0032] Heading correction module: Used to acquire the unmanned surface vessel's navigation parameters, calculate the bearing error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the bearing error, calculate the heading error based on the forward sight distance, and obtain heading correction data based on the heading error;

[0033] The optimized navigation parameters module is used to calculate the rate of curvature change, calculate the measurement step size based on the rate of curvature change, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters.

[0034] Obstacle avoidance path module: used to determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the UAV, construct the UAV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the UAV and the obstacle radius, and obtain the obstacle avoidance path based on the UAV obstacle avoidance constraint;

[0035] Bee colony algorithm optimization module: Used to obtain artificial bee colony algorithm parameters, construct optimization vectors using artificial bee colony algorithm parameters, construct optimization objective function, use optimization objective function to optimize optimization vectors to obtain optimization algorithm parameters, and use optimization algorithm parameters to construct optimized artificial bee colony algorithm;

[0036] Unmanned surface vessel (USV) control module: Used to determine the number of iterations, calculate dynamic weight coefficients and adaptive scaling factors based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficients and adaptive scaling factors, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the USV through the optimal control command and heading correction data.

[0037] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of a collision avoidance path tracking control method as described above.

[0038] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of a collision avoidance path tracking control method as described above.

[0039] The above-described one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0040] This invention provides a collision avoidance path tracking control method, system, device, and medium. It calculates the heading error of an unmanned surface vessel (USV) using dynamic forward-looking distance to correct the heading error, calculates the dynamic measurement step size based on the curvature of the flight path, thereby achieving higher accuracy and a better fit between prediction and reality in error prediction, and then calculates navigation parameters based on this. By constructing obstacle avoidance constraints for the USV, it can avoid obstacles in a timely manner. Finally, it constructs an optimized artificial bee colony algorithm that balances tracking accuracy, control strength, and computational efficiency to obtain the optimal control command.

[0041] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0042] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0043] Figure 1 This is a flowchart illustrating a collision avoidance path tracking control method provided by the present invention.

[0044] Figure 2 This is a schematic diagram of the longitudinal error of the heading tracking in a collision avoidance path tracking control method provided by the present invention.

[0045] Figure 3 This is a schematic diagram of the lateral error in the heading tracking of a collision avoidance path tracking control method provided by the present invention;

[0046] Figure 4 This is a schematic diagram of the obstacle avoidance path of the collision avoidance path tracking control method provided by the present invention;

[0047] Figure 5 This is a schematic diagram of the collision avoidance path tracking control system provided by the present invention.

[0048] Figure 6 This is a schematic diagram of the structure of a collision avoidance path tracking control device provided by the present invention.

[0049] Figure label:

[0050] 100. Course correction module; 200. Optimized navigation parameters module; 300. Obstacle avoidance route module; 400. Swarm algorithm optimization module; 500. Unmanned surface vessel control module; 810. Processor; 820. Communication interface; 830. Memory; 840. Communication bus. Detailed Implementation

[0051] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention. The following embodiments are used to illustrate this invention but cannot be used to limit the scope of this invention.

[0052] In the description of the embodiments of the present invention, it should be noted that the terms "first", "second" and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0053] In the description of the embodiments of the present invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "connected" and "linked" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium. Those skilled in the art can understand the specific meaning of the above terms in the embodiments of the present invention based on the specific circumstances.

[0054] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0055] The following is combined with Figures 1 to 6 Specific embodiments of the present invention are described below. Figure 1 A flowchart illustrating a collision avoidance path tracking control method provided by the present invention includes:

[0056] S1: Obtain the unmanned surface vessel's navigation parameters, calculate the azimuth error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the azimuth error, calculate the heading error based on the forward sight distance, and obtain the heading correction data based on the heading error;

[0057] Furthermore, the objective of this stage is to calculate the bearing error, thereby calculating the forward sight distance and heading error, and to perform heading correction. Specifically, step S1 further includes:

[0058] S11: Obtain the unmanned surface vessel's (USV) navigation parameters, including its current speed and current position, and use its current position to calculate the azimuth error, including lateral and longitudinal errors.

[0059] S12: Determine the length of the unmanned surface vessel (USV), calculate the forward sight distance using the USV length, the bearing error, and the current speed of the USV, calculate the sliding mode virtual input based on the bearing error, calculate the heading error using the sliding mode virtual input and the forward sight distance, and obtain heading correction data based on the heading error.

[0060] The specific implementation method for the above steps in this embodiment is as follows:

[0061] First, it is necessary to obtain the unmanned surface vessel's (USV) current speed U and current position navigation parameters. Then, the path tangent angle is calculated using the USV's current position. :

[0062]

[0063] in, Represents the arctangent function in the four quadrants. Let represent the derivative of the y-axis coordinate of the unmanned surface vessel at time t when it is in the ideal position. This represents the derivative of the x-axis coordinate when the unmanned surface vessel is in its ideal position at time t. In this embodiment, the y-axis is horizontal and the x-axis is vertical. This allows us to calculate the orientation error.

[0064]

[0065] in, For longitudinal error, For lateral error, Let x be the x-axis coordinate of the unmanned surface vessel at time t in its navigation parameters. Let be the y-coordinate of the unmanned surface vessel at time t in its navigation parameters. Let x represent the x-coordinate of the unmanned surface vessel at time t when it is in the ideal position. This represents the y-coordinate of the unmanned surface vessel at time t when it is in the ideal position.

[0066] Then, the length L of the unmanned surface vessel (USV) is determined. This allows for the calculation of the forward-looking distance. :

[0067]

[0068] in, The minimum forward sight distance is preset. The first forward sight distance coefficient is preset. The second forward sight distance coefficient is preset. The third forward sight distance coefficient is preset. This refers to the heading angle error during the previous voyage. It represents the multiplication sign.

[0069] Then calculate the sliding mode virtual input. :

[0070]

[0071] Where sign() represents the sign function, The preset first sliding mode input coefficient, The preset second sliding mode input coefficient, The current heading angle of the unmanned surface vessel. The reference heading angle is calculated as follows:

[0072]

[0073] Where β is the current sideslip angle of the unmanned surface vessel.

[0074] This allows us to calculate the heading error:

[0075]

[0076] in, For parameters of uncertain terms in kinematics, Let be the derivative of the path tangent angle with respect to time. For longitudinal heading error, This represents the lateral heading error. Once the heading error is obtained, heading correction data can be derived from it to reduce the heading error and correct the heading of the unmanned surface vessel.

[0077] Here, to demonstrate the stability of the heading correction calculation, a Lyapunov function is constructed. :

[0078]

[0079] Treating the heading error as the derivative of the azimuth error, substituting it into the expressions for the heading error and the sliding mode virtual input, and differentiating the Lyapunov function, we obtain the derivative of the Lyapunov function. That is:

[0080]

[0081] because The value is in the range [-1, 1], and when When the value is small, It can be approximated as Since it is a linear function, scaling the derivative of the Lyapunov function yields:

[0082]

[0083] Determine the first Lyapunov coefficient that is greater than 0. , making Then we have:

[0084]

[0085] Choose an appropriate first Lyapunov coefficient such that Then the above formula can be further written as:

[0086]

[0087]

[0088] in, The second Lyapunov coefficient, since There is a lower realm, therefore The system is globally stable.

[0089] S2: Calculate the rate of change of curvature, calculate the measurement step size based on the rate of change of curvature, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters;

[0090] Furthermore, the objective of this stage is to calculate the measurement step size, thereby calculating the predicted path error and obtaining optimized navigation parameters. Specifically, step S2 further includes:

[0091] S21: Determine the path curvature, calculate the rate of change of curvature based on the path curvature, and calculate the measurement step size based on the rate of change of curvature;

[0092] S22: Construct a discrete displacement expression, and predict the path error using the discrete displacement expression and the measurement step size to obtain the predicted path error;

[0093] S23: Construct a path error optimization function, substitute the predicted path error into the path error optimization function, and solve the path error optimization function to obtain the optimized navigation parameters.

[0094] In step S2, the measurement step size The calculation method is as follows:

[0095]

[0096] in, For the minimum step size, For the maximum step size, For path curvature, This represents the maximum path curvature. To find the differential, let t be time. For longitudinal error, For lateral error, The first adjustment factor is... This is the second adjustment factor. The coefficient of curvature variation. This is the tracking error coefficient.

[0097] The specific implementation method for the above steps in this embodiment is as follows:

[0098] First, the path curvature needs to be determined based on the initial route planned for the unmanned surface vessel. This allows us to determine the current rate of change of curvature of the flight path based on the path curvature, and thus calculate the measurement step size. :

[0099]

[0100] in, For the minimum step size, For the maximum step size, This represents the maximum curvature of the path, which is the maximum curvature along the flight path. To find the differential, let t be time. The first adjustment factor is... This is the second adjustment factor. The coefficient of curvature variation. To track the error coefficient, the measurement step size can be adjusted based on the path curvature and the rate of change of curvature. When the rate of change of curvature is large, the measurement step size can be increased appropriately to achieve more accurate path tracking when the curvature changes frequently. When the maximum value of the path curvature and the orientation error are large, the measurement step size can be reduced to avoid causing large cumulative errors.

[0101] Then, the discrete displacement expression is constructed:

[0102]

[0103] in, Let be the longitudinal displacement at time k+1. Let be the lateral displacement at time k+1. Let the heading angle be at time k+1. The velocity at time k+1 Let be the lateral velocity at time k+1. Let be the roll angular velocity at time k+1. The rudder angle at time k+1 Let k be the longitudinal displacement at time k. Let k be the lateral displacement at time k. Let k be the heading angle at time k. Let k be the velocity at time k. Let k be the lateral velocity. Let k be the roll angular velocity at time k. Let k be the rudder angle. The interval between two moments. The value of the change in longitudinal displacement at a given moment. This represents the change in lateral displacement at a given moment. The change in heading angle at a given moment. The change in velocity at a given moment. The change in lateral velocity at a given moment. The change in roll angular velocity at a given moment. This represents the change in rudder angle at a given moment.

[0104] The changes of each parameter at a given moment are:

[0105]

[0106] in, Let k be the change in heading angle at time k. For the quality of unmanned surface vessels, Adding lateral mass to the unmanned surface vessel. Add longitudinal mass to the unmanned surface vessel. For the longitudinal viscous hydrodynamics at time k, For the transverse viscous hydrodynamics at time k, Let k be the longitudinal propeller force at time k. Let k be the transverse propeller force. Let k be the longitudinal rudder force. Let k be the lateral rudder force. The longitudinal disturbance estimate at time k is... Let k be the estimated value of the lateral disturbance at time k. Let be the moment of inertia of the unmanned surface vessel about its vertical axis. To add a moment of inertia, For viscous hydrodynamics at time k, Let K be the propeller force at time k. Let k be the rudder force at time k. The disturbance estimate at time k is... For servo control gain, The servo time constant, Let be the command rudder angle at time k.

[0107] This approach uses the lateral displacement error as the primary error to be controlled. By using the expression for lateral displacement in the discrete displacement formula and based on the measurement step size, the path error, represented by the lateral displacement, is predicted. Here, the lateral displacement at each time step first needs to be obtained:

[0108]

[0109] in, for The lateral displacement at time k+1 onwards The expression for the lateral displacement at each moment follows the same pattern. When calculating the lateral displacement at each moment, in the expression for the change in lateral displacement, k represents the moment preceding the predicted lateral displacement. This is done by working from k+1 to... The lateral displacement at time step 1 yields the predicted lateral error, which can be used as the predicted path error.

[0110]

[0111] in, Let be the lateral error of the prediction at time k+1. for The lateral error of the prediction at time k+1 onwards The expression for the lateral error of the prediction at each time step is similar. Let k+1 be the theoretical lateral displacement that the unmanned surface vessel should have on its course. for The theoretical lateral displacement that the unmanned surface vessel should have on its course at time k+1 is calculated from time k+1 onwards. The theoretical lateral displacement that the unmanned surface vessel should have on the course at each moment can be expressed in the same way.

[0112] Finally, the path error optimization function is constructed. And substitute the predicted path error into:

[0113]

[0114] in, Let P be the matrix consisting of the prediction lateral errors from time k to time k+j, where P is a pre-defined weight matrix and T denotes the transpose. As the rudder angle, This is the minimum rudder angle. This is the maximum rudder angle. This means that within the range of the maximum and minimum rudder angle values, the rudder angle is adjusted to minimize the path error optimization function. Solving this path error optimization function allows the obtained rudder angle values ​​at each time point to be used as optimized navigation parameters.

[0115] S3: Determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the unmanned surface vessel (USV), construct the USV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the USV, and the obstacle radius, and obtain the obstacle avoidance route based on the USV obstacle avoidance constraint;

[0116] Furthermore, the objective of this stage is to calculate the collision avoidance radius, thereby constructing the obstacle avoidance constraints for the unmanned surface vessel (USV), and obtaining the obstacle avoidance route based on these constraints. Specifically, step S3 further includes:

[0117] S31: Select a target obstacle, measure the danger radius of the target obstacle, determine the minimum turning radius of the unmanned surface vessel (USV), calculate the collision avoidance radius based on the minimum turning radius and the danger radius, and construct the first obstacle avoidance constraint of the USV based on the collision avoidance radius;

[0118] S32: Measure the radius of the obstacle, calculate the predicted position of the unmanned surface vessel (USV) including the predicted position coordinates of the lateral direction and the predicted position coordinates of the longitudinal direction, and construct the second obstacle avoidance constraint of the USV using the predicted position of the USV and the radius of the obstacle. The obstacle avoidance constraint of the USV includes the first obstacle avoidance constraint of the USV and the second obstacle avoidance constraint of the USV.

[0119] S33: Adjust the predicted route to obtain a predicted route that conforms to the obstacle avoidance constraints of the unmanned surface vessel as the obstacle avoidance route.

[0120] The specific implementation method for the above steps in this embodiment is as follows:

[0121] First, obstacles along the planned route of the unmanned surface vessel (USV) need to be identified as target obstacles. Then, the danger radius of each target obstacle needs to be measured. The danger radius is the radius of the obstacle itself plus the necessary safety distance. The minimum turning radius of the unmanned surface vessel also needs to be determined. This allows us to calculate the collision avoidance radius. :

[0122]

[0123] Where D represents the average turning radius of the unmanned surface vessel (USV) on the predetermined route, and max() represents taking the maximum value within the parentheses. Thus, the first obstacle avoidance constraint of the USV can be constructed based on the collision avoidance radius:

[0124]

[0125] in, The x-coordinate of the center of the target obstacle. The y-coordinate is the center of the target obstacle.

[0126] Then the radius of the obstacle is measured, that is, the radius of the m-th target obstacle. Subsequently, based on the lateral displacement coordinates Y obtained at each moment during the process of obtaining the predicted path error in step S2, and the longitudinal displacement coordinates X obtained at each moment using a similar method, these can be used as the predicted position of the unmanned surface vessel (USV). This allows the construction of the second obstacle avoidance constraint for the USV.

[0127]

[0128] The unmanned surface vessel's (USV) course and future position are adjusted based on obstacle avoidance constraints, including the first and second constraints, until the course conforms to the USV's obstacle avoidance constraints; this course can then be used as an obstacle avoidance course. Once the obstacle avoidance course is generated, the optimized navigation parameters also need to be changed as the course changes.

[0129] S4: Obtain the parameters of the artificial bee colony algorithm, construct an optimization vector using the artificial bee colony algorithm parameters, construct an optimization objective function, use the optimization objective function to optimize the optimization vector, obtain the optimization algorithm parameters, and use the optimization algorithm parameters to construct an optimized artificial bee colony algorithm;

[0130] Furthermore, the objective of this stage is to optimize the optimization vector to obtain the optimization algorithm parameters, thereby constructing an optimized artificial bee colony algorithm. Specifically, step S4 further includes:

[0131] S41: Select an artificial bee colony algorithm, and select artificial bee colony algorithm parameters including a cost function weighting matrix, a control input weighting matrix, a first curvature sensitive parameter, and a second curvature sensitive parameter from the artificial bee colony algorithm, and construct an optimization vector through the artificial bee colony algorithm parameters;

[0132] S42: Determine the optimization regular vector, construct the optimization objective function through the optimization regular vector, substitute the optimization vector into the optimization objective function, and use the optimization objective function to optimize the optimization vector to obtain the optimization algorithm parameters;

[0133] S43: Substitute the optimization algorithm parameters into the artificial bee colony algorithm to complete the construction of the optimized artificial bee colony algorithm.

[0134] The specific implementation method for the above steps in this embodiment is as follows:

[0135] Here, we first select an existing artificial bee colony algorithm, and then select from the artificial bee colony algorithms a weighted matrix Q for the cost function and a weighted matrix for the control input. First curvature sensitive parameter Second curvature sensitive parameter The parameters of the artificial bee colony algorithm can be used to construct an optimization vector. :

[0136]

[0137] Here, vec() represents a vectorized function.

[0138] Subsequently, the optimization of the regular vector was determined. This allows us to construct the optimization objective function. :

[0139]

[0140] in, Let represent the matrix consisting of the rudder angles from the k-th time to the (k+j-th time). This means adjusting the optimization vector to minimize the objective function. This represents the optimization objective coefficients. The goal is to ensure that the values ​​of each parameter in the optimization vector lie between preset maximum and minimum values, while guaranteeing the physical feasibility and system stability of the artificial bee colony algorithm. Solving the objective function yields the values ​​in the resulting optimization vector, which are then used as parameters for the optimization algorithm. Substituting these parameters into the artificial bee colony algorithm completes the construction of the optimized artificial bee colony algorithm.

[0141] S5: Determine the number of iterations, calculate the dynamic weight coefficient and adaptive scaling factor based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficient and adaptive scaling factor, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the unmanned surface vessel through the optimal control command and heading correction data.

[0142] Furthermore, the objective of this stage is to calculate the dynamic weighting coefficients and adaptive scaling factors to obtain the optimal control command and control the unmanned surface vessel. Specifically, in step S5, when the optimized artificial bee colony algorithm solves for the optimal control command using the optimized navigation parameters and obstacle avoidance route, it determines a transition probability threshold such that the state transition probability of the optimized artificial bee colony algorithm is higher than the transition probability threshold.

[0143] The specific implementation method for the above steps in this embodiment is as follows:

[0144] Here, we first need to determine the iteration number n, which is the current iteration number of the artificial bee colony algorithm. Based on the iteration number, we can calculate the dynamic weight coefficient for the nth iteration. and the adaptive scaling factor of the nth iteration :

[0145]

[0146]

[0147] in, The maximum number of iterations is predetermined. This is the minimum value of the adaptive scaling factor. This represents the maximum value of the adaptive scaling factor.

[0148] The optimal manipulation command is then obtained by optimizing the artificial bee colony algorithm:

[0149]

[0150] in, To optimize the solution obtained by the artificial bee colony algorithm in the nth iteration, This refers to the L-th solution selected from multiple solutions obtained by optimizing the artificial bee colony algorithm. To select the H-th solution from multiple solutions obtained by optimizing the artificial bee colony algorithm, rand is a randomly generated random number greater than 0. This is the initial solution for the iteration. The method for calculating the initial solution for the iteration is as follows:

[0151]

[0152] in, This is the current optimal solution. This is the original initial solution. The coefficients of the first initial iteration solution. The coefficients of the second initial iteration solution. It is random Gaussian noise. And we have:

[0153]

[0154] Here, the initial solution of each iteration is used as the original initial solution for the next iteration. When obtaining the optimal control command, it is necessary to determine the transition probability threshold and ensure that the state transition probability of the optimized artificial bee colony algorithm is higher than the transition probability threshold during the solution search process, so as to prevent the algorithm from getting trapped in local optima. The optimized artificial bee colony algorithm is used to further optimize the propeller output, rudder angle, and other parameters of the unmanned surface vessel (USV) based on the optimized navigation parameters and obstacle avoidance route, so as to obtain the optimal control command that enables the USV to accurately track the obstacle avoidance route with the minimum error. In this way, the USV can be controlled by the optimal control command and the heading correction data. The heading correction data is used to immediately correct the error of the current heading during the USV's navigation. In the process of obtaining the optimal control command, the optimized navigation parameters provide a better initial reference, enabling the optimized artificial bee colony algorithm to converge quickly and find the optimal control command quickly.

[0155] The optimized artificial bee colony algorithm obtained here is compared with the traditional artificial bee colony algorithm. Rosenbrock, Rastrigin, and Ackley are used as test functions to test the optimized artificial bee colony algorithm and the traditional artificial bee colony algorithm. The optimized artificial bee colony algorithm has significant improvements in both convergence rate and reduction in the number of iterations. The results are shown in Table 1:

[0156] Table 1. Comparison of Tests between Optimized Artificial Bee Colony Algorithm and Artificial Bee Colony Algorithm

[0157]

[0158] The reduction in the number of relative iterations refers to the reduction in the number of iterations required for the optimized artificial bee colony algorithm to obtain a solution of similar quality compared to the traditional artificial bee colony algorithm.

[0159] Furthermore, the effectiveness of the collision avoidance path tracking control method provided by this invention was verified. The parameters of the unmanned surface vessel (USV) used are as follows: draft 0.8 meters, full load draft 1.25 tons, length 5 meters, width 1.2 meters, block coefficient 0.09, and propeller diameter 0.46 meters. A curved path was designed to demonstrate the effectiveness of this method. The USV was set to a speed of 6 knots to simulate an autonomous seabed topographic survey using side-scan sonar. External interference factors included: wind speed of [missing information]. m / s, wind direction is The wind changes over time, and the flow velocity is m / s, flow direction is The time-varying flow.

[0160] Both MPC and RLOS-MPC in the comparison are existing unmanned surface vessel (USV) path tracking methods. Figure 3 This is a schematic diagram of lateral error. Figure 2 This is a schematic diagram of longitudinal error. Figure 2 and Figure 3 The horizontal axis represents time, in seconds. It can be seen that the method provided by this invention has relatively small vertical and horizontal errors. Figure 4 This is a schematic diagram of an obstacle avoidance route. The black dots represent obstacles located on the original route. As you can see, this method can plan an obstacle avoidance route that can safely bypass obstacles on the original route. The units for the x-axis and y-axis are meters.

[0161] The following describes a collision avoidance path tracking control device provided by the present invention. The collision avoidance path tracking control device described below and the collision avoidance path tracking control method described above can be referred to in correspondence.

[0162] Figure 5 An example is a schematic diagram of a collision avoidance path tracking control system, such as... Figure 5 As shown, a collision avoidance path tracking control method as described above includes:

[0163] Heading correction module 100: Used to acquire unmanned surface vessel (USV) navigation parameters, calculate bearing error based on USV navigation parameters, calculate forward sight distance based on bearing error, calculate heading error based on forward sight distance, and obtain heading correction data based on heading error.

[0164] Optimized navigation parameters module 200: used to calculate the rate of curvature change, calculate the measurement step size based on the rate of curvature change, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters;

[0165] Obstacle avoidance path module 300: used to determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the unmanned surface vessel (USV), construct the USV obstacle avoidance constraints based on the collision avoidance radius, the predicted position of the USV, and the obstacle radius, and obtain the obstacle avoidance path based on the USV obstacle avoidance constraints;

[0166] Bee colony algorithm optimization module 400: used to obtain artificial bee colony algorithm parameters, construct optimization vectors through artificial bee colony algorithm parameters, construct optimization objective function, use optimization objective function to optimize optimization vectors to obtain optimization algorithm parameters, and use optimization algorithm parameters to construct optimized artificial bee colony algorithm;

[0167] Unmanned surface vessel (USV) control module 500: This module is used to determine the number of iterations, calculate the dynamic weight coefficient and adaptive scaling factor based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficient and adaptive scaling factor, and solve using optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command. The USV is then controlled using the optimal control command and heading correction data.

[0168] Figure 6 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 6 As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830, and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other via the communication bus 840. The processor 810 can call a computer program in the memory 830 to execute a collision avoidance path tracking control method, which includes:

[0169] S1: Obtain the unmanned surface vessel's navigation parameters, calculate the azimuth error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the azimuth error, calculate the heading error based on the forward sight distance, and obtain the heading correction data based on the heading error;

[0170] S2: Calculate the rate of change of curvature, calculate the measurement step size based on the rate of change of curvature, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters;

[0171] S3: Determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the unmanned surface vessel (USV), construct the USV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the USV, and the obstacle radius, and obtain the obstacle avoidance route based on the USV obstacle avoidance constraint;

[0172] S4: Obtain the parameters of the artificial bee colony algorithm, construct an optimization vector using the artificial bee colony algorithm parameters, construct an optimization objective function, use the optimization objective function to optimize the optimization vector, obtain the optimization algorithm parameters, and use the optimization algorithm parameters to construct an optimized artificial bee colony algorithm;

[0173] S5: Determine the number of iterations, calculate the dynamic weight coefficient and adaptive scaling factor based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficient and adaptive scaling factor, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the unmanned surface vessel through the optimal control command and heading correction data.

[0174] Furthermore, when the computer program in the aforementioned memory 830 can be implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0175] In another aspect, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to perform one of the collision avoidance path tracking control methods provided above, the method comprising:

[0176] S1: Obtain the unmanned surface vessel's navigation parameters, calculate the azimuth error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the azimuth error, calculate the heading error based on the forward sight distance, and obtain the heading correction data based on the heading error;

[0177] S2: Calculate the rate of change of curvature, calculate the measurement step size based on the rate of change of curvature, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters;

[0178] S3: Determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the unmanned surface vessel (USV), construct the USV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the USV, and the obstacle radius, and obtain the obstacle avoidance route based on the USV obstacle avoidance constraint;

[0179] S4: Obtain the parameters of the artificial bee colony algorithm, construct an optimization vector using the artificial bee colony algorithm parameters, construct an optimization objective function, use the optimization objective function to optimize the optimization vector, obtain the optimization algorithm parameters, and use the optimization algorithm parameters to construct an optimized artificial bee colony algorithm;

[0180] S5: Determine the number of iterations, calculate the dynamic weight coefficient and adaptive scaling factor based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficient and adaptive scaling factor, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the unmanned surface vessel through the optimal control command and heading correction data.

[0181] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0182] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0183] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A collision avoidance path tracking control method, characterized in that, include: S1: Obtain the unmanned surface vessel's navigation parameters, calculate the azimuth error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the azimuth error, calculate the heading error based on the forward sight distance, and obtain the heading correction data based on the heading error; Step S1 further includes: S11: Obtain the unmanned surface vessel's (USV) navigation parameters, including its current speed and current position, and use its current position to calculate the azimuth error, including lateral and longitudinal errors. S12: Determine the length of the unmanned surface vessel (USV), calculate the forward look-ahead distance using the USV length, the bearing error, and the current speed of the USV, calculate the sliding mode virtual input based on the bearing error, calculate the heading error using the sliding mode virtual input and the forward look-ahead distance, and obtain heading correction data based on the heading error; S2: Calculate the rate of change of curvature, calculate the measurement step size based on the rate of change of curvature, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters; S3: Determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the unmanned surface vessel (USV), construct the USV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the USV, and the obstacle radius, and obtain the obstacle avoidance route based on the USV obstacle avoidance constraint; Step S3 further includes: S31: Select a target obstacle, measure the danger radius of the target obstacle, determine the minimum turning radius of the unmanned surface vessel (USV), calculate the collision avoidance radius based on the minimum turning radius and the danger radius, and construct the first obstacle avoidance constraint of the USV based on the collision avoidance radius; S32: Measure the radius of the obstacle, calculate the predicted position of the unmanned surface vessel (USV) including the predicted position coordinates of the lateral direction and the predicted position coordinates of the longitudinal direction, and construct the second obstacle avoidance constraint of the USV using the predicted position of the USV and the radius of the obstacle. The obstacle avoidance constraint of the USV includes the first obstacle avoidance constraint of the USV and the second obstacle avoidance constraint of the USV. S33: Adjust the predicted route to obtain a predicted route that conforms to the obstacle avoidance constraints of the unmanned surface vessel as the obstacle avoidance route; S4: Obtain the parameters of the artificial bee colony algorithm, construct an optimization vector using the artificial bee colony algorithm parameters, construct an optimization objective function, use the optimization objective function to optimize the optimization vector, obtain the optimization algorithm parameters, and use the optimization algorithm parameters to construct an optimized artificial bee colony algorithm; S5: Determine the number of iterations, calculate the dynamic weight coefficient and adaptive scaling factor based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficient and adaptive scaling factor, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the unmanned surface vessel through the optimal control command and heading correction data.

2. The collision avoidance path tracking control method according to claim 1, characterized in that, Step S2 further includes: S21: Determine the path curvature, calculate the rate of change of curvature based on the path curvature, and calculate the measurement step size based on the rate of change of curvature; S22: Construct a discrete displacement expression, and predict the path error using the discrete displacement expression and the measurement step size to obtain the predicted path error; S23: Construct a path error optimization function, substitute the predicted path error into the path error optimization function, and solve the path error optimization function to obtain the optimized navigation parameters.

3. The collision avoidance path tracking control method according to claim 1, characterized in that, In step S2, the measurement step size The calculation method is as follows: in, For the minimum step size, For the maximum step size, For path curvature, This represents the maximum path curvature. To find the differential, let t be time. For longitudinal error, For lateral error, The first adjustment factor is... This is the second adjustment factor. The coefficient of curvature variation. This is the tracking error coefficient.

4. The collision avoidance path tracking control method according to claim 1, characterized in that, Step S4 further includes: S41: Select an artificial bee colony algorithm, and select artificial bee colony algorithm parameters including a cost function weighting matrix, a control input weighting matrix, a first curvature sensitive parameter, and a second curvature sensitive parameter from the artificial bee colony algorithm, and construct an optimization vector through the artificial bee colony algorithm parameters; S42: Determine the optimization regular vector, construct the optimization objective function through the optimization regular vector, substitute the optimization vector into the optimization objective function, and use the optimization objective function to optimize the optimization vector to obtain the optimization algorithm parameters; S43: Substitute the optimization algorithm parameters into the artificial bee colony algorithm to complete the construction of the optimized artificial bee colony algorithm.

5. The collision avoidance path tracking control method according to claim 1, characterized in that, In step S5, when the optimized artificial bee colony algorithm solves for the optimal control command using the optimized navigation parameters and obstacle avoidance route, it determines a transition probability threshold such that the state transition probability of the optimized artificial bee colony algorithm is higher than the transition probability threshold.

6. A collision avoidance path tracking control system, used to execute a collision avoidance path tracking control method as described in any one of claims 1 to 5, characterized in that, include: Heading correction module: Used to acquire the unmanned surface vessel's navigation parameters, calculate the bearing error based on the unmanned surface vessel's navigation parameters, calculate the forward sight distance based on the bearing error, calculate the heading error based on the forward sight distance, and obtain heading correction data based on the heading error; The optimized navigation parameters module is used to calculate the rate of curvature change, calculate the measurement step size based on the rate of curvature change, calculate the predicted path error using the measurement step size, and obtain the optimal parameters based on the predicted path error to obtain the optimized navigation parameters. Obstacle avoidance path module: used to determine the danger radius, calculate the collision avoidance radius based on the danger radius, determine the obstacle radius and the predicted position of the UAV, construct the UAV obstacle avoidance constraint based on the collision avoidance radius, the predicted position of the UAV and the obstacle radius, and obtain the obstacle avoidance path based on the UAV obstacle avoidance constraint; Bee colony algorithm optimization module: Used to obtain artificial bee colony algorithm parameters, construct optimization vectors using artificial bee colony algorithm parameters, construct optimization objective function, use optimization objective function to optimize optimization vectors to obtain optimization algorithm parameters, and use optimization algorithm parameters to construct optimized artificial bee colony algorithm; Unmanned surface vessel (USV) control module: Used to determine the number of iterations, calculate dynamic weight coefficients and adaptive scaling factors based on the number of iterations, optimize the artificial bee colony algorithm based on the dynamic weight coefficients and adaptive scaling factors, and use optimized navigation parameters and obstacle avoidance routes to obtain the optimal control command, and control the USV through the optimal control command and heading correction data.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the collision avoidance path tracking control method as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the collision avoidance path tracking control method as described in any one of claims 1 to 5.

Images