Unmanned ship berthing control method based on stage scheduling and constraint model prediction

Abstract

The application discloses a method for unmanned ship berthing control based on stage scheduling and constraint model prediction, relates to the technical field of unmanned ship autonomous control, and is used for obtaining a control sequence of an autonomous berthing process of an unmanned ship, and completing the autonomous berthing process of the unmanned ship, including information collection and autonomous berthing operation, dividing the berthing process into an approaching section, an aligning section and a berthing section according to the distance between the unmanned ship and a berth, generating a plurality of candidate paths in the approaching section and the aligning section, screening an optimal path from the candidate paths based on a comprehensive path cost function, constructing a predictive control model in the berthing section, generating an optimal control sequence by using a rolling horizon optimization method, generating a control instruction based on the optimal path and the optimal control sequence, and sending the control instruction to an unmanned ship actuator to control the unmanned ship to complete autonomous berthing. The application improves berthing precision and safety, and enhances robustness in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of ship control technology, and more particularly to the field of unmanned vessel autonomous control technology, specifically to an unmanned vessel berthing control method based on stage scheduling and constraint model prediction. Background Technology

[0002] Autonomous berthing of unmanned vessels is a key link in realizing the closed loop of unmanned vessel missions. It directly affects whether unmanned vessels can safely enter the target berth, successfully complete resupply and handover, and ensure repeated voyages. The reliability and accuracy of the unmanned vessel berthing process have an important impact on the operational efficiency, operating costs and safety risks of unmanned vessels.

[0003] Existing unmanned surface vessel (USV) berthing technologies suffer from the following main problems: First, traditional path planning methods often employ a single planning strategy, lacking targeted optimization for different stages of the berthing process, resulting in low efficiency. Second, under multiple constraints, they lack the ability to coordinate and handle factors such as hull attitude, lateral offset, and safety distance, making it difficult to guarantee berthing accuracy and safety. Furthermore, the lack of a trajectory prediction mechanism during the fine berthing phase makes it difficult to adapt to complex water environments and dynamic disturbances. Additionally, fixed weight parameters cannot be dynamically adjusted based on berthing distance and stage characteristics, affecting control effectiveness.

[0004] Therefore, there is an urgent need for an autonomous berthing control method for unmanned vessels that can achieve multi-stage scheduling and multi-constraint collaborative optimization. Summary of the Invention

[0005] The purpose of this invention is to provide an unmanned vessel berthing control method based on stage scheduling and constraint model prediction, so as to solve the problems of insufficient multi-stage collaborative optimization, lack of constraint coordination processing capability, and low berthing control accuracy in the existing unmanned vessel berthing process.

[0006] To address the above objectives, this invention provides an unmanned vessel berthing control method based on phase scheduling and constraint model prediction, comprising:

[0007] S1. Obtain real-time status information and target berth information of the unmanned vessel, and divide the unmanned vessel berthing process into approach segment, alignment segment and berthing segment according to the distance between the unmanned vessel and the target berth;

[0008] S2. Generate multiple candidate paths in the approach and alignment segments, and select the optimal path based on the comprehensive path cost evaluation function;

[0009] S3. During the berthing phase, a predictive control model is constructed. Based on the current state information of the unmanned vessel, the motion trajectory of the unmanned vessel is predicted. The rolling time-domain optimization method is used to optimize the motion trajectory of the unmanned vessel and generate the optimal control sequence.

[0010] S4. Generate control commands based on the optimal path and control sequence, and send the control commands to the unmanned vessel's actuators to control the unmanned vessel to complete the autonomous docking operation.

[0011] The real-time status information of the unmanned surface vessel includes its real-time position, real-time heading angle, real-time speed, and real-time acceleration.

[0012] The target berth information includes the target berth's location, obstacle location, shoreline location, and the target berth's orientation angle.

[0013] Based on the target berth information, construct the berth centerline equation:

[0014] ;

[0015] In the formula, This indicates the position coordinates of a point on the center line of the berth. Let be the normal vector of the berth centerline. This is a constant term for the berth centerline. The distance between the unmanned vessel (UV) and the target berth is calculated based on the UV's position information and the target berth's position information. The UV's berthing process is divided into an approach segment, an alignment segment, and a berthing segment based on this distance. During the approach segment, if the distance between the UV and the target berth is greater than a first distance threshold... The distance between the unmanned surface vessel and the target berth is within the first distance threshold. With the second distance threshold Between these points, the distance between the unmanned vessel in the berthing section and the target berth is less than the second distance threshold. ;

[0016] right Normalization is performed to calculate the lateral offset error between the unmanned vessel's center of mass and the berth centerline. :

[0017] ;

[0018] ;

[0019] In the formula, This indicates the coordinates of the center of mass of the unmanned vessel.

[0020] S2 Integrated Path Cost Evaluation Function for:

[0021] ;

[0022] ;

[0023] ;

[0024] In the formula, The candidate path length, This represents the change in the path heading angle. For the sake of maintaining a safe distance, For heading deviation error, Indicates the speed penalty item. For segment safety speed threshold, , , , , , These respectively represent the berthing process The weight parameters, For the speed of the unmanned ship, The heading angle of the unmanned vessel;

[0025] In the approach and alignment segments, multiple candidate paths are generated, based on... The path cost evaluation value of each candidate path is calculated, and the candidate path with the smallest path cost evaluation value is selected as the optimal path. The unmanned vessel navigates according to the optimal path during the approach and alignment phases.

[0026] During the berthing phase, a predictive control model is constructed, and model predictive control methods are used to precisely control the unmanned surface vessel, including:

[0027] S3.1, Construct an unmanned vessel state prediction model, based on... Prediction of the state vector and control vector of the unmanned surface vessel at any time Real-time status of the unmanned vessel, obtain The predicted state vector of the unmanned surface vessel at time t, with a sampling time interval of t. ;

[0028] S3.2, construct the objective function for predictive control optimization, apply constraints, and use an iterative linear method to solve the unmanned vessel state prediction model to obtain the iterative optimization variables for adjacent sampling periods;

[0029] S3.3, using the rolling time-domain optimization method, steps S3.1 and S3.2 are executed in each sampling period to apply control rate of change constraints to the control vector and trust region constraints to the iterative optimization variables, thereby obtaining the optimal control sequence;

[0030] In S3.2, the constraints include state constraints, control constraints, and safety margin constraints for berth boundaries and hull shape. State constraints include speed constraints to limit the speed range of the unmanned vessel and heading deviation error constraints to limit the deviation error range between the heading angle of the unmanned vessel and the heading of the berth. Control constraints include acceleration constraints to limit the upper limit of the acceleration of the unmanned vessel and turning angular velocity constraints to limit the upper limit of the turning angular velocity of the unmanned vessel. The speed range constraints, acceleration constraints, and turning angular velocity constraints are determined by the performance of the unmanned vessel, while the heading deviation error constraints are jointly determined by the geometry of the berth and the geometry of the unmanned vessel.

