Multi-tug hierarchical model predictive control autonomous accompanying control method

Abstract

The invention discloses a multi-tug hierarchical model predictive control autonomous accompanying control method. A tug dynamic model is constructed, and kinematics constraints meeting accompanying targets and safety problems in multi-tug cable accompanying are constructed based on the model; and further constructing a layered MPC architecture comprising a multi-constraint MPC trajectory planner and an MPC trajectory tracking controller considering a tug double-full-rotation propulsion mode, planning a reference trajectory sequence of each tug through the multi-constraint MPC trajectory planner, and completing tracking of the reference trajectory sequence through the MPC trajectory tracking controller. According to the method, the problem of initial solution conjecture under multiple security constraints is solved through a preposed pre-optimization mechanism; a trajectory tracking controller is designed by considering the characteristics of the double-full-rotation propellers, and the tugs are controlled to track the safe trajectory planned by the top layer, so that safe multi-tug cable autonomous accompanying control is realized, and the problem of accompanying control caused by the double-full-rotation characteristics of the tugs is solved.

Description

Technical Field

[0001] The present invention relates to a technology in the field of ship control, in particular to an autonomous escort control method for multi-tugboat layered model predictive control. Background Art

[0002] With the vigorous development of maritime shipping, the number of ships sailing in the waters and waterways near ports has become more and more dense, and the number of ship loss-of-control incidents has increased year by year, posing a major threat to the port's navigation efficiency and navigation safety. The existing technology uses multiple tugboats to escort cargo ships in a cabled state to deal with emergency situations during the process of entering the port. However, the multi-tugboat cabled escort navigation mission needs to consider the restrictions on the tugboat movement caused by the cabled state, which adds a variety of kinematic safety constraints to the multi-tugboat cabled autonomous escort control problem, making the traditional autonomous formation control method inapplicable. At the same time, the existing technology mainly uses the generalized control force composed of the hull longitudinal force, lateral force and bow turning moment as the control quantity, and the tugboat achieves navigation by controlling the thrust and rotation angle of the dual full-turn thrusters. Therefore, the combination of the control method and the dual full-turn thruster propulsion mode still needs to be further studied. Summary of the invention

[0003] In view of the shortcomings of the prior art that the safety of tugboat navigation and the optimality of the preparatory operation position cannot be ensured during the autonomous escort of multiple tugboats with cables, the present invention proposes a multi-tugboat hierarchical model predictive control autonomous escort control method, considers the safety constraints caused by towboats and constructs a trajectory planner based on a multi-constraint model predictive controller (MPC) to plan a safe tugboat escort trajectory, and solves the initial solution guessing problem under multiple safety constraints through a pre-optimization mechanism; considers the characteristics of dual full-turn thrusters to design a trajectory tracking controller, controls the tugboat to track the top-level planned safe trajectory, thereby realizing safe multi-tugboat autonomous escort control with cables, and solves the problem of escort control caused by the dual full-turn characteristics of the tugboats.

[0004] The present invention is achieved through the following technical solutions:

[0005] The present invention relates to a multi-tugboat hierarchical model predictive control autonomous escort control method, which constructs a tugboat dynamics model and constructs kinematic constraints that meet the escort objectives and safety issues in the multi-tugboat cable escort based on the model; further constructs a hierarchical MPC architecture including a multi-constraint MPC trajectory planner and an MPC trajectory tracking controller considering the tugboat dual full-turn propulsion mode, plans a reference trajectory sequence of each tugboat through the multi-constraint MPC trajectory planner, and completes the tracking of the reference trajectory sequence through the MPC trajectory tracking controller.

[0006] The tugboat dynamics model is: Where: η = [x, y, ψ] Tis the position state of the tugboat in the earth coordinate system, x, y, ψ are the position coordinates and heading angle respectively, ν=[u,v,r] T is the velocity state of the tugboat in the hull coordinate system, u, v, r are the longitudinal velocity, lateral velocity and bow angular velocity respectively, and R(ψ) is the conversion matrix between the velocity of the earth coordinate system and the velocity of the hull coordinate system.

