Ship navigation control method and device, computer equipment and storage medium

Abstract

The invention relates to the technical field of intelligent shipping, and discloses a ship navigation control method and device, computer equipment and a storage medium, and the method comprises the steps: obtaining the real-time navigation information of a target ship and a preset reference path, and the real-time navigation information comprises ship position information and environment perception information; determining an expected course angle according to a preset reference path and the ship position information, and determining expected motion parameters based on the preset reference path, the ship position information and the environment perception information; and controlling the target ship to sail based on the expected course angle and the expected motion parameters. According to the method, the problems of path tracking lag and speed response mismatch caused by mutual independence of course and speed control and fixed foresight distance in a short-distance multi-broken-line navigation channel in the existing ship autonomous navigation method are solved.

Description

Technical Field

[0001] This invention relates to the field of intelligent shipping technology, specifically to ship navigation control methods, devices, computer equipment, and storage media. Background Technology

[0002] Autonomous navigation technology for inland waterway vessels primarily relies on the Global Positioning System (GPS) and preset waypoints, employing geometric methods such as the line-of-sight (LOS) approach and pure tracking algorithms to achieve path tracking. These methods treat the vessel as a point mass, calculating the desired course based on the geometric relationship between the current pose and the target point, and in practice, combining crew experience or constant speed for control. However, inland waterways are generally characterized by short track segments, frequent turns, and pronounced zigzag features. Especially in scenarios such as lock junctions, port junctions, areas with dense bridges, and winding urban waterways, vessels need to complete continuous multi-angle turns within short segments. Existing methods do not fully consider the vessel's high inertia and slow response dynamics, and course and speed control are independent, making it difficult to adapt to the control coupling requirements brought about by high-frequency geometric changes.

[0003] The shortcomings and drawbacks of existing technologies are as follows: Traditional line-of-sight methods use a fixed forward sight distance in short-distance, multi-bend paths, which may cause the ship to cut inside the bend when entering the next segment, posing a risk of hitting the shore or deviating from the channel, or cause the ship to rush past the preset endpoint along the current segment, resulting in severe oscillations at the bend point; at the same time, speed control is mostly reactive, relying on the current tracking error to adjust the speed. When the segment length is limited, it is impossible to complete the dynamic matching of acceleration and deceleration in time, falling into the control lag dilemma of deceleration as soon as acceleration, and failing to achieve dynamic coordination between path tracking and speed safety. Summary of the Invention

[0004] In view of this, embodiments of the present invention provide a ship navigation control method, apparatus, computer equipment, and storage medium to solve the problems of path tracking lag and speed response mismatch caused by the independent control of heading and speed and the fixed forward sight distance in existing ship autonomous navigation methods in short-distance multi-zigzag waterways.

[0005] In a first aspect, embodiments of the present invention provide a ship navigation control method, the method comprising: The system acquires real-time navigation information and a preset reference path for the target vessel, wherein the real-time navigation information includes vessel position information and environmental perception information. The desired heading angle is determined based on the preset reference path and the ship's position information, and the desired motion parameters are determined based on the preset reference path, the ship's position information, and the environmental perception information. The target vessel is controlled to navigate based on the desired heading angle and the desired motion parameters.

[0006] Furthermore, determining the desired heading angle based on the preset reference path and the ship's position information includes: Based on the ship's position information, determine the first reference point on the preset reference path that is closest to the current position of the target ship, and obtain the arc length parameter corresponding to the first reference point on the preset reference path; Calculate the lateral tracking error of the position point relative to the first reference point, and determine the path curvature radius of the first reference point on the preset reference path based on the arc length parameter; The forward sight distance of the target vessel is calculated based on the current speed of the target vessel, the radius of curvature of the path, and the minimum turning radius threshold of the target vessel. Along the arc length direction of the preset reference path, a second reference point is determined based on the forward sight distance, and the basic expected heading angle of the target vessel's current position pointing to the second reference point is calculated; The feedback control term for the lateral tracking error is determined based on the lateral tracking error, and the feedforward control term for the path curvature is determined based on the characteristic coefficient of the target vessel and the arc length parameter. The base desired heading angle is corrected based on the feedback control term of the lateral tracking error and the feedforward control term of the path curvature to obtain the desired heading angle.

[0007] Furthermore, determining the desired motion parameters based on the preset reference path, the ship's position information, and the environmental perception information includes: Construct a target speed profile of the target ship within a planning time domain, wherein the planning time domain includes multiple stages; Based on the preset reference path, the ship's position information, and the environmental perception information, the target stage currently in which the target ship is located is determined; The motion parameters corresponding to the target stage in the target velocity profile are used as the desired motion parameters.

[0008] Furthermore, constructing the target speed profile of the target vessel within the planning time domain includes: Obtain the planning time domain of the current control cycle, and discretize the planning time domain into multiple stages; Under preset constraints, the motion parameters of the target ship at each stage are solved. When the preset cost function satisfies the preset convergence condition, the target speed profile of the target ship in the planning time domain is obtained. The preset cost function includes at least one of curve safety cost, following safety cost and efficiency cost. The constraints include at least one of ship dynamics constraints, curve speed constraints, following safety constraints, channel speed limit constraints and empirical speed constraints.

[0009] Furthermore, the real-time navigation information of the target vessel includes operational status information; the method further includes: During the process of controlling the navigation of the target vessel based on the desired heading angle and the desired motion parameters, the target navigation scenario currently in which the target vessel is located is determined according to the environmental perception information and the operating status information. The target navigation scenario is any one of the following: straight navigation scenario, curve navigation scenario, following navigation scenario, dynamic obstacle avoidance scenario, and narrow waterway navigation scenario. Based on the target navigation scenario, a corresponding cooperative control strategy is determined, and the desired heading angle and the desired motion parameters are adjusted according to the cooperative control strategy during the navigation of the target vessel.

[0010] Furthermore, when the navigation scenario is a curve navigation scenario, the cooperative control strategy includes: During the curve entry phase, the desired motion parameters are adjusted according to the first decreasing rule to obtain updated desired motion parameters; at the same time, the forward sight distance of the target vessel is adjusted according to the second decreasing rule to obtain updated forward sight distance, and the desired heading angle is updated using the updated forward sight distance to obtain updated desired heading angle. During the cornering phase, maintain the updated desired motion parameters and the updated desired heading angle; During the exit phase of the curve, the updated desired motion parameters are adjusted according to the first incremental rule to obtain the restored desired motion parameters; at the same time, the updated forward sight distance is adjusted according to the second incremental rule to obtain the restored forward sight distance, and the updated desired heading angle is updated using the restored forward sight distance to obtain the restored desired heading angle.

[0011] Furthermore, when the navigation scenario is a car-following navigation scenario, the cooperative control strategy includes: Detect a reference vessel located in the direction of travel of the target vessel; Determine the safe following speed based on the motion status information of the reference vessel; The safe following speed is used as a priority constraint, and the desired motion parameters are updated according to the priority constraint to obtain the updated desired motion parameters; at the same time, the forward sight distance of the target vessel is adjusted according to the third incremental rule to obtain the updated forward sight distance, and the desired heading angle is updated using the updated forward sight distance to obtain the updated desired heading angle.

[0012] Furthermore, when the navigation scenario is a dynamic obstacle avoidance scenario, the cooperative control strategy includes: Detect whether there is a risk of collision between the target vessel and obstacles in front of it; When a collision risk is detected, the local path planner is invoked to generate a short-term obstacle avoidance heading sequence, and the desired heading angle is updated according to the short-term obstacle avoidance heading sequence to obtain the updated desired heading angle; at the same time, the desired motion parameters are adjusted according to the third decreasing rule to obtain the updated desired motion parameters.

[0013] Furthermore, when the navigation scenario is a narrow waterway navigation scenario, the cooperative control strategy includes: Obtain the channel width information of the waterway where the target vessel is currently located; When the channel width information is less than a preset width threshold, a speed limit constraint is determined based on the channel width information, and the desired motion parameters are updated according to the speed limit constraint to obtain the updated desired motion parameters; at the same time, the forward sight distance of the target vessel is adjusted according to the fourth decreasing rule to obtain the updated forward sight distance, and the desired heading angle is updated using the updated forward sight distance to obtain the updated desired heading angle.