[0031] The construction of safety margin constraints for berth boundaries and hull shape includes: representing the berth area as a convex polygon in the berth reference coordinate system; using a set of linear inequalities for constraints to obtain berth boundary constraints; calculating the hull projection margin based on the geometry of the unmanned vessel and its heading angle in the berth reference coordinate system; and merging the hull projection margin into the berth boundary constraints.

[0032] The unmanned vessel state prediction model is as follows:

[0033] ;

[0034] ;

[0035] ;

[0036] In the formula, express The predicted state vector of the unmanned surface vessel at any given time. express The state vector of the unmanned ship at any given time. express The control vector of the unmanned surface vessel at any given time. express The coordinates of the unmanned ship's center of mass at all times. express The heading angle of the unmanned ship at all times. express The speed of the unmanned ship at all times. express The longitudinal acceleration of the unmanned ship at all times express The turning angular velocity of the unmanned vessel at all times.

[0037] Constructing the objective function for predictive control optimization :

[0038] ;

[0039] In the formula, For stage cost items, This is a terminal penalty item;

[0040] ;

[0041] In the formula, For the state tracking error term, To control the consumption of capacity, To control the incremental smoothing term;

[0042] ;

[0043] In the formula, For terminal location cost term, For the terminal heading cost item, For terminal speed cost, This is the terminal soft constraint cost term.

[0044] ;

[0045] ;

[0046] ;

[0047] In the formula, The prediction step size is equivalent to the total number of prediction time steps. The square of the weighted norm is used to... Assign weights to each component. For the first The state error vector of the step, Here is the weight matrix for the state error. For predicting the time step index, For the first The tangential offset error component of the step, For the first The tangential position coordinates of the unmanned surface vessel's center of mass in the berth reference coordinate system. For the first The tangential reference position coordinates of the unmanned surface vessel. As the weight of the tangential error, For the first The lateral offset error component of the step. For the first The normal position coordinates of the unmanned surface vessel's center of mass in the berth reference coordinate system. For the first The normal reference position coordinates of the unmanned surface vessel. As the weight of the lateral error, For the first The heading offset error component of the step, For the first The actual heading angle of the unmanned surface vessel. For the first The expected heading angle of the unmanned surface vessel. As the weight of the heading deviation error, For the first The speed error component of the step, For the first The actual speed of the unmanned surface vessel. Indicates the first The expected speed of the unmanned surface vessel. This represents the weight of the speed error.

[0048] ;

[0049] ;

[0050] In the formula, For the first The longitudinal acceleration of the unmanned surface vessel. Indicates the first The turning angular velocity of the unmanned surface vessel. To control capability weights, For the first Relaxation variables of the soft constraint at the berth boundary. The penalty weights for the slack variables under soft constraints; For the acceleration change component, For the first The longitudinal acceleration of the unmanned surface vessel. This represents the component of the change in the bow angular velocity. For the first The turning angular velocity of the unmanned surface vessel. To control the changing weights.

[0051] ;

[0052] ;

[0053] ;

[0054] ;

[0055] In the formula, This represents the penalty coefficient for the terminal state. Let be the coordinates of the unmanned vessel's center of mass in the tangential terminal position of the reference coordinate system at the berth. The coordinates are the tangential terminal reference positions of the unmanned surface vessel. Let be the coordinates of the normal terminal position of the unmanned vessel's center of mass in the berth reference coordinate system. Here are the coordinates of the unmanned surface vessel's normal terminal reference position. The terminal heading angle of the unmanned vessel. The desired terminal heading angle of the unmanned vessel. This is the terminal speed penalty coefficient. The terminal speed of the unmanned vessel. For the slack variables of the soft constraint of the berth boundary at the terminal time.

[0056] berthing section Dynamically adjust based on the distance between the unmanned vessel and the target berth:

[0057] ;

[0058] ;

[0059] ;

[0060] In the formula, The distance between the unmanned vessel and the target berth. , , Divided into The dynamic adjustment coefficient, It is a preset small positive number.

[0061] Compared with the prior art, the present invention has the following advantages:

[0062] This invention employs a multi-stage scheduling strategy, using targeted planning and control methods based on the different characteristics of the berthing process to improve berthing efficiency;

[0063] The comprehensive path cost evaluation function provided by this invention considers multiple factors such as path length, heading change, safety distance, lateral offset error, attitude offset error and speed constraint, and achieves multi-constraint collaborative optimization.

[0064] This invention employs a model predictive control method in the fine berthing phase to continuously optimize the future motion trajectory of the unmanned vessel, thereby improving berthing accuracy and robustness.

[0065] This invention dynamically adjusts weight parameters based on the distance between the unmanned vessel and the target berth, enabling the control strategy to adaptively adjust according to the berthing distance and improving control performance;

[0066] This invention employs a hybrid hard and soft constraint mechanism, setting hard constraints on shoreline boundaries to ensure safety, and setting soft constraints on other boundaries to improve optimization feasibility and enhance system robustness. Attached Figure Description

[0067] Figure 1 A schematic diagram of the unmanned vessel autonomous docking method provided by the present invention;

[0068] Figure 2A schematic diagram of the multi-stage berthing process provided for this invention;

[0069] Figure 3 A schematic diagram of the network architecture of the predictive control model provided by this invention;

[0070] Figure 4 Experimental results of an unmanned vessel berthing control method based on stage scheduling and constraint model prediction. Detailed Implementation

[0071] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0072] Example 1

[0073] A method for unmanned surface vessel (USV) berthing control based on stage scheduling and constraint model prediction includes:

[0074] S1. Obtain real-time status information and target berth information of the unmanned vessel. Based on the distance between the unmanned vessel and the target berth, divide the berthing process of the unmanned vessel into multiple stages, namely the approach stage, the alignment stage, and the berthing stage.

[0075] S2. Generate multiple candidate paths in the approach and alignment segments, and select the optimal path based on the comprehensive path cost evaluation function;

[0076] S3. During the berthing phase, a predictive control model is constructed. Based on the current state information of the unmanned vessel, the motion trajectory of the unmanned vessel is predicted. The rolling time-domain optimization method is used to optimize the motion trajectory of the unmanned vessel and generate the optimal control sequence.

[0077] S4. Generate control commands based on the optimal path and control sequence, and send the control commands to the unmanned vessel's actuators to control the unmanned vessel to complete the autonomous docking operation.

[0078] The unmanned surface vessel (USV) acquires real-time status information through its onboard multi-sensor system. This real-time status information includes the USV's real-time position, heading angle, speed, and acceleration. Target berth information includes the berth's location, obstacle locations, shoreline location, and heading angle.

