A ship port entry-docking autonomous navigation control method and system based on LBMPC

Abstract

The application discloses a kind of based on LBMPC ship port entry-docking autonomous navigation control method, comprising: establishing port environment model, and according to port information and ship navigation state from candidate berth set optimal berth is selected;The whole process of ship port entry-docking is divided into port entry stage, berthing area stage and low-speed into berth stage, and the trajectory node of each stage is calculated based on port environment model;Improved fast exploration random tree star algorithm is used to plan the path of port entry stage, and Dubins curve is used to plan the path of berthing area stage, and global trajectory is obtained by combining nonlinear decay model for speed distribution;In the process of actual navigation, learning model prediction control based on Gaussian process regression compensation is used for local dynamic pose planning and trajectory tracking control, and in low-speed into berth stage, by tightening motion constraint, the docking task of ship is completed.The method can improve the control accuracy and safety of ship navigation in the whole process of port entry.

Description

Technical Field

[0001] This invention relates to the field of waterway transportation technology, specifically to a method and system for autonomous navigation control of ships entering and berthing in port based on LBMPC. Background Technology

[0002] In recent years, with the rapid development of the global shipping industry, port throughput has continued to climb, and the trend towards larger and denser vessels has become increasingly apparent. Port waters are among the most complex and risky areas for ship navigation, characterized by limited water area, narrow channels, dense berths, and significant impact from environmental disturbances such as wind, waves, and currents. Currently, most port vessel operations still rely primarily on manual handling. However, manual handling is susceptible to external disturbances and human factors, such as information delays, fatigue, or judgment errors, which may increase the risk of accidents.

[0003] Existing research on intelligent ship navigation has made key progress in trajectory planning and collision avoidance decision-making, but there are still shortcomings in unmanned autonomous control of the entire process of entering and berthing in port. These shortcomings are mainly reflected in the following aspects: First, trajectory planning methods based on static geometric calculations are difficult to cope with the real-time changing port environment and autonomous collision avoidance requirements; second, traditional model predictive control (MPC) relies on accurate mathematical models, and its performance is easily affected by external disturbances and parameter uncertainties, leading to control deviations or oscillations. These factors limit the applicability and robustness of traditional automatic control technologies in port waters. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a ship autonomous navigation control method and system based on LBMPC for entering and berthing in order to overcome the shortcomings of the prior art. This method can improve the control accuracy and safety of the ship's navigation throughout the entire process of entering the port.

[0005] To achieve the above objectives, according to one aspect of the present invention, a ship entry-berthing autonomous navigation control method based on LBMPC is provided, comprising: Acquire the spatial distribution data of port static facilities and environment, establish a port environment model, and select the optimal berth from the candidate berth set based on the spatial distribution data of port static facilities and environment and the ship navigation status; The entire process of a ship entering the port and berthing at the optimal berth is divided into the port entry stage, the berthing area stage, and the low-speed berthing stage, and the trajectory nodes of each stage are calculated based on the port environment model. Based on the trajectory nodes, the improved Rapidly-exploring RandomTree Star (RRT*) algorithm is used to plan the path for the port entry stage, the Dubins curve is used to plan the path for the berthing area stage, and a nonlinear decay model is combined to perform velocity allocation to obtain the global trajectory. During the actual navigation based on the global trajectory, Learning-Based MPC (LBMPC) based on Gaussian process regression compensation is used for local dynamic pose planning and trajectory tracking control. In the low-speed berthing phase, the motion constraints of the Learning-Based MPC are tightened to complete the berthing task of the ship.

[0006] In the above scheme, the selection criteria for the candidate berths include: The berth length is not less than the total length of the vessel plus the allowable length for berthing; the berth width is not less than the beam of the vessel plus the allowable width for berthing; the width of the water area in front of the berth is not less than the minimum safe water area width required for the berthing area; the maximum offsetting energy of the berth shoreline facilities is not less than the impact energy generated by the residual velocity of the vessel's inertia; the sum of the recent average water depth of the berth and the predicted arrival tide is not less than the sum of the vessel's draft and the required underwater clearance; and the berth is currently in a usable state.

[0007] In the above scheme, the selection of the optimal berth is achieved through a comprehensive cost function; The comprehensive cost function is composed of the following weighted factors: the width of the waterway in front of the berth, the maximum offsetting energy of the berth shoreline facilities, the berth length, the berth width, the current channel historical average entry time, the lateral merging velocity near the berth, and the maneuvering complexity index. The maneuvering complexity index is composed of the following weighted factors: total turning maneuvering coefficient, longitudinal current velocity when the ship is sailing downstream in the berthing area during turning maneuvering, merging current velocity in the berthing area, turning water radius, and turning maneuvering penalty coefficient.

[0008] In the above scheme, the port entry phase refers to the process where the vessel begins to approach the optimal berth (target berth) from an area far from the port. The berthing area phase refers to a vessel approaching the optimal berth at less than 2 knots and possibly making a turning maneuver. The low-speed berthing phase refers to the ship approaching the berth center precisely at a speed of 0.2~0.5kn under the control of the propeller and rudder, reaching the berth node.

