An underactuated unmanned surface vehicle obstacle avoidance method based on distributed model predictive control

Abstract

The embodiment discloses a kind of underactuated unmanned ship obstacle avoidance methods based on distributed model predictive control, through single oar single rudder underactuated unmanned ship kinematics model and dynamics model, the cost function considering dynamic obstacle avoidance cost is established;And the objective function and constraint function based on the cost function are established, the optimal control input of unmanned ship is obtained by solving the objective function, to realize the obstacle avoidance of underactuated unmanned ship.The optimal control input of unmanned ship is obtained by fully utilizing dynamic obstacle information (including neighbor unmanned ship), so that unmanned ship can have higher obstacle avoidance accuracy when performing tasks, effectively improve the autonomous obstacle avoidance ability of multi-ship system in complex environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned surface vessel (USV) control technology, and in particular to an underactuated USV obstacle avoidance method based on distributed model predictive control. Background Technology

[0002] When unmanned surface vessels (USVs) perform patrol or reconnaissance missions in environments such as oceans, rivers, and lakes, they must autonomously avoid various obstacles in order to precisely control their course. Static obstacles (such as islands, reefs, and buoys) have fixed positions, while dynamic obstacles (such as other vessels) have positions that change over time. Therefore, the trajectory tracking and control of USVs needs to consider information from both static and dynamic obstacles to ensure navigational safety.

[0003] For multi-boat formation control, centralized methods are computationally intensive and lack robustness. If the central computing node is disturbed, the cluster system will be significantly affected, thus distributed control has attracted attention. Distributed Model Predictive Control (DMPC) divides the entire system into multiple subsystems, each of which independently optimizes its control strategy based on its own state and information from neighboring boats, ultimately achieving global coordination. Compared with centralized schemes, DMPC has advantages such as good scalability (computational complexity increases linearly with the number of boats), strong fault tolerance (single boat failure does not affect the whole system), and high real-time performance (distributed computing reduces the burden on a single controller). Furthermore, in multi-boat cooperative model predictive control, formation maintenance can be achieved by introducing a neighbor state consistency term into the cost function, and collisions between boats can be forcibly avoided through state constraints.

[0004] However, existing technologies have achieved tracking and control of multi-unmanned surface vessel (USV) formations through methods such as distributed model predictive control, but they have not made full use of information on dynamic obstacles (especially other vessels), resulting in low obstacle avoidance accuracy and unsatisfactory obstacle avoidance performance when USVs are performing tasks. Summary of the Invention

[0005] This invention discloses an obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control, in order to overcome the above-mentioned technical problems.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows:

[0007] An obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control includes the following steps:

[0008] S1: Establish the kinematic model and error dynamic model of the single-propeller, single-rudder, underactuated unmanned surface vessel, so as to obtain the differential kinematic model based on the error dynamic model.

[0009] S2: Establish a dynamic model of a single-propeller, single-rudder, underactuated unmanned surface vessel to obtain a differential dynamic model;

[0010] S3: Based on the kinematic and dynamic models, establish a cost function that considers the avoidance cost of dynamic obstacles, including neighboring unmanned surface vessels;

[0011] S4: Establish an objective function based on the cost function, and establish a constraint function according to the differential kinematic model and the differential dynamic model to solve the objective function, obtain the optimal control input of the unmanned surface vessel, and thus realize obstacle avoidance of the underactuated unmanned surface vessel.

[0012] Beneficial Effects: This invention provides an obstacle avoidance method for underactuated unmanned surface vessels (USVs) based on distributed model predictive control. It establishes a cost function considering the dynamic obstacle avoidance costs using a kinematic and dynamic model of a single-propeller, single-rudder underactuated USV. Furthermore, it establishes an objective function and constraint functions based on this cost function, solves the objective function to obtain the optimal control input for the USV, thereby achieving obstacle avoidance. This invention fully utilizes dynamic obstacle information (including neighboring USVs) to obtain the optimal control input for the USV, enabling it to achieve high obstacle avoidance accuracy during mission execution and effectively improving the autonomous obstacle avoidance capability of multi-USV systems in complex environments. Attached Figure Description

[0013] To more clearly illustrate the technical solutions in the embodiments of the present 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 the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0014] Figure 1 This is a flowchart of the underactuated unmanned surface vessel obstacle avoidance system based on distributed model predictive control of the present invention;

[0015] Figure 2 This is a schematic diagram of the hardware and software architecture of a single unmanned surface vessel according to an embodiment of the present invention;

[0016] Figure 3 This is a schematic diagram of distributed communication according to an embodiment of the present invention;

[0017] Figure 4 This is a flowchart of the distributed model predictive control algorithm according to an embodiment of the present invention;

[0018] Figure 5 This is a schematic diagram of the coordinate system of an unmanned surface vessel according to an embodiment of the present invention;

[0019] Figure 6 This is a flowchart of the distributed model predictive control algorithm code according to an embodiment of the present invention;

[0020] Figure 7 This is a schematic diagram of the single-boat model predictive control trajectory according to an embodiment of the present invention;

[0021] Figure 8 This is a schematic diagram of the three-boat distributed model predictive control trajectory according to an embodiment of the present invention;

[0022] Figure 9 This is a schematic diagram of the time-domain curves of the state and control variables of the No. 1 unmanned surface vessel in an embodiment of the present invention;

[0023] Figure 10 This is a schematic diagram of the time-domain curves of the state and control variables of the No. 2 unmanned surface vessel according to an embodiment of the present invention;

[0024] Figure 11 This is a schematic diagram of the time-domain curves of the state and control variables of the No. 3 unmanned surface vessel in an embodiment of the present invention. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] This embodiment introduces an obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control, such as... Figure 1 As shown, it includes the following steps:

[0027] S1: Establish the kinematic model and error dynamic model of the single-propeller, single-rudder, underactuated unmanned surface vessel, so as to obtain the differential kinematic model based on the error dynamic model.

[0028] Preferably, S1 includes:

[0029] S11: The kinematic model of the single-propeller, single-rudder, underactuated unmanned surface vessel is established as follows:

[0030] (1)

[0031] In the formula: This represents the x-coordinate of the unmanned surface vessel in a fixed coordinate system. express The first derivative; This represents the ordinate of the unmanned surface vessel in a fixed coordinate system; express The first derivative; This represents the heading angle of the unmanned surface vessel, i.e., the angle between the bow and the fixed coordinate system. The angle between the axes; express The first derivative; This represents the longitudinal velocity of the unmanned surface vessel in the moving coordinate system; This indicates the lateral speed of the unmanned surface vessel; This indicates the bow angular velocity of the unmanned surface vessel.