[0014] Secondly, embodiments of the present invention provide a ship navigation control device, the device comprising: The acquisition module is used to acquire the real-time navigation information of the target vessel and the preset reference path, wherein the real-time navigation information includes the vessel's position information and environmental perception information; The determination module is used to determine the desired heading angle based on the preset reference path and the ship position information, and to determine the desired motion parameters based on the preset reference path, the ship position information and the environmental perception information. A control module is used to control the navigation of the target vessel based on the desired heading angle and the desired motion parameters.

[0015] Thirdly, embodiments of the present invention provide a computer device, including: a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the computer instructions to perform the method described in the first aspect or any corresponding embodiment thereof.

[0016] Fourthly, embodiments of the present invention provide a computer-readable storage medium storing computer instructions that cause a computer to perform the method described in the first aspect or any of its corresponding embodiments.

[0017] The method provided in this application has the following beneficial effects: The method provided in this application provides accurate basic data support for subsequent coordinated heading and speed control by acquiring real-time navigation information of the target vessel and a preset reference path, ensuring that control decisions can respond promptly to channel geometric changes and environmental dynamics. By determining the desired heading angle based on the preset reference path and vessel position information, and determining the desired motion parameters based on the preset reference path, vessel position information, and environmental perception information, adaptive matching of forward-looking distance, path curvature, and lateral error in heading control is achieved, avoiding trajectory cutting or overshoot problems in short-distance broken-line scenarios, while simultaneously enabling... Speed ​​planning can take into account curve safety constraints, following distance, channel speed limits, and historical speed profiles, transforming speed adjustment from a passive response to a proactive dynamic optimization, thus solving the inefficiency problems caused by direct speed reduction and delayed speed adjustment. By controlling the target ship's navigation based on the desired heading angle and desired motion parameters, under the unified scheduling of the heading and speed collaborative decision-making unit, it realizes mode switching and adaptive adjustment of control parameters for different navigation scenarios (such as curves, following navigation, dynamic obstacle avoidance, and narrow waterway passage), enabling the ship to maintain smooth, safe, and efficient tracking navigation in continuous multi-turn sections. Attached Figure Description

[0018] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0019] Figure 1 This is a schematic flowchart of a ship navigation control method according to some embodiments of the present invention; Figure 2 This is a schematic flowchart of another ship navigation control method according to some embodiments of the present invention; Figure 3 This is a structural block diagram of a ship navigation control device according to an embodiment of the present invention; Figure 4 This is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. Detailed Implementation

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

[0021] According to embodiments of the present invention, a ship navigation control method, apparatus, computer equipment, and storage medium are provided. It should be noted that the steps shown in the flowcharts in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowcharts, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0022] This embodiment provides a ship navigation control method. Figure 1 This is a flowchart of a ship navigation control method according to an embodiment of the present invention, such as... Figure 1 As shown, the process includes the following steps: Step S101: Obtain the real-time navigation information and preset reference path of the target vessel, wherein the real-time navigation information includes the vessel's position information and environmental perception information.

[0023] In this embodiment, real-time navigation information refers to a set of data collected in real time by shipborne sensors (such as GPS, radar, lidar, visual sensors, etc.) that reflects the current status of the target ship and the dynamics of its surrounding environment. Specifically, it includes ship position information for determining the ship's spatial position and environmental perception information for sensing external conditions such as channel boundaries, the movement of other ships, obstacles, and hydrological and meteorological conditions. The preset reference path refers to a pre-planned global route provided by the cloud or electronic chart system, which is usually composed of a series of waypoints with clear geographical coordinates and is used to guide the ship from the starting point to the destination.

[0024] The process is as follows: First, the real-time latitude and longitude coordinates of the target vessel are obtained through positioning equipment such as the Global Positioning System (GPS) as the vessel's position information. At the same time, multi-source sensing equipment such as radar, lidar, automatic identification systems, and visual sensors are used to collect environmental perception information, including the dynamics of other vessels, channel boundaries, bridges, obstructions, visibility, and wind force. Then, pre-planned and stored global reference path data is retrieved from the vessel's autopilot system or cloud server. This path consists of continuous waypoints and straight lines connecting waypoints, and has clear geometric characteristics. Finally, the real-time collected vessel position information and environmental perception information are spatiotemporally aligned with the pre-set reference path to provide basic input data for subsequent coordinated control of course and speed.

[0025] Step S102: Determine the desired heading angle based on the preset reference path and ship position information, and determine the desired motion parameters based on the preset reference path, ship position information and environmental perception information.

[0026] In this embodiment of the application, determining the desired heading angle based on a preset reference path and ship position information includes: Step A1: Based on the ship's position information, determine the first reference point that is closest to the target ship's current position on the preset reference path, and obtain the arc length parameter corresponding to the first reference point on the preset reference path.

[0027] Specifically, the ship's position information is the spatial coordinates of the target ship currently located, which are obtained in real time through devices such as the Global Positioning System; the first reference point is the point on the preset reference path that has the shortest geometric distance to the target ship's current actual position, and is denoted as P_n in the heading control module; the arc length parameter is the path length accumulated along the path curve from the starting point of the preset reference path to the first reference point, and is denoted as s_n in the heading control module, which is used to quantify the ship's longitudinal position on the reference path. The implementation process is as follows: First, the preset reference path is discretized into a polyline segment composed of several continuous path points. Each path point is pre-associated with an arc length parameter accumulated from the path starting point. Then, based on the current position information of the target vessel, the Euclidean distance from the position point to each path point on the preset reference path is calculated one by one. The path point with the smallest distance is found by traversal or by using a nearest neighbor search algorithm and is determined as the first reference point P_n. Finally, based on the path segment where the first reference point P_n is located, the arc length parameter s_n corresponding to the point is calculated using a linear interpolation method. That is, the path points with known arc length parameters before and after P_n are taken, and interpolation is performed according to the projection ratio of P_n on the line connecting the two points, so as to obtain the precise arc length position of the point on the entire preset reference path, which provides a basis for subsequent calculation of the path curvature radius and forward sight distance at the point.

[0028] Step A2: Calculate the lateral tracking error of the position point relative to the first reference point, and determine the path curvature radius of the first reference point on the preset reference path based on the arc length parameter.

[0029] Specifically, the position point refers to the actual spatial coordinates of the target vessel at the current moment; the first reference point is the point on the preset reference path that is closest to the position point, denoted as P_n; the lateral tracking error is the perpendicular distance from the position point to the tangent of the path where the first reference point is located, used to quantify the degree to which the vessel deviates from the preset reference path; the arc length parameter is the cumulative path length from the starting point of the preset reference path to the first reference point, denoted as s_n, used to locate the longitudinal position of the vessel on the path; the path curvature radius refers to the degree of curvature of the preset reference path at the first reference point. The smaller the curvature radius, the more severe the curvature of the path, used to measure the sharpness of the curve.

[0030] First, the lateral tracking error is calculated. Specifically, the tangent direction of the path at the first reference point P_n and its adjacent path points is determined. Then, a perpendicular line is drawn from the position point to the tangent, with the foot of the perpendicular falling on the tangent or its extension. The length of the perpendicular segment is the lateral tracking error e. This value can be positive or negative, typically defined as positive when the ship is veering to starboard and negative when veering to port. Second, the path curvature radius is determined. Specifically, using the arc length parameter s_n corresponding to the first reference point P_n, several path points adjacent to this point are extracted on the preset reference path. The curvature κ of this point is calculated using the three-point concyclic method or based on the first and second derivative relationships of the parametric curve. The path curvature radius R = 1 / |κ| is then obtained. If the path at this point is a straight segment, the curvature is zero and the curvature radius is infinite. This curvature radius will be used in the subsequent calculation of the adaptive forward look distance, enabling the heading control to adjust in advance according to the curvature of the path.