[0009] In the above scheme, the improved fast-exploration random tree star algorithm includes the following improvements introduced for the standard fast-exploration random tree star algorithm: A dynamic sampling bias mechanism is established so that when the algorithm explores the initial path, the probability of the sampling point being selected as the target point and the radius of the search for new nodes can be adaptively adjusted according to the relative distance between the current node and the destination. Establish a navigation safety collision avoidance detection mechanism by performing obstacle collision detection within a multi-layered concentric rectangular channel centered on the current path segment and calculating a quantified hazard index to ensure that newly expanded nodes are located within the safe zone. A guided sampling path optimization mechanism is established. After obtaining the initial path, an elliptical sampling space is constructed with the start and end points as the focal points and the major axis equal to the current path length. Based on an optimal safe path cost function that simultaneously considers path length and average risk, efficient sampling is performed within this elliptical space using a reconfigurable KD-tree (K-Dimensional Tree) data structure to optimize the path.

[0010] In the above scheme, the step of using the improved fast exploratory random tree-star algorithm to plan the path for the port entry phase further includes: After obtaining the initial path using the improved fast exploration random tree-star algorithm, the following post-processing steps are performed sequentially to improve the smoothness and traceability of the path: First, abrupt corner points in the path with turning angles exceeding a threshold are detected and marked, and then smoothed; then, a sliding window filter based on a Gaussian kernel function is used to smooth the path point sequence as a whole to eliminate high-frequency noise; finally, a Clamped cubic B-spline curve is used to fit the preprocessed path points to generate the final smooth path.

[0011] In the above scheme, the method of using the Durbins curve to plan the path in the berthing area stage further includes: The endpoints of the path in the port entry phase are optimized using Durbins curves. The path in the berthing area phase is planned using the Dubins curve. Under the condition of satisfying the turning radius constraint, the shortest path from the starting pose to the ending pose is one of the six basic motion types. The six basic motion types include: left turn arc-straight line-left turn arc, left turn arc-straight line-right turn arc, right turn arc-straight line-left turn arc, right turn arc-straight line-right turn arc, left turn arc-right turn arc-left turn arc, and right turn arc-left turn arc-right turn arc.

[0012] In the above scheme, the use of a learning model predictive control based on Gaussian process regression compensation for local dynamic pose planning and trajectory tracking control, and the tightening of the motion constraints of the learning model predictive control during the low-speed berthing phase to complete the ship's berthing task, includes: A nominal dynamic model of ship motion is established, which maps the ship's state from the hull coordinate system to the geographic coordinate system and describes the effect of control input on the ship's velocity vector. Based on Gaussian process regression online learning and compensation for the dynamic residual between the nominal dynamic model and the actual ship motion, the Gaussian process regression uses the ship velocity vector and control input vector of the previous moment as feature vectors to output a probability distribution estimate of the dynamic residual, including the mean and variance. The mean residual estimate of the regression output of the Gaussian process is combined with the prediction results of the nominal dynamics model to construct a learning model for model predictive control. Based on the learning model, within a finite prediction time domain, the optimal control input sequence that satisfies ship maneuvering constraints, static obstacle collision avoidance constraints, and dynamic obstacle collision avoidance constraints is solved. The first control command in the optimal control input sequence is executed, and the above steps are re-executed in the next sampling period based on the latest ship status to achieve rolling optimization control and complete the berthing task.

[0013] In the above scheme, the Gaussian process regression uses a squared exponential kernel function to perform online training and prediction based on historical states and control input data, and outputs the mean and variance of the residual estimate, which are used to update the control model and construct the uncertainty compensation distance in dynamic collision avoidance.

[0014] According to another aspect of the present invention, a ship port entry-berthing autonomous navigation control system based on LBMPC is provided, comprising: The environmental perception module is used to acquire data on the spatial distribution of port static facilities and environment, establish a port environment model, and select the optimal berth from the candidate berth set based on the data on the spatial distribution of port static facilities and environment and the ship navigation status. The trajectory planning module is used to divide the entire process of a ship entering the port and berthing to the optimal berth into the port entry stage, the berthing area stage, and the low-speed berthing stage, and calculate the trajectory nodes and corresponding poses of each stage based on the port environment model. The predictive control module is used to plan the path of the port entry stage using an improved fast exploration random tree star algorithm based on the trajectory nodes, plan the path of the berthing area stage using the Durbins curve, and combine the nonlinear decay model to perform speed allocation to obtain the global trajectory. The execution module is used to perform local dynamic pose planning and trajectory tracking control using a learning model predictive control based on Gaussian process regression compensation during actual navigation based on the global trajectory, and to complete the berthing task of the ship by tightening the motion constraints of the learning model predictive control during the low-speed berthing phase.

[0015] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects: This invention provides a ship autonomous navigation control method based on LBMPC for port entry and berthing. This method enables ships to autonomously complete global trajectory planning, local dynamic pose planning and trajectory tracking control based on multi-stage division in dynamic port waters, thereby improving the control accuracy and safety of ship navigation throughout the entire port entry process. Attached Figure Description

[0016] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings: Figure 1 This is a flowchart illustrating a ship autonomous navigation control method for entering and berthing based on LBMPC in Embodiment 1 of the present invention.