[0079] Based on the target berth information, construct the berth centerline equation:

[0080] ;

[0081] In the formula, This indicates the position coordinates of a point on the center line of the berth. These are the horizontal component coefficients of the berth centerline equation. These are the vertical component coefficients of the berth centerline equation. This is a constant term for the berth centerline. , , The normal vector that forms the centerline of the berth. , This is the berth orientation angle.

[0082] like Figure 2 As shown, the distance between the unmanned vessel and the target berth is calculated based on the unmanned vessel's position information and the target berth's position information. The unmanned vessel's berthing process is divided into an approach segment, an alignment segment, and a berthing segment based on the distance between the unmanned vessel and the target berth. The approach segment refers to the process of approaching the unmanned vessel when the distance between the unmanned vessel and the target berth is relatively far. The alignment segment refers to the process of adjusting the attitude and lateral position of the unmanned vessel when it enters a medium distance range. The berthing segment refers to the low-speed, precise berthing process when the unmanned vessel approaches the target berth and meets the preset entry conditions.

[0083] Set the first distance threshold. With the second distance threshold First distance threshold Greater than the second distance threshold Based on the distance between the unmanned surface vessel (USV) and the target berth, the berthing process is divided into multiple stages using two distance thresholds: an approach stage, an alignment stage, and a berthing stage. During the approach stage, the distance between the USV and the target berth is greater than the first distance threshold. The distance between the unmanned surface vessel and the target berth is within the first distance threshold. With the second distance threshold Between these points, the distance between the unmanned vessel in the berthing section and the target berth is less than the second distance threshold. And it meets the preset posture and position entry conditions.

[0084] First distance threshold With the second distance threshold This serves as the switching point for the unmanned vessel's berthing state, dividing the autonomous berthing process into three stages with different control objectives and difficulties, with the first distance threshold being the threshold. With the second distance threshold The threshold needs to be determined through comprehensive calculations based on the unmanned vessel's maneuvering characteristics, sensor performance, environmental conditions, and the physical dimensions of the berth. For example, sensor performance may be the primary basis for determining the threshold. It should be less than the effective working distance of the positioning sensors carried by the unmanned surface vessel. It should be greater than the static error of the unmanned vessel's positioning system, with the handling characteristics of the unmanned vessel serving as the primary basis for determining the threshold. It is typically two to three times the distance required for a ship to brake suddenly at its initial speed. This is typically the distance an unmanned surface vessel (USV) travels by inertia after its power is stopped; the threshold is primarily determined based on environmental conditions and the physical dimensions of the berth. Typically located outside of a physical obstacle relative to the target berth. Normally, it should not exceed the length of the berth.

[0085] When the unmanned surface vessel (USV) has a hull length of 2.5 meters, a width of 1 meter, a target berth length of 4.8 meters, and a target berth width of 2.4 meters, the maximum braking acceleration of the USV is: The initial velocity of the unmanned vessel during the approach phase is Emergency braking distance is When considering maneuverability, berth dimensions, and safety margin, the following is taken: Rice; take rice.

[0086] right Normalization is performed to calculate the lateral offset error between the unmanned vessel's center of mass and the berth centerline. :

[0087] ;

[0088] ;

[0089] In the formula, This indicates the position coordinates of the unmanned vessel's center of mass in the berth reference coordinate system.

[0090] The berth reference coordinate system is a two-dimensional planar coordinate system. The origin is typically the midpoint of the rear edge of the berth or a spatial point on the berth's centerline. The X-axis is along the length of the berth, and the Y-axis is perpendicular to the length of the berth. The X-axis is the tangential axis of the unmanned surface vessel's (USV) motion, and the Y-axis is the normal axis of the USV's motion. The autonomous berthing motion of the USV is decomposed into tangential motion along the length of the berth and normal motion perpendicular to the length of the berth using the berth reference coordinate system. Tangential offset error is used to determine whether the USV's hull has entered the allowable range of the berth's fore and aft edges, while lateral offset error is used to determine whether the hull is close to the berth or deviates from the centerline. In the berth reference coordinate system, the distance between the USV and the target berth is the straight-line distance between the USV's center of mass and the origin.

[0091] S2 Integrated Path Cost Evaluation Function for:

[0092] ;

[0093] ;

[0094] ;

[0095] In the formula, The candidate path length, This represents the change in the path heading angle. For the sake of maintaining a safe distance, The heading offset error is used to represent the deviation between the actual heading angle of the unmanned vessel and the berth orientation angle. This is a speed penalty term used to limit the speed of the unmanned surface vessel during the approach and alignment phases to not exceeding the safe speed threshold for that phase. For segment safety speed threshold, , , , , , These represent the berthing process. The weight parameters, For the speed of the unmanned ship, The heading angle of the unmanned vessel is denoted by , and the safety centerline is a reference line obtained by shifting the berth centerline outward or in the direction of the water. It is used to quantify the safe distance that the unmanned vessel needs to maintain with the dock, collision avoidance facilities, etc. during the berthing process, so as to avoid collisions caused by environmental disturbances or control errors during the autonomous berthing process.

[0096] Segment safety speed threshold The empirical parameters set by humans are determined based on: first, constraints on ship maneuverability, the safe speed threshold should not exceed the upper limit of the continuous cruising speed of the unmanned vessel's propulsion system, typically taken as 70% to 90% of the cruising speed; second, emergency braking distance requirements, based on the braking distance formula. The calculated emergency braking distance should be much smaller than the distance threshold corresponding to the current stage, where, Indicates the distance threshold. The parameters are as follows: 1) Maximum acceleration of the unmanned vessel; 2) Effective detection range of the sensors, the distance the unmanned vessel travels per second should be much smaller than the sensor detection range; 3) Attitude adjustment accuracy requirements, the safe speed threshold for each segment should be set to a lower value to improve control accuracy; 4) Berth size constraints, the safe speed threshold for each segment should be matched with the berth size to ensure that the unmanned vessel has enough time to complete attitude and position adjustments.

[0097] In S2, during the approach and alignment phases, multiple candidate paths are generated using existing unmanned surface vessel (USV) or ship path planning algorithms. The path length, change in heading angle, and safety distance of each candidate path are obtained, and a comprehensive path cost evaluation function is used. The path cost evaluation value of each candidate path is calculated, and the candidate path with the smallest path cost evaluation value is selected as the optimal path. The unmanned vessel navigates according to the optimal path during the approach and alignment phases.

[0098] when Furthermore, when the attitude and position entry conditions are met, the berthing process of the unmanned surface vessel (USV) switches to the berthing phase. The attitude and position entry conditions include the tangential position coordinates of the USV in the berth reference coordinate system. satisfy:

[0099] ;

[0100] In the formula, , These represent the tangential position coordinates of the front edge and rear edge of the target berth in the berth reference coordinate system, respectively. Indicates the front-end tangential position threshold. This represents the rear tangential position threshold; the front and rear tangential position thresholds are determined by the geometry of the unmanned vessel and the target berth, ensuring that the unmanned vessel can dock at the target berth.

[0101] Normal position coordinates of the unmanned vessel in the berth reference coordinate system satisfy:

[0102] ;

[0103] In the formula, This is the normal position scaling factor. The width of the target berth;

[0104] Lateral offset error between the center of mass of the unmanned vessel and the center line of the berth satisfy:

[0105] ;

[0106] In the formula, These are the normal position coordinates of the unmanned surface vessel in the reference coordinate system of the berth. Here are the normal reference position coordinates of the unmanned surface vessel. This is the threshold for lateral offset error.

[0107] Lateral offset error threshold The empirical parameters set by humans are determined based on the following: First, hull size constraints, the lateral offset error threshold should be less than half the hull width, usually taken as 20% to 30% of the hull width; second, berth size constraints, the lateral offset error threshold should be less than half the difference between the berth width and the hull width; third, control accuracy requirements, the lateral offset error threshold should match the positioning accuracy of the unmanned vessel positioning system; and fourth, target normal position constraints, the target normal position is calculated through the projection range of the hull under the maximum heading offset error, and the lateral offset error threshold should ensure that the deviation between the final lateral position of the unmanned vessel and the target normal position is within the allowable range.

[0108] Heading deviation error of unmanned vessel's center of mass from berth centerline satisfy:

[0109] ;

[0110] In the formula, This represents the maximum heading deviation error.

[0111] Maximum heading deviation error The empirical parameters set by the user are determined based on the following: First, the geometric constraints of the berth, ensuring that the maximum heading deviation error is maintained within the berth width even when the vessel is deviating; second, the geometric constraints of the hull, where the projected width of the hull increases with the deviation angle, and the maximum heading deviation error should ensure that the projected width of the hull does not exceed the difference between the berth width and the safety margin; third, the maneuverability constraints, ensuring that the maximum heading deviation error matches the turning capability of the unmanned vessel, allowing it to adjust its course promptly during berthing; and fourth, the safety margin requirement, where a sufficient safety margin should be reserved for the maximum heading deviation error, typically between 15 and 25 degrees.

[0112] like Figure 3 As shown, during the berthing phase, a predictive control model is constructed, and model predictive control methods are used to precisely control the unmanned vessel, including:

[0113] S3.1, Construct an unmanned vessel state prediction model, based on... Prediction of the state vector and control vector of the unmanned surface vessel at any time Real-time status of the unmanned vessel, obtain The predicted state vector of the unmanned surface vessel at time t, with a sampling time interval of t. , Time to A time interval is defined as one sampling period;

[0114] S3.2, construct the objective function for predictive control optimization, apply constraints, and use an iterative linear method to solve the unmanned vessel state prediction model to obtain the iterative optimization variables for adjacent sampling periods;

[0115] S3.3, using the rolling time-domain optimization method, steps S3.1 and S3.2 are executed in each sampling period to apply control change rate constraints to the control vector and trust region constraints to the iterative optimization variables, thereby obtaining the optimal control sequence.

[0116] In S3.2, the constraints include state constraints, control constraints, and safety margin constraints for berth boundaries and hull shape. State constraints include speed constraints to limit the unmanned surface vessel's (USV) speed range and heading deviation error constraints to limit the range of deviation error between the USV's heading angle and the berth's heading. Control constraints include acceleration constraints to limit the upper limit of the USV's acceleration and turning angular velocity constraints to limit the upper limit of the USV's turning angular velocity. The speed range constraints, acceleration constraints, and turning angular velocity constraints are determined by the USV's performance, while the heading deviation error constraints are jointly determined by the berth's geometry and the USV's geometry. The speed constraints are... , This represents the minimum speed, usually a negative value, equivalent to the maximum permissible reversing speed during berthing, and is determined by the unmanned vessel's propulsion system. The maximum permissible longitudinal speed during berthing is determined by a comprehensive calculation based on the unmanned vessel's propulsion system and the upper limit of safe berthing speed; the heading deviation error constraint is that the heading deviation error satisfies... The acceleration constraint is the longitudinal acceleration of the unmanned vessel. satisfy , The upper limit of acceleration is determined by the unmanned surface vessel's propulsion system, and the bow angular velocity constraint is the bow angular velocity of the unmanned surface vessel. satisfy , The maximum turning angular velocity of the unmanned vessel is determined by its lateral thrust capability.

[0117] The safety margin constraints between the berth boundary and the hull shape approximate the hull as a convex polygon centered on a reference point (such as the center of mass or center of gravity) that varies with the heading. This ensures that the polygon maintains a safe distance from the berth shoreline and restricted areas. The construction of these safety margin constraints includes: representing the berth area as a convex polygon in the berth reference coordinate system; using a system of linear inequalities to obtain the berth boundary constraints; calculating the hull projection margin based on the UAV's geometry and heading angle in the berth reference coordinate system; and incorporating the hull projection margin into the berth boundary constraints. The safety margin constraints between the berth boundary and the hull shape are as follows:

[0118] ;

[0119] In the formula, Let be the coefficient matrix of the linear inequality of a convex polygon. Used to quantify the distances from the ship's center of mass to the boundaries of the target berth. This is a constant vector representing the linear inequality of a convex polygon, used to quantify the original positions of each boundary of the target berth. This is a safety margin vector used to quantify the static safety distance that must be maintained between the ship's hull and the berth boundary. Let be the hull projection margin vector. For the first Relaxation variables of the soft constraint at the berth boundary.

[0120] The soft constraints on berth boundaries that are not less than zero include the constraints on the left and right sides of the berth and the constraint on the front end of the berth. These boundaries allow unmanned vessels to exceed them to a certain extent under special circumstances, so as to improve the feasibility of the optimization problem.

[0121] Slack variables To optimize variables, the values ​​are automatically calculated by the optimization solver during the solution process and satisfy the following conditions. The introduction of slack variables employs the standard soft constraint handling method in model predictive control. By adding slack variables and setting penalty weights to the berth boundary constraints, the solver can find a suboptimal solution when the original constraints cannot be met, while minimizing the degree of constraint violation. The penalty weights for the slack variables are as follows. These are empirical parameters set manually, and their determination is based on: firstly, constraints that violate suppression requirements; First, a larger value should be chosen to suppress the value of slack variables and ensure that the degree of boundary constraint violation is minimized; second, optimization feasibility requirements should be considered. The value should not be too large, otherwise it will worsen the numerical conditions of the optimization problem; thirdly, it needs to be balanced with other weights. It should be consistent with other weight parameters in the objective function, and its value is usually in the range of 100 to 1000.