[0031] Step A3: Calculate the forward sight distance of the target vessel based on its current speed, path curvature radius, and minimum turning radius threshold.

[0032] Specifically, the current speed refers to the real-time speed of the target vessel at the current moment, denoted as V, and is usually measured by sensors such as the Global Positioning System or a Doppler log; the path curvature radius refers to the curvature radius at the first reference point P_n on the preset reference path, denoted as R, which is used to characterize the curvature of the path at that position. The smaller the curvature radius, the sharper the curve; the minimum turning radius threshold refers to the minimum theoretical turning radius at which the target vessel can safely complete a turn without sideslip or loss of control at a given speed, denoted as R_min, which is a preset constant related to the vessel's own turning characteristics and load conditions; the forward sight distance refers to the arc length extending forward from the first reference point along the preset reference path, denoted as L, which is used to determine the target point of the desired course and is a key adaptive parameter in line-of-sight path tracking.

[0033] First, the path curvature radius R at the first reference point is obtained, along with the target vessel's current speed V, acquired in real-time by sensors. Simultaneously, the preset minimum turning radius threshold R_min is read from the vessel's parameter configuration. Then, the adaptive forward sight distance is calculated using the formula L = k1 * V + k2 / (1 + R_min / |R|), where k1 and k2 are dimensionless empirical coefficients (usually k1=2, k2=0.5). In this formula, the first term k1 * V linearly increases the forward sight distance with increasing speed to ensure directional stability at high speeds, while the second term k2 / (1 + R_min / |R|) automatically reduces the forward sight distance in curves (where R is smaller) to prevent the vessel from cutting into the inside of the curve. Next, a coupling correction for the lateral tracking error e is introduced, using the correction formula L = L * (1 - k3 * ... The forward sight distance is adjusted by tanh(|e|), where k3 is a dimensionless empirical coefficient (usually taken as 0.3), and tanh is the hyperbolic tangent function. When the absolute value of the lateral tracking error is large, the correction coefficient (1 - k3 * tanh(|e|)) will decrease accordingly, thereby adaptively shortening the forward sight distance to enhance the correction response speed. The final output is the forward sight distance L after triple adaptive adjustment of speed, curvature and lateral error, which is used for the selection of subsequent target points.

[0034] Step A4: Determine the second reference point along the arc length of the preset reference path based on the forward sight distance, and calculate the basic expected heading angle from the current position of the target vessel to the second reference point.

[0035] Specifically, the arc length direction refers to the direction in which the cumulative path length increases along the preset reference path from the starting point to the ending point, used to determine the forward direction of the path; the second reference point refers to the path point reached after searching forward along the preset reference path from the first reference point P_n for a distance equal to the forward sight distance L, denoted as P_los, which serves as the sight target point in the line-of-sight method; the basic expected heading angle refers to the heading angle of the target vessel's current position pointing to the line connecting the second reference point, denoted as ψ_los. This angle is the basic heading instruction without considering feedback correction and feedforward compensation, and is usually taken clockwise with true north as the reference.

[0036] First, taking the first reference point P_n as the starting point and its corresponding arc length parameter s_n as the reference, along the arc length increasing direction of the preset reference path, the forward sight distance L is used as the accumulation step to calculate the arc length parameter s_los = s_n + L of the target point. Then, based on s_los, the target point is located in the discretized data of the preset reference path. If s_los corresponds exactly to a known path point, it is directly used. If it falls between two known path points, a linear interpolation method is used to calculate the precise coordinates (x_plos, y_plos) of the second reference point P_los based on the coordinates of two adjacent points and the ratio of their arc length parameters. Finally, the coordinates (x, y) of the target ship's current position are obtained, and the direction angle from the current position to the second reference point is calculated using the arctangent function ψ_los = atan2(y_plos - y, x_plos - x). This angle is the basic expected heading angle, which is used as the basis value for subsequent heading corrections.

[0037] Step A5: Determine the feedback control term for the lateral tracking error based on the lateral tracking error, and determine the feedforward control term for the path curvature based on the characteristic coefficients and arc length parameters of the target vessel.

[0038] Specifically, the lateral tracking error refers to the perpendicular distance from the target vessel's current position to the tangent at the first reference point on the preset reference path, denoted as e, used to quantify the degree to which the vessel deviates from the preset path; the feedback control term refers to the heading correction calculated using a proportional-integral-derivative control law based on the lateral tracking error and its rate of change and cumulative amount, denoted as ψ_fb, used to suppress deviations and gradually return the vessel to the reference path; "PID" refers to proportional-integral-derivative control, where the proportional term (Kp * e) is used to respond to the current error, the integral term (Ki * ∫e dt) is used to eliminate steady-state error, and the derivative term (Kd * e) is used to respond to the current error. ė) is used to predict the trend of error changes and enhance system damping; the characteristic coefficient refers to the empirical coefficient related to the ship's own physical properties, denoted as c, which comprehensively considers factors such as ship load, turning inertia, acceleration and deceleration response time, and is used to quantify the ship's tracking characteristics to changes in path curvature; the arc length parameter refers to the path length accumulated from the starting point of the preset reference path to the first reference point, denoted as s_n, which is used to locate the longitudinal position of the ship on the path and obtain the path curvature κ(s_n) at that point; the feedforward control term refers to the heading compensation amount pre-calculated based on the inherent geometric characteristics (curvature) of the path, denoted as ψ_ff, which is used to overcome the disturbance caused by path curvature in advance, so that the ship can start turning preparation before entering the curve.

[0039] First, the PID feedback control term for the lateral tracking error is calculated. Specifically, the lateral tracking error e calculated in step A2 is collected in real time, and its derivative is used to obtain the error change rate ė, and its integral is used to obtain the cumulative error ∫e dt. Then, these three are multiplied by the preset proportional coefficient Kp, differential coefficient Kd, and integral coefficient Ki, respectively, and the sum is taken as the arctangent transform to obtain ψ_fb = arctan(-Kp * e - Kd * ė - Ki * ∫e dt). This value is used to dynamically adjust the course according to the current deviation. Second, the feedforward control term for the path curvature is calculated. Specifically, the arc length parameter s_n obtained in step A1 is used to obtain the path curvature κ(s_n) at the preset reference path through the differential or parametric curve differentiation method, and then multiplied by the ship characteristic coefficient c, i.e., ψ_ff = c * κ(s_n), or a more precise form ψ_ff = arcsin(V * κ(s_n) / g) (where V is the current speed and g is the gravitational acceleration), this feedforward term enables the ship to sense the curvature of the path ahead in advance and give a steering command in advance before an actual deviation occurs, thereby reducing the tracking lag when turning.

[0040] Step A6: Based on the feedback control term of the lateral tracking error and the feedforward control term of the path curvature, the basic desired heading angle is corrected to obtain the desired heading angle.

[0041] Specifically, the desired heading angle refers to the comprehensive heading command that is finally output to the ship's heading control execution layer, denoted as ψ_d. It is the result of the superposition of the basic desired heading angle, the feedback control term, and the feedforward control term, and serves as the target input for the ship's rudder system or propulsion steering system. First, the basic desired heading angle ψ_los is used as the reference value for heading control. Then, the lateral tracking error PID feedback control term ψ_fb is superimposed on this reference value to correct path deviations caused by factors such as ship inertia and external disturbances, enabling the ship to gradually converge back to the preset reference path. Next, the path curvature feedforward control term ψ_ff is also superimposed. This feedforward term allows the ship to perceive the curvature trend of the curve ahead before actual deviation occurs and to give steering commands in advance, thereby effectively reducing tracking lag when cornering. Finally, the three terms are summed by ψ_d = ψ_los + ψ_fb + ψ_ff, and the result is normalized according to actual engineering needs (such as limiting the angle value to the range of 0° to 360°) to obtain the final desired heading angle, which is output to the underlying actuator to control the ship's steering motion.

[0042] In this embodiment of the application, the desired motion parameters are determined based on a preset reference path, ship position information, and environmental perception information, including: Step B1: Construct the target speed profile of the target vessel within the planning time domain, where the planning time domain includes multiple stages.

[0043] In this embodiment of the application, constructing the target speed profile of the target ship in the planning time domain includes: obtaining the planning time domain of the current control cycle and discretizing the planning time domain into multiple stages; solving the motion parameters of the target ship in each stage under preset constraints; and obtaining the target speed profile of the target ship in the planning time domain when the preset cost function satisfies the preset convergence condition. The preset cost function includes at least one of curve safety cost, following safety cost, and efficiency cost, and the constraints include at least one of ship dynamics constraints, curve speed constraints, following safety constraints, channel speed limit constraints, and empirical speed constraints.

[0044] Specifically, the planning time domain refers to the range of future flight paths projected forward by the speed control module within the current control cycle. It is typically measured by path length or time and denoted as D_lookahead. It generally covers the next 2 to 5 minutes of flight path and needs dynamic adjustment to ensure it includes at least the next critical curve or traffic conflict point. Discretization involves dividing the continuous planning time domain into several discrete stages along the path arc length, denoted as N stages, for numerical optimization. A stage refers to the i-th segment point in the discretized planning time domain; each stage corresponds to a path position and serves as the basic unit for optimization. Motion parameters... This refers to the key physical quantities describing the motion state of the target ship at each stage, mainly including the target speed V_i (as a state variable) at that stage and the acceleration a_i between adjacent stages (as a decision control variable); the target speed profile is a sequence composed of the optimal speed V_i at each stage within the planning time domain, reflecting the optimal trajectory of the ship's speed change over a future period; the preset constraints are the set of restrictions that must be satisfied when solving the speed profile, used to ensure the feasibility and safety of the planning results, specifically including ship dynamics constraints (upper and lower limits of acceleration a_min≤a_i≤a_max), and cornering speeds. The constraints include: a safe speed limit determined by the curvature of the curve (V_i ≤ sqrt(a_safe_max / |κ_i|)), a following safety constraint (safe speed calculated based on the distance to the preceding vessel V_i ≤ V_following_i), a channel speed limit constraint (inherent channel speed limit V_i ≤ V_limit(s_i)), and an empirical speed constraint (a reasonable speed range extracted from historical crew operation data V_exp_min(s_i) ≤ V_i ≤ V_exp_max(s_i)); the preset cost function is the objective function used to evaluate the quality of the speed profile, which is minimized. This function balances the needs of multi-objective optimization, specifically including curve safety cost J_curve (when the centripetal acceleration V_i²×|κ_i| generated by the curve exceeds the safety threshold a_safe_max, the excess is included in the cost in square form), car-follow safety cost J_follow (based on the time-distance model, a penalty is imposed when the ratio of car-follow distance to speed is less than the minimum safe time distance TTC_min), and efficiency cost (encouraging improved traffic efficiency); the preset convergence condition refers to the criterion for judging the termination of the iteration during the optimization solution process, which is usually the change in the cost function value is less than the set threshold or the number of iterations reaches the upper limit.

[0045] First, based on the target vessel's current position, the geometric features ahead of the preset reference path (such as curve curvature distribution), and environmental perception information (such as the position and speed of other vessels), the planning time domain D_lookahead for the current control cycle is dynamically determined to ensure that this time domain can cover at least one key curve or traffic conflict point. Then, this planning time domain is discretized into N stages along the path arc length, and motion parameters V_i and a_i are defined for each stage. Based on this, preset constraints are used as the feasible region of the optimization problem, and a preset cost function is used as the optimization objective. Numerical optimization algorithms such as quadratic programming are employed to solve the problem. The curve safety cost is used to suppress lateral acceleration during high-speed curves, the following safety cost is used to ensure sufficient safe time distance from the preceding vessel, and the efficiency cost avoids overly conservative speed decisions. During the solution process, the optimizer iteratively adjusts the motion parameters of each stage, gradually reducing the preset cost function value. When the difference in the cost function between two adjacent iterations is less than a preset threshold or the number of iterations reaches the upper limit, the preset convergence condition is satisfied. The optimal solution sequence obtained at this point is the target velocity profile of the target vessel within the planning time domain.

[0046] Step B2: Determine the target stage currently in which the target vessel is located based on the preset reference path, vessel position information, and environmental perception information.

[0047] Specifically, the target stage refers to the stage index among the N stages obtained by discretization that corresponds to the arc length position of the target ship on the preset reference path. It is used to extract the motion parameters to be executed at the current moment from the solved target velocity profile.

[0048] First, the first reference point P_n and its arc length parameter s_n are used as the precise longitudinal position of the target ship on the preset reference path. Then, combined with the determined planning time domain D_lookahead and its discretization method, which extends forward from the current ship position to the future voyage, the ship's current position corresponds to the first stage (i.e., i=1) in the discretized stages. On this basis, environmental perception information is further introduced for verification. If the actual arc length position of the current ship deviates from the starting point of the planning time domain due to dynamic obstacle avoidance or temporary deviation, the starting point of the planning time domain is realigned according to the latest ship position information to ensure that the target stage accurately reflects the actual position of the ship in the current control cycle. Finally, the target stage is determined as i=1 (i.e., the first stage in the planning time domain), and the motion parameters corresponding to this stage will be used as the expected output of this control cycle.

[0049] Step B3: Use the motion parameters corresponding to the target stage in the target velocity profile as the desired motion parameters.

[0050] Specifically, the target speed profile refers to the sequence of optimal speeds V_i obtained through optimization within each stage of the planning time domain, reflecting the optimal trajectory of ship speed changes over a future period; the target stage refers to the stage index corresponding to the arc length position of the target ship on the preset reference path. Since the planning time domain starts from the current ship position, this stage is usually the first stage after discretization (i.e., i=1); motion parameters refer to the key physical quantities describing the ship's motion state, which in the speed control module mainly include the target speed V_i of this stage and the acceleration a_i between adjacent stages; the desired motion parameters refer to the desired speed and desired acceleration commands finally output to the underlying actuators, used to drive the ship to adjust its speed according to the optimization results.

[0051] After optimizing and solving to obtain the target speed profile, this profile includes the optimal speed V_i at each discrete point from the current stage (i=1) to the Nth stage and the acceleration a_i between adjacent stages; the target stage is the starting stage (i=1) of the planning time domain, corresponding to the current path position of the ship; accordingly, the speed value V_1 corresponding to the first stage is extracted from the target speed profile as the desired speed V_desired at the current moment, and the acceleration value a_1 corresponding to the first stage is extracted as the desired acceleration a_desired; finally, these two desired motion parameters are output to the underlying actuator, where the desired speed is used to set the main engine speed or propulsion power, and the desired acceleration is used to guide the smooth transition of the acceleration and deceleration process, thereby realizing closed-loop control of the target ship speed.

[0052] Step S103: Control the target ship's navigation based on the desired heading angle and desired motion parameters.

[0053] In this embodiment, the desired heading angle refers to the final heading command, denoted as ψ_d, which is the result of the superposition of the basic desired heading angle, the lateral tracking error PID feedback control term, and the path curvature feedforward control term, used to guide the ship's turning motion; the desired motion parameters refer to the desired speed V_desired and the desired acceleration a_desired, where the desired speed is the first stage value of the target speed profile, and the desired acceleration is the acceleration command for the corresponding stage. Together, they constitute the control command for the ship's longitudinal motion; the target ship's navigation is mainly controlled by the level in the ship motion control system responsible for directly driving the actuators, after receiving the desired heading angle and the desired motion parameters. The level includes the rudder control system for achieving heading tracking, and the main engine control system or propulsion system for achieving speed regulation.

[0054] First, the desired heading angle ψ_d is sent to the underlying rudder control system. This system uses closed-loop rudder angle control to ensure the ship's actual heading follows the commanded value. Simultaneously, the desired speed V_desired and the desired acceleration a_desired are sent to the underlying main engine control system. This system uses feedforward-feedback composite control based on the deviation between the current actual speed and the desired speed, combined with the desired acceleration. It achieves smooth speed tracking by adjusting the main engine throttle or propeller pitch. In this process, the desired heading angle and desired motion parameters are independent yet synergistic: heading control ensures the ship always travels along the preset reference path, while speed control dynamically adjusts the speed based on factors such as the curvature of the path ahead and traffic conditions. Together, they act on the ship's propulsion and steering systems, enabling the target ship to achieve precise path tracking and safe speed control in highly dynamic inland waterways.

[0055] In this embodiment of the application, the real-time navigation information of the target vessel includes operational status information; such as... Figure 2 As shown, the method also includes: Step S201: During the process of controlling the target ship's navigation based on the desired heading angle and desired motion parameters, the target navigation scenario currently in which the target ship is located is determined according to environmental perception information and operational status information. The target navigation scenario is any one of the following: straight navigation scenario, curve navigation scenario, following navigation scenario, dynamic obstacle avoidance scenario, and narrow waterway navigation scenario.

[0056] In this embodiment, environmental perception information refers to the surrounding environment data collected in real time by devices such as radar, lidar, automatic identification systems, visual sensors, and depth sounders. This mainly includes the position and motion status of other vessels, channel boundaries, the location of obstructions, water depth, visibility, and hydrological and meteorological conditions such as wind force. Operational status information refers to the target vessel's own status data obtained through devices such as global positioning systems, inertial navigation systems, logs, and attitude sensors. This mainly includes the vessel's current position coordinates, speed, heading, bow angle, and lateral tracking error. The target navigation scenario refers to the specific scenario type into which the collaborative decision-maker classifies the current navigation situation based on a comprehensive judgment of the environmental perception information and operational status information. Specifically, this includes any one of the following: straight navigation scenario (long straight channel ahead, no traffic conflict), curve navigation scenario (increased curvature of the path ahead detected), following navigation scenario (a slow-moving target vessel ahead that is not suitable for overtaking), dynamic obstacle avoidance scenario (the nearest encounter distance or minimum encounter time with other target vessels is less than a safety threshold), and narrow waterway navigation scenario (significantly narrowed channel safety boundary). This serves as the basis for subsequent mode switching and collaborative control.

[0057] Within each control cycle, the collaborative decision-maker first gathers environmental perception information (such as the trajectory of other vessels, CPA / TCPA, and channel width) and operational status information (such as the vessel's position, speed, tracking error, and current arc length parameters) to form a comprehensive perception of the navigation situation. Then, it performs parallel judgments based on preset rule-triggered logic: if the curvature of the path ahead continuously increases from s_curve_start, it is determined to be a curve navigation scenario; if the CPA / TCPA is less than the safety threshold and there is a collision risk, it is determined to be a dynamic obstacle avoidance scenario; if there is a slow-moving target vessel ahead and it is passing the window... If the channel width is insufficient, it is determined to be a following navigation scenario. If the channel width is significantly smaller than the safety boundary, it is determined to be a narrow waterway navigation scenario. If none of the above conditions are met and the path ahead is a long straight section with small tracking error and no traffic conflict, it is determined to be a straight navigation scenario. When the triggering conditions of multiple scenarios are met at the same time (such as the presence of a slow boat ahead in a curve), the collaborative decision-maker arbitrates according to the preset priority rules (usually dynamic obstacle avoidance has the highest priority, followed by narrow waterway, curve navigation, following navigation, and straight navigation has the lowest priority) and finally outputs a unique target navigation scenario for the selection of subsequent collaborative control strategies.

[0058] Step S202: Determine the corresponding cooperative control strategy based on the target navigation scenario, and adjust the desired heading angle and desired motion parameters during the target ship's navigation according to the cooperative control strategy.

[0059] In this embodiment of the application, when the navigation scenario is a curve navigation scenario, the cooperative control strategy includes: during the curve entry phase, adjusting the desired motion parameters according to a first decreasing rule to obtain updated desired motion parameters; simultaneously adjusting the forward sight distance of the target vessel according to a second decreasing rule to obtain updated forward sight distance, and using the updated forward sight distance to update the desired heading angle to obtain updated desired heading angle; during the curve exit phase, maintaining the updated desired motion parameters and the updated desired heading angle; during the curve exit phase, adjusting the updated desired motion parameters according to a first increasing rule to obtain restored desired motion parameters; simultaneously adjusting the updated forward sight distance according to a second increasing rule to obtain restored forward sight distance, and using the restored forward sight distance to update the updated desired heading angle to obtain restored desired heading angle.

[0060] Specifically, the "curve navigation scenario" refers to the navigation state determined by the collaborative decision-maker when it detects an increase in curvature on the preset reference path ahead; the collaborative control strategy refers to the set of rules formulated by the collaborative decision-maker for specific navigation scenarios, coordinating the heading control module and the speed control module; the curve entry phase refers to the preparation phase from when the ship detects an increase in curvature ahead (s_curve_start) to when it officially enters the curve; the desired motion parameters refer to the desired speed V_desired and desired acceleration a_desired output by the speed control module, used to guide the ship's longitudinal motion; the first decreasing rule refers to the smooth deceleration mechanism executed by the speed module after the collaborative decision-maker issues a "curve preparation" event to the speed module during the curve entry phase, causing the desired motion parameters to gradually decrease according to a preset gradient; forward sight distance... "Leaving" refers to the arc length L extending forward from the first reference point along the preset reference path in the heading control module, used to determine the target point of the line of sight; "Second decreasing rule" refers to the adjustment mechanism by which the cooperative decision-maker instructs the heading module to gradually reduce the forward sight distance during the curve entry phase; "Desired heading angle" refers to the final heading command ψ_d output by the heading control module, used to guide the ship's turning motion; "Curve phase" refers to the continuous curve entry process where the ship enters the curve and its speed has dropped to a safe range; "Curve exit phase" refers to the recovery process where the ship detects that the path curvature is beginning to decrease and is about to leave the curve; "First increasing rule" refers to the mechanism by which the cooperative decision-maker instructs the speed module to plan acceleration during the curve exit phase, so that the desired motion parameters are gradually restored to the cruising speed; "Second increasing rule" refers to the mechanism by which the cooperative decision-maker instructs the heading module to gradually increase the forward sight distance during the curve exit phase, so that it is restored to the normal tracking mode.

[0061] During the curve entry phase, the collaborative decision-maker, based on the detection result that the curvature of the path ahead begins to increase, sends a "curve preparation" event to the speed control module. The speed module then begins to smoothly decelerate according to the first reduction rule, gradually reducing the desired motion parameters (desired speed and desired acceleration) to within the safe range of the curve. Simultaneously, the collaborative decision-maker instructs the heading control module to gradually reduce the forward sight distance according to the second reduction rule. The heading module uses the updated forward sight distance to recalculate the second reference point, thereby updating the desired heading angle, allowing the ship to prepare for curve entry in advance. During the curve exit phase, once the ship has officially entered the curve and the speed has decreased to a safe range, the collaborative decision-maker... The heading module is switched to "precise cornering mode." During this phase, the updated desired motion parameters and the updated desired heading angle are maintained to ensure the vessel safely passes through the curve in a stable state. In the exit phase, when the cooperative decision-maker detects that the path curvature begins to decrease, the speed module is instructed to accelerate according to the first incremental rule, gradually restoring the desired motion parameters to the cruising speed. At the same time, the heading module is instructed to gradually increase the forward sight distance according to the second incremental rule. The heading module uses the restored forward sight distance to recalculate the second reference point and update the desired heading angle, restoring the heading module to normal tracking mode and preparing for the subsequent segment.

[0062] In this embodiment of the application, when the navigation scenario is a following navigation scenario, the cooperative control strategy includes: detecting a reference vessel located in the direction of the target vessel's movement; determining a safe following speed based on the motion state information of the reference vessel; using the safe following speed as a priority constraint, and updating the desired motion parameters according to the priority constraint to obtain the updated desired motion parameters; simultaneously adjusting the forward sight distance of the target vessel according to the third incremental rule to obtain the updated forward sight distance, and using the updated forward sight distance to update the desired heading angle to obtain the updated desired heading angle.

[0063] Specifically, the following navigation scenario refers to the navigation situation determined by the collaborative decision-maker when it detects a slower target vessel ahead of the target vessel and the overtaking opportunity is insufficient under current conditions; the reference vessel refers to a target vessel located in the direction of the target vessel's movement, forming a following relationship with the current vessel, and moving at a slower speed, usually obtained through automatic identification systems or radar detection; motion state information refers to the real-time dynamic data of the reference vessel, mainly including its position coordinates, speed, heading, and relative distance to the current vessel; safe following speed refers to the permissible speed V_following calculated based on the time-distance model that maintains a sufficient safe time interval between the current vessel and the reference vessel; priority constraint refers to the priority constraint imposed by the collaborative decision-maker during the optimization process of the speed control module, prioritizing safe following speeds. As the highest priority hard constraint, the speed limit forces the output of the speed control module to ensure that the desired motion parameters do not exceed this speed value. The desired motion parameters refer to the desired speed V_desired and desired acceleration a_desired output by the speed control module, which are used to guide the longitudinal motion of the ship. The third incremental rule refers to the adjustment mechanism by which the cooperative decision-maker instructs the heading control module to slightly increase the forward look-ahead distance in the following navigation scenario, so as to increase the smoothness of path tracking and avoid overly sensitive steering due to frequent speed fluctuations. The forward look-ahead distance refers to the arc length L extending forward from the first reference point along the preset reference path in the heading control module, which is used to determine the target point of the line of sight. The desired heading angle refers to the final heading command ψ_d output by the heading control module, which is used to guide the ship's turning motion.

[0064] First, the collaborative decision-maker continuously monitors the dynamics of other vessels in the target vessel's direction of travel using sensors such as automatic identification systems and radar. When a slow-moving reference vessel that is not suitable for overtaking is detected, a follow-the-carry scenario is triggered. Then, based on the motion status information of the reference vessel (including its position, speed, and relative distance), a safe follow-the-carry speed V_following is calculated based on a time-distance model to ensure that the time interval between the current vessel and the reference vessel is not less than the minimum safe time distance TTC_min. Next, the collaborative decision-maker inputs this safe follow-the-carry speed as a priority constraint into the speed control module. When optimizing the solution of the target speed profile, the speed module will... This constraint is given the highest priority, forcing the expected speed V_i at each stage to not exceed the safe following speed, thus obtaining the updated expected motion parameters. At the same time, the co-decision unit instructs the heading control module to slightly increase the forward sight distance according to the third incremental rule, so that the heading module can recalculate the second reference point using the updated forward sight distance, and then update the expected heading angle to enhance the smoothness of path tracking and avoid over-response of the steering system caused by frequent speed fluctuations during following. Finally, the updated expected motion parameters and the expected heading angle work together on the ship, enabling the target ship to achieve smooth path tracking while maintaining a safe following distance.

[0065] In this embodiment of the application, when the navigation scenario is a dynamic obstacle avoidance scenario, the cooperative control strategy includes: detecting whether there is a collision risk between the target ship and the obstacle in front; when a collision risk is detected, calling the local path planner to generate a short-term obstacle avoidance heading sequence, and updating the desired heading angle according to the short-term obstacle avoidance heading sequence to obtain the updated desired heading angle; and adjusting the desired motion parameters according to the third decreasing rule to obtain the updated desired motion parameters.

[0066] Specifically, dynamic obstacle avoidance scenarios refer to the navigational state determined by the collaborative decision-maker when it detects that the nearest encounter distance or minimum encounter time between the target vessel and an obstacle ahead (including other vessels, fixed obstructions, etc.) is less than a safety threshold, thus posing a collision risk. This is the triggering condition for dynamic obstacle avoidance / encounter. Collision risk refers to a dangerous state determined based on CPA / TCPA indicators, where a collision may occur if evasive maneuvers are not taken. The local path planner is a fast path planning module invoked by the collaborative decision-maker during emergency obstacle avoidance. It can employ algorithms such as the artificial potential field method or the dynamic window method (DWA) to generate a local navigation path that avoids obstacles in a short time. It complements and strengthens the existing enhanced line-of-sight heading control module: the enhanced line-of-sight method provides global route reference, while the heading instructions given by the collision avoidance module are used for local obstacle avoidance. To ensure proper execution, the ship will return to the enhanced line-of-sight route after the obstacle avoidance operation is completed. The short-term obstacle avoidance heading sequence refers to the discrete heading command sequence output by the local path planner, which guides the ship to gradually turn during obstacle avoidance, covering the short time domain from the start of obstacle avoidance to escaping the obstacle risk. The third reduction rule refers to the adjustment mechanism adopted by the collaborative decision-maker in dynamic obstacle avoidance scenarios, which automatically adopts a conservative deceleration and follows the "slow-fast-slow" principle based on the intensity of obstacle avoidance maneuvers, i.e., decelerating when approaching the obstacle, maintaining speed when passing through, and recovering after escaping. The desired motion parameters refer to the desired speed V_desired and desired acceleration a_desired output by the speed control module, which guide the ship's longitudinal motion. The desired heading angle refers to the final heading command ψ_d output by the heading control module, which guides the ship's turning motion.

[0067] First, the collaborative decision-maker continuously calculates the CPA / TCPA index of the target vessel and obstacles ahead. When the index is detected to be lower than a preset safety threshold and a collision risk is determined, a dynamic obstacle avoidance scenario is triggered. Subsequently, the collaborative decision-maker temporarily suspends the original enhanced line-of-sight standard path tracking and calls a local path planner (such as the artificial potential field method or dynamic window method) to quickly generate a short-term obstacle avoidance heading sequence based on the current vessel status and obstacle information. The heading control module uses this sequence as a new heading command for tracking, thereby obtaining the updated desired heading angle and realizing local obstacle avoidance maneuvers. Simultaneously... The collaborative decision-maker instructs the speed control module to adjust the desired motion parameters according to the third decreasing rule. It automatically adopts a more conservative deceleration based on the intensity of the obstacle avoidance maneuver and strictly follows the "slow-fast-slow" principle: smoothly decelerates to reduce collision energy during the approach phase to reduce collision energy, maintains a stable speed during the passage phase, and gradually restores the speed to cruising speed after the collision risk is eliminated. After the obstacle avoidance maneuver is completed, the collaborative decision-maker terminates the call to the local path planner, causing the heading control module to switch back to the original enhanced line-of-sight method route for tracking, and the speed control module resumes normal speed planning, thereby achieving safe and efficient dynamic obstacle avoidance.

[0068] In this embodiment of the application, when the navigation scenario is a narrow waterway navigation scenario, the cooperative control strategy includes: obtaining the channel width information of the waterway where the target vessel is currently located; when the channel width information is less than a preset width threshold, determining the speed limit constraint based on the channel width information, and updating the expected motion parameters according to the speed limit constraint to obtain the updated expected motion parameters; at the same time, adjusting the forward sight distance of the target vessel according to the fourth decreasing rule to obtain the updated forward sight distance, and using the updated forward sight distance to update the expected heading angle to obtain the updated expected heading angle.

[0069] Specifically, the narrow waterway navigation scenario refers to the navigation situation determined by the collaborative decision-maker when it detects a significant narrowing of the channel safety boundary, i.e., the triggering condition for narrow waterway navigation; "channel width information" refers to the navigable width data of the waterway currently occupied by the target vessel, obtained through electronic charts, radar, or visual sensors, typically including the distance between the two boundaries of the channel; the preset width threshold refers to a pre-set critical value for the safe channel width, which is determined to be a narrow waterway when the actual channel width is less than this threshold; the speed limit constraint refers to the lower speed limit V_limit forcibly applied by the collaborative decision-maker based on the channel width information, used to ensure that the vessel remains within the restricted waterway. The desired motion parameters refer to the desired speed V_desired and desired acceleration a_desired output by the speed control module, which are used to guide the longitudinal motion of the ship. The fourth reduction rule refers to the adjustment mechanism by which the cooperative decision-maker instructs the heading control module to further reduce the forward sight distance in narrow waterway navigation scenarios, so as to improve the ship's maneuverability in narrow channels. The forward sight distance refers to the arc length L extending forward from the first reference point along the preset reference path in the heading control module, which is used to determine the target point of the line of sight. The desired heading angle refers to the final heading command ψ_d output by the heading control module, which is used to guide the ship's turning motion.

[0070] First, the collaborative decision-maker acquires the channel width information of the target vessel's current location through electronic charts, radar, or visual sensors and compares it with a preset width threshold. When the channel width information is detected to be less than the preset width threshold, it is determined to be a narrow waterway navigation scenario. The collaborative decision-maker determines a lower speed limit constraint V_limit based on the specific channel width value and inputs this speed limit constraint into the speed control module. The speed module enforces this speed limit when optimizing the target speed profile, thereby updating the desired motion parameters according to the speed limit constraint, resulting in updated desired motion parameters. Simultaneously, the collaborative decision-maker instructs the heading control module to further reduce the forward sight distance according to the fourth decreasing rule to improve the vessel's maneuverability in narrow waterways. At the same time, the collaborative decision-maker also adaptively increases the lateral error feedback gain Kp of the heading module to enhance its correction capability. The heading module recalculates the second reference point using the updated forward sight distance, thereby updating the desired heading angle. Finally, the updated desired motion parameters and the desired heading angle work together to enable the target vessel to navigate safely in narrow waterways with reduced speed and higher control precision, effectively preventing the risk of grounding or contact with shore caused by narrow channels.

[0071] This embodiment also provides a ship navigation control device for implementing the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that performs a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0072] This embodiment provides a ship navigation control device, such as... Figure 3 As shown, it includes: The acquisition module 31 is used to acquire the real-time navigation information of the target vessel and the preset reference path, wherein the real-time navigation information includes the vessel's position information and environmental perception information. The determination module 32 is used to determine the desired heading angle based on the preset reference path and the ship's position information, and to determine the desired motion parameters based on the preset reference path, the ship's position information and the environmental perception information. Control module 33 is used to control the navigation of the target ship based on the desired heading angle and desired motion parameters.

[0073] In an optional embodiment of this application, the determining module 32 is configured to: determine a first reference point on a preset reference path that is closest to the current position of the target vessel based on the vessel's position information; obtain the arc length parameter corresponding to the first reference point on the preset reference path; calculate the lateral tracking error of the position point relative to the first reference point; determine the path curvature radius of the first reference point on the preset reference path based on the arc length parameter; calculate the forward sight distance of the target vessel based on the current speed of the target vessel, the path curvature radius, and the minimum turning radius threshold of the target vessel; determine a second reference point along the arc length direction of the preset reference path based on the forward sight distance; calculate the basic expected heading angle from the current position of the target vessel to the second reference point; determine a feedback control term for the lateral tracking error based on the lateral tracking error; determine a feedforward control term for the path curvature based on the characteristic coefficient of the target vessel and the arc length parameter; and correct the basic expected heading angle based on the feedback control term for the lateral tracking error and the feedforward control term for the path curvature to obtain the expected heading angle.

[0074] In an optional embodiment of this application, the determining module 32 is used to construct a target speed profile of the target ship in the planning time domain, wherein the planning time domain includes multiple stages; determine the target stage in which the target ship is currently located based on a preset reference path, ship position information and environmental perception information; and use the motion parameters corresponding to the target stage in the target speed profile as the expected motion parameters.

[0075] In an optional embodiment of this application, the determining module 32 is used to obtain the planning time domain of the current control cycle and discretize the planning time domain into multiple stages; solve the motion parameters of the target ship in each stage under preset constraints; and obtain the target speed profile of the target ship in the planning time domain when the preset cost function satisfies the preset convergence condition. The preset cost function includes at least one of curve safety cost, following safety cost, and efficiency cost, and the constraints include at least one of ship dynamics constraints, curve speed constraints, following safety constraints, channel speed limit constraints, and empirical speed constraints.

[0076] In an optional embodiment of this application, the real-time navigation information of the target vessel includes operational status information; the device further includes: an adjustment module, used to determine the target navigation scenario currently in which the target vessel is located based on environmental perception information and operational status information during the process of controlling the target vessel's navigation based on the desired heading angle and desired motion parameters, wherein the target navigation scenario is any one of a straight navigation scenario, a curve navigation scenario, a following navigation scenario, a dynamic obstacle avoidance scenario, and a narrow waterway navigation scenario; determine a corresponding cooperative control strategy based on the target navigation scenario, and adjust the desired heading angle and desired motion parameters during the target vessel's navigation according to the cooperative control strategy.

[0077] In an optional embodiment of this application, the adjustment module is configured to, when the navigation scenario is a curve navigation scenario, adjust the desired motion parameters according to a first decreasing rule during the curve entry phase to obtain updated desired motion parameters; simultaneously adjust the forward sight distance of the target vessel according to a second decreasing rule to obtain updated forward sight distance, and use the updated forward sight distance to update the desired heading angle to obtain updated desired heading angle; during the curve exit phase, maintain the updated desired motion parameters and the updated desired heading angle; during the curve exit phase, adjust the updated desired motion parameters according to a first increasing rule to obtain restored desired motion parameters; simultaneously adjust the updated forward sight distance according to a second increasing rule to obtain restored forward sight distance, and use the restored forward sight distance to update the updated desired heading angle to obtain restored desired heading angle.

[0078] In an optional embodiment of this application, the adjustment module is used to detect a reference vessel located in the direction of the target vessel's movement when the navigation scenario is a following navigation scenario; determine a safe following speed based on the motion state information of the reference vessel; use the safe following speed as a priority constraint and update the desired motion parameters according to the priority constraint to obtain the updated desired motion parameters; at the same time, adjust the forward sight distance of the target vessel according to the third incremental rule to obtain the updated forward sight distance, and use the updated forward sight distance to update the desired heading angle to obtain the updated desired heading angle.

[0079] In an optional embodiment of this application, the adjustment module is used to detect whether there is a collision risk between the target ship and the obstacle in front when the navigation scenario is a dynamic obstacle avoidance scenario; when a collision risk is detected, the local path planner is called to generate a short-term obstacle avoidance heading sequence, and the expected heading angle is updated according to the short-term obstacle avoidance heading sequence to obtain the updated expected heading angle; at the same time, the expected motion parameters are adjusted according to the third decreasing rule to obtain the updated expected motion parameters.

[0080] In an optional embodiment of this application, the adjustment module is used to obtain the channel width information of the waterway where the target vessel is currently located when the navigation scenario is a narrow waterway navigation scenario; when the channel width information is less than a preset width threshold, determine the speed limit constraint according to the channel width information, and update the expected motion parameters according to the speed limit constraint to obtain the updated expected motion parameters; at the same time, adjust the forward sight distance of the target vessel according to the fourth decreasing rule to obtain the updated forward sight distance, and use the updated forward sight distance to update the expected heading angle to obtain the updated expected heading angle.

[0081] Please see Figure 4 , Figure 4 This is a schematic diagram of the structure of a computer device provided in an optional embodiment of the present invention, such as... Figure 4 As shown, the computer device includes one or more processors 10, memory 20, and interfaces for connecting the components, including high-speed interfaces and low-speed interfaces. The components communicate with each other via different buses and can be mounted on a common motherboard or otherwise installed as needed. The processors can process instructions executed within the computer device, including instructions stored in or on memory to display graphical information of a GUI on external input / output devices (such as display devices coupled to the interfaces). In some alternative implementations, multiple processors and / or multiple buses can be used with multiple memories and multiple memory modules, if desired. Similarly, multiple computer devices can be connected, each providing some of the necessary operations (e.g., as a server array, a group of blade servers, or a multiprocessor system).

[0082] Processor 10 may be a central processing unit, a network processor, or a combination thereof. Processor 10 may further include a hardware chip. The hardware chip may be an application-specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The programmable logic device may be a complex programmable logic device (CAMP), a field-programmable gate array (FPGA), a general-purpose array logic (GPA), or any combination thereof.

[0083] The memory 20 stores instructions executable by at least one processor 10 to cause at least one processor 10 to perform the method shown in the above embodiments.

[0084] The memory 20 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the computer device as shown by a landing page for an app. Furthermore, the memory 20 may include high-speed random access memory and may also include non-transient memory, such as at least one disk storage device, flash memory device, or other non-transient solid-state storage device. In some alternative embodiments, the memory 20 may optionally include memory remotely located relative to the processor 10, which can be connected to the computer device via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0085] The memory 20 may include volatile memory, such as random access memory; the memory may also include non-volatile memory, such as flash memory, hard disk or solid-state drive; the memory 20 may also include a combination of the above types of memory.