[0017] Figure 2 This is a schematic diagram of the ship berthing mode determination mechanism in Embodiment 1 of the present invention.

[0018] Figure 3 This is a schematic diagram illustrating the multi-stage division of the entire process of a ship entering port and berthing in Embodiment 1 of the present invention.

[0019] Figure 4 This is a flowchart of the global trajectory planning process for ships based on the improved RRT* algorithm in Embodiment 1 of the present invention.

[0020] Figure 5 This is a flowchart of the ship local dynamic pose planning and control based on the LBMPC method in Embodiment 1 of the present invention. Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0022] It should be understood that the sequence number of each step in the embodiment does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0023] Example 1 This application provides a method for autonomous navigation control of ships entering and berthing based on LBMPC. Please refer to [link / reference]. Figure 1 ,include: S1. Obtain spatial distribution data of port static facilities and environment, establish port environment model, and select the optimal berth from the candidate berth set based on the spatial distribution data of port static facilities and environment and ship navigation status.

[0024] In this embodiment, specifically, by acquiring spatial distribution data of various static facilities and environment in the port, different static facilities are classified and geometrically represented using a polygonal modeling strategy to obtain a set of candidate berths. Based on port information and navigation status, the optimal berth for this entry into the port is selected, and a unified port environment model is established. In acquiring port environment and berth collection Under the premise of, candidate berth set Specifically, this can be expressed as: (1) in, For candidate berths, The length of the berth. The total length of the vessel, To allow for ample length for berthing, The width of the berth. For the width of the boat, To provide ample berth width, The width of the waterway in front of the berth. The minimum safe water width required for berthing is related to whether a U-turn is required. This represents the recent average water depth of the berth. To predict the arrival tide level, For the ship's draft, For the required underwater clearance, This indicates the occupancy status of the berths. To maximize energy offset for berth shoreline facilities. The impact energy generated by the ship's residual inertial velocity can be approximated as... ,in For equivalent ship mass, The normal inertial residual velocity after entering the berth.

[0025] Comprehensive cost function for selecting the optimal berth for: (2) This is the optimal berth for this berthing.

[0026] Using the method described above, the comprehensive cost function for selecting the optimal berth is... for: (3) in, These are the signed normalized weight coefficients. The current average port entry time is based on the historical data of the current waterway. The lateral merging velocity near the berth. To manipulate complexity metrics, its expression is as follows: (4) in, These are the signed normalized weighting coefficients; This is the total steering control coefficient, which is related to the number of steering inputs and the steering amplitude. The longitudinal current velocity when a vessel is maneuvering downstream within the berthing area for turning maneuvers; The merging velocity within the berthing area; The radius of the waterway where the U-turn takes place; The penalty coefficient for turning around.

[0027] S2 divides the entire process of a ship entering the port and berthing to the optimal berth into the port entry stage, the berthing area stage, and the low-speed berthing stage, and calculates the trajectory nodes of each stage based on the port environment model.

[0028] Specifically, in this embodiment, based on the port environment model, the process for calculating trajectory nodes at each stage is as follows: The entire process of a vessel entering the port and berthing at the optimal berth is divided into three stages: Stage 1 (Entry Stage): The vessel smoothly enters the area surrounding the port structures from an area far from the port until it reaches the vicinity of the berthing area, where its node connects with the berthing area, maintaining a minimum safe distance from the shoreline. The berthing area refers to a safe waterway in front of the berth, allowing the vessel to turn around or approach the berth without considering extreme berthing maneuvers. Stage 2 (Benchmarking Area Stage): Within the berthing area, the vessel controls its attitude or performs a turning maneuver at a speed below 2 knots, while further decelerating to reach the vicinity of the berth at a speed of 0.2~0.5 knots. This node indicates that the vessel's attitude has met the berthing requirements. Stage 3 (Low-Speed ​​Berthing Stage): Based on the adjusted berthing attitude, the vessel laterally approaches the berth through precise propeller and rudder maneuvers. Upon reaching the third stage node, the entry task is completed.

[0029] Following the above port entry process, the trajectory nodes and corresponding poses for each stage are calculated, generating a complete port entry trajectory. The trajectory node generation strategy will be further explained below, focusing on the U-turn determination in step S1. To simplify the port environment model, it is assumed that any berth is adjacent to a certain side of the shoreline or port facilities, and the berth direction is defined. The direction of the vector formed by the two endpoints on the shore in a clockwise direction is used. The sign of the vessel's berthing direction for this port entry mission is calculated using a two-way exploration mechanism. This symbol indicates the ship's orientation when berthing. The relative relationship with the berth direction, and also based on the merging direction in front of the berth. Calculate the sign of the merging direction based on its relative relationship with the berth direction. ,like Figure 2 As shown. Symbol The calculation method is as follows: (5) The U-turn determination mechanism based on direction symbols is as follows: (6) Based on the above steps, the trajectory nodes and desired poses for each stage are determined, such as... Figure 3 The circular nodes in the diagram are shown in relation to the ship's attitude markers.

[0030] S3, based on trajectory nodes, uses an improved fast exploration random tree star algorithm to plan the path in the port entry stage, uses the Durbins curve to plan the path in the berthing area stage, and combines a nonlinear decay model to allocate speed and obtain the global trajectory.

