Autonomous navigation control method for large-inertia unmanned ship

Abstract

The invention relates to the technical field of unmanned ship autonomous navigation control, and discloses a large inertia unmanned ship autonomous navigation control method comprising the following steps: in each sampling period, generating a mixed sampling candidate trajectory cluster comprising a linear trajectory and an arc trajectory; performing multi-constraint fusion evaluation on each candidate track; according to the total evaluation value, selecting an optimal track from the candidate track cluster, and taking the navigational speed and the navigational direction of the first time step in the optimal track as planning results; the planning result is input into a nonlinear gain sliding mode controller to be adjusted to generate a final control result, the nonlinear gain sliding mode controller comprises a nonlinear gain PID controller and a sliding mode controller, in proportion and differential terms of the nonlinear gain PID controller, a nonlinear function is added, sliding mode control is combined, the calculated amount is low, real-time performance is good, and the control precision is high. And the response speed, the course stability and the anti-interference capability of the large-inertia unmanned ship are remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation control technology for unmanned vessels, and particularly to a method for autonomous navigation control of large inertia unmanned vessels. Background Technology

[0002] Large inertia ships exhibit significant dynamic response hysteresis. Classical PID control methods, due to their fixed gain parameters, struggle to balance fast response and stability in large inertia ship control. Specifically, to enable a large inertia ship to quickly track the desired heading value, a large fixed PID parameter value is required. This can lead to oscillations after the heading error converges to the desired value. Conversely, a smaller fixed PID parameter results in slow tracking. Furthermore, model-based control methods rely on model accuracy, but establishing motion models for large inertia ships involves uncertainties such as model parameter instability, unmodeled dynamics, and uncertainties arising from external disturbances like wind, waves, and currents. Moreover, for navigation safety issues of large inertia ships navigating in narrow inland waterways with multiple dynamic obstacle safety assessments, MPC (Multi-Purpose Control) is an effective navigation planning method. It involves constructing an objective function, using dynamic obstacle safety assessments and the ship model as constraints, and then optimizing the solution to provide the planning results. Currently, there are at least three shortcomings in the autonomous navigation control of large inertial ships: (1) MPC performance is heavily dependent on the accuracy of the prediction model, and it is difficult to establish an accurate motion model for large inertial ships; (2) The optimization solution has a large computational load and poor real-time performance, making it difficult to balance collision avoidance safety and control feasibility; (3) Currently, the focus is mainly on the safety evaluation of dynamic obstacles and the constraints of static obstacles in open waters, which are quite different from the actual situation. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention provides an autonomous navigation control method for large inertial unmanned vessels, aiming to solve the problems of slow response, poor maneuverability, and difficulty in collision avoidance faced by large transport vessels in restricted inland waterways.

[0004] This invention provides an autonomous navigation control method for large inertia unmanned surface vessels, comprising the following steps: Within each sampling period, based on the ship's current state and large inertial characteristics, a mixed sampling candidate trajectory cluster containing straight-line trajectories and circular arc trajectories is generated; For each candidate trajectory, a multi-constraint fusion evaluation is performed, including a comprehensive evaluation of static obstacle safety, dynamic obstacle safety, large inertia constraint, target approach, and collision avoidance rules, to obtain the total evaluation value; Based on the overall evaluation value, the optimal trajectory is selected from the candidate trajectory cluster, and the th best trajectory among the optimal trajectories is selected. The speed and heading at each time step are used as the planning result, among which To meet the requirements of large inertia and collision avoidance turning capability in the shortest possible time step; The planning results are input into the nonlinear gain sliding mode controller for adjustment to generate the final control result. The nonlinear gain sliding mode controller includes a nonlinear gain PID controller and a sliding mode controller. Nonlinear functions are added to the proportional and derivative terms of the nonlinear gain PID controller.

[0005] Optionally, the method for generating a cluster of mixed-sample candidate trajectories includes the following steps: Based on the environmental adaptive speed scaling factor and large inertia constraint evaluation, candidate speed intervals and candidate heading intervals are constructed respectively. Uniform sampling is performed within the candidate speed and candidate heading intervals, and candidate straight-line trajectories for all time steps are generated using the current position and heading as the initial state. Within the constraints of the large inertia ship's turning performance, multiple numbers of turning rates are sampled, the heading is calculated, and then combined with the sampling results of the candidate speed intervals to generate candidate circular arc trajectories for all time steps, with the current position and heading as the initial state. By fusing candidate straight-line trajectories and candidate circular arc trajectories under different combinations of speed and heading, a hybrid sampling candidate trajectory cluster is formed.

