A method for hull contour - boundary closest point guaranteed corridor navigation control

Abstract

This invention discloses a navigation control method within a corridor that ensures the closest distance between the hull outline and the boundary. The method is characterized by using a closed-loop structure of "geometry-reference-constraint-execution" to achieve the coordination of path tracking and safety constraints. This invention comprehensively utilizes techniques such as Control Barrier Function (CBF) constraints, bow reference continuity / rate limiting, and torque-rate constrained optimization. It proposes a safety constraint construction based on the contact point set CBF, an I-LOS+rate limiting continuity tracker to suppress bow jitter, and pre-aiming curvature and boundary centering correction to improve robustness within corridors. Furthermore, it combines disturbance observation and feedback linearization with a one-dimensional QP analytical projection solution to achieve coordinated optimization of torque and its rate of change. This enables safe verification of the nearest distance not less than a threshold and coordinated satisfaction of path tracking error and actuator constraints in narrow corridor scenarios such as inland waterways, port channels, and lock chambers / docks. It improves disturbance resistance, smoothness, and real-time engineering feasibility, and is applicable to unmanned surface vessels (USVs / ASVs) and manned vessels in corridor scenarios such as inland waterways, ports and basins, and lock chambers / docks for autonomous driving, berthing and unberthing, convoy navigation, and precision maneuvering.

Description

Technical Field

[0001] This invention relates to a navigation control method based on online assessment and safety assurance of the closest distance between the hull outline and the boundary, belonging to the fields of ship and marine engineering and automatic control technology, specifically the field of automatic navigation and path tracking control technology for ships under corridor (narrow waterway / channel / port basin channel) constraints. Background Technology

[0002] In recent years, domestic approaches to path tracking for ships / unmanned vessels have largely adopted guidance and heading tracking based on the concept of LOS (Landing on Surface). For example, patent CN112068550A proposes to transform path following into heading tracking using LOS and combines rudder angle-PID control, typically utilizing the "acceptance circle" to achieve path segment switching. Patent CN109116857A provides a nonlinear control scheme for path tracking of underactuated ships, and the background section also considers LOS as the mainstream guidance. For large ships, patent CN116859933A further proposes a variant of predicting LOS to suppress overshoot. On a specific platform, patent CN114527744B introduces intelligent optimization to generate a reference heading. In formation / cooperative scenarios, patent CN117539252A integrates maritime collision avoidance rules into cooperative tracking guidance. Furthermore, collision avoidance decisions on the engineering side are mostly based on DCPA / TCPA or fuzzy / crowdsourcing optimization: for example, patent CN111063218A uses DCPA / TCPA and fuzzy evaluation to generate collision avoidance strategies, and patent CN115601998A identifies encounters based on AIS data and provides avoidance suggestions for inland waterway scenarios. These publications generally focus on course geometry following and global / on-ship collision avoidance, and rarely address the issue of ensuring the closest distance between the ship's geometric outer contour and its side boundaries in "narrow channels / corridors" online.

[0003] Objectively speaking, the above solutions have three common shortcomings:

[0004] Firstly, most studies focus on the error from the centroid to the centerline / straight line segment or the switching of the "acceptance circle," lacking explicit modeling and online calculation of the minimum margin from the hull's outer contour (bow semicircle, stern angle, long side) to the corridor boundary (related patents do not clearly provide a detailed process for real-time evaluation of hull-to-wall proximity and triggering constraints). Secondly, although some work has introduced optimization / constraints, most of them focus on heading / rudder angle / speed limits or global collision avoidance, and few establish a one-dimensional analytical QP with guarded scaling + rate constraints on torque and its rate of change at the execution level. Thirdly, while safety filtering using the Control Barrier Function (CBF) has been studied in the robotics / UAV field, publicly available information on constructing corridor constraints at the hull contact point set (stern angle + midpoint of long side + bow semicircle) level is scarce (most published materials discuss general CBF safety navigation without detailing the engineering implementation of the hull geometric contact set and corridor normal). Therefore, the combined control of hull outer contour-boundary minimum distance assurance, bow reference continuity for jitter suppression, torque-rate limited optimization, and contact point set CBF under corridor constraints still needs further improvement and systematic public disclosure. Summary of the Invention

[0005] The technical problem to be solved by the present invention is that the existing technical solutions only use centroid / centerline error constraints, lack explicit modeling and online assurance of the minimum safety margin of the hull geometry (bow semicircle, stern angle, long side) and corridor boundary, cause jitter caused by bow reference jump, and lack of executability and stability due to the lack of execution torque rate limitation and guard scaling.