[0086] The computer device also includes a communication interface 30 for communicating with other devices or communication networks.

[0087] This invention also provides a computer-readable storage medium. The methods described above according to embodiments of the invention can be implemented in hardware or firmware, or implemented as computer code that can be recorded on a storage medium, or implemented as computer code downloaded via a network and originally stored on a remote storage medium or a non-transitory machine-readable storage medium and then stored on a local storage medium. Thus, the methods described herein can be processed by software stored on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, optical disk, read-only memory, random access memory, flash memory, hard disk, or solid-state drive, etc.; further, the storage medium can also include combinations of the above types of memory. It is understood that computers, processors, microprocessor controllers, or programmable hardware include storage components capable of storing or receiving software or computer code, which, when accessed and executed by the computer, processor, or hardware, implements the methods shown in the above embodiments.

[0088] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A method for controlling ship navigation, characterized in that, The method includes: The system acquires real-time navigation information and a preset reference path for the target vessel, wherein the real-time navigation information includes vessel position information and environmental perception information. The desired heading angle is determined based on the preset reference path and the ship's position information, and the desired motion parameters are determined based on the preset reference path, the ship's position information, and the environmental perception information. The target vessel is controlled to navigate based on the desired heading angle and the desired motion parameters.

2. The method according to claim 1, characterized in that, Determining the desired heading angle based on the preset reference path and the ship's position information includes: Based on the ship's position information, determine the first reference point on the preset reference path that is closest to the current position of the target ship, and obtain the arc length parameter corresponding to the first reference point on the preset reference path; Calculate the lateral tracking error of the position point relative to the first reference point, and determine the path curvature radius of the first reference point on the preset reference path based on the arc length parameter; The forward sight distance of the target vessel is calculated based on the current speed of the target vessel, the radius of curvature of the path, and the minimum turning radius threshold of the target vessel. Along the arc length direction of the preset reference path, a second reference point is determined based on the forward sight distance, and the basic expected heading angle of the target vessel's current position pointing to the second reference point is calculated; The feedback control term for the lateral tracking error is determined based on the lateral tracking error, and the feedforward control term for the path curvature is determined based on the characteristic coefficient of the target vessel and the arc length parameter. The base desired heading angle is corrected based on the feedback control term of the lateral tracking error and the feedforward control term of the path curvature to obtain the desired heading angle.