[0006] Optionally, the environment-adaptive speed scaling factor includes the environment-adaptive speed scaling factor for narrow channels and the environment-adaptive speed scaling factor for open waters; Environmentally Adaptive Speed ​​Scaling Factor in Narrow Channels Calculated using the following formula: , Environment-adaptive speed scaling factor in open waters Calculated using the following formula: , In the formula, The number of dynamic obstacles surrounding the vessel for safety assessment; Used to determine whether a dynamic obstacle safety assessment is too close to the vessel. This indicates that the distance is too close.

[0007] Optionally, the equation of state for the candidate straight-line trajectory is as follows: , In the formula, and Here are the position coordinates of the candidate trajectory points. The heading angle of the candidate trajectory point, Sample the flight speed for candidate trajectory points; The state equations for the candidate circular arc trajectory are as follows: , In the formula, The sampling rotation rate.

[0008] Optionally, the target proximity evaluation includes a directional proximity evaluation index. and location proximity evaluation index Directional convergence evaluation index The position proximity evaluation index is obtained by comparing the azimuth angles of the current position and the target waypoint with the angular difference between the heading of the last trajectory point in the candidate trajectory. It is obtained from the distance between the position of the last trajectory point in the candidate trajectory and the position of the target waypoint.

[0009] Optional, large inertial constraint evaluation includes trajectory large rotation evaluation and curvature smoothing evaluation; Large turn evaluation value The following formula is used to calculate: , In the formula, The current heading angle of the unmanned vessel. The heading angle of the candidate trajectory's terminal point; Curvature smoothing evaluation value The following formula is used to calculate: , In the formula, This represents the number of trajectory points for a single candidate trajectory. For the candidate trajectory, the first N i The heading angle of each trajectory point For the first N i - The heading angle of a trajectory point, For the first N i- The heading angles of the two trajectory points.

[0010] Optional, collision avoidance rule evaluation includes: Based on the dynamic obstacle safety assessment results Implement a strategy of slowing down high-risk activities and accelerating low-risk activities; Establish a right-hand avoidance evaluation value When the navigation risk reaches or exceeds the limit required for obstacle avoidance by the ship, a reward will be given to the candidate trajectory in the area to the right front of the current course, and the higher the dynamic risk of the candidate trajectory, the lower the reward.

[0011] Optionally, the overall evaluation score is obtained using the following method: The weight coefficients for each of the following are determined based on their respective priorities: static obstacle safety assessment, dynamic obstacle safety assessment, large inertia constraint assessment, target approach assessment, and collision avoidance rule assessment. The total evaluation value is obtained by multiplying the evaluation values ​​of static obstacle safety evaluation, dynamic obstacle safety evaluation, large inertia constraint evaluation, target approach evaluation, and collision avoidance rule evaluation by their respective weight coefficients.

[0012] Optionally, the design method for a nonlinear gain sliding mode controller includes the following steps: Define heading deviation; Design a nonlinear gain PID heading controller and sliding mode surface based on heading deviation; By combining a nonlinear gain PID heading controller and a sliding mode surface, a nonlinear gain sliding mode controller is obtained.

[0013] Optionally, the sliding surface can be smoothed using the tanh function after construction.

[0014] The technical solution provided by the embodiments of the present invention has the following advantages compared with the prior art: The autonomous navigation control method for large-inertia unmanned surface vessels (USVs) provided in this invention addresses the challenges of narrow inland waterways, dense traffic flow, and complex environmental disturbances. Through collaborative optimization at the planning and control layers, it achieves high-precision heading control and safe obstacle avoidance for USVs. First, at the planning layer, this invention employs a hybrid sampling multi-constraint MPC method. Unlike traditional MPC, this algorithm does not require a precise model; instead, it generates a cluster of hybrid sampling candidate trajectories for trajectory cost evaluation. It utilizes both straight-line and circular trajectories, enhancing both navigation efficiency and collision avoidance safety for the USV. Simultaneously, it integrates multi-constraint evaluations, including static obstacle safety evaluation, dynamic obstacle safety evaluation, large-inertia constraint evaluation, target approach evaluation, and collision avoidance rule evaluation. Through real-time sampling in each sampling cycle, the optimal trajectory is selected, providing the optimal control output for the current situation. This method is model-independent, resulting in lower computational complexity and better real-time performance. Through rolling optimization in each cycle, it can quickly generate safe trajectories that meet collision avoidance rules in complex traffic flow scenarios, significantly reducing the risk of collisions for large-inertia USVs in dense waterways. Subsequently, at the control level, this invention proposes a control method for a nonlinear gain sliding mode converter. This method, based on the traditional PID controller, adds a nonlinear gain function to the proportional and derivative terms, and incorporates a sliding mode term. By dynamically adjusting this gain, it achieves rapid adjustment of the dynamic response of large-inertia vessels. This controller can automatically adjust the control law strength according to speed and heading errors, exhibiting stability in the small error region, rapid response in the large error region, and strong robustness under large disturbance environments. This significantly improves the response speed, heading stability, and anti-interference capability of large-inertia unmanned surface vessels (USVs). This control method effectively overcomes the control lag problem caused by large inertia, achieving high dynamic response and high-precision control of USVs in confined waters. Attached Figure Description