[0006] To address the aforementioned technical problems, the present invention discloses a corridor navigation control method that ensures the closest distance between the hull outline and the boundary. The method is characterized by employing a closed-loop structure of "geometry-reference-constraint-execution" to achieve synergy between path tracking and safety constraints. Within each control cycle, the following steps are sequentially completed: Step 1: Corridor geometric modeling and coordinate convention; Step 2: Calculate Frenet error and perform numerical protection; Step 3: I-LOS track angle and heading reference continuity / rate limiting; Step 4, the curvature command for pre-aiming curvature and boundary centering correction, includes: Obtain the reference curvature by clicking "Forward Time Preview"; Curvature command is obtained by integrating I-LOS, lateral / velocity coupling, and boundary centering correction. ; The rate of change of curvature is differentially limited to avoid second-order excitation; Step 5: Generate bow roll rate command , With longitudinal speed threshold ,in, For bow roll rate reference, This is a reference command for the yaw rate generated based on the curvature command. Step 6: Disturbance observation and feedback linearization (bow rock channel); Step 7: Corridor safety constraints for the contact point set CBF; Step 8: Set the nearest safety margin Mapped to "guard scaling" coefficient, calculated Guard zoom tighten The interval, where, For torque (near-wall actuable pivot); Step 9: Obtain the rate of change of torque One-dimensional QP analytical projection solution with constraints (execution optimization hub); Step 10: Online nearest-point assessment and feedback coupling (monitoring hub): Based on the contact point set, a small number of hull outline sampling points were added, and the local radius of the left and right boundaries was adjusted. Nearest point search; if the neighborhood is empty, scale up proportionally. If empty, the global fallback nearest point will be used; Get the closest distance between the inner and outer sides with global minimum margin ; Will Feedback is sent to steps 7 and 8 to form a safety-execution dual closed-loop coupling; Step 11: State / pose update and centerline progress.

[0007] Preferably, in step 1, the arc length parameter of the centerline is known. Centerline position Tangential angle curvature Total width of the corridor Minimum safety threshold Tangential With the direction of the law Define the left / right boundaries (continuous definition, actually approximated by dense discrete points): , ,in, This represents the total width of the corridor.

[0008] Preferably, in step 2, the ship's center of mass... The local coordinates of the point closest to the centerline are represented as follows:

[0009] In the formula, The longitudinal and lateral errors in the Frenet local coordinate system established at the point closest to the centerline of the ship's center of mass; To avoid numerical noise, the differential velocity of the lateral error is limited:

[0010] In the formula, For k pairs of lateral errors at discrete sampling times The difference approximation derivative (rate of change of lateral error). The sampling period / control period of the control system. The lateral error is calculated at the k-th sampling time. This is the threshold value for the rate of change of the lateral error / the maximum permissible absolute value.

[0011] Preferably, in step 3, the larger the lateral error, the shorter the aiming time, and vice versa, thus forming an adaptive I-LOS:

[0012] In the formula, This is the maximum upper limit of the adaptive aiming distance. The scaling gain is the sum of the aiming distance and the lateral error (and its integral). This is the integral gain of the transverse error integral term. This is the minimum lower bound for adaptive aiming distance; Track angle:

[0013] In the formula, The desired track angle / desired heading angle generated by I-LOS. The adaptive aiming distance is calculated for the current control cycle; A first-order rate-limiting tracker is used to suppress bow jitter caused by reference jumps:

[0014]

[0015]

[0016] In the formula, For the track angle error in the heading reference generation, The bow reference angle after continuumization and speed limiting. The bow roll rate reference is the output of the first-order rate limiting tracker. The time constant / response time parameter for the forward reference tracker. The time derivative of the heading reference angle. This is the effective upper limit of the bow roll rate. The step / large deflection angle stage can be improved.

[0017] Preferably, in step 4, the reference curvature is obtained by "forward time pre-aiming", as shown in the following formula:

[0018]

[0019]

[0020] In the formula, The longitudinal velocity in the ship's coordinate system. The bow angle of the ship. Let be the lateral velocity in the ship's coordinate system. Let w be the first derivative of the centerline parametric equation with respect to the arc length parameter w. Pre-aiming time; The curvature command obtained by fusing I-LOS, lateral / velocity coupling, and boundary centering correction is expressed as follows:

[0021] In the formula: , This is the gain coefficient for the boundary centering (return to center / repulsion near the wall) term. For symbolic functions, This represents the absolute value of the lateral error. To prevent small positive quantities with a denominator of zero, Let be the power exponent parameter of the power-law term. This is the upper limit of the amplitude limit; The proportional gain from lateral error to curvature correction; This is the minimum speed protection threshold in the speed division; The proportional gain from heading error to curvature correction; This refers to the heading angle error; This is the integral gain of the heading error integral term; This is the boundary centering correction function; The rate of change of curvature, expressed as a differential limit, is as follows:

[0022] In the formula, This represents the threshold value for the rate of change of curvature / the maximum permissible absolute value.

[0023] Preferably, in step 5, the command to generate the bow roll rate is... , And add feedforward and smoothing, as shown in the following formula:

[0024]

[0025] In the formula, Smoothing / filtering coefficients for bow roll rate reference. This refers to the actual bow roll rate currently measured on the ship. The longitudinal reference velocity adopts the minimum of the three laws of "curvature / roll / lateral acceleration", as shown in the following formula:

[0026] In the formula, This represents the maximum permissible / upper limit of longitudinal speed. To prevent small positive quantities with a denominator of zero (consistent with the above, can be used interchangeably). The maximum permissible lateral acceleration threshold; And when When the speed is high, the deceleration gate will be activated:

[0027] In the formula, The deceleration threshold speed; Execution layer Acceleration-limited follower:

[0028] In the formula, This is the maximum acceleration limit value for the longitudinal velocity following element.

[0029] Preferably, in step 6, the bow rolling dynamics are expressed as:

[0030] In the formula, For the known / identifiable system drift terms of the bow rocking channel, For the control input gain of the bow rocker channel, , To provide control torque commands applied to the bow rocker channel, For the combined disturbance term of the bow rocking channel; The first-order DOB estimates the external disturbance as shown in the following equation:

[0031] In the formula, This refers to the bandwidth / observation gain parameter of the perturbation observer. The online estimate of the disturbance; The nominal torque for eliminating steady-state error, including PI, is expressed as:

[0032] In the formula, The proportional feedback gain is used to calculate the bow roll rate tracking error. This is the integral feedback gain for the bow roll rate tracking error.

[0033] Preferably, in step 7, the coordinates are selected as follows: "2 points at the stern angle + 2 points at the midpoints of the two long sides + the bow semicircle". A set is composed of "points". over posture With the center of mass World System:

[0034] Technical normal distance and security function:

[0035]

[0036]

[0037] In the formula, This is a reference point used to calculate the normal distance. The sign distance / normal projection of the contact point relative to the reference point along the corridor normal. The available boundary margin from the contact point to the corridor boundary / the remaining normal distance to the boundary. For security functions / barrier functions; Using the relative degree 2CBF condition:

[0038]

[0039]

[0040] In the formula, and Positive parameter gain; Will With respect to the rate of change of torque By linearizing the dependency, we obtain the pointwise inequalities:

[0041] In the formula, The rate of change of torque, For the barrier constraint at the j-th contact point, regarding linear coefficients, For the constant term / upper bound term of the barrier constraint at the j-th contact point, The number of contact points in the contact point set; Assembled as follows:

[0042] In the formula, The constraint coefficient matrix, To constrain the upper bound vector; Increase CBF stiffness when near a wall:

[0043]

[0044] In the formula, for The baseline value, for The enhancement amplitude, for The baseline value, for The amplitude of the enhancement.

[0045] Preferably, in step 8, the most recent safety margin is... The mapping is represented by the "guard scaling" coefficient, as shown in the following formula:

[0046]

[0047]

[0048] In the formula, To protect the soft lower bound of the scaling factor To protect the scaling factor margin buffer parameter, The limit of the allowable torque amplitude of the actuator. This is the lower limit of the effective torque obtained under the guard scaling effect. This represents the upper limit of the effective torque obtained under the scaling effect of the guard. This is the guard scaling factor calculated based on the most recent safety margin.

[0049] Preferably, in step 9, with As variables, establish a one-dimensional quadratic cost that satisfies the linear inequalities of rate box, interval, and CBF:

[0050] In the formula, For reference to the nominal rate of change of torque, The torque command applied in the previous control cycle; The constraints are:

[0051]

[0052] Unconstrained extrema:

[0053] The speed box and the torque range correspond to Interval: , BF constraint By forming a half-space interval and finding its intersection, we get:

[0054] In the formula, The inequality derived from the j-th barrier is about The feasible range, Combination operators for finding the intersection of intervals; final:

[0055]

[0056] In the formula, The optimal rate of change of torque solution that satisfies all constraints and minimizes the objective function. For projection operators of scalars over interval I; In step 10, Feedback to step 7 , And step 8 This forms a safety-execution dual closed-loop coupling; In step 11, pose dynamics:

[0057]

[0058]

[0059] Centerline progress and parameter updates:

[0060]

[0061] In the formula, This is the first derivative of the centerline parametric equation with respect to the arc length parameter w.

[0062] The above cycle execution sequence can be summarized as follows: (1) Corridor geometry and Frenet error; (2) I-LOS and rate limiting heading reference; (3) Pre-aiming curvature and boundary centering correction results ; (4) Generation , , And speed limit; (5) DOB+ feedback linearization yields (Can be stacked) ); (6) Construct the contact point set CBF: ; (7) Calculate Guard zoom tighten interval; (8) Solve the one-dimensional QP to get ,renew ; (9) Dynamics and pose integration, progress update; (10) Online nearest distance assessment and feedback (7) – (8).

[0063] This invention comprehensively utilizes techniques such as Control Barrier Function (CBF) constraints, bow reference continuity / rate limiting, and torque-rate constrained optimization. It proposes a safety constraint construction based on the contact point set CBF, an I-LOS+rate limiting continuity tracker to suppress bow jitter, and pre-aiming curvature and boundary centering correction to improve robustness within corridors. Furthermore, it combines disturbance observation and feedback linearization with a one-dimensional QP analytical projection solution to achieve coordinated optimization of torque and its rate of change. This enables safe verification of the nearest distance not less than a threshold and coordinated satisfaction of path tracking error and actuator constraints in narrow corridor scenarios such as inland waterways, port channels, and lock chambers / docks. It improves disturbance resistance, ride comfort, and real-time engineering feasibility, and is applicable to autonomous driving, berthing and unberthing, convoy navigation, and precision maneuvering of unmanned surface vessels (USVs / ASVs) and manned vessels in corridor scenarios such as inland waterways, ports and basins, and lock chambers / docks.