[0031] Specifically, in this embodiment, a global trajectory is generated based on trajectory nodes. The first stage (port entry stage) uses an improved rapidly-exploring random tree star (RRT) algorithm to generate a path that balances collision avoidance safety, ship maneuverability, and optimal route selection. The second stage (berthing area stage) uses Dubins curves to generate a path within the berthing area that meets maneuverability requirements or turning radius, reaching the outside of the berth. The final stage (low-speed berthing stage) involves side-to-side berthing under precise propeller and rudder control, completing the berthing task. Simultaneously, a nonlinear decay model is used to allocate speed along the path to obtain the global trajectory.

[0032] The first stage (port entry stage), as the main trajectory, first uses an improved RRT* algorithm to generate the desired path. It's understandable that port scenarios are characterized by large obstacle areas but low density; the global trajectory for port entry needs to prioritize collision avoidance safety, maneuverability, and path efficiency, rather than the exploration advantages in high-density, complex obstacle scenarios. Therefore, with the goal of stable initial path exploration and efficient solution of the optimal safe path, the following improvements are introduced to the standard RRT* algorithm: Establish a dynamic sampling bias mechanism when exploring the initial path. Define the dynamic target bias strategy as follows: (7) in, Marking upper and lower boundaries with symbols is equivalent to , Indicates the next sampling point The probability of reaching the endpoint. Based on the probability, As a regulating factor, This represents the distance between the current node and the destination. Starting point To the finish line The distance.

[0033] Define a dynamic radius sampling method based on the current new node: (8) in, This indicates the newly generated node. The radius of the circle for the next sampling. The radius of the base sampling circle, This is an adjustment factor. In this embodiment, the dynamic sampling bias mechanism makes the sampling process both exploratory and guided, and adaptively adjusts based on the relative orientation of the endpoint.

[0034] Establish a navigation safety collision avoidance detection mechanism. Based on geometric port scene modeling, for the current node... With parent node The length of the connecting line segment is: (9) Set kernel rectangle The length and width are respectively: (10) Further construct the outer channel rectangle , , , Its shape expands proportionally: (11) in , And follow Increment, and with With one heart and one mind.

[0035] This yields a rectangular channel set: (12) In this embodiment, the set of environmental obstacles is set as follows: ,as well as The danger level of each passageway. , , , , Define the collision indication function for each channel: (13) Risk of creating a new node: (14) For any new node , must meet .

[0036] A guided sampling (Informed-sampling) path optimization mechanism based on the optimal safety cost function and a reconfigurable KD-tree is established. The specific method is as follows: After obtaining the initial path, the RRT tree structure is reconstructed, retaining only the initial path points. An elliptical space is introduced, with the two foci of the ellipse located at the start and end points, respectively, and its major axis length is equivalent to the current path length. For any sampling point All must satisfy the basic properties of an ellipse: (15) A random sampling point is generated within the unit circle. Based on the parameters of the ellipse and its center position, the sampling point is mapped to the ellipse through a transformation. The coordinate transformation process from the unit circle to the ellipse is as follows: (16) The semi-major axis and semi-minor axis of the ellipse are respectively represented as... and , and The subscripts correspond to the center of the unit circle and the center of the ellipse, respectively.

[0037] Introduce the following optimal safe path cost function As the basis for updating the guided sampling path: (17) in, These are the signed normalized weight coefficients. This represents the average risk level of the current path. Based on the aforementioned cost function, the path length and risk level continuously approach the theoretical optimal value.

[0038] KD-trees are based on the concept of binary search trees. They divide a multidimensional space into multiple disjoint subspaces using a hyperplane, thereby enabling fast data retrieval. For a point set containing several nodes... Calculate the variance of the latitude of each of the two coordinate axes: (18) choose and The larger value in the middle is used as the dividing axis. The nodes are sorted according to the latitude of the dividing axis, and the median point is calculated. Based on the dividing axis and the median point, left and right branches are divided to obtain the left branch point set of the first-level sub-branch. With the right branch point set Repeat the above steps until the [number]th [number]. The smallest branch of the layer The number of nodes on the branch threshold is less than the number of branches. Construct a KD tree.

[0039] The neighbor search process based on a KD-tree is roughly as follows: given a new node... and neighbor radius ,from Search the branch tree from near to far, consisting of sub-branches, branches at the same level, and branches at the previous level. At each level, keep only branches that may intersect the radius circle and discard the rest. or Latitude coordinates exceed The branch. The neighbor set is continuously maintained during the search process. and nearest neighbor distance Until the range of all branch routes is completely exceeded. The search process is now complete.

[0040] Set reconstruction threshold ,exist Newly added nodes will be directly inserted into the specific minimum branch, and will follow... It splits on its own. The number of new nodes reaches [a certain threshold]. Then, the KD-tree is reconstructed to synchronize the latest spatial features. Introducing the KD-tree significantly reduces the time spent on traversal neighbor searches. The flowchart for ship global trajectory planning based on the improved RRT* algorithm is shown below. Figure 4 As shown.