[0015] Figure 1 A flowchart illustrating an autonomous navigation control method for a large inertial unmanned surface vessel provided in an embodiment of the present invention; Figure 2 A schematic diagram of the overall process of an autonomous navigation control method for a large inertial unmanned vessel provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of a hybrid sampling candidate trajectory cluster provided in an embodiment of the present invention. Detailed Implementation

[0016] The following detailed description of a specific embodiment of the present invention is provided in conjunction with the accompanying drawings. However, it should be understood that the scope of protection of the present invention is not limited to the specific embodiment.

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

[0018] The present invention will be described below through several specific embodiments. To keep the following description of the embodiments clear and concise, detailed descriptions of known functions and components may be omitted. When any component of an embodiment of the present invention appears in more than one drawing, the component may be represented by the same reference numerals in each drawing.

[0019] like Figure 1 As shown, this embodiment of the invention provides an autonomous navigation control method for a large inertial unmanned surface vessel, comprising the following steps: Within each sampling period, based on the ship's current state and large inertia characteristics, a hybrid sampling candidate trajectory cluster containing straight-line and circular trajectories is generated. To balance the ship's navigation efficiency and collision avoidance capabilities during turns, a method for generating the hybrid sampling candidate trajectory cluster containing both straight lines and circular arcs is proposed. Any candidate trajectory in the trajectory cluster must be selected within a reasonable speed and heading candidate range before virtual navigation generation.

[0020] For each candidate trajectory, a multi-constraint fusion evaluation is performed, including a comprehensive evaluation of static obstacle safety, dynamic obstacle safety, large inertia constraint, target approach, and collision avoidance rules to obtain the total evaluation value. Based on the overall evaluation value, the optimal trajectory is selected from the candidate trajectory cluster, and the th best trajectory among the optimal trajectories is selected. The speed and heading at each time step are used as the planning result, among which To meet the requirements of large inertia and collision avoidance turning capability in the shortest possible time step; The planning results are input into the nonlinear gain sliding mode controller for adjustment to generate the final control result. The nonlinear gain sliding mode controller includes a nonlinear gain PID controller and a sliding mode controller. Nonlinear functions are added to the proportional and derivative terms of the nonlinear gain PID controller.

[0021] Optionally, the method for generating a cluster of mixed-sample candidate trajectories includes the following steps: Based on the environmental adaptive speed scaling factor and the large inertia constraint evaluation, candidate speed intervals and candidate heading intervals are constructed respectively. Uniform sampling is performed within the candidate intervals for speed and heading, and candidate straight-line trajectories for all time steps are generated with the current position and heading as the initial state. Within the constraints of the ship's rotation performance, multiple rotation rates are sampled, the heading is calculated, and then combined with the sampling results of the candidate speed intervals to generate candidate circular trajectories for all time steps, with the current position and heading as the initial state. The candidate straight-line trajectory and candidate circular trajectory under different combinations of speed and heading are combined to form a hybrid sampling candidate trajectory cluster.

[0022] According to the 1972 Collision Avoidance Regulations Evaluation Convention, navigation in dangerous areas with numerous obstacles or narrow channels requires lower speeds to ensure safe maneuvering time; while in open, safe waters, speeds can be appropriately increased to improve navigation efficiency. Based on this, an environment-adaptive speed scaling factor was designed when constructing the candidate speed range. The calculation formula is as follows:

[0023] Narrow waterway ) Open waters ) In the formula, The number of dynamic obstacles surrounding the vessel for safety assessment; Used to determine whether a dynamic obstacle safety assessment is too close to the vessel. This indicates that the distance is too close; The local map water area ratio (i.e., the ratio of the effective navigation water area to the entire local map area) is used to determine whether the waterway is narrow. Threshold for determining narrow waterways.

[0024] definition The minimum speed at which the ship can maintain steering and rudder effectiveness. The candidate design speed range for the ship's maximum speed is as follows: In the formula, The sampling speed.