[0032] Specifically, Figure 5 The coordinate system represents the motion of the unmanned surface vessel (USV), where, This represents a fixed coordinate system that does not change over time. This represents the motion coordinate system, which moves along with the unmanned surface vessel (USV). The origin of the motion coordinate system is chosen as the center of gravity of the USV. The axis is selected along the ship's length, with the direction pointing towards the bow being positive. The axis is perpendicular to the longitudinal section.

[0033] S12: The error dynamic model is established as follows:

[0034] Specifically, assume that the trajectory that the unmanned surface vessel needs to track is composed of discrete points. It is obtained by fitting a cubic curve and can be expressed as a function: ;in, This indicates that the unmanned surface vessel needs to track the first [unclear] in a fixed coordinate system. The x-coordinates of discrete points; This indicates that the unmanned surface vessel needs to track the first [unclear] in a fixed coordinate system. The ordinates of discrete points; Indicates the number of discrete points; This represents the trajectory curve that the unmanned surface vessel needs to track in a fixed coordinate system; Represents the coefficient of the cubic term; Represents the coefficient of the quadratic term; Denotes the coefficient of the linear term; Represents a constant term;

[0035] Therefore, at each prediction step, it is possible to determine the outcome based on the unmanned surface vessel's... and Calculate the lateral tracking error deviation from heading angle Then, an error dynamic model is established, with the specific formula as follows:

[0036] (2)

[0037] In the formula: Indicates the unmanned surface vessel (USV) Lateral tracking error at each discrete time step; Indicates the unmanned surface vessel (USV) The heading angle error at each discrete time step; Indicates and The corresponding ordinate value; Indicates the unmanned surface vessel (USV) The ordinate of each discrete time step; This represents the trajectory curve that the unmanned surface vessel (USV) needs to track in a fixed coordinate system. The x-axis of each discrete time step; Indicates the index number of the discrete time step; This indicates the trajectory curve that the unmanned surface vessel (USV) needs to track in a fixed coordinate system at the [number]th [position]. The slope of the tangent at each discrete time step; Indicates that the unmanned surface vessel is in The heading angle at each discrete time step;

[0038] S13: Based on the aforementioned error dynamic model, the kinematic model after differentiation is obtained as follows:

[0039] Specifically, the continuous kinematic differential model is discretized into a differential kinematic model, and the lateral tracking error and heading angle error are calculated.

[0040] (3)

[0041] In the formula: Indicates the unmanned surface vessel's position in the moving coordinate system. The longitudinal velocity of each discrete time step; Indicates the unmanned surface vessel (USV) The heading angle at each discrete time step; Indicates the difference interval; Indicates the unmanned surface vessel's position in the moving coordinate system. Lateral velocity at each discrete time step; Indicates the unmanned surface vessel (USV) The heading angular velocity at each discrete time step.

[0042] S2: Establish a dynamic model of a single-propeller, single-rudder, underactuated unmanned surface vessel to obtain a differential dynamic model;

[0043] Preferably, the dynamic model of the single-propeller, single-rudder, underactuated unmanned surface vessel is established as follows:

[0044] Specifically, in order to fully consider the dynamic characteristics of the system, such as inertia, damping, hydrodynamic nonlinearity, and input response delay, a dynamic model of the unmanned surface vessel is introduced to enhance the control system's ability to characterize the actual response behavior of the vessel and improve the accuracy, robustness, and practicality of the obstacle avoidance control algorithm.

[0045] (4)

[0046] In the formula: express The first derivative of is the longitudinal acceleration of the unmanned surface vessel in the moving coordinate system; Indicates the mass of the unmanned surface vessel; Represents the longitudinal velocity of the unmanned surface vessel in the moving coordinate system. The corresponding linear damping derivative; Represents longitudinal acceleration The corresponding additional mass derivative; This indicates the bow angular velocity of the unmanned surface vessel; The nonlinear damping derivative represents the quadratic term of the longitudinal velocity; Indicates the lateral speed of the unmanned surface vessel The corresponding linear transverse force derivative; Indicates lateral acceleration The corresponding lateral additional mass derivative; The nonlinear derivative of the transverse force, representing the quadratic term of the transverse velocity; Indicates longitudinal thrust; for The first derivative of , i.e., the lateral acceleration of the unmanned surface vessel; express The absolute value; The derivative of the gyratory torque corresponding to the heading angular velocity; The derivative of the additional moment of inertia corresponding to the heading angular acceleration; The derivative of the nonlinear gyratory torque representing the second term of the heading angular velocity; This represents the moment of inertia of the unmanned surface vessel about its vertical axis. Indicates the turning torque; This represents the absolute value of the heading angular velocity.

[0047] Discretize the dynamic differential model of the unmanned surface vessel in equation (4) into a difference-based dynamic model:

[0048] (5)

[0049] At this point, the kinematic and dynamic models of the unmanned surface vessel (USV) have been constructed, and both have been discretized into difference models. Lateral tracking error and heading angle error are introduced, and the dynamic equations for the discrete errors of the kinematic model are established, thus constructing a complete prediction model.

[0050] In this embodiment, a basic control unit is built, and a communication and computing architecture is established. With the support of the communication and computing architecture in this embodiment, a kinematic and dynamic model suitable for a single-propeller, single-rudder, underactuated unmanned surface vessel is established, providing a system modeling foundation for distributed model predictive control algorithms.

[0051] Specifically, the basic control unit of the unmanned surface vessel includes an integrated controller module and a computing module, and is configured with corresponding firmware and an embedded operating system to form a scalable hardware and software operating environment, laying the foundation for distributed computing and multi-sensor data processing.

[0052] Specifically, the basic control unit of the unmanned surface vessel consists of a controller module and a computing module, which are used to realize the underlying motion control and the upper-level algorithm execution and communication coordination functions, respectively.