[0064] Compared with existing technical solutions, the present invention has the following beneficial effects: 1. Safety Verification: Based on the contact point set CBF formed by "stern angle + midpoint of long side + bow semicircle", the shortest distance between "hull outline and corridor boundary" is directly constrained. And when near the wall, it tightens the torque through gain adaptation and guard scaling to ensure... Online security margin.

[0065] 2. Jitter Suppression and Smoothness: Employs I-LOS + Bow Reference Continuum / Rate Limiting Suppression Jitter caused by jumps; combined with curvature change rate limiting and speed threshold, significantly reduces and Peak and overshoot, improving ride / work smoothness.

[0066] 3. Robust tracking around the edge: By linking pre-aiming curvature and boundary centering correction, it automatically returns to center in narrow bends and edge-close scenarios, taking into account both trajectory error and constraint boundaries (lateral acceleration, yaw rate), reducing "edge pull-back-swaying".

[0067] 4. Feasible and easy to implement: One-dimensional QP analytical projection simultaneously satisfies the rate box, torque range, and CBF linear inequality, reducing actuator saturation and reverse impact; it has low computational complexity, is suitable for embedded real-time implementation, and combines DOB+PI to improve robustness under external disturbances and model errors. Attached Figure Description

[0068] Figure 1 This is a flowchart of a navigation control method within a corridor that ensures the closest distance between the hull outline and the boundary according to the present invention.

[0069] Figure 2 This demonstrates the scene and the closest distance: The algorithm displays the S-shaped corridor, centerline, and left and right boundaries, and overlays multiple frames of the ship's hull shape. The distance between the inner and outer nearest points is represented by the line connecting the candidate outer contour points and the boundary. At the same time, the candidate point cloud is also provided. It can be seen that at the edge and curve, the algorithm automatically identifies "which side is the inner / outer" and can adaptively expand when the local neighborhood is insufficient, and finally obtains the nearest distance line stably. This proves that the nearest distance evaluation algorithm remains stable and solvable at geometric abrupt changes.

[0070] Figure 3 The heading and roll rate are shown: In (a), after continuumization (Dashed line) and I-LOS (Dotted line) Smooth, no phase jump, actual heading (Solid line) Follows the reference line; In (b), the actual bow roll rate (Solid line) around the reference (Dashed line) Oscillates slightly and is always affected by (Point-and-line) constraints. This indicates that the reference layer velocity limiting effectively suppressed... Jitter is amplified at the execution layer, while overshoot is controlled; Figure 4 This demonstrates torque commands and rate constraints: In (a), The solid line is always constrained by the envelope (dashed line) of the "upper and lower limits after guard scaling"; when approaching the corridor boundary, the envelope can be seen to automatically tighten (guard scaling), thereby reducing the risk of large movements; In (b), (Solid line) is mostly within the speed box, only touching during strong constraints such as turning / hugging the edge. (Points and lines); This demonstrates that the analytical projection of one-dimensional QP achieves a balance between real-time performance and executability.

[0071] Figure 5 This shows the closest distance (overall) from the hull outline to the inner / outer boundary: inner side (Solid line) and the outer side (Dotted line) remained within the safe threshold throughout the entire process. Above (dots and lines); the gray area is The curve did not enter the danger zone. It can be seen that the minimum distance between the bend and the edge decreases significantly, but never falls below the threshold, verifying the online guarantee of minimum safety margin provided by "Contact Point Set CBF + Guardian Scaling".

[0072] Figure 6 This demonstrates the difference between the pre-aiming reference curvature and the final curvature: (Solid line) relative to The (dotted line) shows a moderate offset at the edge / sharp bend, reflecting the centering effect of the boundary centering correction; while the two basically overlap in the straight line or the wide range, indicating that the correction has the adaptive characteristic of "intervention on demand" and avoids unnecessary manipulation. Detailed Implementation

[0073] The present invention will be further illustrated below with reference to specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. Furthermore, it should be understood that after reading the teachings of this invention, those skilled in the art can make various alterations or modifications to the invention, and these equivalent forms also fall within the scope defined by the appended claims.

[0074] In this embodiment of the invention, the system comprises a geometry and safety module, a reference generation module, a constraint construction (CBF) module, and an execution optimization module. The system consists of a one-dimensional QP module, a dynamics and observation module (DOB), an online nearest-distance monitoring module, and a plot generation module. The system input is the channel centerline geometry (including arc length parameters). , , , Total width of the corridor Minimum safety threshold With the ship's condition The system output is the actuator torque command. Longitudinal velocity Reference and limit, and the monitoring quantity of "closest distance between inner / outer boundaries - over time".

