Human-like constrained adaptive model predictive control method with risk-adjustable control barrier function

Abstract

The present application relates to a kind of human-like constraint adaptive model predictive control method with risk adjustable control barrier function, constraint adaptive MPC is combined with risk adjustable CBF to avoid collision, constraint relaxation is added in MPC-CBF, and the attenuation coefficient of risk adjustable CBF is adjusted for different obstacles;At the same time, human-like MPC is combined, and event-triggered mechanism is used, and human-like constraint adaptive MPC control strategy with risk adjustable CBF is constructed;Ship control and risk avoidance are carried out by the control strategy constructed, if general navigation risk is detected, the attenuation coefficient of risk adjustable CBF is adjusted for different obstacles, if imminent danger is detected, acceptable control input is obtained by constraint relaxation, if in safe state, human-like model predictive control based on event-triggered mechanism is used.The method is conducive to achieving the balance between control efficiency, safety, stability and energy saving.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of safety control, and particularly relates to a human-like constraint adaptive model predictive control method with a risk-adjustable control barrier function. BACKGROUND

[0002] In recent years, safety control methods to prevent ship systems from reaching dangerous or unstable states have become particularly important. With the increase in traffic density, the maneuvering space available to ships is further restricted. Therefore, the distance between ships will be closer. The waterway traffic flow in the restricted area is more complex, greatly increasing the difficulty of maneuvering and the risk of collision. Therefore, the development of safety control is crucial.

[0003] Since the pioneering work of Japanese scholar Nagumo on the necessary and sufficient conditions for invariance sets in the 1940s, many studies have been conducted on the safety of control systems from multiple perspectives. Currently, methods for safety analysis and control mainly include model checking, reachability analysis, and barrier functions. The control barrier function (CBF) proposed by Wieland and Allgower defines a constraint condition that guarantees the safety and stability of a dynamic system. Implementing these constraints is to prevent the system from entering an undesirable, dangerous, or unsafe state.

[0004] Barrier functions act as boundaries that the system should not cross and impose penalties or costs when the system violates these constraints.

[0005] Although great progress has been made in safety control, there are still some problems and challenges, such as:

[0006] (1) The classical model predictive control (MPC) is a general-purpose controller, not specifically designed for human driving. It focuses on narrowing the gap between the actual trajectory and the reference trajectory. The controller guides the system to reach the target position, ensuring a certain degree of safety within the system state constraints and control input limits. In complex and crowded scenarios, any probability of insecurity is considered unacceptable. In addition, quadratic programming (QP) may encounter unstable situations without a solution due to factors such as sensor noise, modeling errors, and complex ocean environment disturbances, which may result in solutions that do not satisfy the given inequality constraints. In these unstable situations, it may be impossible to generate actual control inputs, which may lead to collision risks. Therefore, it is necessary to learn from human behavior and find solutions from a higher level within appropriate constraint ranges, such as reducing the number of inequality constraints.

[0007] ​(2) How to construct CBF is still a challenge. Although solutions have been found for certain special structures of the system (such as polynomial systems), further exploration is needed for more general nonlinear systems to determine the most advantageous barrier function. In addition, the constraint model needs to take into account various factors such as tidal models and sea conditions. These constraints change over time, and their time characteristics vary with uncertainty, making it challenging to create an accurate model. Conservativeness of CBF can also be considered. CBF imposes robustness on uncertainties in the worst-case scenario. However, these uncertainties can result in significant time efficiency and control cost in dynamic environments, making autonomous task execution practically infeasible in some applications. Depending on the scenario, safety and danger can vary greatly. In the case of a single CBF, the introduction of slack variables can guarantee feasibility. However, in the presence of multiple CBF constraints, conflicts between different CBFs can arise, resulting in a lack of feasible solutions. The challenge lies in balancing the priorities of different CBFs to ensure feasibility. Most existing work involves first designing an optimization problem and then incorporating CBF constraints with minimal modifications to improve safety. However, this approach can result in suboptimal controllers, and there is no rigorous discussion of whether the proposed CBF is effective.

[0008] (3) The excellent driving experience of humans has not been thoroughly explored and imitated, learned, and inherited by autonomous ships. Compared to autonomous ships, human ship operators do not need precise path tracking when the starting point and destination are clearly determined. Instead, they focus on navigation within the channel and avoiding collision accidents. They do not need to engage in high-frequency refresh control rates and modify operations such as acceleration, deceleration, and steering. In some applications, slight violations of constraints can be tolerated to improve task efficiency. In specific applications, a trade-off between risk and efficiency is needed. For activities such as yacht sightseeing tours, strict route specifications are not required. After specifying the starting point and destination, the route can be automatically adjusted based on control cost and collision avoidance. SUMMARY

[0009] The purpose of the present application is to provide a human-like constraint adaptive model predictive control method with a risk-adjustable control barrier function, which is beneficial to achieve a balance between control efficiency, safety, stability, and energy saving.

