Unmanned surface vessel dynamic obstacle avoidance control method based on real-time nonlinear model predictive control

Abstract

The invention relates to an unmanned surface vessel dynamic obstacle avoidance control method based on real-time nonlinear model predictive control, and the method comprises the steps: constructing a dynamic model, a static obstacle model and a dynamic obstacle model of an unmanned surface vessel; based on the dynamic model, the static obstacle model and the dynamic obstacle model, a multi-objective optimization model is constructed, and the multi-objective optimization model takes trajectory tracking cost, obstacle avoidance cost and control smoothing cost as objective functions and takes a motion range, speed limitation, control input limitation and a collision avoidance rule as constraint conditions; and obtaining the state of the unmanned surface vessel and the obstacle state in real time, inputting a multi-target optimization problem for solving, obtaining control input, and completing dynamic obstacle avoidance control of the unmanned surface vessel based on the control input. According to the invention, the adaptability and safety of the unmanned surface vessel in a complex navigation environment can be improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent marine equipment technology, and in particular to a dynamic obstacle avoidance control method for an unmanned surface vessel based on real-time nonlinear model predictive control. The method is applied to an autonomous navigation system in a complex marine environment, and aims to address the technical challenges in the existing technology regarding navigation safety, autonomy, and regulatory compliance of unmanned surface vessels. Background Art

[0002] With the growing demand for ocean exploration and intelligent maritime operations, unmanned surface vehicles (USVs) have been widely used in a variety of fields, including deep-sea observation, environmental monitoring, search and rescue, and offshore equipment maintenance. While USVs demonstrate significant potential for these missions, achieving reliable navigation in complex and volatile ocean environments remains challenging. Factors such as communication disruptions, narrow waterways, and adverse sea conditions lead to variations in hydrodynamic resistance, significantly impacting control accuracy and navigation stability.

[0003] To improve the autonomous performance of unmanned surface vehicles, existing research has proposed a variety of control strategies, including backstepping control, linear quadratic regulators, sliding mode control, and adaptive or fuzzy logic control. However, many traditional methods rely on simplified kinematic models and often ignore the dynamic behavior of the vessel during collision avoidance. This can lead to significant discrepancies between predicted and actual trajectories, particularly in time-sensitive environments or in densely populated waters. Furthermore, traditional methods typically treat trajectory tracking and obstacle avoidance as separate modules, reducing the overall responsiveness and safety of the system in real-world scenarios.

[0004] Model predictive control (MPC) has emerged as an effective approach for explicitly handling system dynamics and physical constraints within a unified optimization framework. Some research has extended MPC to support multi-agent coordination and rule-compliant path planning, enabling standardized navigation by embedding Rule 14 of the International Regulations for Preventing Collisions at Sea into the decision-making process. Despite these advances, achieving real-time control performance in practical applications remains a key technical bottleneck. The computational complexity of nonlinear MPC can limit the responsiveness of unmanned surface vehicles (USVs) in highly dynamic environments or when dealing with multiple targets, where timely control decisions are crucial for safe navigation. Summary of the Invention

[0005] The purpose of the present invention is to address the technical problems in the prior art of unmanned surface vehicles such as insufficient accuracy of autonomous navigation control, poor system responsiveness and lack of rule compliance, and to provide an unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control to improve the adaptability and safety of unmanned surface vehicles in complex navigation environments.

[0006] To achieve the above object, the present invention provides the following solutions:

[0007] A dynamic obstacle avoidance control method for an unmanned surface vehicle based on real-time nonlinear model predictive control, comprising:

[0008] Construct the dynamic model of the unmanned surface vehicle, as well as the static obstacle model and dynamic obstacle model;

[0009] Based on the dynamic model, the static obstacle model, and the dynamic obstacle model, a multi-objective optimization model is constructed, wherein the multi-objective optimization model uses trajectory tracking cost, obstacle avoidance cost, and control smoothing cost as objective functions, and uses motion range, speed limit, control input limit, and collision avoidance rules as constraints;

[0010] The state of the unmanned surface vehicle and the state of obstacles are obtained in real time, input into the multi-objective optimization model for solution, control input is obtained, and dynamic obstacle avoidance control of the unmanned surface vehicle is completed based on the control input.