[0075] The system operates with a fixed sampling period: In this embodiment, we take... (i.e., 20Hz), total duration .

[0076] This invention, using narrow corridor simulation as an example, further illustrates a corridor navigation control method disclosed in this invention that ensures the closest distance between the hull outline and the boundary. The method specifically includes the following steps: Step 1, Geometry and Safety Threshold Settings: Hull dimensions: Length , width ; Corridor parameters: Total width Minimum safety threshold Buffer zone ; Guard minimum coefficient: .

[0077] Step 2, Ship Yaw Channel Model and Disturbance: Employing the identified low-order coupling model, where the input gain of the bow-rock channel varies with longitudinal velocity: ,in, This represents the control input gain, or control torque, of the yaw channel. Rate of change of bow roll angular velocity The effect coefficient (control effectiveness coefficient); This represents the longitudinal velocity (surge velocity) of the vehicle in the ship's coordinate system.

[0078] External disturbance injection: , This represents the combined disturbance term or equivalent external force disturbance component acting on the sway channel (which can characterize the equivalent acceleration / force term caused by wind, current, waves, and unmodeled sway hydrodynamics). , This represents the combined disturbance term or equivalent external torque disturbance component acting on the yaw channel (which can characterize changes in nearshore effects, wind, waves, currents, and equivalent torque terms caused by unmodeled yaw hydrodynamics).

[0079] Step 3, Key Control and Protection Parameters: Bow reference tracker time constant: ; Velocity and attitude boundaries: , ,in, This indicates the maximum permissible upper limit of the longitudinal reference speed (maximum speed constraint). This indicates the maximum permissible lateral acceleration threshold (used to limit cornering speeds by curvature to improve safety and ride comfort). Bow roll rate limit: Starting / Large Deviation Angle Boost Window: ; Curvature link gain: , , , ,in, This represents the proportional gain from lateral error to curvature correction. This represents the proportional gain from heading error to curvature correction; This represents the integral gain from the integration of heading error to curvature correction (used to eliminate steady-state angle deviation). This represents the gain from the lateral error rate of change or the velocity coupling term to the curvature correction (used to suppress lateral dynamics and improve damping and edge stability). Speed ​​gates and longitudinal execution: acceleration limit ; Disturbance observer: , This represents the bandwidth / observation gain parameter of the disturbance observer (which determines the response speed and filtering strength of the disturbance estimation). Torque amplitude and velocity boundary: , ,in, This indicates the upper limit of the amplitude of the bow roll control torque (or equivalent rudder torque); It represents the upper limit of the torque change rate (first derivative of torque), used to constrain the actuator's action speed and improve its operability and smoothness.

[0080] Step 4, Corridor Geometry and Frenet Error: Construct Frenet coordinates using the tangent and normal directions of the centerline, and calculate... And obtained by limiting the difference ,in, This represents the error components of the spacecraft's center of mass relative to the nearest point on the centerline in the Frenet coordinate system. This refers to the longitudinal deviation along the tangential direction. This represents the lateral deviation along the normal direction; Indicates lateral error The discrete difference approximation derivative (rate of change of lateral error). Using the rotation matrix... Complete the coordinate transformation and use To achieve numerical protection, among which, This represents the increment of the lateral error between adjacent control cycles (i.e. ); This indicates the sampling period of the control system.

[0081] Step 5: I-LOS track angle and heading reference continuity / speed limit: Adaptive Pre-aiming Based on the self-scaling of the transverse error integral, we obtain , This represents the desired track angle / desired track direction angle generated by the I-LOS guidance law (used to feed back lateral error to the desired heading trend). Generated using a reference speed limit tracker First Phase expansion yields continuity Then use , ,in, Indicates to Continuous track angle reference obtained after phase unrolling / de-jumping; This represents the continuous heading reference angle; The time constant (control) of the heading reference tracker. follow (speed) This indicates the upper limit of the effective bow roll rate, and can be increased to a "boost value" according to a preset strategy when starting or when there is a large bow misalignment, in order to improve the alignment speed while suppressing spikes.

[0082] Step 6: Pre-aiming curvature and boundary centering correction: With non-negative arc length speed Pre-aiming position ,Pick ,in, This represents the longitudinal velocity in the ship's coordinate system; Indicates the ship's heading angle; Indicates the tangential angle of the centerline (centerline direction angle); This represents the lateral velocity in the ship's coordinate system. Indicates the reference curvature at the aiming position; This represents the centerline curvature function (input is the centerline arc length / parameter, output is the curvature at that point).

[0083] Curvature command integrates I-LOS, lateral / velocity coupling, boundary centering, and Correction; when approaching the boundary, an additional term is generated and its amplitude is limited according to "boundary margin power law suppression", thus forming a retracement trend; for Implement differential speed limiting to prevent second-order excitation.