[0010] In order to achieve the above object, the technical scheme adopted by the present application is: a human-like constraint adaptive model predictive control method with a risk-adjustable control barrier function, which combines constraint adaptive model predictive control (MPC) with a risk-adjustable control barrier function (CBF) to avoid collision, adds constraint relaxation to MPC-CBF and adjusts the decay coefficient of the risk-adjustable CBF for different obstacles; at the same time, a human-like constraint adaptive MPC control strategy with a risk-adjustable CBF is constructed by combining human-like MPC and using an event-triggered mechanism; ship control and risk avoidance are performed through the constructed control strategy, if a general navigation risk is detected, the decay coefficient of the risk-adjustable CBF is adjusted for different obstacles, if an imminent danger is detected, acceptable control input is obtained through constraint relaxation, and if in a safe state, human-like model predictive control based on an event-triggered mechanism is adopted.

[0011] Further, the method specifically comprises the following steps:

[0012] Step 1: establishing a ship dynamics model;

[0013] Step 2: constructing a risk-adjustable CBF for complex navigation obstacles;

[0014] Step 3: when the system detects a general navigation risk, i.e. the distance between the ship and the surrounding obstacles is less than a safety threshold, the decay coefficient of the risk-adjustable CBF is adjusted for different obstacles, and a constructed quadratic programming problem QP1 is solved, the ship trajectory is controlled and the distance is increased based on the planning result until the distance is not less than the safety threshold; then the system resumes free driving;

[0015] Step 4: when the system detects an imminent danger, the constraints are relaxed based on the quadratic programming problem QP1, a quadratic programming problem QP2 is solved, and acceptable control input is obtained through constraint relaxation;

[0016] Step 5: if the ship does not exist navigation threat and is in a safe state, a quadratic programming problem QP3 based on event triggering is solved, and only when the set condition is reached, the control is intervened.

[0017] Further, in step 1, the ship model is established as follows:

[0018]

[0019]

[0020]

[0021] η=[x,y,ψ] T

[0022] υ=[u,v,r] T

[0023] where η is the position and orientation vector, is the first derivative of the position and orientation vector, R is the rotation matrix, and υ is the velocity vector, is the first derivative of the velocity vector, M is the inertia matrix, C is the Coriolis and centripetal force matrix of the rigid body, D is the damping matrix, and τ is the input force and moment, τ d is the disturbance, u is the forward velocity, v is the lateral velocity, r is the yaw angular velocity, x is the forward position, y is the lateral position, and ψ is the yaw angle;

[0024] The disturbance is caused by wind and wave;

[0025] τ d = F w + F s

[0026] F w = [F wx , F wy , N w ] T

[0027] F s = [F sx , F sy , N s ] T

[0028] where F w is the wind disturbance, F s is the wave disturbance, F wx is the wind disturbance in the forward direction, F wy is the wind disturbance in the lateral direction, N w is the wind disturbance in the yaw direction, N s is the wave disturbance in the yaw direction, F sx is the wave disturbance in the forward direction, and F sy is the wave disturbance in the lateral direction;

[0029]

[0030] where C X is the longitudinal wind coefficient, C Y is the lateral wind coefficient, C N is the wind moment coefficient, A f is the projected area above waterline, A s is the projected area of the side surface, L oa is the total length of the ship, p a is the air density, and V r is the relative velocity;

[0031] the encounter frequency ωe is:

[0032]

[0033] where ω w is the frequency of the wave, v w is the speed of the wave, χ is the encounter angle, and g is the gravitational acceleration;

[0034] The disturbance force and torque of each sub-wave are:

[0035]

[0036] where X si is the disturbance force of the i-th wave in the longitudinal direction, Y si is the disturbance force of the i-th wave in the transverse direction, N si is the disturbance torque of the i-th wave in the bow direction, and ζ ai is the amplitude of each harmonic wave in the random wave, T is the draft, B is the waterline width, A is the cross-sectional area of the ship, and k1 and k2 are set coefficients;

[0037] The wave disturbance is:

[0038]

[0039] where n represents the number of sub-waves.

[0040] Substituting into we get:

[0041]

[0042] The system state equation is represented as:

[0043]

[0044] where I is the identity matrix,

[0045] The expected pose vector η d is denoted as:

[0046] η d = [x d , y d , ψ d ] T

[0047] The pose vector error η e and the derivative error of the pose vector are denoted as:

[0048]

[0049] where x d is the desired longitudinal position, y d is the desired lateral position, ψ d is the desired heading angle, and η e is the tracking error of the position vector.

[0050] The state equation of the tracking error system is:

[0051]

[0052] where, The purpose of the control is to calculate the force and torque vector U.

[0053] Further, the specific method for constructing a risk-adjustable CBF for complex navigation obstacles in step 2 is:

[0054] Design a CBF constraint based on distance to ensure that no collision occurs between the ship and the obstacle;

[0055] Let C k be a closed set,

[0056] where X k represents the X value of the kth time series, represents an n-dimensional real-time space, and h(X k ) represents the control barrier function of the kth time series.