[0007] The transformation matrix Where: M is the inertial mass matrix, C is the Coriolis centripetal force matrix, D is the hydrodynamic damping matrix, τ = [τ u ,τ v ,τ r ] T is the generalized control force vector, where: τ u , τ v , τ r They represent longitudinal force, lateral force and bow moment respectively.

[0008] The inertial mass matrix Coriolis Centripetal Force Matrix Hydrodynamic Damping Matrix Where: m t is the mass of the tugboat, is the additional mass, X uu , X vv , X vr , X rr ,Y v , Y r , N v , N r , Y |v|v , Y |v|r , Y |r|r , N vvr , N |r|r , N vrr is the hydrodynamic coefficient.

[0009] The escort target in the multi-tugboat cable escort, i.e. the optimal emergency preparedness state, is specifically: during the escort process, the tugboat must always be located on one side of the large ship to avoid collision between the tow cable and the large ship. At the same time, the tugboat should be located at the set formation position as much as possible to ensure that it can enter the operating state as quickly as possible when emergency operations are required.

[0010] The security issues described include:

[0011] ① Internal collision avoidance: During escort, tugboats should always avoid collision with large ships and other tugboats in the formation.

[0012] ② Avoid obstacles inwards: During the escort process, the tugboat should avoid external obstacles. Due to the towline connection, the tugboat must avoid obstacles inwards of the formation to prevent the towline from colliding with obstacles.

[0013] ③ Towline slack: During escort, the towline should not be stretched tight, that is, the distance between the tugboat and the mooring point of the large ship should be less than the set maximum cable length. Technical Effects

[0014] The present invention introduces the hierarchical model predictive control of the tugboat's cable escort safety constraint and the tugboat's dual full-rotation thruster propulsion mode. Compared with the prior art, the present invention ensures that the tugboat meets the kinematic safety constraint introduced by the towing cable during the escort process while considering the tugboat's dual full-rotation thruster propulsion mode, thereby ensuring the safety of the tugboat's cable escort system while putting the tugboat in the best emergency preparedness state. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a flow chart of the present invention;

[0016] Figure 2 It is a diagram of the hierarchical MPC architecture;

[0017] Figure 3 This is a simulation diagram of the tugboat trajectory curve based on the hierarchical MPC architecture;

[0018] Figure 4 It is a simulation diagram of the error curve between the tugboat position and the optimal reference formation;

[0019] Figure 5 It is a simulation diagram of the relaxation constraint curve of the tow cable based on the hierarchical MPC architecture;

[0020] Figure 6 This is the simulation diagram of the thrust curve of the tugboat propeller;

[0021] Figure 7 This is a simulation diagram of the tugboat thruster rotation angle curve. DETAILED DESCRIPTION

[0022] like Figure 1 As shown, this embodiment involves a multi-tugboat hierarchical model predictive control autonomous escort control method, by constructing a tugboat dynamics model and constructing kinematic constraints based on the model to meet the escort objectives and safety issues in the multi-tugboat cable escort; further constructing Figure 2 The hierarchical MPC architecture shown includes a multi-constrained MPC trajectory planner and an MPC trajectory tracking controller considering the dual full-turn propulsion mode of the tugboat. The reference trajectory sequence of each tugboat is planned by the multi-constrained MPC trajectory planner, and the reference trajectory sequence is tracked by the MPC trajectory tracking controller.

[0023] After specific practical experiments, the thruster parameters are set as shown in Table 1.

[0024] Table 1 Main parameter settings

[0025] like Figure 2 As shown, the hierarchical MPC architecture includes: a top-level multi-constraint MPC trajectory planner and a bottom-level MPC trajectory tracking controller (Azimuth thruster-based MPC, ATMPC) that considers the full-turn propulsion characteristics of the tugboat, wherein: the multi-constraint MPC trajectory planner uses a relatively long prediction field of view to complete trajectory planning, and the MPC trajectory tracking controller uses a shorter prediction field of view to complete trajectory tracking, and the top-level planned trajectory is matched with the bottom-level control cycle through cubic Hermite interpolation.