[0122] Trust region constraints are:

[0123] ;

[0124] In the formula, , The prediction step size is equivalent to the total number of prediction time steps. Indicates the first The state variables of the step, Indicates the first In the nth iteration, the 1st The reference state variable of the step, , They are respectively , Trust threshold radius, Indicates the first The control vector of the step, Indicates the first In the nth iteration, the 1st The reference control variable for the step.

[0125] The constraint on the rate of change is:

[0126] ;

[0127] Perform first-order smoothing filtering on the control commands:

[0128] ;

[0129] In the formula, For the first The longitudinal acceleration of the unmanned surface vessel. For the first The longitudinal acceleration of the unmanned surface vessel; For the first The turning angular velocity of the unmanned surface vessel. For the first The bow angular velocity of the unmanned surface vessel. The maximum rate of change of acceleration, The maximum rate of change of the bow angular velocity. This is the actual output instruction after filtering. This is a smoothing coefficient, with a value between 0 and 1. The closer it is to 1, the stronger the smoothing effect, but the slower the response.

[0130] The maximum rate of change of acceleration and the maximum rate of change of bow angular velocity are empirical parameters set manually. The determination of the maximum rate of change of acceleration is based on the following: First, the response characteristics of the propulsion system, the maximum rate of change of acceleration should match the dynamic response capability of the unmanned vessel's propulsion system; second, the structural strength constraints of the hull, as rapid acceleration changes will generate large inertial forces, the maximum rate of change of acceleration should ensure that the inertial forces are within the allowable range of the structure; third, the requirements for passenger comfort, as rapid acceleration changes will affect passenger comfort, and the value is usually taken in the range of 1.0 m / s³ to 3.0 m / s³. The determination of the maximum rate of change of the bow angular velocity is based on the following criteria: First, the response characteristics of the side thrust system; the maximum rate of change of the bow angular velocity should match the dynamic response capability of the unmanned vessel's side thrust system. Second, the inertial constraints of the hull; a rapid change in the bow angular velocity will generate a large inertial moment, and the maximum rate of change of the bow angular velocity should ensure that the inertial moment is within the allowable range of the structure. Third, the control stability requirements; a rapid change in the bow angular velocity will lead to unstable heading control, and the value is usually taken in the range of 60 degrees per square second to 180 degrees per square second.

[0131] The unmanned vessel state prediction model is as follows:

[0132] ;

[0133] ;

[0134] ;

[0135] In the formula, express The predicted state vector of the unmanned surface vessel at any given time. express The state vector of the unmanned ship at any given time. express The control vector of the unmanned surface vessel at any given time. express The position coordinates of the unmanned vessel's center of mass in the berth reference coordinate system at all times. express The heading angle of the unmanned ship at all times. express The speed of the unmanned ship at all times. express The longitudinal acceleration of the unmanned ship at all times express The turning angular velocity of the unmanned vessel at all times.

[0136] Constructing the objective function for predictive control optimization :

[0137] ;

[0138] In the formula, For stage cost items, This is a terminal penalty item;

[0139] ;

[0140] In the formula, For the state tracking error term, To control the consumption of capacity, To control the incremental smoothing term;

[0141] ;

[0142] In the formula, For terminal location cost term, For the terminal heading cost item, For terminal speed cost, This is the terminal soft constraint cost term.

[0143] ;

[0144] ;

[0145] ;

[0146] In the formula, The prediction step size is equivalent to the total number of prediction time steps. The square of the weighted norm is used to... Assign weights to each component. For the first The state error vector of the step, Here is the weight matrix for the state error. For predicting the time step index, For the first The tangential offset error component of the step, For the first The tangential position coordinates of the unmanned surface vessel's center of mass in the berth reference coordinate system. For the first The tangential reference position coordinates of the unmanned surface vessel. As the weight of the tangential error, For the first The lateral offset error component of the step. For the first The normal position coordinates of the unmanned surface vessel's center of mass in the berth reference coordinate system. For the first The normal reference position coordinates of the unmanned surface vessel. The weight of the normal error, For the first The heading offset error component of the step, For the first The actual heading angle of the unmanned surface vessel. For the first The expected heading angle of the unmanned surface vessel. As the weight of the heading deviation error, For the first The speed error component of the step, For the first The actual speed of the unmanned surface vessel. Indicates the first The expected speed of the unmanned surface vessel. This represents the weight of the speed error.

[0147] The berth reference coordinate system is a two-dimensional plane coordinate system, with the midpoint of the rear edge of the berth as the origin. The X-axis is the tangential axis, running along the length of the berth and pointing towards the front of the berth; the Y-axis is the normal axis, perpendicular to the length of the berth and pointing towards the width of the berth, also known as the transverse axis. The heading is the direction in which the bow of the unmanned vessel points, expressed as a heading angle.

[0148] Tangential offset error is the offset between the tangential position coordinates of the UAV's center of mass in the berth reference coordinate system and the tangential reference position coordinates. It is used to determine whether the UAV's hull has entered the allowable range of the fore and aft edges of the berth. Lateral offset error is the offset between the normal position coordinates of the UAV's center of mass in the berth reference coordinate system and the normal reference position coordinates. It is also called normal offset error and is used to determine whether the hull is close to the berth or deviates from the centerline. Heading offset error is the offset between the UAV's actual heading angle and the desired heading angle. It is used to determine whether the UAV's hull attitude is aligned with the berth orientation.

[0149] For the first The tangential reference position coordinates of the unmanned surface vessel are equivalent to the coordinates of the first unmanned surface vessel. In each sampling period, the desired position of the unmanned vessel's center of mass along the tangential direction is preferably selected by choosing the tangential position coordinates of a spatial point on the safety center line. The target berthing point is typically set as the midpoint of the rear edge of the berth. In the berth reference coordinate system with the midpoint of the rear edge of the berth as the origin, along with Proceeding along the length of the berth to zero is equivalent to the unmanned vessel's trajectory being designed to smoothly move from its current tangential position to the origin of the coordinate system, ultimately... It is zero.

[0150] Control capability consumption item for:

[0151] ;

[0152] Control Incremental Smoothing Term for:

[0153] ;

[0154] In the formula, For the first The longitudinal acceleration of the unmanned surface vessel. Indicates the first The turning angular velocity of the unmanned surface vessel. To control capability weights, For the first Relaxation variables of the soft constraint at the berth boundary. The penalty weights for the slack variables under soft constraints; For the acceleration change component, For the first The longitudinal acceleration of the unmanned surface vessel. This represents the component of the change in the bow angular velocity. For the first The turning angular velocity of the unmanned surface vessel. To control the changing weights.