[0011] Optionally, the dynamic model of the unmanned surface vehicle is:

[0012]

[0013] in, represents the derivative of the posture state vector of the unmanned surface vehicle in the northeast coordinate system, ν represents the velocity state vector of the unmanned surface vehicle in the hull coordinate system, R(ψ) represents the rotation matrix, M represents the mass matrix, represents the derivative of the velocity state vector of the unmanned surface vehicle in the hull coordinate system, C(ν) represents the centrifugal force matrix, D(ν) represents the damping matrix, τ=[τ u ,0,τ r ] T is the control input vector of the unmanned surface vehicle, τ u and τ r represents the propulsion force and torque, τ w It is the interference force of the external environment.

[0014] Optionally, the static obstacle model is:

[0015]

[0016] Among them, [x s ,y s ] T is the current position coordinate of the unmanned surface vehicle, [x s0 ,y s0 ] T is the center coordinate of the static obstacle; r s is the radius of the safety boundary; r is the radius of the circumscribed circle of the obstacle; k is the magnification factor.

[0017] Optionally, the dynamic obstacle model is:

[0018]

[0019] Among them, [x T (t),y T (t)] T is the predicted position of the target ship at time t, [x(t0),y(t0)] T is the position of the target ship at time t0; ψ T is the heading angle of the target ship at time t0, [u(t0),v(t0)] T is the longitudinal and transverse velocity of the target ship at time t0, Δt is the sampling time, and T(ψ) is the transformation matrix from the ship coordinate system to the northeast coordinate system.

[0020] Optionally, the objective function is expressed as:

[0021]

[0022] Among them, J track 、J avoid and J control They represent the trajectory tracking cost, obstacle avoidance cost and control smoothing cost respectively, and J final represents the final objective function after considering the collision constraint, g(x k ,x o,j,k ) represents the collision constraint function between the position of the unmanned boat and the obstacle, x k =[x k ,y k ,ψ k ] T represents the state vector at time k, x o,j,k represents the position of the j-th obstacle at time k, and ρ represents the weight coefficient of the collision avoidance constraint.

[0023] Optionally, the trajectory tracking cost in the objective function is used to evaluate the accuracy of the unmanned surface vehicle following the predetermined trajectory, and is expressed as:

[0024]

[0025] Among them, J track represents the trajectory tracking cost, x k+i∣k 、y k+i∣k , ψ k+i∣k They represent the lateral position, longitudinal position and heading angle of the unmanned boat predicted at time k+i in the future; x ref,k+i 、y ref,k+i , ψ ref,k+i Represent the reference position and heading angle of the reference trajectory k+i, Q1 and Q2 represent the weight matrix, N pIndicates the prediction step size.

[0026] Optionally, the obstacle avoidance cost in the objective function is used to ensure that the unmanned surface vessel safely bypasses obstacles, and is expressed as:

[0027]

[0028] Among them, J avoid represents the obstacle avoidance cost, N o is the number of obstacles, w j is the weight coefficient of the j-th obstacle, σ j is the coefficient related to obstacle size and safety distance, x k+i∣k represents the lateral position of the unmanned boat at the future k+i moment predicted at the k moment, x o，j，k+i represents the position of the jth obstacle at time k+i, N p Indicates the prediction step size.

[0029] Optionally, the control smoothing cost in the objective function is used to optimize the amplitude and rate of change of the control input, and is expressed as:

[0030]

[0031] Among them, J control represents the control smoothing cost, u k+i represents the control input at time k+i predicted at time k, Δu k+i It represents the control input change at the future k+i moment predicted at the k moment; R and S are weight matrices, N c Indicates the control step size.

[0032] Optionally, the collision avoidance rule is used to stipulate that when two unmanned surface vessels meet in opposite or nearly opposite directions and are about to collide, each vessel turns right, as expressed as:

[0033]

[0034] Among them, g represents the collision avoidance rule constraint, ψ r represents the relative course deviation of the unmanned surface vehicle in the collision avoidance direction, ψ s represents the safe heading deviation threshold, θ r represents the relative azimuth angle between the unmanned surface vehicle and the obstacle, θ m Indicates the angle threshold for head-on encounter, d r Represents the relative distance between the unmanned surface vehicle and the obstacle, d s Indicates the safety distance threshold.