[0026] The multi-constrained MPC trajectory planner is constructed in the following way:

[0027] Step 1) Construct the model prediction equation of multi-constraint MPC: For the i-th tugboat, the position and speed information X ui =[x ui ,y ui ,ψ ui ,u ui ,v ui ,r ui ] T As a state variable, the speed change U ui =[Δu ui ,Δv ui ,Δr ui ] T As the control variable, the kinematic equations in the tugboat dynamics model are discretized based on the forward Euler method to obtain the model prediction equation:

[0028] in: (·)(k+m|k) represents the predicted value at time k for time k+m, which is abbreviated as (·) m (k), and the complete prediction sequence at time k is recorded as

[0029] Step 2) Construct a multi-constrained MPC rolling optimization problem, that is, whether a local obstacle is detected; solve the optimal reference trajectory based on the pre-optimization method

[0030] The multi-constrained MPC rolling optimization problem is: as well as in: Maintain the objective function for the optimal job location, is the collision avoidance objective function within the tugboat, is the objective function for the optimal preparation cable length, The objective function for avoiding external obstacles is the kinematic constraints for safety problems, which include: velocity constraint and velocity increment constraint One-side limited half-space safety restraint Towline slack safety restraint Inward obstacle avoidance ring constraint and half-space constraint The superscripts u and l represent the top-level controller and the bottom-level controller respectively.

[0031] The optimal operation position maintenance objective function in: Predicted position η for the tugboat ui The best working position The error between According to the set formation and large ship position η ship Calculate, Q u is the set weight matrix.

[0032] The objective function of avoiding collision within the tugboat is in: The tugboats in the formation are in the collision avoidance group. The tugboats behind the large ship in the formation have a lower priority and avoid collision with the tugboats on the same side in front. is the distance between the corresponding tugboats, D t =2L i is the collision avoidance radius inside the tugboat, w oT is the set weight.

[0033] The optimal preparatory operation cable length objective function in: is the distance between tugboat i and the corresponding mooring point, w ic To set the weight, the subscript b is the mooring point.

[0034] The external obstacle avoidance objective function Where: N o is the number of obstacles locally perceived by the tugboat on the same side of the large vessel as the tugboat, is the distance between the tugboat and the obstacle, D oj =L oj +L i , L oj is the radius of the obstacle, w oo is the set weight, and subscript o is the obstacle.

[0035] The speed constraint and speed increment constraint means:

[0036] The one-side limited half-space safety constraint means: in: in: By setting the relative distance D p With η ship OK. shipm (k), are the position coordinates of the escort target and the tugboat, respectively. This constraint ensures that the tugboat is always located on one side of the large ship. The subscript p is the half space, and the subscript ship is the escort target ship.

[0037] The towline slack safety restraint means: Where: R oc The maximum cable length is set.

[0038] The inward obstacle avoidance ring constraint and half-space constraint Includes: Inward obstacle avoidance circular constraint and the inward obstacle avoidance half-space constraint Where: d obj (k) is the distance between the obstacle and the corresponding mooring point, is the intersection of the line connecting the mooring point and the obstacle center and the obstacle boundary, p oj (k) is the coordinate of the obstacle, The obstacle boundary passes P bm The tangent vector of the half-space

[0039] The method based on pre-optimization is to set a feasible formation and a position sequence based on the formation and the accompanying target ship. Calculate the pre-optimized reference trajectory sequence Constructing a pre-optimization problem in: is the tugboat position predicted by the model With the pre-optimized reference trajectory sequence The error between the two. The pre-optimization problem is solved by taking the zero control amount as the initial guess of the pre-optimization problem, and then the optimal solution of the pre-optimization problem is As a feasible initial solution guess for multi-constrained MPC, the subscript f refers to the pre-optimization.

[0040] The pre-optimization is an approximate solution to maximize feasibility, and thus can guarantee the initial feasibility of the multi-constrained MPC to a large extent.

[0041] The MPC trajectory tracking controller considering the tugboat dual full-turn propulsion mode is constructed in the following way:

[0042] Step a) Constructing the ATMPC prediction model: Y li (k+m|k)=C li X li (k+m|k), where: P ξ =[I 6×6 ,0 6×4 ], P δ =[0 4×6 ,I 4×4 ],P f =[0 2×6 ,I 2×2 ,0 2×2 ],C li =[I 3×3 ,0 3×10 ], T l is the control period, D' is the hydrodynamic damping matrix ignoring high-order hydrodynamic terms, is the dynamic state transfer matrix of the tugboat, is the dynamic input matrix of the tugboat, C li is the output matrix, P ξ , P δ , P f Select the matrix for the state quantity, is the state variable, is the control variable, the subscripts l and r represent the left and right thrusters of the tugboat respectively, and the motion state ξ li =[x li ,y li ,ψ li ,u li ,v li ,r li ], thruster manipulated variables

[0043] Step b) Construct the ATMPC rolling optimization problem: Among them: J li is the optimization objective function of the ATMPC problem, for T l is the sequence obtained by periodic discretization, Depend on T lThe interval is obtained based on piecewise cubic Hermite interpolation, and the constraints are: the upper and lower limits of the change of the thrust and angle of the propeller set by the user, and the subscript m represents the time.

[0044] Step c) The ATMPC rolling optimization problem in step b is solved in real time by the casadi solver, and the first component of the optimal control quantity sequence is used as the control input at the current moment to control the tugboat to complete trajectory tracking.

[0045] like Figure 3 As shown in the figure, the simulation is performed in MATLAB software with the main parameters in Table 1. It can be seen that the tugboat formation realizes the cable escort task under the action of the designed controller, and each tugboat can avoid obstacles inward during the escort process and return to the relatively optimal preparatory operation position after avoiding obstacles.

[0046] like Figure 4 As shown, after the tugboat formation changes to avoid obstacles, it can converge back to the original formation in time and continue to accompany the tugboat formation.

[0047] like Figure 5 As shown, compared with the prior art which cannot ensure that the tugboat always satisfies the tow cable relaxation constraint during the process of navigating with a cable, the present method can ensure the satisfaction of the constraint in a suitable navigating with a cable scenario.

[0048] like Figure 6 As shown in the figure, both the saturation constraint and the incremental constraint of the thruster are satisfied, indicating that the method proposed in this paper can complete the multi-tugboat cable escort control under the consideration of the propulsion mode of the tugboat's dual full-rotation thrusters and the thrust and rotation angle constraints of the thrusters.

[0049] like Figure 7 As shown in the figure, both the saturation constraint and the incremental constraint of the thruster angle are satisfied, indicating that the method proposed in this paper can complete the multi-tugboat cable escort control under the consideration of the propulsion mode of the tugboat's dual full-rotation thrusters and the thrust and rotation angle constraints of the thrusters.

[0050] Compared with the prior art, the present invention can derive the thrust, angle and increment of the thrusters to ensure that the tugboat can complete the autonomous cable-guided navigation task while taking into account the propulsion mode of the tugboat's dual full-rotation thrusters, and ensure that the tugboat meets the kinematic safety constraints introduced by the cables during the navigation process, thereby ensuring the safety of the tugboat's cable-guided navigation system while placing the tugboat in the best emergency preparedness state.

[0051] The above-mentioned specific implementation can be partially adjusted in different ways by those skilled in the art without departing from the principle and purpose of the present invention. The protection scope of the present invention shall be based on the claims and shall not be limited by the above-mentioned specific implementation. Each implementation scheme within its scope shall be subject to the constraints of the present invention.

Claims