[0155] Terminal location cost item for:

[0156] ;

[0157] Terminal heading cost item :

[0158] ;

[0159] Terminal speed cost for:

[0160] ;

[0161] Terminal soft constraint cost term for:

[0162] ;

[0163] In the formula, This represents the penalty coefficient for the terminal state. Let be the coordinates of the unmanned vessel's center of mass in the tangential terminal position of the reference coordinate system at the berth. The coordinates are the tangential terminal reference positions of the unmanned surface vessel. Let be the coordinates of the normal terminal position of the unmanned vessel's center of mass in the berth reference coordinate system. Here are the coordinates of the unmanned surface vessel's normal terminal reference position. The terminal heading angle of the unmanned vessel. The expected terminal heading angle of the unmanned vessel. This is the terminal speed penalty coefficient. The terminal speed of the unmanned vessel. For the slack variables of the soft constraint of the berth boundary at the terminal time.

[0164] In the near segment, the weight parameters satisfy... , , The weight parameters are preset constants; in the alignment segment, the weight parameters satisfy... , and During the berthing phase, the weighting parameters satisfy... , Terminal state penalty coefficient It should be greater than 1 to enhance terminal state convergence performance; terminal speed penalty coefficient. It should be no less than 1 to ensure that the speed is sufficiently reduced during berthing. (Control the rate of change weight) Appropriate values ​​should be chosen to balance control smoothness and response speed, with slack variable penalty weights. A larger value should be chosen to suppress boundary violations.

[0165] Preset constants Empirical parameters set manually are used to limit the weight of lateral offset error in the approach segment. Weight relative to path length The upper limit of the proportion.

[0166] Preset constants The criteria for determining the approach phase control objective are as follows: First, the approach phase control objective requires that the unmanned vessel be far from the target berth, and the main control objective is to approach the berth quickly. Therefore, the weight of the lateral offset error should be relatively small to avoid premature lateral adjustments by the unmanned vessel, which could affect approach efficiency. Second, the path planning stability requirement dictates that an excessively large weight for the lateral offset error could lead to oscillations or instability in the path planning results. Therefore, a preset constant is used. The following should be ensured: First, the weight of the lateral offset error should be within a reasonable range; second, the smoothness of the stage transition should be guaranteed, and a preset constant should be used. The weight parameters of the approach segment and the alignment segment should be smoothly transitioned, and the values ​​are usually in the range of 0.05 to 0.2.

[0167] In S4, such as Figure 2 As shown, based on the multi-stage switching conditions of the berthing process, the berthing stage is dynamically switched during the berthing process of the unmanned vessel. At that time, the unmanned vessel was in the approach phase of its berthing process; when At that time, the unmanned vessel's berthing process switches to the alignment phase; when At this point, the unmanned surface vessel's (USV) berthing process switches to the berthing phase. To avoid excessive initial deviation leading to uncontrolled instability, attitude and position entry conditions are set during the berthing phase. Furthermore, the unmanned vessel's berthing process is only allowed to switch from the alignment phase to the berthing phase when both attitude and position entry conditions are met simultaneously. Otherwise, the berthing process remains in the alignment phase, and the control sequence is generated based on the optimal path in S3. The unmanned vessel's autonomous berthing is controlled according to the control scheme of the alignment phase until the attitude and position entry conditions of the berthing phase are met.

[0168] During the approach and alignment phases, the forward sight point is determined based on the optimal path, and the desired heading angle is generated. with expected speed The yaw rate command is calculated based on the heading deviation error and the speed error. With longitudinal acceleration command In the fine berthing segment, the control sequence obtained from model predictive control optimization is directly used as... and The control commands are subject to rate-of-change constraints and first-order smoothing filters to suppress jitter.

[0169] Control commands are sent to the actuators (such as thrusters and servos) of the unmanned vessel via the communication module, driving the unmanned vessel to move along the planned path or optimized trajectory to complete the autonomous docking operation.

[0170] The distance between the unmanned boat and the berth Lateral offset error Heading deviation and speed When all values ​​are less than their respective thresholds, the unmanned vessel is deemed to have successfully docked.

[0171] The threshold for successful berthing includes the distance threshold. Lateral offset error threshold Heading deviation threshold and speed threshold These are all empirical parameters set manually.

[0172] The determination of the lateral offset error threshold is based on the following criteria: first, the hull dimension constraint, which should be less than half the hull width; second, the berth dimension constraint, which should be less than half the difference between the berth width and the hull width; and third, the control accuracy requirement, which should match the positioning accuracy of the unmanned vessel positioning system.

[0173] The determination of the heading deviation threshold is based on the following: First, the geometric constraints of the berth, which should ensure that the hull remains completely within the berth width range even when it is deviating; second, the geometric constraints of the hull, which should ensure that the projected width of the hull does not exceed the difference between the berth width and the safety margin; and third, the berthing attitude requirements, which are usually taken in the range of 3 to 10 degrees.

[0174] The determination of the speed threshold is based on the following criteria: first, berthing safety requirements, which should ensure that the speed of the unmanned vessel is low enough when it successfully berths; second, propulsion system constraints, which should match the low-speed control capability of the unmanned vessel's propulsion system; and third, berthing accuracy requirements, which are usually taken in the range of 0.1 m / s to 0.2 m / s.

[0175] Example 2

[0176] Based on Example 1, a dynamic adjustment strategy for model predictive control weight parameters is adopted in the berthing section, using the distance between the unmanned vessel and the berth in the current sampling period. The independent variable is the berthing segment. The distance between the unmanned vessel and the target berth is dynamically adjusted, where:

[0177] ;

[0178] ;

[0179] ;

[0180] In the formula, The distance between the unmanned vessel and the target berth. , , Divided into The dynamic adjustment coefficient satisfies , Preset small positive numbers, Not following change.

[0181] Preset small positive numbers These are manually set empirical parameters used to prevent the denominator from being zero during dynamic weight calculations, and to adjust the sensitivity of the weights to changes in distance. Preset to small positive numbers. The basis for this determination is: firstly, the requirement for numerical stability, presupposing small positive numbers. Ensure that the distance between the unmanned vessel and the target berth is [not specified]. First, the denominator should not be zero when the value is close to zero to avoid numerical oddities in the weight calculation; second, there are requirements for weight sensitivity adjustment, with a preset small positive number. The larger the value, the lower the sensitivity of the weight to changes in distance; a small positive number is preset. The smaller the value, the more sensitive the weight becomes to changes in distance; thirdly, the weight value range is constrained, with a preset small positive number. The weights should be kept within a reasonable range during berthing, typically between 0.1 and 0.5.