[0035] Optionally, the constraint condition is expressed as:

[0036]

[0037] Among them, x k+1 represents the state vector at time k+1, x k represents the state vector at time k, u k represents the control input vector at time k, f(x k ,u k ) represents the dynamic model of the unmanned surface vehicle, x min and x max They represent the lower and upper limits of the state of the unmanned surface vehicle, u min and u max They represent the lower and upper limits of the control input of the unmanned surface vehicle, h static (x k ,y k ) and h dynamic (x k ,y k , v k , r k ) are the static obstacle avoidance constraint function and the dynamic obstacle avoidance constraint function to ensure the collision avoidance of the unmanned surface vehicle, Represents the collision avoidance rule constraint function.

[0038] The beneficial effects of the present invention are:

[0039] This invention utilizes the three-degree-of-freedom dynamic model of the Otter unmanned surface vehicle (USV) to organically integrate high-precision trajectory tracking with static and dynamic obstacle avoidance capabilities into a unified control framework, while ensuring compliance with Article 14 of the International Regulations for Preventing Collisions at Sea. A rule-based intelligent obstacle avoidance strategy is implemented through a designed multi-objective optimization scheme that explicitly considers actuator limitations and Article 14 compliance requirements of the International Regulations for Preventing Collisions at Sea. Static obstacles are accurately modeled using an extended safety margin, dynamic obstacles are predicted using linear assumptions, and regulatory constraints are incorporated as soft penalties in the objective function, enabling efficient, rule-compliant avoidance maneuvers.

[0040] The present invention can achieve reliable and smooth trajectory tracking and obstacle avoidance functions, and is suitable for static and dynamic complex environments; it can maintain excellent real-time computing performance under the constraints of onboard computing resources, ensuring the practical application feasibility of autonomous maritime navigation; by following international standardized navigation specifications, it significantly improves the system's adaptability and safety in complex navigation environments, and at the same time lays a solid foundation for the future commercial application of unmanned surface vessels in international waters. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0042] Figure 1 Schematic diagram of a three-degree-of-freedom motion model of the Otter unmanned surface vehicle according to an embodiment of the present invention;

[0043] Figure 2 This is a schematic diagram of Otter autonomous navigation according to an embodiment of the present invention;

[0044] Figure 3 This is a schematic diagram of the multi-obstacle path planning of the Otter unmanned surface vehicle according to an embodiment of the present invention;

[0045] Figure 4 This is a schematic diagram of control input for an unmanned surface vehicle according to an embodiment of the present invention;

[0046] Figure 5 Schematic diagram of velocity and position components according to an embodiment of the present invention;

[0047] Figure 6 Schematic diagram of nonlinear model predictive control calculation performance evaluation according to an embodiment of the present invention;

[0048] Figure 7 A schematic diagram of a trajectory of an unmanned surface vehicle avoiding dynamic obstacles according to an embodiment of the present invention;

[0049] Figure 8 This is a schematic diagram of dynamic obstacle avoidance control input according to an embodiment of the present invention;

[0050] Figure 9 A schematic diagram of the predictive control performance of a nonlinear model for navigation of an unmanned surface vehicle according to an embodiment of the present invention;

[0051] Figure 10 This is a flow chart of a dynamic obstacle avoidance control method for an unmanned surface vehicle based on real-time nonlinear model predictive control according to an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0053] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0054] This embodiment provides a method for dynamic obstacle avoidance control of an unmanned surface vehicle based on real-time nonlinear model predictive control, including:

[0055] Construct the dynamic model of the unmanned surface vehicle, as well as the static obstacle model and dynamic obstacle model;

[0056] Based on the dynamic model, the static obstacle model, and the dynamic obstacle model, a multi-objective optimization model is constructed, wherein the multi-objective optimization model uses trajectory tracking cost, obstacle avoidance cost, and control smoothing cost as objective functions, and uses motion range, speed limit, control input limit, and collision avoidance rules as constraints;