[0025] When in high-risk environments such as "narrow waterways" or "approaching vessels", Reduce speed to lower the sampling speed limit; in low-risk environments such as "open channels" or "no neighboring vessels nearby," Increase the sampling speed limit to allow unmanned ships to travel at higher speeds.

[0026] The construction of candidate heading intervals consists of two parts. Candidate straight-line trajectories are uniformly sampled within a certain threshold range (±60 degrees) of the current heading, and then combined with the heading speed to generate a total of [number missing] trajectories using the current position and heading as the initial state. The candidate straight-line trajectory for each time step (the candidate trajectory consists of a series of candidate trajectory points, and the number of trajectory points determines the length of the candidate trajectory. Each trajectory point is calculated iteratively based on the trajectory point state of the previous time step, according to the sampling results of air speed and heading). This represents the number of trajectory points in the candidate trajectory, which is also the number of prediction time steps for the candidate trajectory. If the value is 300, it means that each candidate trajectory is obtained by predicting 300 time steps, containing a total of 300 trajectory points. Conclusion: This represents the number of trajectory points in the candidate trajectory. The state equations for the candidate straight-line trajectories are as follows:

[0027] In the formula, and Here are the position coordinates of the candidate trajectory points. The heading angle of the candidate trajectory point.

[0028] The primary function of candidate circular trajectories is to enable emergency obstacle avoidance maneuvers when candidate straight trajectories fail to allow the unmanned vessel to avoid obstacles. Within the constraints of the large inertia ship's turning performance, at least two gyration rates (e.g., four gyration rates, including two left turns and two right turns) are sampled, the heading is calculated, and then, by combining it with the speed, a trajectory is generated using the current position and heading as the initial state. The candidate circular arc trajectories at each time step, and their state equations are as follows:

[0029] In the formula, The sampling rotation rate.

[0030] By combining straight lines and candidate circular arc trajectories, a pattern is formed as follows: Figure 3 The candidate trajectory cluster shown is used for subsequent evaluation and decision-making. The candidate trajectory cluster includes both candidate straight trajectories that allow unmanned vessels to navigate efficiently in safe and wide waters, and candidate circular trajectories that provide better turning and obstacle avoidance in dangerous and narrow waters, thus covering the safe navigation requirements of unmanned vessels in inland waterway scenarios.

[0031] Optionally, the target proximity evaluation includes a directional proximity evaluation index. and location proximity evaluation index Directional convergence evaluation index The position proximity evaluation index is obtained by comparing the azimuth angles of the current position and the target waypoint with the angular difference between the heading of the last trajectory point in the candidate trajectory. It is obtained from the distance between the position of the last trajectory point in the candidate trajectory and the position of the target waypoint.

[0032] Optional, large inertial constraint evaluation includes trajectory large rotation evaluation and curvature smoothing evaluation; Large turn evaluation value The following formula is used to calculate: , In the formula, The current heading angle of the unmanned vessel. The heading angle of the candidate trajectory's terminal point; Curvature smoothing evaluation value The following formula is used to calculate: , In the formula, This represents the number of trajectory points for a single candidate trajectory. For the candidate trajectory, the first N i The heading angle of each trajectory point For the first N i - The heading angle of a trajectory point, For the first N i- The heading angles of the two trajectory points.

[0033] Optional, collision avoidance rule evaluation includes: Based on the dynamic obstacle safety assessment results Implement a strategy of slowing down high-risk activities and accelerating low-risk activities; Establish a right-hand avoidance evaluation value When the navigation risk reaches or exceeds the limit required for obstacle avoidance by the ship, a reward will be given to the candidate trajectory in the area to the right front of the current course, and the higher the dynamic risk of the candidate trajectory, the lower the reward.

[0034] Optionally, the overall evaluation score is obtained using the following method: The weight coefficients for each of the following are determined based on their respective priorities: static obstacle safety assessment, dynamic obstacle safety assessment, large inertia constraint assessment, target approach assessment, and collision avoidance rule assessment. The total evaluation value is obtained by multiplying the evaluation values ​​of static obstacle safety evaluation, dynamic obstacle safety evaluation, large inertia constraint evaluation, target approach evaluation, and collision avoidance rule evaluation by their respective weight coefficients.

[0035] like Figure 2 As shown, to enable unmanned surface vessels (USVs) to perform dynamic obstacle safety assessments and avoid obstacles such as shorelines within limited inland waterways, this embodiment of the invention employs a hybrid sampling (MPC) method to construct a multi-constraint fusion planning algorithm. Its core lies in enabling USVs to navigate safely and efficiently in complex and dynamically changing aquatic environments through real-time sampling, multi-dimensional evaluation, and decision-making of candidate trajectories.