[0057] If there exists a K ∞ class function α such that the following formula is true, then the function h is a control barrier function CBF:

[0058] infh(X k+1 )-h(X k )≥-α(h(X k ))

[0059] Where inf represents the lower bound.

[0060] The conditional definition of CBF is as follows:

[0061] Δh(X k )=h(X k+1 )-h(X k )≥-γh(X k )

[0062] Where Δh(X k ) represents the difference of h, and γ > 0 represents the decay coefficient.

[0063] The complex navigation obstacle is represented by the union set of reachable points.

[0064] The variable area of the restricted navigation area is described and adjusted by the risk-adjustable CBF for different obstacles.

[0065] Further, the ship should comply with the following guidelines according to its actual draft to ensure sufficient navigation gap depth: the navigation gap depth should be no less than 0.4m for the ship with draft less than 5m; the navigation gap depth should be no less than 0.5m for the ship with draft less than 7m and no less than 5m; the navigation gap depth should be no less than 0.7m for the ship with draft less than 9.7m and no less than 7m; the navigation gap depth should be no less than 0.8m for the ship with draft between 9.7m and 10.5m; the navigation gap depth should be no less than 10% of the draft for the ship with draft greater than 10.5m; the navigation gap depth should be increased by 0.1m for the ship carrying dangerous goods; the navigation gap depth should be increased by 0.1m for the ship with speed exceeding 12 knots; the minimum safe water depth is calculated to determine whether the ship can safely pass through the shallow water area; the minimum safe water depth calculation formula of the ship is: draft + salinity difference + rapid ship stern trim + draft increased due to height + half wave height + reserve depth - draft reduced due to fuel consumption.

[0066] Further, in step 3, the quadratic programming problem QP1 is specifically:

[0067] At time t k , the objective function J in the MPC constraint optimization problem is:

[0068]

[0069] subjectto

[0070] Δh(X k )=h(X k+1 )-h(X k )≥-γh(X k )

[0071] X e ∈X o

[0072] U∈U o

[0073] where t is the integral variable, t k is the kth time sequence, N p > 0 is the prediction interval time length range, X e (t) is the prediction of the system state error at time t k , Q is the weight matrix of X e (t), and R1 is the weight positive definite matrix of U(t); represents the square of the 2-norm of the vector, where U iR represents the i-th element of vector U. 1i Represents the i-th element on the diagonal of matrix R1; Let X be the square of the 2-norm of a vector, where X is the vector with respect to the 2-norm. ei Represents vector X e The i-th element, Q i X represents the i-th element on the diagonal of matrix Q; o U is the set of system states; o X is the set of ranges of system control inputs. e (t) is at time t k Prediction of system state error at a given location.

[0074] Furthermore, in step 4, the quadratic programming problem QP2 is specifically as follows:

[0075]

[0076] subjectto

[0077] Δh(X k )=h(X k+1 )-h(X k )≥-γh(X k )

[0078] X e ∈X Rel

[0079] U∈U Rel

[0080] Among them, X Rel It is the set of states after constraint relaxation. It is X Rel A subset of; U Rel It is the control input set after constraint relaxation. It's U Rel A subset of.

[0081] Furthermore, in step 5, the quadratic programming problem QP3 is specifically as follows:

[0082]

[0083] subjectto

[0084] X e ∈X o

[0085] U∈U o

[0086]

[0087] Where P is X e(t k +N p ) weight matrix, s is at a certain moment between time interval (t k ,t k +N p ], epsilon is a set coefficient, alpha is a constant in (0,1);

[0088] The order of the moment of event triggering is 0<=t1<t2<…t k <…;The moment when the deviation between the actual state X(s;t k ) and the predicted state X * (s;t k ) reaches a threshold value is:

[0089]

[0090] Wherein, L is the Lipschitz constant of f, rho is the maximum value of the infinite norm of disturbance, i.e. ||omega|| ∞ <=rho, beta is a constant in (0,N p ), and lambda is the maximum eigenvalue;

[0091] The event triggering strategy is as follows:

[0092]

[0093] Compared with the prior art, the present application has the following beneficial effects:

[0094] 1、The present application combines constraint adaptive MPC with CBF to avoid collision, and creatively adds constraint relaxation in MPC-CBF design.

[0095] 2、The present application constructs a human-like MPC based on an event triggering mechanism for ship control and risk avoidance, so that the close connection between ship maneuvering control and risk is avoided and strengthened. The method combines the advantages of human drivers and machines, and realizes the hybrid enhancement of human-machine intelligence. When a danger is detected, the focus is on collision avoidance.

[0096] 3、Unlike traditional complex channel ship tracking control, the present application allows adjusting the risk boundary according to specific requirements to meet safety constraints. For ships sailing near shoals, glaciers, underwater fishing nets and reefs under variable sailing conditions, the attenuation coefficient of the adjustable CBF is adjusted. When the attenuation coefficient is large, the keel clearance is also large, and vice versa, so as to provide guidance for reducing the risk of grounding when the water depth is uncertain, or maintaining a safe distance from environmentally sensitive areas. BRIEF DESCRIPTION OF DRAWINGS