[0057] The state of the unmanned surface vehicle and the state of obstacles are obtained in real time, input into the multi-objective optimization model for solution, control input is obtained, and dynamic obstacle avoidance control of the unmanned surface vehicle is completed based on the control input.

[0058] Specifically, this embodiment adopts the three-degree-of-freedom dynamic model of the Otter unmanned surface vessel, organically integrating high-precision trajectory tracking with static and dynamic obstacle avoidance functions into a unified control framework, while ensuring compliance with Article 14 of the International Regulations for Preventing Collisions at Sea. Through the designed multi-objective optimization scheme, the actuator limitations and the compliance requirements of Article 14 of the International Regulations for Preventing Collisions at Sea are explicitly considered to achieve a rule-oriented intelligent obstacle avoidance strategy. Static obstacles are accurately modeled by extending the safety boundary, dynamic obstacles are predicted by linear assumptions, and regulatory constraints are included as soft penalty terms in the objective function to achieve efficient risk avoidance maneuvers that comply with the rules. Figure 10 As shown, the specific contents of the unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control provided by this embodiment are as follows:

[0059] 1. Construct a dynamic model of an unmanned surface vehicle;

[0060] The Otter unmanned boat uses a tail twin-propeller propulsion system to achieve steering through propeller speed difference. Due to the lack of transverse thrusters, the system has a typical under-actuated characteristic. Unlike manned ships that require multi-degree-of-freedom comfort design, Otter focuses on three basic horizontal degrees of freedom (surge, sway, and bow pitch) while omitting vertical degrees of freedom (heave, pitch, and roll). This simplified strategy effectively simplifies the coordinate conversion calculation between the north-east coordinate system and the hull fixed coordinate system, such as Figure 1 shown.

[0061] For ship motion control simulation, this embodiment adopts the classic Fossen model. The unmanned surface vehicle kinematic model mainly describes the conversion relationship between motion variables between two coordinate systems:

[0062]

[0063] Where η = [x, y, ψ] T is the posture state vector of the unmanned surface vehicle in the northeast coordinate system, x is the north position, y is the east position, ψ is the bow angle of the unmanned surface vehicle, and the clockwise direction is defined as positive; are the derivatives of the posture state vector, representing the north velocity, east velocity and steering angular velocity of the unmanned boat respectively; ν = [u, v, r] T is the velocity state vector in the UAV hull coordinate system, and R(ψ) is the transformation matrix from the hull coordinate system to the northeastern coordinate system:

[0064]

[0065] To establish an accurate USV dynamic model, this example relies on two key assumptions: the USV structure satisfies transverse symmetry and can be considered a rigid body with uniform mass distribution. In a fixed ship coordinate system, the Newton-Euler equations of motion are applied, combined with these assumptions, to derive a three-degree-of-freedom (3-DOF) USV dynamic model:

[0066]

[0067] Where, is the derivative of the velocity vector, which represents the acceleration of the unmanned boat; τ = [τ u ,0,τ r ] T is the control input vector of the unmanned boat, τ u and τ r represents the propulsion force and torque, where τ u =T p +T s , τ r =(T p -T s )B / 2, B is the distance between the two thrusters. τ w =[τ w,u ,τ w,v ,τ w,r ] T is the external environmental interference force; M is the additional mass matrix of the unmanned boat, which includes the weight inertia of the unmanned boat and the inertia added by hydrodynamics; C(ν) is the centrifugal force matrix; D(ν) is the damping matrix.

[0068] The specific forms of M, C(ν) and D(ν) are shown in equations (4)-(6):

[0069]

[0070] Where m 11 、m 22 、m 23 、m 32 and m 33 is the inertial mass parameter. Since the values ​​of D(ν) on the off-diagonal lines differ greatly from those on the diagonal lines, their influence on the control effect can be ignored. When the speed is small, d 23 (ν,r)=d 32 (ν,r)=0; the nonlinear damping term is very small, so the nonlinear damping term can be ignored and only the linear damping is considered, so d 11 (u) = -X u , d 22 (v,r)=-Y ν , d 33 (v,r)=-N r .