[0053] The controller module uses the open-source Pixhawk 2.4.8 flight controller, which features a highly integrated inertial navigation unit (IMU) containing an accelerometer, gyroscope, and magnetometer, enabling basic navigation functions such as attitude estimation and heading determination. The Pixhawk 2.4.8 then connects to an M9N GPS module via SWITCH, GPS, and I2C interfaces for high-precision positioning, acquiring real-time position, heading angle, and velocity information to meet surface positioning requirements. This completes the connection of the necessary hardware sensors for the controller. Finally, an electronic speed controller and servo motor are connected to the Pixhawk 2.4.8's PWM output port, with the electronic speed controller connected to the motor, forming a basic motion control unit.

[0054] The Pixhawk 2.4.8 flight controller runs on PX4-based open-source flight controller firmware, supporting flexible parameter configuration and multiple control modes. It is widely used in unmanned systems and possesses excellent real-time performance and stability. This embodiment downloads the 1.12 version of the PX4 source code from the internet and flashes it to Pixhawk 2.4.8 using QGroundControl software. After successful firmware flashing, the corresponding parameters are configured in the QGroundControl software. For the driver parameters, the settings should be based on the PWM output ports of the external ESC and servo motors. The parameter MAV_1_CONFIG is changed to TELEM2 for connecting to the microcomputer described later. Sensor parameter configuration can be done by manually changing the attitude of Pixhawk 2.4.8 according to the interface prompts, and the parameters will be automatically adjusted. Other less important parameters have default configurations and will not be described in detail here.

[0055] The computing module uses a Raspberry Pi 4B embedded microcomputer, running the Ubuntu 18.04 operating system. It features a multi-core ARM Cortex-A72 processor and strong edge computing capabilities, meeting the requirements for real-time optimization computing and multi-threaded task scheduling. To achieve efficient collaboration between modules and system component integration, the ROS (Robot Operating System) operating system is deployed on the computing module. ROS, as an open-source middleware for robot software architecture, provides mechanisms such as node communication, topic publishing / subscription, and service invocation, making data interaction and scheduling between the controller and upper-layer algorithm modules more efficient and modular. However, Pixhawk 2.4.8 uses the MAVLink protocol to transmit control commands and sensor data. To facilitate the interoperability and mutual conversion between ROS messages and MAVLink messages for development purposes, this platform uses the open-source MAVROS software package. In this embodiment, the SIM card of the Raspberry Pi 4B is connected to the computer via a card reader, and the Ubuntu 18.04 operating system is burned to the SIM card using the Raspberry Pi Imager software, allowing the Raspberry Pi 4B to run the Ubuntu 18.04 operating system. Next, the physical pins 8, 10, and 12 of the Raspberry Pi 4B BOARD are connected to the TELEM2 port of the Pixhawk 2.4.8. This completes the hardware connection between the Raspberry Pi 4B and Pixhawk 2.4.8, enabling communication between them via this serial port. The Raspberry Pi is then installed with the noetic version of ROS and the corresponding MAVROS via apt. It's important to note that ROS and MAVROS are merely middleware for the algorithms within a single unmanned surface vessel (USV) and Pixhawk 2.4.8; other versions can be used.

[0056] Specifically, the hull of this unmanned surface vessel (USV) is made of lightweight fiberglass composite material, and the overall structure is a monohull. The hull has a streamlined design, with dimensions of 120cm × 40cm × 30cm. The interior of the hull features compartments forming independent waterproof chambers, and a reserved electronics bay for installing controllers and power modules. The motion execution module consists of a DC brushed motor with a propeller, and a servo motor with rudder blades. The propeller diameter is 5.0cm, and the servo motor model is MG995, with a rudder length of 10cm and a rudder width of 5.4cm. The power system uses lithium batteries to perform voltage conversion and current distribution for each module, ensuring stable power supply to each unit.

[0057] Through the above integration, a single vessel possesses complete state awareness, task computation, and execution control capabilities, providing feasible platform support for the subsequent deployment of distributed cooperative control and obstacle avoidance algorithms. For example... Figure 2 As shown, this illustrates the hardware and software structure of a single-boat basic structure.

[0058] Specifically, the overall flow of the distributed model predictive control algorithm in this embodiment is as follows: Figure 4 As shown. The basic idea of ​​distributed model predictive control is: at each sampling time, the system's mathematical model (i.e., the kinematic and dynamic model of a single-propeller, single-rudder, underactuated unmanned surface vessel) is used to predict the state within a finite time domain in the future. Based on this, a cost function and constraints are constructed, and the optimal control input is obtained through rolling optimization. The control quantity from the first step is then applied to the system. As time progresses, the optimization problem is repeatedly solved, thereby achieving real-time trajectory tracking and obstacle avoidance control in dynamic environments.

[0059] Within this framework, an accurate system model is a prerequisite for prediction. To implement an obstacle avoidance strategy based on distributed model predictive control, a mathematical model suitable for this system's single-propeller, single-rudder, underactuated unmanned surface vessel (USV) must first be established as a prediction model to describe the USV's motion characteristics on the water surface. Specifically, a kinematic model is first introduced to characterize the evolution of the USV's position and heading angle in a two-dimensional plane over time. This model is simple in structure, computationally efficient, and suitable for the preliminary design of the controller and trajectory generation. Subsequently, based on the kinematic model, a dynamic model is introduced to more accurately reflect the actual impact of the propulsion system and servo actuators on the hull's motion, thereby improving the accuracy and reliability of the prediction results.

[0060] To simplify the complexity of model motion and analysis, the unmanned surface vessel (USV) is assumed to be a rigid body, and only its motion on the water surface is considered, i.e., only the motion in the three directions of sway, roll, and pitch is considered, while the influence of viscous fluid on the USV's motion is ignored.

[0061] In this embodiment, a wireless communication module is integrated into each terminal node to construct a distributed communication and control architecture. This architecture supports independent operation of control algorithms by each node, while achieving information interconnection and state sharing through a highly reliable communication network.

[0062] Specifically, after assembling a single unmanned surface vessel (USV), in order to realize the distributed collaborative control and state sharing mechanism of the USV swarm, this step involves deploying communication modules at multiple terminal nodes and building a unified wireless communication network and message transmission protocol system to form a multi-node distributed communication and control platform.

[0063] The core objective of the multi-node distributed communication and control platform is to ensure stable, low-latency, and bidirectional data exchange between nodes, thereby supporting each node to independently run control algorithms and achieve interconnection. This multi-node distributed communication and control platform adopts a Wi-Fi-based wireless communication method, offering the following significant advantages: support for the IP protocol stack, enabling direct TCP / UDP data transmission and facilitating network expansion of the system; each node can be assigned a unique IP address, supporting remote access and management (e.g., Anji can remotely SSH into a Raspberry Pi 4B); it boasts high data throughput and network compatibility; and it can be seamlessly integrated with existing middleware systems such as MQTT.