3. The method according to claim 1, characterized in that, The step of determining the desired motion parameters based on the preset reference path, the ship's position information, and the environmental perception information includes: Construct a target speed profile of the target ship within a planning time domain, wherein the planning time domain includes multiple stages; Based on the preset reference path, the ship's position information, and the environmental perception information, the target stage currently in which the target ship is located is determined; The motion parameters corresponding to the target stage in the target velocity profile are used as the desired motion parameters.

4. The method according to claim 3, characterized in that, The construction of the target speed profile of the target ship in the planning time domain includes: Obtain the planning time domain of the current control cycle, and discretize the planning time domain into multiple stages; Under preset constraints, the motion parameters of the target ship at each stage are solved. When the preset cost function satisfies the preset convergence condition, the target speed profile of the target ship in the planning time domain is obtained. The preset cost function includes at least one of curve safety cost, following safety cost and efficiency cost. The constraints include at least one of ship dynamics constraints, curve speed constraints, following safety constraints, channel speed limit constraints and empirical speed constraints.

5. The method according to claim 1, characterized in that, The real-time navigation information of the target vessel includes operational status information; the method further includes: During the process of controlling the navigation of the target vessel based on the desired heading angle and the desired motion parameters, the target navigation scenario currently in which the target vessel is located is determined according to the environmental perception information and the operating status information. The target navigation scenario is any one of the following: straight navigation scenario, curve navigation scenario, following navigation scenario, dynamic obstacle avoidance scenario, and narrow waterway navigation scenario. Based on the target navigation scenario, a corresponding cooperative control strategy is determined, and the desired heading angle and the desired motion parameters are adjusted according to the cooperative control strategy during the navigation of the target vessel.