[0084] Step 7, Bow roll rate command and longitudinal speed threshold: Depend on generate and according to Smooth, in which, Indicates the reference command for bow roll rate; This represents the nominal yaw rate (feedforward term from curvature to yaw rate) obtained by converting the curvature command to the longitudinal velocity. This represents a saturation limiting function, used to restrict the input to a given upper / lower bound; Indicates smoothing / filtering gain (used for enhancement) For reality (convergence and suppression of instruction jitter). This represents the actual measured bow roll angular velocity.

[0085] Vertical reference When the large misalignment occurs, then... Take the minimum, where, Indicates the maximum permissible longitudinal speed; A scalar value representing the curvature of the centerline at the current (or pre-aimed) point; This indicates a small positive quantity (value protection item) to prevent the denominator from being zero. This indicates the maximum permissible lateral acceleration threshold.

[0086] Execution layer Follow, ensuring smooth vertical movement, among which... Indicates the longitudinal reference velocity; This indicates the upper limit of the longitudinal acceleration amplitude.

[0087] Step 8: DOB + Feedback Linearization + Nominal Torque of PI: Bow rolling dynamics abstracted as ,in, The term representing the drift term of the bow rocking channel / known model term (including damping, coupling, and nonlinear hydrodynamics, etc.) (the impact); This represents the actual bow roll rate; Indicates the control input gain (control efficiency coefficient); This represents the combined disturbance term of the bow rocking channel (external disturbances and unmodeled dynamics).

[0088] First-order DOB: ,in, This represents the perturbation observer bandwidth / observation gain parameter.

[0089] Nominal torque: ,in, This represents the integral gain of the bow roll rate tracking error integral element, used to reduce steady-state error.

[0090] Step 9, Corridor safety constraints for the contact point set CBF: Contact point set: 2 points at the stern angle + 2 points at the midpoints of the two long sides + the bow semicircle There are 10 points, totaling 100 points. indivual; The body coordinate contact point is passed through Mapped to the world system, calculated along the corridor normal, we have

[0091] in, Represents the safety function / barrier function (representing the remaining amount of boundary margin relative to the minimum safety threshold); Indicates the total width of the corridor; The sign of the distance (normal projection, used to distinguish left and right sides) between the contact point and the reference point along the corridor normal direction. This represents the minimum safe distance threshold.

[0092] Using relative degree two CBF: and will right Linearize the effect into pointwise ,in, Indicates the rate of change of torque; Indicates the first In the barrier inequality at each contact point, regarding The linear coefficients; Indicates the first The upper bound of the contact point barrier inequality is determined by geometric quantities, state quantities, model terms, and disturbance estimates.

[0093] Hardening against the wall: With the latest margin An increase in the index triggers stronger damping and recovery.

[0094] Step 10, Guard Scaling and One-Dimensional QP Optimization Execution: in accordance with Calculate guard scaling tighten The amplitude range, where, This represents the lower bound of the guard scaling factor (the minimum tightening factor, used to ensure that some control is still retained while avoiding excessive tightening that would make it infeasible):

[0095] in, This indicates the lower limit of the effective torque after tightening; This indicates the hard upper limit of the torque amplitude; Indicates the guard scaling factor; This indicates the upper limit of the effective torque after tightening.

[0096] Find the analytical projection solution of a one-dimensional quadratic target in the "velocity box + interval + CBF half-space": First come, first served ,in, This represents the free minimum point (unconstrained optimal solution) of a one-dimensional objective function when no constraints are imposed. This represents the weighting coefficients in the cost function (used for weighted following). Approaching ); Indicates the reference for the nominal rate of change of torque; Indicates the nominal torque; This indicates the torque command that was applied in the previous control cycle.

[0097] Then project it onto the intersection interval ; at last ,in, This represents the optimal torque rate of change solution after satisfying the rate box, tightening interval, and CBF constraints.

[0098] For cases that are infeasible or have an empty set, prioritize preserving the CBF constraint, automatically reduce speed and shrink size. .

[0099] Step 11: Online nearest distance assessment and feedback coupling: Add a small number of outer contour points to the contact point set, using "local radius" Nearest Neighbor Point - Adaptive Zoom —The "global fallback" three-step process obtains the closest distance between the inner and outer boundaries; form ,in, This indicates the shortest distance from the outer contour of the ship's exterior to the inner boundary of the corridor (the inner side of the bend); This indicates the shortest distance from the outer contour of the ship's exterior to the outer boundary of the corridor; This represents the minimum safe distance threshold.

[0100] And feed its closed-loop feedback back to step 9. With step 10 .

[0101] Step 12: Dynamics Integration and Pose / Progress Update according to Perform Euler integrals, with Update centerline arc length .