[0071] Through reasonable simplification and parameter optimization, combined with equations (1)-(6), the complete unmanned surface vehicle dynamic motion equation (7) is finally established, providing an accurate mathematical basis for the subsequent control strategy design.

[0072]

[0073] 2. Construct static obstacle models and dynamic obstacle models;

[0074] 2.1 Static obstacle modeling;

[0075] During the autonomous navigation of an unmanned surface vehicle, safe avoidance of static obstacles is achieved through accurate modeling. This method constructs a static obstacle model by appropriately expanding the minimum circumscribed circle of the obstacle and using the outer boundary of the circle as the safety boundary of the unmanned surface vehicle:

[0076]

[0077] In the formula, [x s ,y s ] T is the current position coordinate of the unmanned boat, [x s0 ,y s0 ] T is the center coordinate of the static obstacle; r s is the radius of the safety margin; r is the radius of the obstacle circumcircle; and k is the magnification factor. By adjusting the k value, the size of the safety margin can be flexibly set according to the navigation environment and safety requirements.

[0078] 2.2 Dynamic obstacle modeling;

[0079] During autonomous navigation, the dynamic obstacles encountered by unmanned surface vehicles are primarily other moving ships. Accurately predicting the future trajectory of a dynamic ship is challenging. To simplify the problem, this method assumes that the target ship will maintain linear motion from its current state. The target ship's position prediction equation is:

[0080]

[0081] In the formula, [x T (t),y T (t)] T is the predicted position of the target ship at time t, [x(t0),y(t0)] T is the position of the target ship at time t0; ψ T is the heading angle of the target ship at time t0, [u(t0),v(t0)] T is the longitudinal and transverse velocity of the target ship at time t0, Δt is the sampling time, and T(ψ) is the transformation matrix from the ship coordinate system to the northeast ground coordinate system. The prediction model achieves a reasonable balance between computational efficiency and prediction accuracy.

[0082] 3. Build a multi-objective optimization model;

[0083] This embodiment proposes a comprehensive control strategy for unmanned surface vehicles (USVs), which not only achieves high-precision trajectory tracking but also integrates intelligent collision avoidance functionality compliant with Article 14 of the International Regulations for Preventing Collisions at Sea. This design enables the USV to strictly adhere to internationally standardized navigation specifications during actual navigation, significantly enhancing the system's adaptability and safety in complex navigation environments. It also lays a solid regulatory foundation for the commercial use of USVs in international waters. The proposed method comprehensively considers the USV's physical constraints and environmental interference factors, enabling safe and efficient autonomous navigation in dynamic and complex marine environments. Figure 2 The system framework of Otter's autonomous navigation under this control strategy is intuitively demonstrated.

[0084] Design a real-time nonlinear model predictive control strategy with integrated collision avoidance functionality, specifically including the following:

[0085] 3.1 Constraints:

[0086] Considering the mechanical limitations of the propulsion and steering systems of the unmanned surface vehicle, its range of motion and speed are inevitably constrained. Therefore, the control strategy needs to fully consider constraints such as control input limits and control increments. In addition, to ensure the safety of the ship's navigation, reasonable boundary constraints must be imposed on its motion trajectory. The complete constraint conditions are given by Equation (10):

[0087]

[0088] Where x k+1 =[x k+1 ,y k+1 ,ψ k+1 ] T represents the state vector at time k+1, x k =[x k ,y k ,ψ k ] T represents the state vector at time k, u k =[T p ,T s ] T represents the control input vector at time k, x min and x max They represent the lower and upper limits of the state of the unmanned surface vehicle respectively. min =-120N, indicating the minimum control input, u max =120N represents the maximum control input, f(x k ,u k ) represents the dynamic model of the unmanned surface vehicle, h static and h dynamic They represent the static and dynamic obstacle avoidance constraints that ensure the unmanned surface vehicle avoids collision, Represents the constraint function that ensures compliance with the collision avoidance rules.

[0089] 3.2 Objective function:

[0090] The multi-objective optimization function is designed to ensure that the USV accurately follows the reference trajectory, effectively avoids obstacles, and minimizes control input changes, while strictly complying with physical constraints and collision avoidance rules. The complete objective function expression is:

[0091]