[0064] To increase the communication distance and anti-interference capability of each node, the multi-node distributed communication and control platform adopts a new generation of low-power long-distance Wi-Fi communication technology—Wi-Fi HaLow (IEEE 802.11ah), which operates at a frequency of around 900MHz. While maintaining low power consumption, it can provide a communication distance of hundreds of meters, making it particularly suitable for the deployment needs of unmanned platforms in outdoor and open water areas.

[0065] Subsequently, by configuring multiple Wi-Fi HaLow communication modules, the unmanned surface vessels at each node were able to access the same local area network, and a unified network communication architecture was built on this basis.

[0066] In terms of communication protocol design, a publish / subscribe mechanism based on the MQTT (Message Queuing Telemetry Transport) protocol is adopted to achieve efficient and lightweight data interaction between multiple nodes. The MQTT protocol has the following advantages: simple architecture, low bandwidth consumption, especially suitable for low bandwidth and low power consumption scenarios; support for QoS levels and message retention mechanisms to enhance communication reliability; flexible topic structure, which facilitates nodes to subscribe to data of interest on demand; and the ability to realize asynchronous message communication between multiple nodes.

[0067] One of the methods involves deploying an MQTT Broker on a selected node. This broker serves as the central coordinating node, responsible for receiving, forwarding, and managing the data streams published by each unmanned surface vessel (USV) node. It also facilitates real-time monitoring of the operational status of all USVs on the server node, including location information, system status, and trajectory execution.

[0068] Each unmanned surface vessel is equipped with an MQTT Client node, which periodically publishes its own status information, including but not limited to: real-time location information (latitude and longitude or planar coordinates); heading angle; longitudinal and lateral speeds; and the current predicted trajectory sequence.

[0069] At the same time, each unmanned surface vessel (USV) also subscribes to the status and predicted trajectory information of other USVs, so that when running its local control algorithm (such as DMPC), it can fully obtain the dynamic information of "neighboring vessels" and achieve effective dynamic obstacle avoidance and swarm coordinated control.

[0070] In this embodiment, after setting up the basic control unit for the unmanned surface vessel (USV), each USV Raspberry Pi connects to a Wi-Fi HaLow communication module via an Ethernet interface and is powered. Each Raspberry Pi node is configured with a different IP address, allowing multiple nodes to connect to the same local area network. Subsequently, under the Raspberry Pi Ubuntu operating system, CMake version 3.10 or higher is upgraded, and the open-source libraries paho.mqtt.c-1.3.10 and paho.mqtt.cpp-1.3.2 are installed. At this point, the MQTT library can be imported and used when writing code.

[0071] The client node sets the published topic name to "shipnum_value," where "shipnum" is the ID of each unmanned surface vessel (USV) to distinguish data sources. It then sets the corresponding published topic data format to "lat:gpsbk_lat,lon:gpsbk_lon,vel:gpsbk_vel,yaw:real_yaw,pre_xy:x_pre,y_pre," where gpsbk_lat, gpsbk_lon, gpsbk_vel, real_yaw, x_pre, and y_pre represent the USV's current latitude, longitude, speed, heading angle, and predicted x and y coordinates in the local Cartesian coordinate system, respectively. The data is then converted to a string format and published to the MQTT Broker cyclically at 0.2-second intervals. Simultaneously, this node continuously subscribes to data published by other nodes, retrieving topic names and parsing corresponding data to update the status information of each node in this distributed communication architecture.

[0072] like Figure 3 The diagram illustrates a multi-node distributed communication system. Multiple unmanned surface vessels (USVs) communicate via a Wi-Fi HaLow module, placing them on the same local area network. The communication protocol is MQTT, enabling the exchange of information between the USVs.

[0073] At this point, the hardware and software platform of this embodiment has been completed. It possesses a complete onboard perception and decision-making unit, a stable and reliable communication and data interaction mechanism, an architecture that supports the operation of distributed control algorithms, and good scalability and remote monitoring capabilities. This platform provides a solid foundation for the subsequent deployment and experimental verification of distributed obstacle avoidance control algorithms.

[0074] S3: Based on the kinematic and dynamic models, establish a cost function that considers the cost of dynamic obstacle avoidance;

[0075] Preferably, the cost function is expressed as follows:

[0076]

[0077] in,

[0078] (6)

[0079] In the formula: Indicates the cost of tracking error; This represents the prediction time domain of model predictive control; Indicates the index number of the discrete time step; This represents the weighting coefficient for the lateral tracking error; The model predictive control is represented by the first Lateral tracking error at each discrete time step; Denotes the Euclidean norm; This represents the heading angle error weighting coefficient; Indicates the unmanned surface vessel (USV) The heading angle error at each discrete time step; This represents the speed weighting coefficient; Indicates the unmanned surface vessel's position in the moving coordinate system. The longitudinal velocity of each discrete time step; This represents the desired longitudinal velocity of the unmanned surface vessel;