[0097] Figure 1 is the control principle diagram of the embodiment of the present application;​

[0098] Figure 2 is a method implementation flowchart of an embodiment of the present application;

[0099] Figure 3 is a CBF for ship anti-grounding in an embodiment of the present application;

[0100] Figure 4 is a schematic diagram of a predicted control result (trajectory) in an embodiment of the present application;

[0101] Figure 5 is a schematic diagram of a predicted control result (speed) in an embodiment of the present application;

[0102] Figure 6 is a schematic diagram of a predicted control result (position and heading angle) in an embodiment of the present application;

[0103] Figure 7 is a schematic diagram of a predicted control result (input) in an embodiment of the present application. DETAILED DESCRIPTION

[0104] The present application will be further described below in conjunction with the accompanying drawings and embodiments.

[0105] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the present application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0106] It is to be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments according to the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, devices, components and / or combinations thereof, but do not preclude the presence or addition of one or more other features, steps, operations, devices, components and / or combinations thereof.

[0107] In order to integrate human driving characteristics and decision-making process into the control system, the present embodiment provides a human-like constraint adaptive model predictive control method with a risk-adjustable control barrier function, aiming to bridge the gap between control efficiency, safety, stability and energy saving. The control principle is as follows: Figure 1The method combines constraint adaptive MPC with risk-adjustable CBF to avoid collision, adds constraint relaxation to MPC-CBF, and adjusts the attenuation coefficient of risk-adjustable CBF for different obstacles; at the same time, it combines human-like MPC and uses an event-triggered mechanism to build a human-like constraint adaptive MPC control strategy with risk-adjustable CBF; through the control strategy built, the ship is controlled and risk is avoided, if a general navigation risk is detected, the attenuation coefficient of risk-adjustable CBF is adjusted for different obstacles, if an imminent danger is detected, an acceptable control input is obtained through constraint relaxation, and if in a safe state, a human-like model predictive control based on an event-triggered mechanism is adopted.

[0108] The method learns from human behavior, and once the starting point and destination are clearly determined, precise path tracking and high-frequency refresh control rate and modification operations are not required, but focus is placed on navigation within the navigation area and compliance with safety constraints. The risk boundary is allowed to be adjusted according to specific requirements to meet safety constraints. The method first adjusts the attenuation coefficient of risk-adjustable CBF for ships navigating near shoals, icebergs, underwater fishing nets, and reefs, where the variable area of the restricted channel is affected by factors such as tides. When the attenuation coefficient is large, the keel clearance is also large, and vice versa.

[0109] Human-like model predictive control (HMPC) creates a comprehensive control strategy by incorporating human driving characteristics and decision-making processes into the MPC system to improve its performance and adaptability. This can enhance system safety and human-machine collaboration and open up possibilities for more intelligent control systems. The method detects whether the ship has a risk of colliding with other vehicles or obstacles. Only when necessary will appropriate action be taken, making it easier to drive without any problems. The method learns from human behavior and seeks solutions from a higher level within the appropriate constraint range, proposing a human-like control method with uncertainty awareness. Factors such as energy consumption, navigation risk, operational workload, and tracking error are considered when switching between different operating states to achieve optimal driving performance. The dynamic adjustment process of the human driver includes smooth transitions between safety control and tracking control modes, as well as optimization of the online switching strategy. Human-like model predictive control (HMPC) creates a comprehensive control strategy by incorporating human driving characteristics and decision-making processes into the human-like model predictive control system, thereby improving its performance and adaptability. This can enhance system safety and human-machine collaboration and open up possibilities for more intelligent control systems. The method detects whether the ship has a risk of colliding with other vehicles or obstacles. Only when necessary will appropriate action be taken, making it easier to drive without any problems.

[0110] In complex and emergency scenarios, slight constraint violations and tracking errors are tolerable. Frequent refreshing of control laws and modification of operations are not needed even in the presence of some observations and tracking errors. Unlike the general objective and cost function that focus on reaching the destination and minimizing tracking errors, it derives acceptable control inputs through constraint relaxation.

[0111] Under normal environmental conditions, the ship should remain stable under control inputs, avoiding sudden acceleration or deceleration. However, during collision avoidance, restrictions on factors such as acceleration can be relaxed. Therefore, it may not be necessary to strictly adhere to the constraints. After specifying the starting and ending points, human ship operators do not need to engage in high-frequency refreshing of control laws and modification of operations, such as acceleration, deceleration, turning, etc. For example, in activities such as yacht sightseeing tours, strict route specifications and precise trajectory tracking control are not required, and the route can be adjusted.

[0112] As shown in Figure 2 , the method specifically includes the following steps:

[0113] Step 1: Establish a ship model;

[0114]

[0115]

[0116]

[0117] η=[x,y,ψ] T

[0118] υ=[u,v,r] T