[0041] The improved RRT* algorithm can theoretically plan paths that infinitely approach the optimal risk and length. However, when considering planning efficiency, iterative limits are usually imposed on the optimization process, which may still result in noise issues inherent in the RRT algorithm. By establishing a noise reduction and smoothing mechanism, a balance is achieved between path planning efficiency, optimal solution, and preservation of path characteristics, while also ensuring its traceability.

[0042] First, the path is smoothed using abrupt change angle smoothing mechanism to ensure stable ship tracking. Abrupt change angle refers to any sharp angle that may exist within the path. Given a two-dimensional path point sequence... , detection Points At that time, with three points , and Given the angle between two adjacent path segments, the following detection logic applies: (19) in, The mutation angle symbol is used. For the first The angle formed by the path points of the group This represents the maximum allowed turning angle along the current path segment. The entire path is marked sequentially, at each point... The marking method at the location is as follows: (20) Based on the marked path, adjacent control points are used to smooth each marked point to eliminate abrupt angles and preserve path features to the greatest extent. Subsequently, a sliding Gaussian kernel window is used to eliminate overall high-amplitude noise in the path. Given a sliding window size... with Gaussian kernel standard deviation To obtain a smooth path The smoothing process is defined as: (twenty one) in, Gaussian weighting function: (twenty two) When index When the boundary is crossed, compensation is made using boundary reflection: (twenty three) Following the above method, a preprocessing path that preserves features can be obtained.

[0043] B-spline curves exhibit good smoothing performance, with their functions primarily composed of control points, basis functions, and node vectors. However, a drawback is their inability to completely eliminate noise and adaptively preserve path features. Therefore, this embodiment introduces a Clamped-type cubic B-spline curve smoothing method based on preprocessed paths to generate smooth paths. The definition of a B-spline curve is shown in the following equation: (twenty four) in, Indicates the first indivual order basis functions, For the first One control point, Let be the parameter. The recursive formula for the B-spline basis function is as follows: (25) The original B-spline curve does not retain the local features of the start and end points. Multiplicity at both ends ensures that the B-spline curve passes through both the start and end points, thus satisfying the Clamped type: (26) The second stage (berthing area stage) path is planned using Dubins curves, and the ends of the first stage (entry stage) path are also optimized using Dubins curves to ensure the vessel can smoothly navigate to the berthing area and approach the berth to complete attitude control or turning maneuvers. Let the attitude of the moving body on the plane be... ,in , and These are the position coordinates and the orientation angle, respectively, and their curvature is affected by the minimum turning radius. Constraints. Therefore, the motion path can be composed of three basic motions: with radius... left-turning arc ; with radius A right-turning arc straight line segment Under this constraint, given the starting pose... and the final pose Its shortest path must satisfy one of the following six types: (27) Therefore, respectively in and Establish left-turn and right-turn circles by drawing tangents along the heading angle, and find the shortest feasible path that satisfies the turning radius constraint, allowing the ship to navigate to the vicinity of the berth and maintain its berthing attitude. Connect the endpoint of the second stage with the center of the optimal berth and set attitude constraints, allowing the ship to reach the optimal berth through precise maneuvering. Based on the complete entry path, a nonlinear attenuation model is used for speed adjustment, as follows: (28) in, The initial velocity, The preset minimum speed threshold, and These represent the length of the current path segment and the total path length, respectively. This represents the attenuation coefficient. Based on the speed distribution mechanism and combined with the heading distribution along the path, a complete port entry trajectory can be obtained.

[0044] S4, during the actual navigation based on the global trajectory, adopts a learning model predictive control based on Gaussian process regression compensation for local dynamic pose planning and trajectory tracking control, and completes the berthing task of the ship by tightening the motion constraints of the learning model predictive control during the low-speed berthing phase.

[0045] Specifically, in this embodiment, during actual navigation based on the global trajectory, LBMPC is used for real-time trajectory planning and control. During the calculation process, static facilities and dynamic obstacles are incorporated into collision avoidance constraints. The optimal control input for future time series is calculated through an optimization problem, and the corresponding motion trajectory is generated. Finally, during the berthing phase, LBMPC with strict pose and velocity constraints is used to solve for precise control commands, completing the berthing control. The specific LBMPC method based on the global berthing trajectory is as follows: A structure combining a nominal dynamics model with a Gaussian process regression learning module is adopted to achieve real-time compensation and uncertainty estimation of nonlinear hydrodynamic effects and external disturbances during ship motion, thereby improving the adaptability and robustness of the control system.

[0046] The ship dynamics model is established as follows: (29) in It is the ship's pose vector in the geographic coordinate system. , The coordinates are in a planar coordinate system. The bow angle of the ship; velocity vector in the ship's coordinate system , and These are the sway velocity, roll velocity, and bow roll angular velocity, respectively. This is a rotation matrix used to describe the ship's speed from the hull coordinate system to the geographic coordinate system, as shown below: (30) In equation (29), , and These are the inertia matrix, Coriolis matrix, centripetal matrix, and damping matrix, respectively, which include the added mass. express , and The generalized thrust vector in the direction, To control the input vector, express , and The direction does not show the perturbation modeled.

[0047] for , and The following uncertainty handling is performed: (31) in , and For the nominal matrix, , and The matrix is ​​uncertain. Therefore, the actual model of the ship can be represented as: (32) Based on equation (32), the ship state vector is defined as follows: The control input vector is , and Given propeller speed command and rudder angle command respectively, the ship's state-space model can be represented as: (33) in, For the nominal model, This is an uncertain model.