1. A multi-tugboat hierarchical model predictive control autonomous escort control method, characterized in that: By constructing a tugboat dynamics model and constructing kinematic constraints based on the model to meet the escort objectives and safety issues in multi-tugboat cable escort, a hierarchical MPC architecture is further constructed, which includes a multi-constrained MPC trajectory planner and an MPC trajectory tracking controller considering the tugboat's dual full-turn propulsion mode. The reference trajectory sequence of each tugboat is planned by the multi-constrained MPC trajectory planner, and the reference trajectory sequence is tracked by the MPC trajectory tracking controller. The tugboat dynamics model is: Where: η = [x, y, ψ] T is the position state of the tugboat in the earth coordinate system, x, y, ψ are the position coordinates and heading angle respectively, ν=[u,v,r] T is the velocity state of the tugboat in the hull coordinate system, u, v, r are the longitudinal velocity, transverse velocity and bow angular velocity respectively, and R(ψ) is the conversion matrix between the velocity of the earth coordinate system and the velocity of the hull coordinate system; The transformation matrix Where: M is the inertial mass matrix, C is the Coriolis centripetal force matrix, D is the hydrodynamic damping matrix, τ = [τ u ,τ v ,τ r ] T is the generalized control force vector, where: τ u , τ v , τ r represent the longitudinal force, lateral force and bow moment respectively; The inertial mass matrix Coriolis Centripetal Force Matrix Hydrodynamic Damping Matrix Where: m t is the mass of the tugboat, is the additional mass, X uu , X vv , X vr , X rr ,Y v , Y r , N v , N r , Y |v|v , Y |v|r , Y |r|r , N vvr , N |r|r , N vrr is the hydrodynamic coefficient.

2. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 1 is characterized in that: The escort target in the multi-tugboat cable escort, that is, the optimal emergency preparedness state, is specifically: during the escort process, the tugboat must always be located on one side of the large ship to avoid collision between the tow cable and the large ship. At the same time, the tugboat should be located at the set formation position as much as possible to ensure that it can enter the operating state as quickly as possible when emergency operations are required.

3. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 1 is characterized in that: , the security issues include: ① Internal collision avoidance: During escort, tugboats should always avoid collision with large ships and other tugboats in the formation; ② Avoid obstacles inwards: During the escort process, the tugboat should avoid external obstacles. Due to the towline connection, the tugboat must avoid obstacles inwards of the formation to prevent the towline from colliding with obstacles; ③ Towline slack: During the escort process, the towline should not be tightened, that is, the distance between the tugboat and the mooring point of the large ship should be less than the set maximum cable length; The kinematic constraints include: speed constraints and speed increment constraints, unilaterally limited half-space safety constraints, tow cable relaxation safety constraints, and inward obstacle avoidance annular constraints and half-space constraints.

4. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 1 is characterized in that: The hierarchical MPC architecture includes: a top-level multi-constraint MPC trajectory planner and a bottom-level MPC trajectory tracking controller that takes into account the full-turn propulsion characteristics of the tugboat, wherein: the multi-constraint MPC trajectory planner uses a relatively long prediction field of view to complete trajectory planning, and the MPC trajectory tracking controller uses a shorter prediction field of view to complete trajectory tracking, and uses cubic Hermite interpolation to achieve matching between the top-level planned trajectory and the bottom-level control cycle.

5. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 1 or 4, characterized in that: The multi-constrained MPC trajectory planner is constructed in the following way: Step 1) Construct the model prediction equation of multi-constraint MPC: For the i-th tugboat, the position and speed information X ui =[x ui ,y ui ,ψ ui ,u ui ,v ui ,r ui ] T As a state variable, the speed change U ui =[Δu ui ,Δv ui ,Δr ui ] T As the control variable, the kinematic equations in the tugboat dynamics model are discretized based on the forward Euler method to obtain the model prediction equation: in: (·)(k+m|k) represents the predicted value at time k for time k+m; Step 2) Construct a multi-constrained MPC rolling optimization problem, that is, whether a local obstacle is detected; solve the optimal reference trajectory based on the pre-optimization method The multi-constrained MPC rolling optimization problem is: as well as in: Maintain the objective function for the optimal job location, is the collision avoidance objective function within the tugboat, is the objective function for the optimal preparation cable length, is the external obstacle avoidance objective function, speed constraint and speed increment constraint One-side limited half-space safety restraint Towline slack safety restraint Inward obstacle avoidance ring constraint and half-space constraint The superscripts u and l represent the top-level controller and the bottom-level controller, respectively; The method based on pre-optimization is to set a feasible formation and a position sequence based on the formation and the accompanying target ship. Calculate the pre-optimized reference trajectory sequence Constructing a pre-optimization problem in: is the tugboat position predicted by the model With the pre-optimized reference trajectory sequence The error between them is solved by taking the zero control amount as the initial guess of the pre-optimization problem, and then the optimal solution of the pre-optimization problem is As a feasible initial solution guess for multi-constrained MPC, the subscript f refers to the pre-optimization.

6. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 5 is characterized in that: The optimal operation position maintenance objective function in: Predicted position η for the tugboat ui and the best working position The error between According to the set formation and large ship position η ship Calculate, Q u is the set weight matrix; The objective function of avoiding collision within the tugboat is in: The tugboats in the formation are in the collision avoidance group. The tugboats behind the large ship in the formation have a lower priority and avoid collision with the tugboats on the same side in front. is the distance between the corresponding tugboats, D t =2L i is the collision avoidance radius inside the tugboat, w oT is the set weight; The optimal preparatory operation cable length objective function in: is the distance between tugboat i and the corresponding mooring point, w ic To set the weight, subscript b is the mooring point; The external obstacle avoidance objective function Where: N o is the number of obstacles locally perceived by the tugboat on the same side of the large vessel as the tugboat, is the distance between the tugboat and the obstacle, D oj =L oj +L i , L oj is the radius of the obstacle, w oo is the set weight, and subscript o is the obstacle.

7. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 5 is characterized in that: In the kinematic constraints described: Speed ​​Constraint and Speed ​​Increment Constraint means: One-side limited half-space safety restraint means: in: in: By setting the relative distance D p With η ship OK, p shipm (k), are the position coordinates of the escort target and the tugboat, respectively. This constraint ensures that the tugboat is always located on one side of the large ship. The subscript p is the half space, and the subscript ship is the escort target ship. Towline slack safety restraint means: Where: R oc is the maximum cable length set; Inward obstacle avoidance ring constraint and half-space constraint Includes: Inward obstacle avoidance ring constraint and the inward obstacle avoidance half-space constraint Where: d obj (k) is the distance between the obstacle and the corresponding mooring point, is the intersection of the line connecting the mooring point and the obstacle center and the obstacle boundary, p oj (k) is the coordinate of the obstacle, The obstacle boundary passes P bm The tangent vector of the half-space 8. The multi-tugboat hierarchical model predictive control autonomous escort control method according to claim 1 or 4, characterized in that: The MPC trajectory tracking controller considering the tugboat dual full-turn propulsion mode is constructed in the following way: Step a) Constructing the ATMPC prediction model: in: P ξ =[I 6×6 ,0 6×4 ],P δ =[0 4×6 ,I 4×4 ],P f =[0 2×6 ,I 2×2 ,0 2×2 ],C li =[I 3×3 ,0 3×10 ], T l is the control period, D' is the hydrodynamic damping matrix ignoring high-order hydrodynamic terms, is the dynamic state transfer matrix of the tugboat, is the dynamic input matrix of the tugboat, C li is the output matrix, P ξ , P δ , P f Select the matrix for the state quantity, is the state variable, is the control variable, the subscripts l and r represent the left and right thrusters of the tugboat respectively, and the motion state ξ li =[x li ,y li ,ψ li ,u li ,v li ,r li ], thruster manipulated variables Step b) Construct the ATMPC rolling optimization problem: Among them: J li is the optimization objective function of the ATMPC problem, for T l is the sequence obtained by periodic discretization, Depend on T l The interval is obtained based on piecewise cubic Hermite interpolation, and the constraints are: the upper and lower limits of the change of thrust and angle of the propeller set by the user, and the subscript m represents the time; Step c) The ATMPC rolling optimization problem in step b is solved in real time by the casadi solver, and the first component of the optimal control quantity sequence is used as the control input at the current moment to control the tugboat to complete trajectory tracking.

Images