[0119] where η is the position and attitude vector, is the first derivative of the position and attitude vector, R is the rotation matrix, υ is the velocity vector, is the first derivative of the velocity vector, M is the inertia matrix, C is the Coriolis force and centripetal force matrix of the rigid body, D is the damping matrix, τ is the input force and torque, τ d is the disturbance, u is the forward speed, v is the lateral speed, r is the yaw rate, x is the forward position, y is the lateral position, and ψ is the yaw angle.

[0120] The disturbance is caused by wind and waves;

[0121] τ d =F w +F s

[0122] F w =[F wx ,F wy ,Nw ] T

[0123] F s = [F sx , F sy , N s ] T

[0124] where F w is the wind disturbance, F s is the wave disturbance, F wx is the wind disturbance along the forward direction, F wy is the wind disturbance along the lateral direction, N w is the wind disturbance along the yaw direction, N s is the wave disturbance along the yaw direction, F sx is the wave disturbance along the forward direction, F sy is the wave disturbance along the lateral direction.

[0125]

[0126] where C X is the longitudinal wind coefficient, C Y is the lateral wind coefficient, C N is the wind moment coefficient, A f is the above-water projected area, A s is the side projected area, L oa is the total length of the ship, p a is the air density, V r is the relative velocity.

[0127] The encounter frequency ω e is:

[0128]

[0129] where ω w is the wave frequency, v w is the wave velocity, χ is the encounter angle, and g is the gravitational acceleration.

[0130] The disturbance force and moment of each sub-wave are:

[0131]

[0132] where X si is the disturbance force of the i-th wave in the longitudinal direction, Y si is the disturbance force of the i-th wave in the lateral direction, N si is the disturbance moment of the i-th wave in the bow direction, ζ ai is the amplitude of each harmonic wave in the random wave, T is the draft, B is the waterline width, A is the ship cross-sectional area, and k1, k2 are set coefficients.

[0133] The wave disturbance is:

[0134]

[0135] where n represents the number of wavelets.

[0136] Substitute into to obtain:

[0137]

[0138] The system state equation is expressed as:

[0139]

[0140] where, is a unit matrix,

[0141] The expected pose vector η d is denoted as:

[0142] η d = [x d , y d , ψ d ] T

[0143] The pose vector error η e and the pose vector derivative error is denoted as:

[0144]

[0145] where x d is the expected longitudinal position, y d is the expected lateral position, ψ d is the expected heading angle, and η e is the tracking error of the position vector.

[0146] The state equation of the tracking error system is:

[0147]

[0148] where,

[0149] where the force and torque vector U = τ, and the purpose of control is to calculate the force and torque vector U.

[0150] Step 2: Construct a risk-adjustable CBF for complex navigation obstacles.

[0151] The CBF method uses Lyapunov-like parameters to ensure the invariance of the desired safety set. Distance-based CBF constraints are designed to guarantee that collisions between ships and obstacles are avoided.

[0152] Given a closed set C k ,

[0153] Among them, X k This represents the X value of the k-th time series. Representing an n-dimensional real-time space, h(X) k ) represents the control barrier function for the k-th time series.

[0154] If K exists ∞ If a class function α makes the following equation hold, then the function h is a control barrier function (CBF):

[0155] infh(X k+1 )-h(X k )≥-α(h(X k ))

[0156] Here, inf represents the infimum.

[0157] The conditions for CBF are defined as follows:

[0158] Δh(X k )=h(X k+1 )-h(X k )≥-γh(X k )

[0159] Wherein, Δh(X) k ) represents the difference of h, and γ>0 represents the attenuation coefficient.

[0160] Complex navigation obstacles can be represented by the union of the sets of reachable points.

[0161] When navigating near shoals, glaciers, underwater fishing nets, and reefs, the variable area of ​​the restricted navigation zone is described and adjusted using risk-adjustable risk factor (CBF) to address different obstacles. Examples of CBFs for preventing ship grounding include... Figure 3 As shown. Figure 3 The horizontal plane in the diagram represents the sea level at different tidal times. The intersection of the hemisphere and the horizontal plane represents the unsafe area. Safe zones near shoals are affected by tides. At low tide, the restricted navigation area is larger. At high tide, the restricted navigation area is smaller.