[0036] After generating candidate trajectories, this invention establishes a multi-constraint fusion planning algorithm based on the MPC (model predictive control) framework. Unlike traditional MPC, this invention uses a hybrid sampling candidate trajectory generation method to replace MPC's prediction based on an accurate model. Its core idea is still to use the rolling optimization mechanism of MPC. Within each sampling period, multiple sets of candidate trajectories are regenerated based on the ship's current position and velocity state to predict the motion state at multiple future time steps. Simultaneously, multiple constraint evaluations are incorporated, including static obstacle safety evaluation, dynamic obstacle safety evaluation, large inertia constraint evaluation, target approach evaluation, and collision avoidance rule evaluation. After comprehensive evaluation, a reasonable candidate trajectory is selected to obtain the action plan for the next moment, constructing a rolling optimization mechanism within a finite time domain (at each sampling moment, the process of generating a candidate trajectory cluster, multi-constraint evaluation, and selecting the optimal trajectory must be implemented; this is the rolling optimization mechanism).

[0037] The following are multidimensional evaluation calculations for candidate trajectories: 1) Static obstacle safety assessment Safety assessment of static obstacles is mainly used to measure the collision risk between the trajectory and fixed obstacles such as shorelines, bridge piers, and shoals. Through a multi-dimensional penalty mechanism, it ensures that the unmanned vessel's navigation trajectory stays away from these static danger zones.

[0038] For any candidate trajectory Determine its first trajectory points at each time step Whether the trajectory point has collided with the shoreline, bridge piers in the middle of the channel, or dock obstacles. If so, it means that the trajectory point faces a serious safety risk and is given a high penalty value. If none of the candidate trajectory points have collided with static obstacles, a soft buffer evaluation is then performed on the candidate trajectory points, i.e., the calculation of each candidate trajectory point... Shortest distance to the obstacle The formula is as follows:

[0039] In the formula, For the search scope, and Based on trajectory points The coordinates of the center of the circle. This is a function to determine whether a location point is an obstacle; a value of 1 indicates that the point is an obstacle.

[0040] Based on trajectory points Shortest distance to the obstacle Calculate the cost of soft buffering This involves converting distance information into numerical values ​​with a reasonable penalty level, specifically in the following form: In the formula, and All of these are soft buffer cost parameters.

[0041] Furthermore, considering that the impact of candidate trajectory points at different times on the overall trajectory safety varies, a time weight is assigned to each predicted point, as follows: In the formula, This is the weight value.

[0042] In summary, by summing the static obstacle assessments of all trajectory points along the candidate trajectory, the overall static obstacle safety assessment of the candidate trajectory is obtained. for: In the formula, For a fixed penalty value, The static evaluation value for each candidate trajectory point.

[0043] When evaluating candidate trajectories, It is an important reference indicator used to screen out candidate trajectories with higher safety.

[0044] 2) Safety assessment of dynamic obstacles Dynamic obstacle safety assessment is primarily used to quantify the collision risk between unmanned surface vessels (USVs) and other dynamic obstacle safety assessments. First, the collision risk between the USV and the USV is predicted based on the Distance at Closest Point of Approach (DCPA) and Time to Closest Point of Approach (TCPA). The candidate trajectory... The time step is the location of my ship's trajectory point. ,course The speed is The velocity components are respectively and .

[0045] Assuming there are around this ship The first dynamic obstacle safety assessment, the first The current position of each ship is The heading is The speed is The velocity components are respectively and . No. The predicted location for the dynamic obstacle safety assessment at each time step is:

[0046] In the formula, For the prediction period.

[0047] Unmanned ships and the first The dynamic obstacle safety assessment is in the first The relative position and relative velocity at each time step are calculated as follows: The DCPA / TCPA calculation is as follows: In the formula, Minimum meeting time, This represents the minimum encounter distance.

[0048] These two parameters intuitively reflect the degree of collision risk between the unmanned vessel and dynamic obstacles in the safety assessment. The smaller the value, the more urgent the meeting time. The smaller the value, the closer the distance at the time of encounter, and the higher the risk of collision.

[0049] Calculate the distance attenuation risk for: In the formula, For a safe distance, To adjust the parameters.

[0050] according to Calculate the time urgency and risk level for In the formula, This is the sensitivity attenuation factor.