6. The method according to claim 5, characterized in that, When the navigation scenario is a curve navigation scenario, the cooperative control strategy includes: During the curve entry phase, the desired motion parameters are adjusted according to the first decreasing rule to obtain updated desired motion parameters; at the same time, the forward sight distance of the target vessel is adjusted according to the second decreasing rule to obtain updated forward sight distance, and the desired heading angle is updated using the updated forward sight distance to obtain updated desired heading angle. During the cornering phase, maintain the updated desired motion parameters and the updated desired heading angle; During the exit phase of the curve, the updated desired motion parameters are adjusted according to the first incremental rule to obtain the restored desired motion parameters; at the same time, the updated forward sight distance is adjusted according to the second incremental rule to obtain the restored forward sight distance, and the updated desired heading angle is updated using the restored forward sight distance to obtain the restored desired heading angle.

7. The method according to claim 5, characterized in that, When the navigation scenario is a car-following navigation scenario, the cooperative control strategy includes: Detect a reference vessel located in the direction of travel of the target vessel; Determine the safe following speed based on the motion status information of the reference vessel; The safe following speed is used as a priority constraint, and the desired motion parameters are updated according to the priority constraint to obtain the updated desired motion parameters; at the same time, the forward sight distance of the target vessel is adjusted according to the third incremental rule to obtain the updated forward sight distance, and the desired heading angle is updated using the updated forward sight distance to obtain the updated desired heading angle.