[0162] The size and draft of the vessel, the depth of the waterway and port, hydrographic and tidal conditions, the geography and topography of the water area, and weather and climate conditions are important factors that affect the navigation clearance depth. Large vessels with a larger draft generally require a deeper water depth to navigate safely, while smaller vessels with a shallower draft may be able to navigate in shallower waters. Vessels need to consider the depth restrictions imposed by the waterway and port to ensure their safe passage. Changes in tidal water levels can cause changes in water depth, affecting the navigation clearance depth of vessels. Changing hydrographic and tidal conditions require vessels to adjust their trajectory and speed in real time to ensure safe navigation at the appropriate water depth. Geographic features such as shoals, reefs, and islands impose restrictions on vessel navigation, affecting the required navigation clearance depth. Adverse weather conditions such as typhoons and storms can cause water levels to rise, affecting the navigation clearance depth of vessels. Vessels should adhere to the prescribed navigation clearance depth while considering variables such as vessel type, draft, speed, seabed conditions, and shoals. Vessels should adhere to the following guidelines based on their actual draft to ensure sufficient navigation clearance depth: for vessels with a draft less than 5 meters, the navigation clearance depth should be no less than 0.4 meters; for vessels with a draft less than 7 meters and not less than 5 meters, the navigation clearance depth should be no less than 0.5 meters; for vessels with a draft less than 9.7 meters and not less than 7 meters, the navigation clearance depth should be no less than 0.7 meters; for vessels with a draft between 9.7 meters and 10.5 meters, the navigation clearance depth should be no less than 0.8 meters; for vessels with a draft greater than 10.5 meters, the navigation clearance depth should be no less than 10% of their draft; for vessels carrying dangerous goods, the navigation clearance depth is increased by 0.1 meters; for vessels with a speed exceeding 12 knots, the navigation clearance depth is increased by 0.1 meters; when passing through shallow water areas, calculate the minimum safe water depth to determine whether the vessel can safely pass through; the formula for calculating the minimum safe water depth of a vessel is: draft + salinity difference + rapid ship stern trim + draft increase due to height + half wave height + reserve depth - draft reduction due to fuel consumption.

[0163] The constrained optimization problem in HMPC is composed of appropriate terminal invariant sets, system constraints, and optimization indices. When the system detects that the distance between the ship and the surrounding obstacles is less than a certain threshold, it controls its trajectory and increases the distance until the distance is not less than the safety threshold. The future dynamics of the system are predicted, and the t k The lower MPC constrained optimization problem is solved. When the new sampling time t k+1 is reached, the new sampling state is used as the initial state to construct the MPC constrained optimization problem at this new sampling time. It is necessary to incorporate motion constraints into the optimization process of MPC while ensuring invariance in the set. The feasibility of MPC-CBF depends on whether there is an intersection between the feasible set of each step and the upper set of CBF inequalities.

[0164] Step 3: When the system detects a general navigation risk, i.e., the distance between the ship and the surrounding obstacles is less than a safety threshold, the attenuation coefficient of the risk-adjustable CBF is adjusted for different obstacles, and a constructed quadratic programming problem QP1 is solved, the ship trajectory is controlled and the distance is increased based on the planning result until the distance is not less than the safety threshold; then the system resumes free driving.

[0165] At time t k , the objective function J in the MPC constraint optimization problem is:

[0166]

[0167] subjectto

[0168] Δh(X k )=h(X k+1 )-h(X k )≥-γh(X k )

[0169] X e ∈X o

[0170] U∈U o

[0171] where t is the integral variable, t k is the kth time sequence, N p > 0 is the prediction interval time length range, X e (t) is the prediction of the system state error at time t k , Q is the weight matrix of X e (t), and R1 is the weight positive definite matrix of U(t); represents the square of the 2-norm of the vector, where U i represents the ith element of the vector U, and R 1i represents the ith element on the diagonal of the matrix R1; represents the square of the 2-norm of the vector, where X ei represents the ith element of the vector X e , and Q i represents the ith element on the diagonal of the matrix Q; X o is the set of system states; U o is the set of system control input ranges, and X e (t) is the prediction of the system state error at time t k .

[0172] Step 4: When the system detects an imminent danger, relax the constraints based on the quadratic programming problem QP1, solve the quadratic programming problem QP2, and obtain an acceptable control input by constraint relaxation.

[0173] In the sudden complex and emergency situation, slight violation of constraints and tracking errors can be tolerated. Frequent refreshing of control law and modification of operation are not required even with some observation and tracking errors. Unlike the general problem where the objective and cost function focus on reaching the destination and minimizing tracking errors, it derives acceptable control inputs through constraint relaxation. If the control input resulting from solving the original QP1 and satisfying inequality constraints is infeasible, the control input of constraint relaxation is computed by solving another quadratic programming problem QP2 of safety control problem. Under normal environmental conditions, the ship should remain stable with constraints. However, during collision avoidance, the restrictions on inputs and states can be relaxed. Therefore, it can not be necessary to strictly follow the X o and U o .

[0174]

[0175] subjectto

[0176] Δh(X k )=h(X k+1 )-h(X k )≥-γh(X k )

[0177] X e ∈X Rel

[0178] U∈U Rel

[0179] where X Rel is the set of states after constraint relaxation, is a subset of X Rel ; U Rel is the set of control inputs after constraint relaxation, is a subset of U Rel .

[0180] Step 5: If the ship does not exist the threat of navigation and is in a safe state, solve the event-triggered quadratic programming problem QP3, only when the set conditions are reached to intervene control.

[0181] After the starting point and the end point are specified, the human ship operator does not need to perform high-frequency refreshing of control law and modification of operation, such as acceleration, deceleration and turning. For example, in activities such as yacht sightseeing, strict route specification and accurate trajectory tracking control are not required, and the route can be adjusted.