[0051] Considering that unmanned vessels and dynamic obstacles will suffer greater damage in head-on collisions, a head-on collision risk component is set. for: When the unmanned surface vessel travels in the same direction as the dynamic obstacle safety assessment, that is ( Set the same-direction approximation factor. for: In summary, by summing the dynamic safety assessments of all trajectory points along the candidate trajectory, the overall dynamic obstacle safety assessment of the candidate trajectory is obtained. for: 3) Target approach evaluation The approach assessment to the target waypoint consists of two parts: directional approach assessment and positional approach assessment. The azimuth is calculated based on the current position and the target waypoint position. as follows:

[0052] In the formula, Current position The location of the target waypoint.

[0053] Directional proximity evaluation index for: In the formula, To adjust the parameters, The heading angle of the candidate trajectory's final trajectory point.

[0054] Location proximity evaluation index for: In the formula, and The coordinates of the position of the trajectory point at the end of the candidate trajectory.

[0055] 4) Evaluation of large inertia constraints Considering the characteristics of large inertia ships, such as weak maneuverability and slow response, a large inertia constraint evaluation is performed on the candidate trajectory, including trajectory large rotation evaluation and curvature smoothing evaluation.

[0056] Large turn evaluation value for: In the formula, The current heading angle of the unmanned vessel. The heading angle of the candidate trajectory's final trajectory point.

[0057] Curvature smoothing evaluation value for: In the formula, This represents the number of trajectory points for a single candidate trajectory.

[0058] 5) Evaluation of collision avoidance rules The 1972 International Convention for Preventing Collisions at Sea states that environmental conditions and traffic density determine safe speed. Therefore, in this embodiment of the invention, the safe speed is determined based on the results of a dynamic obstacle safety assessment. Increase the strategy of slowing down in high-risk situations and accelerating in low-risk situations, specifically when... When the speed exceeds a certain threshold, the current navigation is considered high-risk, and speed reduction is encouraged. When the risk level is below a certain threshold, the current navigation is considered low-risk, and increasing speed is encouraged. The calculation method is as follows:

[0059] In the formula, and To adjust the parameters, and The threshold for dynamic obstacle safety assessment. For safe speed evaluation.

[0060] According to the 1972 International Convention for Preventing Collisions at Sea, vessels navigating through narrow channels or waterways should, as far as safety practicable, navigate as close as possible to the outer edge of that channel or waterway on their starboard side. Based on this, an incentive mechanism is established to encourage right-hand collision avoidance. When the navigational risk reaches or exceeds the threshold required for obstacle avoidance (i.e., dynamic risk reaches 0.5 or higher), a reward is given for candidate trajectories located in the area to the starboard front of the current course. The higher the dynamic risk of the candidate trajectory, the lower the reward. The specific form is as follows:

[0061] In the formula, To adjust the parameters, To avoid giving way to the evaluation.

[0062] 6) Multi-constraint integrated planning The evaluation system integrates static obstacle safety assessment, dynamic obstacle safety assessment, large inertia constraint assessment, target approach assessment, and collision avoidance rule assessment to form a complete multi-constraint overall assessment. as follows: In the formula, to avoid negative scores for each trajectory that would affect the selection of the optimal trajectory, a base score is set, and penalties and rewards (i.e., adding or subtracting points) are then applied based on this base score. The basic evaluation score is 100, with different subscripts. The value is a weighting coefficient. This weighting coefficient is determined based on the priority of each objective (static obstacle safety assessment, dynamic obstacle safety assessment, large inertia constraint assessment, target approach assessment, and collision avoidance rule assessment), and its value is determined through experimental fine-tuning. For example, if safety is the highest priority, then the weighting of the scores for static obstacles and dynamic vessels will play a more crucial role. This weighting coefficient is adjusted based on actual results; dynamic obstacle avoidance and static obstacle avoidance have the highest safety priority and the largest coefficient, with safety obstacle avoidance assessment having the highest priority, followed by large inertia characteristics and target approach.

[0063] Based on the overall evaluation score The optimal trajectory is selected from the candidate trajectory cluster based on its size, and the optimal trajectory is selected based on the size of the first optimal trajectory. The speed and heading at each time step are used as the planning results and input into the nonlinear gain sliding mode controller to complete the control process. The total evaluation score is the score value of each trajectory line calculated by combining multi-dimensional evaluation after predicting the future multiple steps at the current moment. In the next cycle, the candidate trajectory generation, multi-constraint fusion evaluation planning, and control execution process will be repeated to realize the rolling optimization mechanism of MPC.

[0064] Optionally, the design method for a nonlinear gain sliding mode controller includes the following steps: Define heading deviation; Design a nonlinear gain PID heading controller and sliding mode surface based on heading deviation; By combining a nonlinear gain PID heading controller and a sliding mode surface, a nonlinear gain sliding mode controller is obtained.