[0048] In MPC, the system is discretized using the Euler method, resulting in the following state equations: (34) in, This represents the discrete time step.

[0049] Based on the state equation, Gaussian process regression is used for online learning compensation of the model. The regression feature vector of the Gaussian process regression is defined as follows: Then the first The discrete ship dynamics model of the step can be expressed as: (35) in, It is the error input mapping matrix, used to describe the direction of the effect of the learning term on the state change; It is a dynamic residual term used to compensate for the difference between the nominal model and the real system.

[0050] At every moment True dynamic residual The following relationship must be satisfied: (36) in, It is the observation residual. It is a real but unknown nonlinear function. It is a Gaussian noise term, satisfying... ,in This represents the noise variance.

[0051] Gaussian process regression hypothesis It follows a mean function and kernel function Defined random process: (37) Among them, the mean function Functions that are usually set to zero Kernel function Using the squared exponent kernel: (38) In the formula, hyperparameters It is the magnitude function of the kernel function, representing the scale of the function's change; hyperparameters For the feature scale matrix, each The correlation length of the input latitude can be controlled by optimizing the hyperparameters through logarithmic marginal likelihood.

[0052] Given a training dataset Its predicted mean With variance It can be calculated using the following formula: (39) (40) in, Here is the sample kernel matrix, and each element is... , , and ; The output vector of the training sample; It is an identity matrix.

[0053] Therefore, the predicted output is... obey: (41) in, Reflects the estimation of unmodeled dynamics. It reflects a measure of uncertainty.

[0054] Incorporate the mean of the residuals predicted by Gaussian process regression into the prediction model: (42) The MPC method finds the optimal control input sequence based on a given prediction time domain, minimizing the following cost function: (43) in, To predict the length of the time domain, and This is the weight matrix; This is the terminal weight matrix.

[0055] In the optimization problem of MPC, to ensure that the control input meets the physical constraints of the propulsion and servo systems, the following maneuvering constraints are established: (44) (45) in, and These represent the minimum and maximum allowed values ​​for the control input, respectively. and To control the maximum rate of change of the input.

[0056] To ensure the integration of ships and static facilities safe distance It must meet the following requirements: (46) in, This is the minimum Euclidean distance function between the predicted point and the obstacle boundary. For safety margin.

[0057] Let the set of dynamic obstacles within a certain range be denoted as . Its motion state can be obtained by the ship's environmental perception system, and its... The predicted pose within the time interval is: (47) in It is a directional velocity vector, which is predicted based on the recent motion behavior of dynamic obstacles.

[0058] To accurately predict the short-term motion trend of dynamic obstacles, the method constructs a curve motion fitting model based on historical trajectory data, obtains the obstacle's velocity, acceleration, and rate of change of turning angle through local weighted regression, and predicts the obstacle's future trajectory based on the "Constant Turn Rate and Acceleration" (CTRA) assumption to obtain the pose of the obstacle in the future for several steps.

[0059] Record No. The trajectories of the dynamic obstacles at the most recent sampling time are as follows: (48) in These are the location coordinates of the obstacle and the sampling time, respectively. This is the length of the historical window used for local fitting.

[0060] Within this window, the speed is calculated using the first-order difference method. With heading angle : (49) in, and for , The change in coordinates over a time step.

[0061] Time-weighted least squares regression is performed on velocity and heading angle to estimate the acceleration and angular velocity at the current moment. The weighting function is defined as follows: (50) in is the time decay constant.

[0062] Fitting the velocity and heading angle sequences: (51) (52) in, and The regression intercept of the current speed and heading. and The slope is the fitting slope, where represents acceleration and angular velocity, respectively.

[0063] From this, we can obtain the current speed of the obstacle. heading angle acceleration With angular velocity Based on the CTRA assumption, the obstacle will in the future... Step (time interval) The pose prediction of ) can be calculated from the above physical quantities.

[0064] Establish relative position vectors Then the first The dynamic collision avoidance constraint at any given moment is expressed as: (53) in, The uncertainty compensation radius is the variance predicted by the Gaussian process regression. Together with the sensor positioning variance, the system will appropriately increase the collision avoidance distance when the speed of the dynamic obstacle is high or the variance is large.

[0065] Within each sampling period, the optimal control and trajectory point sequence is obtained through nonlinear optimization. , and in the next moment The internal optimization problem is based on the reconstruction of the latest state variables, forming a rolling time-domain closed-loop control. The process of the LBMPC method is as follows: Figure 5 As shown.

[0066] In summary, this embodiment provides a vessel autonomous navigation control method for port entry and berthing based on LBMPC. This method integrates global multi-stage pose planning with local real-time optimization control to achieve intelligent control throughout the entire process from port entry navigation and collision avoidance decision-making to autonomous berthing. The method comprehensively utilizes port environment modeling and berthing strategies, improved RRT* algorithm and Dubins curve global trajectory planning, and learning model predictive control based on Gaussian process regression compensation, enabling high-precision trajectory generation and stable tracking in dynamic port environments. By planning global navigation trends and updating collision avoidance trajectories in real time, it provides intelligent ships with a safe, efficient, and intelligent autonomous navigation solution for the entire port entry process, thereby improving the automation level of port vessel scheduling and navigation safety.