[0182]

[0183] subjectto

[0184] X e ∈X o

[0185] U∈U o

[0186]

[0187] where P is the weight matrix of X e (t k +N p ), s is some time between t k , t k +N p , ε is a set coefficient, and α∈(0,1) is a constant.

[0188] The order of the instant of event-triggered is 0≤t1 k <…t k <…; the time when the deviation between the actual state X(s;t * ) and the predicted state X k (s;t ∞ ) reaches the threshold value is:

[0189]

[0190] where L is the Lipschitz constant of f, ρ is the maximum value of the infinite norm of the disturbance, i.e. ||ω|| p ≤ρ, β∈(0,N o ) is a constant, is the maximum eigenvalue.

[0191] The event-triggered strategy is as follows:

[0192]

[0193] When tracking the "8" trajectory and avoiding the shallow water located at (-6m, -14m), the control results are shown in FIG. 8. The decay coefficient is γ = 0.1. The main control parameters are Figures 4-7

[0194] The initial position is (0, 0), the prediction horizon is Np = 15, the obstacle is located at (xo, yo) = (-6, -14), and the safety set is defined as h(x, y) = (x-x 2 ) o +(y-y 2 )

[0195] ​The embodiment also provides a human-like constrained adaptive model predictive control system with a risk-adjustable control barrier function, comprising a memory, a processor, and computer program instructions stored in the memory and capable of being executed by the processor, and when the processor executes the computer program instructions, the above method can be implemented.

[0196] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, etc.) containing computer-usable program code.

[0197] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus produce a device implemented in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 means for performing the function specified by the one or more blocks.

[0198] These computer program instructions can also be stored in a computer-readable memory capable of causing a computer or other programmable data processing apparatus to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including instruction means, which implements the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 one or more blocks.

[0199] These computer program instructions can also be loaded into a computer or other programmable data processing apparatus, so that a series of operation steps are performed on the computer or other programmable data processing apparatus to produce a computer-implemented process, so that the instructions executed on the computer or other programmable data processing apparatus provide a process for implementing the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 one or more blocks.

[0200] The above merely describes preferred embodiments of the present application, but does not limit the present application in other forms, and any skilled person in the art can modify or change the above disclosed technical content into equivalent embodiments with equivalent changes. However, any simple modification, equivalent change and modification made on the above embodiments without departing from the technical solution content of the present application and according to the technical essence of the present application still belongs to the protection scope of the technical solution of the present application.

Claims

1. A human-like constrained adaptive model predictive control method with risk-adjustable control barrier function, characterized in that, The method comprises the following steps: Step 1: establishing a ship dynamics model; Step 2: constructing a risk-adjustable control barrier function for complex navigation obstacles; Step 3: when the system detects general navigation risks, i.e., the distance between the ship and the surrounding obstacles is less than a safety threshold, adjusting the decay coefficient of the risk-adjustable control barrier function for different obstacles, and solving a constructed quadratic programming problem QP1, controlling the ship trajectory and increasing the distance based on the programming result until the distance is not less than the safety threshold; Then the system resumes free driving; Step 4: when the system detects imminent danger, relaxing the constraints on the basis of the quadratic programming problem QP1, solving a quadratic programming problem QP2, and obtaining acceptable control input through constraint relaxation; Step 5: if the ship does not exist navigation threat and is in a safe state, solving an event-triggered quadratic programming problem QP3, and intervening in control only when a set condition is reached; In step 2, the specific method for constructing the risk-adjustable CBF for complex navigation obstacles is as follows: The CBF constraint is designed based on distance to ensure that no collision occurs between the ship and the obstacles; provided with a closed set C k , where X k represents the X value of the kth time series, represents an n-dimensional real-time space, h(X k ) represents the control barrier function of the kth time series; If there exists K ∞ A function h is a control barrier function CBF if the following holds for a class of functions a: infh(X k+1 )-h(X k )≥-α(h(X k )) Where inf represents the lower limit; The condition of the CBF is defined as follows: Δh(X k ) = h(X k+1 ) - h(X k ) ≥ -γh(X k ) where Δh(X k ) denotes the difference of h, and γ > 0 denotes an attenuation coefficient; The complex navigation obstacles are represented by the union set of reachable points; For different obstacles, the risk-adjustable CBF is used to describe and adjust the variable area of the restricted navigation area; In step 3, the quadratic programming problem QP1 is specifically as follows: At time t k The objective function J in the MPC constrained optimization problem at time t is: subject to Δh(X k ) = h(X k+1 ) - h(X k ) ≥ -γh(X k ) where t is the integral variable, t k is the kth time series, N p > 0 is the prediction horizon range, X e (t) is the prediction of the system state error at time t k , Q is the weight matrix of X e (t), and R1 is the weight positive definite matrix of U(t). denotes the square of the 2-norm of a vector, where U i denotes the ith element of the vector U, R 1i denotes the ith diagonal element of the matrix R1. denotes the square of the 2-norm of a vector, where X ei denotes the ith element of the vector X e , Q i denotes the ith diagonal element of the matrix Q. is a set of system states; is a set of system control input ranges, X e (t) is the prediction of the system state error at time t k . In step 4, the quadratic programming problem QP2 is specifically as follows: subject to Δh(X k ) = h(X k+1 ) - h(X k ) ≥ -γh(X k ) wherein, is the set of states after constraint relaxation, is a subset of ; is the set of control inputs after constraint relaxation, is a subset of .