[0065] Optionally, the sliding surface can be smoothed using the tanh function after construction.

[0066] The heading controller design is based on nonlinear gain sliding mode. The motion planning for the next moment is the desired speed and heading. After inputting into the nonlinear gain sliding mode controller, the control commands for the corresponding propulsion system of the unmanned vessel, such as the speed and angle of the jet pump, are obtained.

[0067] Large inertia vessels have slow response speeds and poor maneuverability. When performing missions in complex waters, traditional PID control parameters are fixed, making it difficult to balance speed and stability, and lacking robustness to external disturbances, thus unsuitable for large inertia inland waterway transport vessels. This invention integrates nonlinear gain PID control with sliding mode control in its control layer design, forming a nonlinear gain sliding mode controller. The nonlinear gain PID controller adaptively adjusts the gain based on the magnitude of the unmanned vessel's heading deviation; the sliding mode controller provides strong robust control force under large disturbances or sudden changes. The design of the heading controller control law for the nonlinear gain sliding mode is as follows:

[0068] If the desired course is The unmanned vessel's heading deviation is defined as , For the desired course, Design a nonlinear gain PID heading controller with the current heading as the output result. as follows: In the formula, and This is a proportional control parameter; These are integral control parameters; and These are differential control parameters. and This refers to the sensitivity parameter.

[0069] Unlike the fixed gain of traditional PID controllers, this invention adds nonlinear functions to the proportional and derivative terms. The nonlinear gain can be adaptively adjusted according to the magnitude of the error. For ships with large inertia, the adaptive gain will increase when the initial error is large, and it can quickly approach the desired value. As the error decreases, the adaptive gain will decrease, achieving a fast response under large errors and stable control under small errors. The tanh function is selected for smoothing the nonlinear function.

[0070] Considering external disturbances during navigation, a sliding mode surface is further constructed and incorporated into the control law. The final nonlinear gain sliding mode controller is as follows: In the formula, These are the sliding mode control parameters; To prevent jittering and smoothing factors; For the defined sliding surface function; and These are the parameters of the sliding surface; This is the output value of the nonlinear gain sliding mode heading controller.

[0071] By selecting error and error change rate as the sliding surface, and considering the chattering phenomenon of the sign function of the sliding surface, the tanh function is used for smoothing after the sliding surface is constructed to ensure that the control of a large inertia ship can take into account both stability and robustness against external disturbances.

[0072] To enable the large-inertia unmanned surface vessel (USV) to also have speed control capabilities, a speed controller was designed concurrently. The current speed of the USV is... The expected speed is The speed deviation of the unmanned surface vessel is defined as: Design of a nonlinear gain sliding mode speed controller as follows:

[0073] In the formula, These are sliding mode control parameters; To prevent jittering and smoothing factors; For the defined sliding surface function; and These are the parameters of the sliding surface; and For sensitivity parameters; and This is a proportional control parameter; These are integral control parameters; and These are differential control parameters; This is the output value of the nonlinear gain PID speed controller; This is the output value of the nonlinear gain sliding mode speed controller.

[0074] The above inventions are merely a few specific embodiments of the present invention. However, the embodiments of the present invention are not limited thereto, and any variations that can be conceived by those skilled in the art should fall within the protection scope of the present invention.

Claims

1. A method for autonomous navigation control of a large inertial unmanned surface vessel, characterized in that, Includes the following steps: Within each sampling period, based on the ship's current state and large inertial characteristics, a mixed sampling candidate trajectory cluster containing straight-line trajectories and circular arc trajectories is generated; For each candidate trajectory, a multi-constraint fusion evaluation is performed, including a comprehensive evaluation of static obstacle safety, dynamic obstacle safety, large inertia constraint, target approach, and collision avoidance rules, to obtain the total evaluation value; Based on the overall evaluation value, the optimal trajectory is selected from the candidate trajectory cluster, and the th trajectory among the optimal trajectories is selected. The speed and heading at each time step are used as the planning result, among which To meet the requirements of large inertia and collision avoidance turning capability in the shortest possible time step; The planning results are input into a nonlinear gain sliding mode controller for adjustment to generate the final control result. The nonlinear gain sliding mode controller includes a nonlinear gain PID controller and a sliding mode controller. Nonlinear functions are added to the proportional and derivative terms of the nonlinear gain PID controller.