[0067] Example 2 This application provides a ship port entry-berthing autonomous navigation control system based on LBMPC, including: The environmental perception module is used to acquire data on the spatial distribution of port static facilities and environment, establish a port environment model, and select the optimal berth from the candidate berth set based on the data on the spatial distribution of port static facilities and environment and the ship navigation status. The trajectory planning module is used to divide the entire process of a ship entering the port and berthing to the optimal berth into the port entry stage, the berthing area stage, and the low-speed berthing stage, and calculates the trajectory nodes and corresponding poses of each stage based on the port environment model. The predictive control module is used to plan the path in the port entry stage based on trajectory nodes, and to plan the path in the berthing area stage using the Dubins curve, and to obtain the global trajectory by combining the nonlinear decay model for speed allocation. The execution module is used to perform local dynamic pose planning and trajectory tracking control using a learning model predictive control based on Gaussian process regression compensation during actual navigation based on the global trajectory. In the low-speed berthing phase, the module tightens the motion constraints of the learning model predictive control to complete the berthing task of the ship.

[0068] It should be noted that, depending on the implementation needs, the various steps described in this application can be broken down into more steps, or two or more steps or parts of the steps can be combined into new steps to achieve the purpose of this invention.

[0069] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A ship port entry-docking autonomous navigation control method based on LBMPC, characterized in that, include: Acquire the spatial distribution data of port static facilities and environment, establish a port environment model, and select the optimal berth from the candidate berth set based on the spatial distribution data of port static facilities and environment and the ship navigation status; The entire process of a ship entering the port and berthing at the optimal berth is divided into the port entry stage, the berthing area stage, and the low-speed berthing stage, and the trajectory nodes of each stage are calculated based on the port environment model. Based on the trajectory nodes, an improved fast exploration random tree star algorithm is used to plan the path for the port entry stage, a Durbins curve is used to plan the path for the berthing area stage, and a nonlinear decay model is combined to perform velocity allocation to obtain the global trajectory. During the actual navigation based on the global trajectory, a learning model predictive control based on Gaussian process regression compensation is used for local dynamic pose planning and trajectory tracking control. In the low-speed berthing phase, the motion constraints of the learning model predictive control are tightened to complete the berthing task of the ship. The improved fast-exploratory random tree star algorithm includes the following improvements introduced for the standard fast-exploratory random tree star algorithm: A dynamic sampling bias mechanism is established so that when the algorithm explores the initial path, the probability of the sampling point being selected as the target point and the radius of the search for new nodes can be adaptively adjusted according to the relative distance between the current node and the destination. Establish a navigation safety collision avoidance detection mechanism by performing obstacle collision detection within a multi-layered concentric rectangular channel centered on the current path segment and calculating a quantified hazard index to ensure that newly expanded nodes are located within the safe zone. A guided sampling path optimization mechanism is established. After obtaining the initial path, an elliptical sampling space is constructed with the starting point and the ending point as the focus and the major axis equal to the current path length. Based on an optimal safe path cost function that simultaneously considers the path length and the average risk, efficient sampling is performed in this elliptical sampling space using a reconfigurable KD tree data structure to optimize the path.

2. The LBMPC-based autonomous ship approach-docking navigation control method according to claim 1, characterized in that, The selection criteria for the candidate berths include: The berth length is not less than the total length of the vessel plus the allowable length for berthing; the berth width is not less than the beam of the vessel plus the allowable width for berthing; the width of the water area in front of the berth is not less than the minimum safe water area width required for the berthing area; the maximum offsetting energy of the berth shoreline facilities is not less than the impact energy generated by the residual velocity of the vessel's inertia; the sum of the recent average water depth of the berth and the predicted arrival tide is not less than the sum of the vessel's draft and the required underwater clearance; and the berth is currently in a usable state.

3. The LBMPC-based autonomous ship approach-docking navigation control method according to claim 1, characterized in that, The selection of the optimal berth is achieved through a comprehensive cost function; The comprehensive cost function is composed of the following weighted factors: the width of the waterway in front of the berth, the maximum offsetting energy of the berth shoreline facilities, the berth length, the berth width, the current channel historical average entry time, the lateral merging velocity near the berth, and the maneuvering complexity index. The maneuvering complexity index is composed of the following weighted factors: total turning maneuvering coefficient, longitudinal current velocity when the ship is sailing downstream in the berthing area during turning maneuvering, merging current velocity in the berthing area, turning water radius, and turning maneuvering penalty coefficient.

4. The method for autonomous navigation control of ships entering and berthing based on LBMPC according to claim 1, characterized in that, The port entry phase refers to the process by which a vessel begins to approach the optimal berth from an area far from the port. The berthing area phase refers to the vessel approaching the optimal berth at a speed of less than 2 knots and possibly performing a turning maneuver. The low-speed berthing phase refers to the ship approaching the berth center at a speed of 0.2~0.5kn under the control of the propeller and rudder, reaching the berth node.