[0092] Where, J track 、J avoid and J control They represent the trajectory tracking, obstacle avoidance and control smoothness performance evaluation indicators, J final represents the final objective function after considering the collision constraint, g(x k ,x o,j,k ) represents the collision constraint function between the position of the unmanned boat and the obstacle, x k =[x k ,y k ,ψ k ] T represents the state vector at time k, x o,j,k represents the position of the j-th obstacle at time k, and ρ represents the weight coefficient of the collision avoidance constraint.

[0093] (1)Jtrack Evaluate the accuracy of the USV in following a planned trajectory:

[0094]

[0095] Where x k+i∣k 、y k+i∣k , ψ k+i∣k They represent the lateral position, longitudinal position and heading angle of the unmanned boat at the future k+i moment predicted at the k moment; x ref,k+i 、y ref,k+i , ψ ref,k+i They represent the reference position and heading angle of the reference trajectory at time k+i, Q1 and Q2 are weight matrices, and N p Indicates the prediction step size.

[0096] This function achieves a comprehensive evaluation of position and heading tracking accuracy by calculating the weighted sum of squared deviations between the predicted trajectory and the reference trajectory.

[0097] (2)J avoid Ensure that the unmanned surface vehicle can safely bypass obstacles:

[0098]

[0099] Where N o is the number of obstacles, w j is the weight coefficient of the j-th obstacle, σ j This function generates a "repulsive force field" that increases exponentially as the USV approaches the obstacle, naturally guiding the USV away from the danger zone.

[0100] (3)J control Optimize the amplitude and rate of change of the control input:

[0101]

[0102] Where u k+i represents the control input at time k+i predicted at time k, Δu k+i It represents the control input change at the future k+i moment predicted at the k moment; R and S are weight matrices, N c Indicates the control step size.

[0103] By adjusting the weight matrices Q1, Q2, R and S, a dynamic balance is achieved among the three objective functions, so that the unmanned surface vehicle can follow the predetermined trajectory as accurately as possible while ensuring safety, while maintaining a smooth change in the control input.

[0104] (4) To ensure compliance with the International Regulations for Preventing Collisions at Sea, particularly Article 14 regarding head-on encounters, and that when two power-driven vessels meet on opposite or nearly opposite courses and present a risk of collision, each vessel should turn starboard so as to pass the other vessel on its port side, the following soft constraints are designed in this embodiment:

[0105]

[0106] Among them, the constraint function g COLREGS Designed to:

[0107]

[0108] Where, ψ r represents the relative heading deviation of the unmanned surface vehicle in the collision avoidance direction, ψ s represents the safe heading deviation threshold, θ r represents the relative azimuth angle between the unmanned surface vehicle and the obstacle, θ m Indicates the angle threshold for head-on encounter (usually set at 15 degrees), d r Represents the relative distance between the unmanned surface vehicle and the obstacle, d s Indicates the safety distance threshold. In this embodiment, d s =r s .

[0109] Finally, by integrating trajectory tracking, obstacle avoidance, and smooth control objectives, a complete nonlinear model predictive control optimization problem is constructed:

[0110]

[0111] The following example simulation is used to further illustrate and verify this method:

[0112] To fully validate the effectiveness and practicality of the proposed control strategy, simulation tests were conducted using an Otter dual-thruster unmanned surface vehicle (USV) platform in two typical scenarios. The first scenario evaluated fixed-point navigation performance with static obstacle avoidance, while the second verified dynamic collision avoidance capabilities in compliance with Article 14 of the International Maritime Regulations. These simulation scenarios formed a systematic testing framework, fully verifying the practicality and effectiveness of the control strategy in maritime environments. The main parameter settings are detailed in Table 1.

[0113] Table 1

[0114]

[0115] (1) Fixed-point navigation performance evaluation:

[0116] In the static obstacle avoidance scenario, the initial point of the unmanned surface vehicle is η0 = [0,0,0] T, the initial velocity is ν0=[0,0,0] T The target point is η=[80,60,π / 12] T , the coordinates of the first obstacle are η ob1 =[35,10] T , the radius of the circumscribed circle is r1 = 15m; the coordinates of the second obstacle η ob2 =[50,50] T , the radius of the circumscribed circle is r2 = 10m; the safety distance threshold d s =0.5m.