[0182] After adopting a dynamic adjustment strategy, when the unmanned surface vessel (USV) is far from the target berth, the values ​​of each weight parameter are small, and the control of the USV is relatively relaxed; as the USV gradually approaches the berth, the values ​​of each weight parameter increase accordingly, and the control precision requirements increase. Because The maximum value indicates that the weight of lateral offset error consistently dominates during precise berthing, ensuring the unmanned vessel accurately approaches the target berth. This dynamic adjustment mechanism adaptively adjusts the control strategy based on the berthing distance, improving berthing accuracy while maintaining berthing efficiency.

[0183] When the unmanned vessel enters the berthing section, precise control is achieved using Model Predictive Control (MPC). For example... Figure 3 As shown, the predictive control model provided by this invention is a closed-loop control system composed of an MPC controller, an optimization solver, an unmanned surface vessel (USV) system, a predictive model, and constraints. The target berth information obtained in S1 and the optimal path selected based on the comprehensive path cost evaluation function obtained in S3 are used as inputs to the predictive control model, providing the desired motion direction and final berthing target for the USV's autonomous berthing process. The information acquisition period, i.e., the sampling time interval, is... One sampling period corresponds to one control period. A state prediction model for the unmanned surface vessel (USV) is constructed. In each control period, based on the acquired real-time state information of the USV, and according to the current state vector and control vector, the state of the USV at the next moment is predicted, providing the MPC controller with future state information. An objective function for predictive control optimization is constructed. In each sampling period, using the current state combined with the future state predicted by the USV state prediction model, a predictive objective function value is calculated to quantify the merits of the current control strategy and provide direction for optimization. Constraints are applied in the MPC controller. In each control period, the MPC, based on the current system state and the prediction model, coordinates the optimal path, objective function, USV state prediction model, and constraints to generate an optimization problem and transmit it to the optimization solver. The optimization solver uses a rolling time-domain optimization method to find the optimal solution to the objective function while satisfying the constraints, obtaining the optimal control sequence. This sequence is used to generate the control commands for the USV at the current moment and transmits them to the USV control system via the communication module. The USV control system, based on the received control commands, drives the thrusters and servos to produce corresponding motions on the USV. Meanwhile, the sensors of the unmanned vessel system measure its own state information in real time, such as position, speed, and attitude, and transmit the measured state information to the MPC controller. The MPC controller uses this feedback information to update the prediction model and optimize the problem, providing a basis for decision-making in the next control cycle.

[0184] In each control cycle, the MPC controller re-predicts, reconstructs and solves the optimization problem based on the latest feedback status information, and updates the control variables to achieve dynamic adjustment and real-time control in response to system uncertainties and external disturbances. In each control cycle, the MPC controller executes only the first control variable, and then repeats the process in the next cycle, achieving rolling optimization.

[0185] Figure 1 This is a schematic diagram of the unmanned vessel autonomous berthing method provided by the present invention, as shown below. Figure 1 As shown, after the automatic control of the unmanned vessel's berthing process begins, the system acquires the unmanned vessel's real-time status information and target berth information. Based on the distance between the unmanned vessel and the target berth, the berthing process is divided into multiple stages, and the current berthing stage (current berthing segment) is determined. According to the multi-stage switching conditions of the berthing process, the berthing stages are dynamically switched during the unmanned vessel's berthing process. At that time, the unmanned vessel was in the approach phase of its berthing process; when At that time, the unmanned vessel's berthing process switches to the alignment phase; when When both attitude and position entry conditions are met, the unmanned surface vessel (USV) berthing process switches from the alignment phase to the berthing phase. A comprehensive path cost evaluation function is used to assess the overall path cost and select the optimal path. Based on the USV's current state information, its trajectory is predicted, and a rolling time-domain optimization method is employed to optimize the trajectory, generating an optimal control sequence. Control commands are generated based on the optimal path and control sequence and sent to the USV's actuators to determine if berthing is successful. The distance between the USV and the berth is then considered. Lateral offset error Heading deviation and speed If all values ​​are less than their respective thresholds, the unmanned vessel is deemed to have successfully docked; otherwise, the process returns and the docking stage of the current docking process is reassessed.

[0186] Figure 4 This is a simulation verification diagram of the intelligent berthing path planning method for ships described in this invention under a complex obstacle environment. The black dashed curve in the diagram represents the path of the unmanned vessel during the berthing process. The arrow at the starting end of the path (the end furthest from the berth) indicates the initial position and initial heading of the vessel, and the arrow at the ending end of the path (one end within the berth) indicates the target berthing position and desired berthing attitude. The black shaded area represents obstacles within the port waters, including breakwaters, shoals, other moored vessels, and restricted navigation areas. Figure 4 As shown, the present invention adopts a three-stage progressive berthing control strategy, including an approach stage, an alignment stage, and a fine control stage, with the final stage using model predictive control to achieve precise positioning. Figure 4 The dashed path terminates precisely at the target berth, and the direction of the arrow at the termination point is consistent with the desired berthing posture, indicating that this invention achieves high-precision berthing. This invention introduces berth safety margin constraints, collision prediction and protection mechanisms, and hard and soft constraints on berth boundaries during path planning and control to ensure that the vessel maintains a safe distance from obstacles and berth boundaries. Figure 4 The dashed path smoothly bypasses all obstacle areas, demonstrating that this invention effectively ensures berthing safety. This invention employs a planning strategy combining candidate path generation and optimal path selection, and uses sequential convex programming iteratively to solve the model predictive control problem, ensuring a feasible solution under complex constraints. Figure 4 This invention demonstrates the successful planning of a feasible optimal path in a complex environment with multiple obstacles, indicating that it has strong environmental adaptability and robustness.