[0102] The simulation results obtained are as follows Figures 2 to 6 As shown. Figure 2 To illustrate the corridor scene and the closest distance: Draw the centerline and left and right boundaries in the S-shaped channel, and overlay multiple frames of the ship's shape; the closest points on the inner and outer sides are given by connecting the "candidate outer contour points - boundaries", showing that the algorithm can still stably obtain the closest distance at the curve and edge positions. Figure 3 Showing heading and roll rate separately: actual heading and continuous reference heading. and I-LOS track angle Basically overlapping, bow roll rate Around restricted reference Small fluctuations and always affected The constraint indicates that the reference layer rate limit effectively suppresses jitter and overshoot. Figure 4Given control inputs and rate constraints: Torque command Always enveloped by the upper and lower limits of the "guardian scaling," the upper and lower limits automatically tighten when near a wall; torque change rate Most of the time it stays within the rate box, only touching the upper limit during strong constraints such as turning / grabbing edges, indicating that the analytical projection of one-dimensional QP achieves a balance between real-time performance and executability. Figure 5 The entire curve is a "hull outline - closest distance between inner / outer boundaries" curve, with the inner side... and the outside None entered The following gray danger zones must be verified throughout the entire process. Safety margin. Figure 6 Comparison of aiming curvature With final curvature At the edge or sharp bend relatively On-demand offsets are generated to achieve boundary centering, while the two largely overlap on straight or wide road sections.

Claims

1. A method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary, characterized in that, The path tracking and safety constraints are coordinated using a closed-loop structure of "geometry-reference-constraint-execution". Within each control cycle, the following steps are completed sequentially: Step 1: Corridor geometric modeling and coordinate convention; Step 2: Calculate Frenet error and perform numerical protection; Step 3: I-LOS track angle and heading reference continuity / rate limiting; Step 4, the curvature command for pre-aiming curvature and boundary centering correction, includes: Obtain the reference curvature by clicking "Forward Time Preview"; Curvature command is obtained by integrating I-LOS, lateral / velocity coupling, and boundary centering correction. ; The rate of change of curvature is differentially limited to avoid second-order excitation; Step 5: Generate bow roll rate command , With longitudinal velocity threshold ,in, For bow roll rate reference, A reference command for the bow roll rate generated based on the curvature command; Step 6: Perturbation observation and feedback linearization; Step 7: Corridor safety constraints for the contact point set CBF; Step 8: Set the nearest safety margin Mapped to "guard scaling" coefficient, calculated Guard zoom tighten The interval, where, For torque; Step 9: Obtain the rate of change of torque One-dimensional QP analytical projection solution with constraints; Step 10: Online nearest distance assessment and feedback coupling: Based on the contact point set, a small number of hull outline sampling points were added, and the local radius of the left and right boundaries was adjusted. Nearest point search; if the neighborhood is empty, scale up proportionally. If empty, the global fallback nearest point will be used; Get the closest distance between the inner and outer sides with global minimum margin ; Will Feedback is sent to steps 7 and 8 to form a safety-execution dual closed-loop coupling; Step 11: State / pose update and centerline progress.

2. The method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary as described in claim 1, characterized in that, In step 1, the arc length parameter of the centerline is known. Centerline position Tangential angle curvature Total width of the corridor Minimum safety threshold Tangential With the direction of the law Define the left / right boundaries: , ,in, This represents the total width of the corridor.

3. The method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary as described in claim 2, characterized in that, In step 2, the ship's center of mass The local coordinates of the point closest to the centerline are represented as follows: In the formula, The longitudinal and lateral errors in the Frenet local coordinate system established at the point closest to the centerline of the ship's center of mass; To avoid numerical noise, the differential velocity of the lateral error is limited: In the formula, For k pairs of lateral errors at discrete sampling times The difference approximation derivative (rate of change of lateral error). The sampling period / control period of the control system. The lateral error is calculated at the k-th sampling time. This is the threshold for limiting the rate of change of the lateral error / the maximum permissible absolute value.

4. The method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary as described in claim 3, characterized in that, In step 3, the larger the lateral error, the shorter the aiming time, and vice versa, thus forming an adaptive I-LOS: In the formula, This is the maximum upper limit of the adaptive aiming distance. The scaling gain is the sum of the aiming distance and the lateral error (and its integral). This is the integral gain of the transverse error integral term. This is the minimum lower bound for adaptive aiming distance; Track angle: In the formula, The desired track angle / desired heading angle generated by I-LOS. The adaptive aiming distance is calculated for the current control cycle; A first-order rate-limiting tracker is used to suppress bow jitter caused by reference jumps: In the formula, For the track angle error in the heading reference generation, The bow reference angle after continuumization and speed limiting. The bow roll rate reference is the output of the first-order rate limiting tracker. The time constant / response time parameter for the forward reference tracker. The time derivative of the heading reference angle. This is the effective upper limit of the bow roll rate. The step / large deflection angle stage can be improved.