[0080] Specifically, a tracking error cost is established to measure the deviation between the current state of the unmanned surface vessel (USV) (including position, heading angle, and velocity) and the desired trajectory. This cost includes: lateral tracking error (the lateral offset between the vessel's center of gravity and the desired trajectory); heading angle error (the deviation between the actual and desired heading angle); and velocity error (the difference between the longitudinal velocity and the desired velocity). By incorporating these error terms into the cost function, the system can be guided to approach the desired state within the prediction domain.

[0081] Specifically, to avoid excessive control input impacting the platform and reduce energy consumption, a control input cost is introduced into the cost function, including the costs related to the longitudinal thrust input and the yaw torque input, ultimately balancing control accuracy and control amplitude. The specific formula is as follows:

[0082] (7)

[0083] In the formula: Indicates the cost of controlling input; This represents the longitudinal thrust weighting coefficient; The model predictive control is represented by the first Longitudinal thrust at discrete time steps; This represents the weighting coefficient for the turning torque; The model predictive control is represented by the first The turning torque at each discrete time step.

[0084] Specifically, considering the physical limitations and response inertia of the actuators in the propulsion and servo systems, a control smoothing cost is introduced into the cost function, including the cost of longitudinal thrust variation and the cost of yaw torque variation. This term is used to suppress drastic changes in control commands, improve the smoothness and feasibility of control signals, and ensure stable system operation. The specific formula is as follows:

[0085] (8)

[0086] In the formula: This indicates the cost of controlling smoothing; Indicates the weighting coefficient for longitudinal thrust variation; This represents the weighting coefficient for the change in bow torque;

[0087] Specifically, to achieve safe navigation and obstacle avoidance control in a multi-vessel environment, the cost function simultaneously introduces: static obstacle avoidance cost: a rejection term is constructed based on the preset coordinates of known obstacles in the environmental map to ensure a safe distance from obstacles during path planning; dynamic obstacle avoidance cost: the trajectories of other unmanned surface vessels in the prediction domain are introduced as time-varying obstacles, and each vessel must consider the predicted paths of the other two vessels and avoid them in its own optimization. This strategy leverages the shared predicted trajectories in a distributed communication structure, enhancing the system's coordination and foresight. The specific formula is as follows:

[0088] (9)

[0089] In the formula: It represents the cost of obstacle avoidance, including static obstacle avoidance cost and dynamic obstacle avoidance cost; This represents the obstacle avoidance weight coefficient for static obstacles; The model predictive control is represented by the first The distance between the unmanned surface vessel and static obstacles at each discrete time step; This indicates a tendency to avoid odd positive numbers when the distance is 0; This represents the obstacle avoidance weight coefficient for dynamic obstacles; The model predictive control is represented by the first The discrete time step and the prediction of the first The Euclidean distance of a dynamic obstacle at a discrete time step;

[0090] Specifically, to achieve coordinated maintenance of multiple unmanned surface vessel (USV) swarms, an inter-vessel attraction cost term is introduced into the cost function to maintain the spatial relationships between USVs and ensure the overall structural stability of the swarm. This term constrains the distance between adjacent USVs in the prediction time domain, ensuring that each USV maintains a preset relative position or distance relationship during motion, thus reflecting the swarm's coordination and consistency. The specific formula is as follows:

[0091] (10)

[0092] In the formula: This refers to the inter-ship attraction cost used to maintain spatial relationships between ships; Indicates the neighboring vessel number of the current unmanned surface vessel; This indicates the number of neighboring unmanned surface vessels (USVs) currently in use. Indicates the current unmanned surface vessel and the first The weighting coefficients of the attraction costs between unmanned surface vessels; Indicates the current unmanned surface vessel and the first The unmanned surface vessel in the The Euclidean distance of the step; Indicates the current unmanned surface vessel and the first The expected distance between the unmanned surface vessels.

[0093] Specifically, this embodiment, based on a system model (including the kinematic and dynamic models of a single-propeller, single-rudder underactuated unmanned surface vessel), comprehensively considers factors such as its own desired trajectory, the predicted trajectories of other vessels, and static obstacles, and designs a cost function that includes multi-vessel cooperation and obstacle avoidance constraints. This embodiment constructs the total cost function through a linear weighted combination.

[0094] Specifically, after establishing the kinematics and dynamics model of the unmanned surface vessel (USV), it is still necessary to transform the control objectives, such as "how to achieve desired trajectory tracking, maintain inter-vessel cooperation, and safely avoid obstacles," into a mathematical form that can be directly solved by the optimizer. The core idea of ​​distributed model predictive control is to make the system state satisfy the predetermined objectives and constraints as much as possible within the finite prediction time domain. To this end, this embodiment constructs a cost function specifically for the needs of multi-vessel cooperation and obstacle avoidance control. This function not only measures the quality of the predicted trajectory and control input but also determines the performance index pursued by the optimization process.

[0095] S4: Establish an objective function based on the cost function, and establish a constraint function according to the differential kinematic model and the differential dynamic model to solve the objective function and obtain the optimal control input of the unmanned surface vessel to achieve obstacle avoidance of the underactuated unmanned surface vessel.

[0096] Specifically, by solving the cost function that includes multi-vessel cooperation and obstacle avoidance constraints, the distributed optimization problem is solved in real time, and the optimal control input of each vessel in the prediction time domain is obtained, thereby achieving the cooperative optimization of path tracking and obstacle avoidance objectives.

[0097] Specifically, after completing the system kinematics and dynamics modeling, constructing the cost function, and discretizing it, the optimal control sequence is obtained within the given prediction time domain by solving the optimization problem.

[0098] Preferably, S4 includes:

[0099] S41: Define state variables and control variables to determine decision variables:

[0100] (11)

[0101] In the formula: Indicates that the unmanned surface vessel is in A system state vector at discrete time steps; Indicates transpose; Indicates that the unmanned surface vessel is in The system control vector for each discrete time step; Represent decision variables; Represents the initial state vector; This represents the state vector at the end of the prediction time domain; Indicates the initial control vector; This represents the control vector at the end of the prediction time domain; The model predictive control is represented by the first The turning torque at each discrete time step; The model predictive control is represented by the first Longitudinal thrust at discrete time steps;

[0102] S42: Based on the aforementioned decision variables, establish the objective function:

[0103] Therefore, in order to solve the optimal control problem with nonlinear constraints, this embodiment uniformly models it as a nonlinear programming problem of the following general form:

[0104] (12)

[0105] In the formula: Represent decision variables; Let be the cost function to be minimized, that is, the performance metric that we want to minimize under the constraints. The constraint function represents the constraints of the prediction model established in S1, including the kinematic model after difference, the dynamic model after difference, and the error dynamic model. and These are the lower and upper bounds of the decision variable, used to describe the physical or practical feasible region.

[0106] Specifically, The specific expression is:

[0107] (13)

[0108] In the formula, Indicates the initial prediction model constraints; This represents the constraints of the prediction model at the end of the prediction time domain; Indicates the first Constraint functions for each discrete time step;

[0109] S43: Introducing Lagrange multipliers, constructing a Lagrange function based on a cost function that includes multi-ship cooperation and obstacle avoidance constraints:

[0110] Subsequently, in order to solve the constrained optimization problem of formula (12), Lagrange multipliers are introduced. Construct a Lagrangian function based on a cost function that includes multi-ship cooperation and obstacle avoidance constraints:

[0111] (14)

[0112] In the formula: Represent the Lagrange function; For Lagrange multipliers;

[0113] S44: Solve for the Lagrangian function to obtain the optimal control input for the unmanned surface vessel, including:

[0114] S441: KKT conditions required to construct a local optimum:

[0115] Specifically, the Lagrangian function in formula (14) integrates the original cost function and equality constraints into the solution framework. Under certain regularity conditions, a local optimum is found. The Karush-Kuhn-Tucker (KKT) optimality condition must be satisfied, specifically expressed as:

[0116] (15)

[0117] In the formula: This represents the gradient of the cost function at the local optimum. The Jacobian matrix representing the constraint functions; Indicate the corresponding Lagrange multiplier solution; Indicates a local optimal solution Nonlinear equality constraints at the location; This represents the local optimal solution of the decision variable; Indicates a local optimal solution The cost function value at that location;

[0118] S442: Construct the Newton-step of the KKT system, in the following form:

[0119] Specifically, the KKT conditions constitute the necessary first-order optimality conditions. To solve the KKT system described in formula (15), this embodiment employs the primal-dual interior-point method. Its core idea is to gradually approximate the optimal solution by solving the linearized approximation of the KKT system in each iteration. In each iteration, it is necessary to construct the Newton step of the KKT system, in the following form:

[0120] (16)

[0121] In the formula: Indicates the number of iterations; Indicates the first Decision variables for the next iteration; Indicates the first Lagrange multipliers in the next iteration; Indicates in Jacobian matrix of the constraint function; This represents the Newton increment of the decision variable in the current iteration; This represents the Newton increment of the Lagrange multipliers in the current iteration; Indicates in The gradient of the cost function with respect to the decision variables; Indicates in Transpose of the Jacobian matrix of the constraint function; Indicates in The constraint function at the location; The Hessian matrix representing the Lagrange function with respect to variables has the following specific form:

[0122] (17)

[0123] In the formula: The Hessian matrix represents the cost function with respect to the decision variables; Indicates the first Lagrange multipliers for discrete time steps; Indicates the first The Hessian matrix of the constraint functions at discrete time steps with respect to the decision variables; Indicates the first Constraint functions for each discrete time step;

[0124] Specifically, the increments of the decision variables and Lagrange multipliers are obtained by solving the linear equation system formula (16). , Then, the solution vector is updated using a line search strategy:

[0125] (18)

[0126] In the formula: Indicates the first Decision variables for the next iteration; Indicates the first Lagrange multipliers in the next iteration; Indicates the line search step size; This represents the Newton increment of the decision variable in the current iteration; This represents the Newton increment of the Lagrange multipliers in the current iteration;

[0127] Specifically, step size The decision is made by either a filter line search or a trust region mechanism, while simultaneously balancing two objectives: ensuring the value of the cost function. On the one hand, the decrease, and on the other hand, minimizing the constraint residuals. This ensures that the optimization trajectory both decreases and converges within the feasible region. This process is repeated continuously, and iteration stops when the KKT residual is below a threshold, the constraint satisfaction reaches the tolerance range, the variable update magnitude approaches zero, or the maximum number of iterations / time limit is reached.

[0128] Finally, the optimal solution for the decision variables is returned. With the corresponding Lagrange multipliers Satisfying the KKT conditions constitutes a local optimum solution to this nonlinear programming problem within the feasible region. If the linear independence constraint eligibility condition is satisfied and the cost function and constraint functions are smooth, then this optimum solution mathematically possesses sufficient first-order optimality.

[0129] In the above optimization process, the optimal control sequence and the corresponding predicted state trajectory under the current state are finally obtained:

[0130] (19)

[0131] In the formula: This represents the optimal control quantity sequence obtained by optimization at the current moment; This represents the sequence of predicted state variables obtained under the action of the optimal control variable; This represents the first optimal control variable applied at the current moment; This represents the last optimal control variable in the prediction time domain; Indicates the amount of control applied The predicted state quantity for the next time step is then obtained; This represents the last predicted state quantity obtained at the end of the prediction time domain;

[0132] Specifically, according to formula (11), , This indicates the first longitudinal thrust applied at the current moment; This indicates the first turning torque applied at the current moment;

[0133] However, this control sequence is only used to guide control decisions in the short term. To enhance adaptability to model errors, environmental disturbances, and nonlinear dynamic changes, model predictive control does not execute all optimal control inputs at once. Instead, it employs a rolling time-domain optimization strategy, executing only the first control input... This is applied to real-world systems. The system then enters the next control cycle, acquiring new current state information via sensors or a state estimator. Based on this updated state, the controller reconstructs the optimization problem and initiates a new round of solving, thus forming a continuous feedback closed-loop control structure.

[0134] This mechanism tightly couples prediction and feedback, enabling the system to possess stronger robustness and adaptability by updating the control strategy in real time during each control cycle. This approach not only minimizes the cost function while ensuring constraints are met, but also possesses the ability to cope with dynamic environments and external disturbances. By continuously repeating the above prediction-optimization-execution process, the system gradually approaches the optimal trajectory, ultimately achieving the control objective.

[0135] Specifically, based on the principle of distributed model predictive control algorithm and optimization results, this embodiment develops corresponding embedded code, deploys the algorithm to the computing modules of each terminal node, and performs real-time data interaction and command issuance with the controller to realize the overall function of multi-vessel path tracking and obstacle avoidance control of unmanned surface vessels.

[0136] Based on the principle of distributed model predictive control, the unmanned surface vessel (USV) first acquires its own state information. This is achieved by cyclically subscribing to longitude, latitude, speed, and heading angle information using middleware from the open-source MAVROS software package. While the speed information can be directly used by the algorithm, the longitude and latitude information need to be processed by the open-source GeographicLib library to convert them into coordinates in a local Cartesian coordinate system. The heading angle obtained by the Raspberry Pi through MAVROS represents the angle between the bow and true north, which is then converted to a coordinate system between the bow and a fixed coordinate system. The included angle of the axis. The unmanned surface vessel subscribes to the status information and predicted trajectories of other vessels via MQTT.

[0137] To improve solution efficiency, this embodiment uses the open-source CppAD library to optimize the cost function. and constraint functions Modeling is performed, and its automatic differentiation function is used to generate the gradient of the cost function. constraint function Jacobian matrix Lagrange function Hessian matrix This avoids the computational errors and efficiency bottlenecks that may be introduced by manual derivation and numerical difference, enabling efficient numerical solutions to complex nonlinear problems. Subsequently, the derivative information generated by CppAD is directly used as the solver input, and the open-source nonlinear optimization solver Ipopt (Interior Point OPTimizer) is called to perform constrained optimization.

[0138] Based on the kinematics and dynamics model of the unmanned surface vessel (USV), predict its own state variables and control variables in the time domain, including the horizontal coordinate, vertical coordinate, and heading angle in the fixed coordinate system, and the longitudinal velocity, lateral velocity, bow angular velocity, lateral tracking error, heading angle deviation, longitudinal thrust, and bow moment in the moving coordinate system. Set the cost function in the prediction time domain. Use the formula (13) described in step S3 as the constraint function. Then, set the variable boundaries, set the minimum and maximum values ​​of the longitudinal velocity, longitudinal thrust, and bow moment, and set the Ipopt output control detail level to 0. Enable sparse matrix calculation, with a maximum calculation time of 0.2 seconds, and solve the optimization problem.

[0139] In this way, this embodiment significantly reduces computation time while ensuring optimization accuracy, enabling unmanned surface vessels to solve for the optimal control sequence in the future prediction time domain in real time on an embedded computing platform, thereby supporting the online control requirements for path tracking and obstacle avoidance.

[0140] Ultimately, the onboard Raspberry Pi 4B computer will solve for the first control input during the predictive optimization process. The system publishes its status information and predicted trajectory to the Pixhawk 2.4.8 flight controller via MAVROS and via MQTT. Upon receiving the control command, the flight controller, following the basic control unit of the unmanned surface vessel (USV) constructed in this embodiment, drives the electronic speed controller (ESC) to control the thrust output of the propellers and drives the servo motors to adjust the rudder angle. This allows for precise execution of the longitudinal thrust and yaw moment calculated by the optimization algorithm, enabling the USV to perform path tracking and obstacle avoidance control.

[0141] By combining the above implementation steps, ROS communication, MQTT communication, cost functions and constraint functions in the distributed model predictive control algorithm, and CppAD and Ipopt solvers are organically integrated to construct a real-time control system suitable for multi-vehicle cooperation and obstacle avoidance. The final overall algorithm flow is as follows: Figure 6 As shown, this process intuitively demonstrates the operational logic of the present invention in multi-vessel cooperative autonomous navigation missions.

[0142] Figure 7 This diagram illustrates the predicted control trajectory for a single-boat model, and sets the cost function parameters. , , The value is 0, and the expected trajectory of the unmanned surface vessel is... The maximum longitudinal velocity is 2 m / s, and the prediction time domain is... The value is 15. The figure shows the trajectory tracking of a single unmanned surface vessel under non-collision avoidance conditions, and it has a good tracking trend.

[0143] like Figure 8 This diagram illustrates the distributed model predictive control trajectory of three unmanned surface vessels (USVs) under a distributed communication architecture. The desired trajectory of USV No. 1 is set as follows: The expected trajectory of the No. 2 unmanned surface vessel is The expected trajectory of the No. 3 unmanned surface vessel is The maximum longitudinal velocity is 2 m / s, and the prediction time domain is... The value is 15. Running the distributed model predictive control algorithm on the designed underactuated unmanned surface vessel system platform, the overall system trajectory is good, with a clear obstacle avoidance trend, and as... Figures 9 to 11 Each unmanned surface vessel has well-defined state and control variables, including longitudinal velocity, lateral velocity, bow angular velocity, lateral tracking error, heading angular error, longitudinal thrust, and bow turning moment.

[0144] This embodiment presents an underactuated unmanned surface vessel (USV) obstacle avoidance platform based on distributed model predictive control (MMDC), focusing primarily on the distributed communication framework and distributed model predictive control algorithm. From a holistic perspective, the underactuated USV hardware and software framework is designed to realize a minimum USV control unit. Each USV is then equipped with distributed communication hardware, enabling communication between the USVs in the cluster to obtain each other's status and prediction information, preparing for the execution of the distributed model predictive control algorithm. Within the Raspberry Pi computing unit of each USV, prediction models and cost functions are designed sequentially. Subsequently, optimization problems are solved, and optimal control is executed, with rolling optimization, ultimately forming a complete, lightweight, and efficient USV obstacle avoidance system that achieves excellent control and obstacle avoidance performance.

[0145] The distributed model predictive control (DMPC) obstacle avoidance platform for underactuated unmanned surface vessel (USV) swarms established in this embodiment achieves information sharing and collaborative obstacle avoidance among multiple vessels through a distributed communication mechanism. Optimized and adapted to actual distributed communication and computing platforms, it combines hardware, software, and algorithms, achieving moderate communication load and fault tolerance. This allows the algorithm to overcome problems such as communication latency, limited computing resources, and network packet loss when deployed in real-world environments, significantly improving real-time performance and stability. It is suitable for distributed collaborative motion control of USV swarms in complex water environments. By exchanging information and independently running the distributed model predictive control algorithm, it achieves global coordination and dynamic obstacle avoidance control for the swarm. It effectively enhances the autonomous obstacle avoidance capability of multi-vessel systems in complex environments, possesses good scalability and practical application value, and is suitable for the execution of USV tasks in complex environments.

[0146] 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 or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control, characterized in that, Includes the following steps: S1: Establish the kinematic model and error dynamic model of the single-propeller, single-rudder, underactuated unmanned surface vessel, so as to obtain the differential kinematic model based on the error dynamic model; S2: Establish a dynamic model of a single-propeller, single-rudder, underactuated unmanned surface vessel to obtain a differential dynamic model; S3: Based on the kinematic and dynamic models, establish a cost function that considers the avoidance cost of dynamic obstacles, including neighboring unmanned surface vessels; S4: Establish an objective function based on the cost function, and establish a constraint function based on the differential kinematic model and the differential dynamic model to solve the objective function, obtain the optimal control input of the unmanned surface vessel, and thus realize obstacle avoidance of the underactuated unmanned surface vessel; The cost function is expressed as follows: in, In the formula: Indicates the cost of tracking error; This represents the prediction time domain of model predictive control. Indicates the index number of the discrete time step; This represents the weighting coefficient for the lateral tracking error; The model predictive control is represented by the first Lateral tracking error at each discrete time step; Denotes the Euclidean norm; This represents the heading angle error weighting coefficient; Indicates the unmanned surface vessel (USV) The heading angle error at each discrete time step; This represents the speed weighting coefficient; Indicates the unmanned surface vessel's position in the moving coordinate system. The longitudinal velocity of each discrete time step; This represents the desired longitudinal velocity of the unmanned surface vessel; In the formula: Indicates the cost of controlling input; This represents the longitudinal thrust weighting coefficient; The model predictive control is represented by the first Longitudinal thrust at discrete time steps; This represents the weighting coefficient for the turning torque; The model predictive control is represented by the first The turning torque at each discrete time step; In the formula: This indicates the cost of controlling smoothing; Indicates the weighting coefficient for longitudinal thrust variation; Indicates the weighting coefficient for the change in bow torque; In the formula: It represents the cost of obstacle avoidance, including static obstacle avoidance cost and dynamic obstacle avoidance cost; This represents the obstacle avoidance weight coefficient for static obstacles; The model predictive control is represented by the first The distance between the unmanned surface vessel and static obstacles at each discrete time step; This indicates a tendency to avoid odd positive numbers when the distance is 0; This represents the obstacle avoidance weight coefficient for dynamic obstacles; The model predictive control is represented by the first The discrete time step and the prediction of the first The Euclidean distance of a dynamic obstacle at a discrete time step; In the formula: This refers to the inter-ship attraction cost used to maintain spatial relationships between ships; Indicates the neighboring vessel number of the current unmanned surface vessel; This indicates the number of neighboring unmanned surface vessels (USVs) currently in use. Indicates the current unmanned surface vessel and the first The weighting coefficients of the attraction costs among unmanned surface vessels; Indicates the current unmanned surface vessel and the first The unmanned surface vessel in the The Euclidean distance of the step; Indicates the current unmanned surface vessel and the first The expected distance between the unmanned surface vessels.

2. The obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control according to claim 1, characterized in that, S1 includes: S11: The kinematic model of the single-propeller, single-rudder, underactuated unmanned surface vessel is established as follows: In the formula: This represents the x-coordinate of the unmanned surface vessel in a fixed coordinate system. express The first derivative; This represents the ordinate of the unmanned surface vessel in a fixed coordinate system; express The first derivative; This represents the heading angle of the unmanned surface vessel, i.e., the angle between the bow and the fixed coordinate system. The angle between the axes; express The first derivative; This represents the longitudinal velocity of the unmanned surface vessel in the moving coordinate system; This indicates the lateral speed of the unmanned surface vessel; This indicates the bow angular velocity of the unmanned surface vessel; S12: The error dynamic model is established as follows: In the formula: Indicates the unmanned surface vessel (USV) Lateral tracking error at each discrete time step; Indicates the unmanned surface vessel (USV) The heading angle error at each discrete time step; Indicates and The corresponding ordinate value; Indicates the unmanned surface vessel (USV) The ordinate of each discrete time step; This represents the trajectory curve that the unmanned surface vessel (USV) needs to track in a fixed coordinate system. The x-axis of each discrete time step; Indicates the index number of the discrete time step; This indicates the trajectory curve that the unmanned surface vessel (USV) needs to track in a fixed coordinate system at the [number]th [position]. The slope of the tangent at each discrete time step; Indicates that the unmanned surface vessel is in The heading angle at each discrete time step; S13: Based on the aforementioned error dynamic model, the kinematic model after differentiation is obtained as follows: In the formula: Indicates the unmanned surface vessel's position in the moving coordinate system. The longitudinal velocity of each discrete time step; Indicates the unmanned surface vessel (USV) The heading angle at each discrete time step; Indicates the difference interval; Indicates the unmanned surface vessel's position in the moving coordinate system. Lateral velocity at each discrete time step; Indicates the unmanned surface vessel (USV) The heading angular velocity at each discrete time step.

3. The obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control according to claim 2, characterized in that, The dynamic model of the single-propeller, single-rudder, underactuated unmanned surface vessel is established as follows: In the formula: express The first derivative of is the longitudinal acceleration of the unmanned surface vessel in the moving coordinate system; Indicates the mass of the unmanned surface vessel; Represents the longitudinal velocity of the unmanned surface vessel in the moving coordinate system. The corresponding linear damping derivative; Represents longitudinal acceleration The corresponding additional mass derivative; This indicates the bow angular velocity of the unmanned surface vessel; The nonlinear damping derivative represents the quadratic term of the longitudinal velocity; Indicates the lateral speed of the unmanned surface vessel The corresponding linear transverse force derivative; Indicates lateral acceleration The corresponding lateral additional mass derivative; The nonlinear derivative of the transverse force, representing the quadratic term of the transverse velocity; Indicates longitudinal thrust; for The first derivative of , i.e., the lateral acceleration of the unmanned surface vessel; express The absolute value; The derivative of the gyratory torque corresponding to the heading angular velocity; The derivative of the additional moment of inertia corresponding to the heading angular acceleration; The derivative of the nonlinear gyratory torque representing the second term of the heading angular velocity; This represents the moment of inertia of the unmanned surface vessel about its vertical axis. Indicates the turning torque; Represents the absolute value of the heading angular velocity; After discretizing the dynamic model, the differential dynamic model is obtained as follows: 。 4. The obstacle avoidance method for underactuated unmanned surface vessels based on distributed model predictive control according to claim 3, characterized in that, S4 includes: S41: Define state variables and control variables to determine decision variables: In the formula: Indicates that the unmanned surface vessel is in A system state vector at discrete time steps; Indicates transpose; Indicates that the unmanned surface vessel is in The system control vector for each discrete time step; Represent decision variables; Represents the initial state vector; This represents the state vector at the end of the prediction time domain; Indicates the initial control vector; This represents the control vector at the end of the prediction time domain; The model predictive control is represented by the first The turning torque at each discrete time step; The model predictive control is represented by the first Longitudinal thrust at discrete time steps; S42: Based on the aforementioned decision variables, establish the objective function: In the formula: Represent decision variables; Let be the cost function to be minimized, that is, the performance metric that we want to minimize under the constraints. Represents the constraint function; and These are the lower and upper bounds of the decision variable, respectively. in, In the formula, Indicates the initial prediction model constraints; This represents the constraints of the prediction model at the end of the prediction time domain; Indicates the first Constraint functions for each discrete time step; S43: Introducing Lagrange multipliers, constructing a Lagrange function based on a cost function that includes multi-ship cooperation and obstacle avoidance constraints: In the formula: Represent the Lagrange function; For Lagrange multipliers; S44: Solve for the Lagrangian function to obtain the optimal control input for the unmanned surface vessel.

Images