8. The method according to claim 5, characterized in that, When the navigation scenario is a dynamic obstacle avoidance scenario, the cooperative control strategy includes: Detect whether there is a risk of collision between the target vessel and obstacles in front of it; When a collision risk is detected, the local path planner is invoked to generate a short-term obstacle avoidance heading sequence, and the desired heading angle is updated according to the short-term obstacle avoidance heading sequence to obtain the updated desired heading angle; at the same time, the desired motion parameters are adjusted according to the third decreasing rule to obtain the updated desired motion parameters.

9. The method according to claim 5, characterized in that, When the navigation scenario is a narrow waterway navigation scenario, the cooperative control strategy includes: Obtain the channel width information of the waterway where the target vessel is currently located; When the channel width information is less than a preset width threshold, a speed limit constraint is determined based on the channel width information, and the desired motion parameters are updated according to the speed limit constraint to obtain the updated desired motion parameters; at the same time, the forward sight distance of the target vessel is adjusted according to the fourth decreasing rule to obtain the updated forward sight distance, and the desired heading angle is updated using the updated forward sight distance to obtain the updated desired heading angle.

10. A ship navigation control device, characterized in that, The device includes: The acquisition module is used to acquire the real-time navigation information of the target vessel and the preset reference path, wherein the real-time navigation information includes the vessel's position information and environmental perception information; The determination module is used to determine the desired heading angle based on the preset reference path and the ship position information, and to determine the desired motion parameters based on the preset reference path, the ship position information and the environmental perception information. A control module is used to control the navigation of the target vessel based on the desired heading angle and the desired motion parameters.

11. A computer device, characterized in that, include: A memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, the processor executing the computer instructions to perform the method of any one of claims 1 to 9.

12. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing a computer to perform the method of any one of claims 1 to 9.

Images