2. The autonomous navigation control method for a large inertia unmanned surface vessel as described in claim 1, characterized in that, The method for generating the hybrid sampling candidate trajectory cluster includes the following steps: Based on the environmental adaptive speed scaling factor and the large inertia constraint evaluation, candidate speed intervals and candidate heading intervals are constructed respectively. Uniform sampling is performed within the candidate intervals for speed and heading, and candidate straight-line trajectories for all time steps are generated with the current position and heading as the initial state. Within the constraints of the ship's rotation performance, multiple rotation rates are sampled, the heading is calculated, and then combined with the sampling results of the candidate speed intervals to generate candidate circular trajectories for all time steps, with the current position and heading as the initial state. The candidate straight-line trajectory and candidate circular trajectory under different combinations of speed and heading are combined to form a hybrid sampling candidate trajectory cluster.

3. The autonomous navigation control method for large inertia unmanned surface vessels as described in claim 2, characterized in that, The environmental adaptive speed scaling factor includes the environmental adaptive speed scaling factor for narrow waterways and the environmental adaptive speed scaling factor for open waterways. The environmental adaptive speed scaling factor of the narrow waterway Calculated using the following formula: ， The environmental adaptive speed scaling factor for open waters Calculated using the following formula: ， In the formula, The number of dynamic obstacles surrounding the vessel for safety assessment; Used to determine whether a dynamic obstacle safety assessment is too close to the vessel. This indicates that the distance is too close.

4. The autonomous navigation control method for large inertia unmanned surface vessels as described in claim 2, characterized in that, The state equation of the candidate straight-line trajectory is as follows: ， In the formula, and Here are the position coordinates of the candidate trajectory points. The heading angle of the candidate trajectory point, Sample the flight speed for candidate trajectory points; The state equation of the candidate circular arc trajectory is as follows: ， In the formula, The sampling rotation rate.

5. The autonomous navigation control method for a large inertia unmanned surface vessel as described in claim 1, characterized in that, The target approach evaluation includes a directional approach evaluation index. and location proximity evaluation index The directional convergence evaluation index The position proximity evaluation index is obtained by comparing the azimuth angles of the current position and the target waypoint with the angle difference between the heading of the last trajectory point in the candidate trajectory. It is obtained from the distance between the position of the last trajectory point in the candidate trajectory and the position of the target waypoint.

6. The autonomous navigation control method for a large inertia unmanned surface vessel as described in claim 1 or 5, characterized in that, The large inertia constraint evaluation includes trajectory large rotation evaluation and curvature smoothing evaluation; The trajectory large turn evaluation value The following formula is used to calculate: ， In the formula, The current heading angle of the unmanned vessel. The heading angle of the candidate trajectory's terminal point; The curvature smoothing evaluation value The following formula is used to calculate: ， In the formula, This represents the number of trajectory points for a single candidate trajectory. For the candidate trajectory, the first N i The heading angle of each trajectory point For the first N i - The heading angle of a trajectory point, For the first N i- The heading angles of the two trajectory points.

7. The autonomous navigation control method for a large inertia unmanned surface vessel as described in claim 1, characterized in that, The evaluation of the collision avoidance rules includes: Based on the dynamic obstacle safety assessment results Implement a strategy of slowing down high-risk activities and accelerating low-risk activities; Establish a right-hand avoidance evaluation value When the navigation risk reaches or exceeds the limit required for obstacle avoidance by the ship, a reward will be given to the candidate trajectory in the area to the right front of the current course, and the higher the dynamic risk of the candidate trajectory, the lower the reward.

8. The autonomous navigation control method for a large inertia unmanned surface vessel as described in claim 1, characterized in that, The overall evaluation value is obtained through the following method: The weight coefficients for each of the following are determined based on their respective priorities: static obstacle safety assessment, dynamic obstacle safety assessment, large inertia constraint assessment, target approach assessment, and collision avoidance rule assessment. The total evaluation value is obtained by multiplying the evaluation values ​​of static obstacle safety evaluation, dynamic obstacle safety evaluation, large inertia constraint evaluation, target approach evaluation, and collision avoidance rule evaluation by their respective weight coefficients.

9. The autonomous navigation control method for a large inertia unmanned surface vessel as described in claim 1, characterized in that, The design method of the nonlinear gain sliding mode controller includes the following steps: Define heading deviation; Design a nonlinear gain PID heading controller and sliding mode surface based on heading deviation; By combining a nonlinear gain PID heading controller and a sliding mode surface, a nonlinear gain sliding mode controller is obtained.

10. The autonomous navigation control method for a large inertial unmanned surface vessel as described in claim 9, characterized in that, After the sliding surface is constructed, it is smoothed using the tanh function.

Images