5. The method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary as described in claim 4, characterized in that, In step 4, the reference curvature is obtained by "forward time pre-aiming", as shown in the following formula: In the formula, The longitudinal velocity in the ship's coordinate system. The bow angle of the ship. Let be the lateral velocity in the ship's coordinate system. Let w be the first derivative of the centerline parametric equation with respect to the arc length parameter w. Pre-aiming time; The curvature command obtained by fusing I-LOS, lateral / velocity coupling, and boundary centering correction is expressed as follows: In the formula: , This is the gain coefficient for the boundary centering (return to center / repulsion near the wall) term. For symbolic functions, This represents the absolute value of the lateral error. To prevent small positive quantities with a denominator of zero, Let be the power exponent parameter of the power-law term. This is the upper limit of the amplitude limit; The proportional gain from lateral error to curvature correction; This is the minimum speed protection threshold in the speed division; The proportional gain from heading error to curvature correction; This refers to the heading angle error; This is the integral gain of the heading error integral term; This is the boundary centering correction function; The rate of change of curvature, expressed as a differential limit, is as follows: In the formula, This represents the threshold value for the rate of change of curvature / the maximum permissible absolute value.

6. The method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary as described in claim 5, characterized in that, In step 5, the command to generate the bow roll rate is... , And add feedforward and smoothing, as shown in the following formula: In the formula, Smoothing / filtering coefficients for bow roll rate reference. This refers to the actual bow roll rate currently measured on the ship. The longitudinal reference velocity adopts the minimum of the three laws of "curvature / roll / lateral acceleration", as shown in the following formula: In the formula, This represents the maximum permissible / upper limit of longitudinal speed. To prevent small positive quantities with a denominator of zero (consistent with the above, can be used interchangeably). The maximum permissible lateral acceleration threshold; And when When the speed is high, the deceleration gate will be activated: In the formula, The deceleration threshold speed; Execution layer Acceleration-limited follower: In the formula, This is the maximum acceleration limit value for the longitudinal velocity following element.

7. The method for navigation control within a corridor with minimum distance protection for the hull outline-boundary as described in claim 6, characterized in that, In step 6, the bow roll dynamics are expressed as: In the formula, For the known / identifiable system drift terms of the bow rocking channel, For the control input gain of the bow rocker channel, , To provide control torque commands applied to the bow rocker channel, For the combined disturbance term of the bow rocking channel; The first-order DOB estimates the external disturbance as shown in the following equation: In the formula, This refers to the bandwidth / observation gain parameter of the perturbation observer. The online estimate of the disturbance; The nominal torque for eliminating steady-state error, including PI, is expressed as: In the formula, The proportional feedback gain is used to calculate the bow roll rate tracking error. This is the integral feedback gain for the bow roll rate tracking error.

8. The method for navigation control within a corridor with minimum distance protection based on the hull outline-boundary as described in claim 7, characterized in that, In step 7, select "2 points at the stern angle + 2 points at the midpoints of the two long sides + the bow semicircle" in the ship's coordinate system. A set is composed of "points". over posture With the center of mass World System: Technical normal distance and security function: In the formula, This is a reference point used to calculate the normal distance. The sign distance / normal projection of the contact point relative to the reference point along the corridor normal. The available boundary margin from the contact point to the corridor boundary / the remaining normal distance to the boundary. For security functions / barrier functions; Using the relative degree 2CBF condition: In the formula, and Positive parameter gain; Will With respect to the rate of change of torque By linearizing the dependency, we obtain the pointwise inequalities: In the formula, The rate of change of torque, For the barrier constraint at the j-th contact point, regarding linear coefficients, For the constant term / upper bound term of the barrier constraint at the j-th contact point, The number of contact points in the contact point set; Assembled as follows: In the formula, The constraint coefficient matrix, To constrain the upper bound vector; Increase CBF stiffness when near a wall: In the formula, for The baseline value, for The enhancement amplitude, for The baseline value, for The amplitude of the enhancement.

9. A method for navigation control within a corridor with minimum distance protection for the hull outline-boundary as described in claim 8, characterized in that, In step 8, the most recent safety margin is... The mapping is represented by the "guard scaling" coefficient, as shown in the following formula: In the formula, To protect the soft lower bound of the scaling factor To protect the scaling factor margin buffer parameter, The limit of the allowable torque amplitude of the actuator. This is the lower limit of the effective torque obtained under the guard scaling effect. This represents the upper limit of the effective torque obtained under the scaling effect of the guard. This is the guard scaling factor calculated based on the most recent safety margin.

10. The method for navigation control within a corridor with minimum distance protection for the hull outline-boundary as described in claim 9, characterized in that, In step 9, with Let be the variable, establish a one-dimensional quadratic cost that satisfies the linear inequalities of rate box, interval, and CBF: In the formula, For reference to the nominal rate of change of torque, The torque command applied in the previous control cycle; The constraints are: Unconstrained extrema: The speed box and the torque range correspond to Interval: , BF constraint By forming a half-space interval and finding its intersection, we get: In the formula, The inequality derived from the j-th barrier is about The feasible range, Combination operators for finding the intersection of intervals; final: In the formula, The optimal rate of change of torque solution that satisfies all constraints and minimizes the objective function. For projection operators of scalars over interval I; In step 10, Feedback to step 7 , And step 8 This forms a safety-execution dual closed-loop coupling; In step 11, pose dynamics: Centerline progress and parameter updates: In the formula, This is the first derivative of the centerline parametric equation with respect to the arc length parameter w.

Images