2. The human-like constrained model predictive control method with risk-adjustable control barrier function according to claim 1, wherein, In step 1, the ship model is established as follows: η = [x, y, ψ] T u = [u, v, r] T where η is the position and orientation vector, is the first derivative of the position and orientation vector, R is the rotation matrix, and υ is the velocity vector, is the first derivative of the velocity vector, M is the inertia matrix, C is the Coriolis and centripetal force matrix of the rigid body, D is the damping matrix, τ is the input force and moment, τ d is the disturbance, u is the forward velocity, v is the lateral velocity, r is the yaw angular velocity, x is the forward position, y is the lateral position, and ψ is the yaw angle. The disturbance is caused by wind and waves; τ d = F w + F s F w = [F wx , F wy , N w ] T F s = [F sx , F sy , N s ] T where F w is the wind disturbance, F s is the wave disturbance, F wx is the wind disturbance in the forward direction, F wy is the wind disturbance in the lateral direction, N w is the wind disturbance in the yaw direction, N s is the wave disturbance in the yaw direction, F sx is the wave disturbance in the forward direction, F sy is the wave disturbance in the lateral direction; where C X is the longitudinal wind coefficient, C Y is the lateral wind coefficient, C N is the wind moment coefficient, A f is the waterplane projected area, A s is the side projected area, L oa is the total length of the ship, p a is the air density, V r is the relative velocity; The encounter frequency ω e is: Where, ω w v is the frequency of the wave. w Let be the wave velocity, χ be the encounter angle, and g be the gravitational acceleration; The disturbance force and torque of each sub-wave are as follows: wherein X si is the disturbance force of the i-th wave in the longitudinal direction, Y si is the disturbance force of the i-th wave in the transverse direction, N si is the disturbance moment of the i-th wave in the bow direction, ζ ai is the amplitude of each harmonic wave in the random wave, T is the draft, B is the waterline width, A is the cross-sectional area of the ship, and k1 and k2 are set coefficients. The wave disturbance is as follows: Where n represents the number of sub-waves; Substituting into gives: The system state equation is expressed as: wherein I is the identity matrix, Desired pose vector η d Noted as: η d = [x d , y d , ψ d ] T pose vector error η e with pose vector derivative error denoted by: where x d is the desired longitudinal position, y d is the desired lateral position, ψ d is the desired heading angle, and η e is the tracking error of the position vector; The state equation of the tracking error system is as follows: wherein wherein the control aims at calculating the force and torque vector U = τ.

3. The human-like constrained model predictive control method with risk-adjustable control barrier function according to claim 1, wherein, The ship should follow the following criteria according to its actual draft to ensure sufficient navigation clearance depth: for a ship with a draft less than 5 meters, the navigation clearance depth should be not less than 0.4 meters; for a ship with a draft less than 7 meters and not less than 5 meters, the navigation clearance depth should be not less than 0.5 meters; for a ship with a draft less than 9.7 meters and not less than 7 meters, the navigation clearance depth should be not less than 0.7 meters; for a ship with a draft between 9.7 m and 10.5 m, the navigation clearance depth should be not less than 0.8 m; for a ship with a draft greater than 10.5 m, the navigation clearance depth should be not less than 10% of the draft; for a ship carrying dangerous goods, the navigation clearance depth is increased by 0.1 m; for a ship with a speed exceeding 12 knots, the navigation clearance depth is increased by 0.1 m; when passing through a shallow water area, the minimum safe water depth is calculated to determine whether the ship can safely pass through; the minimum safe water depth calculation formula of the ship is: draft + salinity difference + rapid ship stern trim + draft increased due to height + half wave height + reserve depth - draft reduced due to fuel consumption.

4. The human-like constrained model predictive control method with risk-adjustable control barrier function according to claim 1, wherein, In step 5, the quadratic programming problem QP3 is specifically as follows: subject to where P is X e (t k +N p ) the weight matrix, s is at some time between time interval (t k ,t k +N p ], and ε is a set coefficient, and a e (0, 1) is a constant. The sequence of the instant of the event trigger is 0≤t1<t2<…t k …; actual state X(s; t k ) and the predicted state X * (s; t k ) reaches the threshold value is: where L is the Lipschitz constant of f, p is the maximum of the infinite norm of the perturbation, i.e. ||ω||∞ ∞ ≤ p, β ∈ (0, N p ) is a constant, is the largest eigenvalue; The event-triggered strategy is as follows:

Images