[0117] like Figure 3 、 Figure 4 and Figure 5 As shown, the unmanned surface vehicle planned an optimal navigation route, precisely avoiding two static obstacles and accurately reaching the preset target point. Throughout the navigation process, the dual thruster control signals remained within the physical constraints, exhibiting smooth and continuous changes, effectively avoiding drastic control actions that could cause system damage.

[0118] Figure 6 The real-time performance of a nonlinear model predictive control controller in a fixed-point navigation scenario was demonstrated. The average iteration computation time was only 14.47 milliseconds, ensuring the controller's stable operation within real-time constraints. Despite minor peaks in computational load, the overall computational process remained stable and predictable, demonstrating the controller's efficiency in handling static obstacle avoidance tasks. This computational time stability is crucial for ensuring a smooth system response, making the nonlinear model predictive control approach particularly suitable for real-time unmanned surface vehicle navigation applications.

[0119] (2) Head-on collision avoidance capability verification:

[0120] The collision avoidance capability of the unmanned surface vehicle in a head-on encounter was tested, with a focus on verifying compliance with Article 14 of the International Regulations for Preventing Collisions at Sea. The initial state of the unmanned surface vehicle is η0 = [0, 0, π / 2] T , sailing in a straight line at a speed of u0 = 1.5 m / s, the initial state of the obstacle ship is η0′ = [0, 50, 3π / 2] T , coming towards you at a speed of u0′=1.2m / s.

[0121] Figure 7 、 Figure 8 Simulation results show that when the two ships approach the safety threshold, the controller automatically executes an evasive maneuver to starboard, as required by Regulation 14 of the International Maritime Convention for Preventing Collisions at Sea. After completing the avoidance maneuver, the ship intelligently returns to its original trajectory. The entire obstacle avoidance process demonstrates agile response and smooth operation, avoiding unnecessary large movements and fully demonstrating the practicality and regulatory compliance of the control strategy.

[0122] like Figure 9 As shown, the nonlinear model predictive control controller maintains efficient and reliable real-time computing performance during dynamic head-on encounters. The controller successfully handled both routine trajectory tracking and complex collision avoidance tasks, with an average computation time of 14.65ms per iteration, consistently staying within real-time control requirements. The temporary increase in computational load during the obstacle avoidance maneuver remained within the system's acceptable range, further demonstrating the feasibility and reliability of the controller for practical applications in autonomous marine systems.

[0123] The embodiments described above are merely descriptions of preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by persons skilled in the art should fall within the scope of protection defined by the claims of the present invention.

Claims

1. A dynamic obstacle avoidance control method for an unmanned surface vehicle based on real-time nonlinear model predictive control, characterized in that: include: Construct the dynamic model of the unmanned surface vehicle, as well as the static obstacle model and dynamic obstacle model; Based on the dynamic model, the static obstacle model, and the dynamic obstacle model, a multi-objective optimization model is constructed, wherein the multi-objective optimization model uses trajectory tracking cost, obstacle avoidance cost, and control smoothing cost as objective functions, and uses motion range, speed limit, control input limit, and collision avoidance rules as constraints; The state of the unmanned surface vehicle and the state of obstacles are obtained in real time, input into the multi-objective optimization model for solution, control input is obtained, and dynamic obstacle avoidance control of the unmanned surface vehicle is completed based on the control input.

2. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 1 is characterized in that: The dynamic model of the unmanned surface vehicle is: in, represents the derivative of the posture state vector of the unmanned surface vehicle in the northeast coordinate system, ν represents the velocity state vector of the unmanned surface vehicle in the hull coordinate system, R(ψ) represents the rotation matrix, M represents the mass matrix, represents the derivative of the velocity state vector of the unmanned surface vehicle in the hull coordinate system, C(ν) represents the centrifugal force matrix, D(ν) represents the damping matrix, τ=[τ u ,0,τ r ] T is the control input vector of the unmanned surface vehicle, τ u and τ r represents the propulsion force and torque, τ w It is the interference force of the external environment.

3. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 1, characterized in that: The static obstacle model is: Among them, [x s ,y s ] T is the current position coordinate of the unmanned surface vehicle, [x s0 ,y s0 ] T is the center coordinate of the static obstacle; r s is the radius of the safety boundary; r is the radius of the circumscribed circle of the obstacle; k is the magnification factor.

4. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 1, characterized in that: The dynamic obstacle model is: Among them, [x T (t),y T (t)] T is the predicted position of the target ship at time t, [x(t0),y(t0)] T is the position of the target ship at time t0; ψ T is the heading angle of the target ship at time t0, [u(t0),v(t0)] T is the longitudinal and transverse velocity of the target ship at time t0, Δt is the sampling time, and T(ψ) is the transformation matrix from the ship coordinate system to the northeast coordinate system.

5. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 1, characterized in that: The objective function is expressed as: Among them, J track 、J avoid and J control They represent the trajectory tracking cost, obstacle avoidance cost and control smoothing cost respectively, and J final represents the final objective function after considering the collision constraint, g(x k ,x o,j,k ) represents the collision constraint function between the position of the unmanned boat and the obstacle, x k represents the state vector at time k, x o,j,k represents the position of the j-th obstacle at time k, and ρ represents the weight coefficient of the collision avoidance constraint.

6. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 5, characterized in that: The trajectory tracking cost in the objective function is used to evaluate the accuracy of the unmanned surface vehicle following the predetermined trajectory, which is expressed as: Among them, J track represents the trajectory tracking cost, x k+i∣k 、y k+i∣k , ψ k+i∣k They represent the lateral position, longitudinal position and heading angle of the unmanned boat predicted at time k+i in the future; x ref,k+i 、y ref,k+i , ψ ref,k+i Represent the reference position and heading angle of the reference trajectory k+i, Q1 and Q2 represent the weight matrix, N p Indicates the prediction step size.

7. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 5, characterized in that: The obstacle avoidance cost in the objective function is used to ensure that the unmanned surface vehicle safely bypasses obstacles and is expressed as: Among them, J avoid represents the obstacle avoidance cost, N o is the number of obstacles, w j is the weight coefficient of the j-th obstacle, σ j is the coefficient related to obstacle size and safety distance, x k+i∣k represents the lateral position of the unmanned boat at the future k+i moment predicted at the k moment, x o，j，k+i represents the position of the jth obstacle at time k+i, N p Indicates the prediction step size.

8. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 5, characterized in that: The control smoothing cost in the objective function is used to optimize the amplitude and rate of change of the control input, which is expressed as: Among them, J control represents the control smoothing cost, u k+i represents the control input at time k+i predicted at time k, Δu k+i It represents the control input change at the future k+i moment predicted at the k moment; R and S are weight matrices, N c Indicates the control step size.

9. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 5, characterized in that: The collision avoidance rules are used to stipulate that when two unmanned surface vessels meet in opposite or nearly opposite directions and are about to collide, they should each turn right, as expressed as: Among them, g represents the collision avoidance rule constraint, ψ r represents the relative course deviation of the unmanned surface vehicle in the collision avoidance direction, ψ s represents the safe heading deviation threshold, θ r represents the relative azimuth angle between the unmanned surface vehicle and the obstacle, θ m Indicates the angle threshold for head-on encounter, d r Represents the relative distance between the unmanned surface vehicle and the obstacle, d s Indicates the safety distance threshold.

10. The unmanned surface vehicle dynamic obstacle avoidance control method based on real-time nonlinear model predictive control according to claim 9, characterized in that: The constraints are expressed as: Among them, x k+1 represents the state vector at time k+1, x k represents the state vector at time k, u k represents the control input vector at time k, f(x k ,u k ) represents the dynamic model of the unmanned surface vehicle, x min and x max They represent the lower and upper limits of the state of the unmanned surface vehicle, u min and u max They represent the lower and upper limits of the control input of the unmanned surface vehicle, h static (x k ,y k ) and h dynamic (x k ,y k , v k , r k ) are the static obstacle avoidance constraint function and the dynamic obstacle avoidance constraint function to ensure the collision avoidance of the unmanned surface vehicle, Represents the collision avoidance rule constraint function.

Images