5. The method for autonomous navigation control of ships entering and berthing based on LBMPC according to claim 1, characterized in that, The method of using an improved fast exploratory random treestar algorithm to plan the path for the port entry phase also includes: After obtaining the initial path using the improved fast exploration random tree-star algorithm, the following post-processing steps are performed sequentially to improve the smoothness and traceability of the path: First, abrupt corner points in the path with turning angles exceeding a threshold are detected and marked, and then smoothed; then, a sliding window filter based on a Gaussian kernel function is used to smooth the path point sequence as a whole to eliminate high-frequency noise; finally, a Clamped cubic B-spline curve is used to fit the preprocessed path points to generate the final smooth path.

6. The method for autonomous navigation control of ships entering and berthing based on LBMPC according to claim 1, characterized in that, The method of using the Durbins curve to plan the path for the berthing area stage also includes: The endpoints of the path in the port entry phase are optimized using Durbins curves. The path in the berthing area phase is planned using the Dubins curve. Under the condition of satisfying the turning radius constraint, the shortest path from the starting pose to the ending pose is one of the six basic motion types. The six basic motion types include: left turn arc-straight line-left turn arc, left turn arc-straight line-right turn arc, right turn arc-straight line-left turn arc, right turn arc-straight line-right turn arc, left turn arc-right turn arc-left turn arc, and right turn arc-left turn arc-right turn arc.

7. The method for autonomous navigation control of ships entering and berthing based on LBMPC according to claim 1, characterized in that, The method of employing a learning model predictive control based on Gaussian process regression compensation for local dynamic pose planning and trajectory tracking control, and tightening the motion constraints of the learning model predictive control during the low-speed berthing phase to complete the ship's berthing task includes: A nominal dynamic model of ship motion is established, which maps the ship's state from the hull coordinate system to the geographic coordinate system and describes the effect of control input on the ship's velocity vector. Based on Gaussian process regression online learning and compensation for the dynamic residual between the nominal dynamic model and the actual ship motion, the Gaussian process regression uses the ship velocity vector and control input vector of the previous moment as feature vectors to output a probability distribution estimate of the dynamic residual, including the mean and variance. The mean residual estimate of the regression output of the Gaussian process is combined with the prediction results of the nominal dynamics model to construct a learning model for model predictive control. Based on the learning model, within a finite prediction time domain, the optimal control input sequence that satisfies ship maneuvering constraints, static obstacle collision avoidance constraints, and dynamic obstacle collision avoidance constraints is solved. The first control command in the optimal control input sequence is executed, and the above steps are re-executed in the next sampling period based on the latest ship status to achieve rolling optimization control and complete the berthing task.

8. A ship autonomous navigation control method based on LBMPC for port entry and berthing according to claim 7, characterized in that, The Gaussian process regression uses a squared exponential kernel function to perform online training and prediction based on historical states and control input data, and outputs the mean and variance of the residual estimate, which are used to update the control learning model and construct the uncertainty compensation distance in dynamic collision avoidance. The collision avoidance constraints include: Static facility safety distance constraints ensure that ships maintain a minimum safe distance from static obstacles; Dynamic obstacle collision avoidance constraints predict the future pose of obstacles and calculate the collision avoidance distance based on the constant steering rate-constant acceleration assumption.

9. A ship port entry-berthing autonomous navigation control system based on LBMPC, characterized in that, include: The environmental perception module is used to acquire data on the spatial distribution of port static facilities and environment, establish a port environment model, and select the optimal berth from the candidate berth set based on the data on the spatial distribution of port static facilities and environment and the ship navigation status. The trajectory planning module is used to divide the entire process of a ship entering the port and berthing to the optimal berth into the port entry stage, the berthing area stage, and the low-speed berthing stage, and calculate the trajectory nodes and corresponding poses of each stage based on the port environment model. The predictive control module is used to plan the path of the port entry stage using an improved fast exploration random tree star algorithm based on the trajectory nodes, plan the path of the berthing area stage using the Durbins curve, and combine the nonlinear decay model to perform speed allocation to obtain the global trajectory. The execution module is used to perform local dynamic pose planning and trajectory tracking control by adopting a learning model predictive control based on Gaussian process regression compensation during the actual navigation based on the global trajectory, and to complete the berthing task of the ship by tightening the motion constraints of the learning model predictive control during the low-speed berthing phase. The improved fast-exploratory random tree star algorithm includes the following improvements introduced for the standard fast-exploratory random tree star algorithm: A dynamic sampling bias mechanism is established so that when the algorithm explores the initial path, the probability of the sampling point being selected as the target point and the radius of the search for new nodes can be adaptively adjusted according to the relative distance between the current node and the destination. Establish a navigation safety collision avoidance detection mechanism by performing obstacle collision detection within a multi-layered concentric rectangular channel centered on the current path segment and calculating a quantified hazard index to ensure that newly expanded nodes are located within the safe zone. A guided sampling path optimization mechanism is established. After obtaining the initial path, an elliptical sampling space is constructed with the starting point and the ending point as the focus and the major axis equal to the current path length. Based on an optimal safe path cost function that simultaneously considers the path length and the average risk, efficient sampling is performed in this elliptical sampling space using a reconfigurable KD tree data structure to optimize the path.

Images