[0187] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. 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. Such 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. A method for unmanned vessel berthing control based on stage scheduling and constraint model prediction, characterized in that, include: S1. Obtain real-time status information and target berth information of the unmanned vessel, and divide the unmanned vessel berthing process into approach segment, alignment segment and berthing segment according to the distance between the unmanned vessel and the target berth; S2. Generate multiple candidate paths in the approach and alignment segments, and select the optimal path based on the comprehensive path cost evaluation function; S3. During the berthing phase, a predictive control model is constructed. Based on the current state information of the unmanned vessel, the motion trajectory of the unmanned vessel is predicted. The rolling time-domain optimization method is used to optimize the motion trajectory of the unmanned vessel and generate the optimal control sequence. S4. Generate control commands based on the optimal path and control sequence, and send the control commands to the unmanned vessel's actuators to control the unmanned vessel to complete the autonomous docking operation; S2 Integrated Path Cost Evaluation Function for: ； ； ； In the formula, The candidate path length, This represents the change in the path heading angle. For the sake of maintaining a safe distance, For heading deviation error, Indicates the speed penalty item. For segment safety speed threshold, , , , , , These respectively represent the berthing process The weight parameters, For the speed of the unmanned ship, The heading angle of the unmanned vessel; In the approach and alignment segments, multiple candidate paths are generated, based on... Calculate the path cost evaluation value of each candidate path, select the candidate path with the smallest path cost evaluation value as the optimal path, and the unmanned vessel navigates according to the optimal path during the approach and alignment phases. During the berthing phase, a predictive control model is constructed, and model predictive control methods are used to precisely control the unmanned surface vessel, including: S3.1, Construct an unmanned vessel state prediction model, based on... Prediction of the state vector and control vector of the unmanned surface vessel at any time Real-time status of the unmanned vessel, obtain The predicted state vector of the unmanned surface vessel at time t, with a sampling time interval of t. ; S3.2, construct the objective function for predictive control optimization, apply constraints, and use an iterative linear method to solve the unmanned vessel state prediction model to obtain the iterative optimization variables for adjacent sampling periods; S3.3, using the rolling time-domain optimization method, steps S3.1 and S3.2 are executed in each sampling period to apply control rate of change constraints to the control vector and trust region constraints to the iterative optimization variables, thereby obtaining the optimal control sequence; In S3.2, the constraints include state constraints, control constraints, and safety margin constraints for berth boundaries and hull shape. State constraints include speed constraints to limit the speed range of the unmanned vessel and heading deviation error constraints to limit the deviation error range between the heading angle of the unmanned vessel and the heading of the berth. Control constraints include acceleration constraints to limit the upper limit of the acceleration of the unmanned vessel and turning angular velocity constraints to limit the upper limit of the turning angular velocity of the unmanned vessel. The speed range constraints, acceleration constraints, and turning angular velocity constraints are determined by the performance of the unmanned vessel, while the heading deviation error constraints are jointly determined by the geometry of the berth and the geometry of the unmanned vessel. The construction of safety margin constraints for berth boundaries and hull shape includes: representing the berth area as a convex polygon in the berth reference coordinate system; using a set of linear inequalities for constraints to obtain berth boundary constraints; calculating the hull projection margin based on the geometry of the unmanned vessel and its heading angle in the berth reference coordinate system; and merging the hull projection margin into the berth boundary constraints.

2. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 1, characterized in that, The real-time status information of the unmanned surface vessel includes its real-time position, real-time heading angle, real-time speed, and real-time acceleration. The target berth information includes the target berth's location, obstacle location, shoreline location, and the target berth's orientation angle. Based on the target berth information, construct the berth centerline equation: ； In the formula, This indicates the position coordinates of a point on the center line of the berth. Let be the normal vector of the berth centerline. This is a constant term for the berth centerline. The distance between the unmanned vessel (UV) and the target berth is calculated based on the UV's position information and the target berth's position information. The UV's berthing process is divided into an approach segment, an alignment segment, and a berthing segment based on this distance. During the approach segment, if the distance between the UV and the target berth is greater than a first distance threshold... The distance between the unmanned surface vessel and the target berth is within the first distance threshold. With the second distance threshold Between these points, the distance between the unmanned vessel in the berthing section and the target berth is less than the second distance threshold. ; right Normalization is performed to calculate the lateral offset error between the unmanned vessel's center of mass and the berth centerline. : ； ； In the formula, This indicates the coordinates of the center of mass of the unmanned vessel.

3. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 2, characterized in that, The unmanned vessel state prediction model is as follows: ； ； ； In the formula, express The predicted state vector of the unmanned surface vessel at any given time. express The state vector of the unmanned ship at any given time. express The control vector of the unmanned surface vessel at any given time. express The coordinates of the unmanned ship's center of mass at all times. express The heading angle of the unmanned ship at all times. express The speed of the unmanned ship at all times. express The longitudinal acceleration of the unmanned ship at all times express The turning angular velocity of the unmanned vessel at all times.

4. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 3, characterized in that, Constructing the objective function for predictive control optimization : ； In the formula, For stage cost items, This is a terminal penalty item; ； In the formula, For the state tracking error term, To control the consumption of ability items, To control the incremental smoothing term; ； In the formula, For terminal location cost term, For the terminal heading cost item, For terminal speed cost, This is the terminal soft constraint cost term.

5. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 4, characterized in that, ； ； ； In the formula, The prediction step size is equivalent to the total number of prediction time steps. The square of the weighted norm is used to... Assign weights to each component. For the first The state error vector of the step, Here is the weight matrix for the state error. For predicting the time step index, For the first The tangential offset error component of the step, For the first The tangential position coordinates of the unmanned surface vessel's center of mass in the berth reference coordinate system. For the first The tangential reference position coordinates of the unmanned surface vessel. As the weight of the tangential error, For the first The lateral offset error component of the step. For the first The normal position coordinates of the unmanned surface vessel's center of mass in the berth reference coordinate system. For the first The normal reference position coordinates of the unmanned surface vessel. As the weight of the lateral error, For the first The heading offset error component of the step, For the first The actual heading angle of the unmanned surface vessel. For the first The expected heading angle of the unmanned surface vessel. As the weight of the heading deviation error, For the first The speed error component of the step, For the first The actual speed of the unmanned surface vessel. Indicates the first The expected speed of the unmanned surface vessel. This represents the weight of the speed error.

6. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 5, characterized in that, ； ； In the formula, For the first The longitudinal acceleration of the unmanned surface vessel. Indicates the first The turning angular velocity of the unmanned surface vessel. To control capability weights, For the first Relaxation variables of the soft constraint at the berth boundary. The penalty weights for the slack variables under soft constraints; For the acceleration change component, For the first The longitudinal acceleration of the unmanned surface vessel. This represents the component of the change in the bow angular velocity. For the first The turning angular velocity of the unmanned surface vessel. To control the changing weights.

7. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 6, characterized in that, ； ； ； ； In the formula, This represents the penalty coefficient for the terminal state. Let be the coordinates of the unmanned vessel's center of mass in the tangential terminal position of the reference coordinate system at the berth. The coordinates are the tangential terminal reference positions of the unmanned surface vessel. Let be the coordinates of the normal terminal position of the unmanned vessel's center of mass in the berth reference coordinate system. Here are the coordinates of the unmanned surface vessel's normal terminal reference position. The terminal heading angle of the unmanned vessel. The expected terminal heading angle of the unmanned vessel. This is the terminal speed penalty coefficient. The terminal speed of the unmanned vessel. For the slack variables of the soft constraint of the berth boundary at the terminal time.

8. The unmanned vessel berthing control method based on stage scheduling and constraint model prediction according to claim 7, characterized in that, berthing section Dynamically adjust based on the distance between the unmanned vessel and the target berth: ； ； ； In the formula, The distance between the unmanned vessel and the target berth. , , Divided into The dynamic adjustment coefficient, It is a preset small positive number.

Images