Unmanned ship autonomous decision control method and system under dynamic and static obstacle objects

By constructing the kinematic and dynamic models of unmanned ships, combining nominal guidance law and expansion state observer, designing anti-interference dynamic controllers and optimizing speed differential trackers, the autonomy of unmanned ships under dynamic and static obstacles is solved. Decision control issues improve navigation safety and collision avoidance capabilities and reduce computing resource consumption.

CN118938908APending Publication Date: 2024-11-12DALIAN MARITIME UNIVERSITY
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202410983439.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-22
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

The existing unmanned ship control method lacks effective obstacle avoidance and collision avoidance capabilities when facing dynamic and static obstacles, and anti-interference control depends on model parameters, and its impact on established operating tasks is ignored, which consumes a lot of computing resources.

Method used

Build a kinematic model and dynamic model of under-driven unmanned ships, combine the nominal guidance law and expansion state observer, design an anti-interference dynamic controller, and optimize the speed differential tracker through the secondary planning model to realize dynamic and static obstacles autonomous decision-making control.

Benefits of technology

Improve the navigation safety of unmanned ships, avoid static and dynamic hindering collisions, optimize collision avoidance strategies, reduce the impact on established operation tasks, and realize precise dynamic control under unknown model parameters, reducing computing resources need.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938908A_ABST
    Figure CN118938908A_ABST
Patent Text Reader

Abstract

The invention discloses an autonomous decision control method and system for an unmanned ship under dynamic and static obstacle objects. The method comprises the following steps: constructing an under-actuated unmanned ship kinematics model and a dynamics model; according to the kinematics model and the dynamics model of the under-actuated unmanned ship, constructing a nominal guidance law and an expansion state observer; state constraints of the under-actuated unmanned ship and navigation safety constraints under dynamic and static obstacle objects are constructed; according to the constructed navigation safety constraint and state constraint of the underactuated unmanned ship, a quadratic programming model is obtained in combination with the nominal guidance law, and the quadratic programming model is solved to obtain the optimal nominal guidance law of the underactuated unmanned ship; constructing a speed differential tracker according to the optimal nominal guidance law; and designing an anti-interference dynamic controller according to the constructed speed differential tracker and the extended state observer so as to realize autonomous decision control of the under-actuated unmanned ship under the dynamic and static obstacle navigation objects. According to the method, the under-actuated unmanned ship can be ensured not to collide with a static navigation obstacle and a dynamic navigation obstacle, and accurate dynamics control can be realized under the condition that parameters of the unmanned ship model are completely unknown.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of anti-interference control of underactuated unmanned ships, and in particular to an autonomous decision-making control method and system for unmanned ships under dynamic and static obstacles. Background Art

[0002] The ocean covers more than 70% of the earth's surface area, which contains rich resources, including oil, natural gas, minerals and fish. As a marine carrier platform that integrates advanced scientific and technological technologies such as perception, communication, control and artificial intelligence, unmanned ships play an indispensable role in maritime transportation, seabed mapping, ocean measurement and ocean surveillance. However, the problem of obstacle avoidance and collision avoidance for unmanned ships is one of the key technologies to ensure the safety of unmanned ships, which to a certain extent reflects the level of intelligence of unmanned ships. In the actual marine environment, unmanned ships may encounter various obstacles that hinder navigation, such as ships, buoys, reefs and shoals. Without the ability to autonomously avoid obstacles and collisions, the safe navigation of unmanned ships will face great threats. At the same time, unmanned ships are highly nonlinear and strongly coupled complex systems. There are internal model uncertainties such as changes in hydrodynamic coefficients caused by additional mass and unmodeled dynamics in the unmanned ship model. In addition, unmanned ships sail at the interface between water and air, and are affected by external environmental disturbances such as wind, waves and currents. These internal model uncertainties and external environmental disturbances bring severe challenges to the motion control of unmanned ships.

[0003] In order to overcome the obstacle avoidance and collision avoidance problems of unmanned ships, domestic and foreign scholars have conducted a lot of relevant research and proposed a variety of methods. These methods can be mainly summarized into two categories: global obstacle avoidance algorithms based on known marine environment information and local obstacle avoidance and collision avoidance algorithms based on sensor information. In terms of global obstacle avoidance algorithms, common ones include Dijkstra algorithm, A* algorithm, ant colony algorithm, genetic algorithm, particle swarm optimization algorithm, etc.; these algorithms usually rely on a comprehensive understanding of the marine environment, including obstacle locations, ocean currents and other information, so as to plan the optimal or suboptimal navigation path. In terms of local obstacle avoidance and collision avoidance algorithms, there are artificial potential field method, vector field histogram method, dynamic window method, speed obstacle method, control obstacle function method, vector field method, etc.; these methods focus more on real-time perception and processing of obstacle information around unmanned ships, and make quick decisions and responses through data obtained by sensors. In terms of unmanned ship dynamics control, researchers have also proposed a variety of unmanned ship dynamics control methods, such as proportional integral differential control, model predictive control, sliding mode control, neural network control, fuzzy control, data-driven control, etc. These methods are designed to ensure that unmanned ships can stably and accurately perform navigation tasks in complex marine environments. Although the above research has made a lot of progress, there are still many challenges and problems in practical applications, as follows:

[0004] First, most of the existing underactuated unmanned ship control methods do not fully consider the issue of navigation safety. However, in practical applications, unmanned ships will inevitably encounter dynamic and static obstacles such as reefs and buoys. If the unmanned ship does not take timely and effective obstacle avoidance measures, it is likely to cause collisions with static and dynamic obstacles, and in serious cases, even catastrophic consequences.

[0005] Second, the existing unmanned ship obstacle avoidance methods often ignore the impact of collision avoidance on the established operation tasks. However, in maritime navigation practice, when an unmanned ship encounters an obstacle, it is particularly critical and important to reasonably adopt different strategies to balance the contradiction between the established operation tasks and the obstacle avoidance tasks to avoid the impact of obstacles on the established navigation tasks of the unmanned ship.

[0006] Third, the existing anti-interference control methods for unmanned ships generally rely on the model parameters of the unmanned ship, or presuppose that the nominal model parameters of the unmanned ship are known. However, the acquisition of the model parameters of the unmanned ship often requires complex parameter identification experiments. Although some data-driven anti-interference controllers can obtain the model parameters of the unmanned ship online, this method often consumes a lot of storage space and computing resources. Summary of the invention

[0007] The present invention provides an autonomous decision-making control method and system for an unmanned ship under dynamic and static obstacles to overcome the above technical problems.

[0008] In order to achieve the above object, the technical solution of the present invention is:

[0009] An autonomous decision-making control method for an unmanned ship under dynamic and static obstacles, comprising:

[0010] S1: Construct the kinematic and dynamic models of underactuated unmanned ship;

[0011] S2: Based on the kinematic model and dynamic model of the underactuated unmanned ship, a nominal guidance law and an extended state observer for realizing path tracking of the underactuated unmanned ship are constructed;

[0012] S3: Construct the state constraints of the underactuated unmanned ship and the navigation safety constraints under dynamic and static obstacles;

[0013] S4: Based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship, a quadratic programming model for obtaining the optimal nominal guidance law of the underactuated unmanned ship is constructed in combination with the nominal guidance law;

[0014] S5: constructing a velocity differential tracker for the underactuated unmanned ship according to the optimal nominal guidance law obtained by solving the quadratic programming model;

[0015] S6: Based on the constructed velocity differential tracker and extended state observer, an anti-interference dynamic controller is designed to achieve autonomous decision-making control of the underactuated unmanned vessel under dynamic and static obstacles.

[0016] Furthermore, in S2, a nominal guidance law is constructed according to the kinematic model and dynamic model of the underactuated unmanned ship, including the following steps:

[0017] Define a series of waypoints for the underactuated unmanned ship, and any two adjacent waypoints are expressed as

[0018] p d,(m-1) =[x d,(m-1) ,y d,(m-1) ] T ,p d,m =[x d,m ,y d,m ] T

[0019] Where: d represents the waypoint; m represents the sequence number of the waypoint;

[0020] Calculate and obtain the desired heading angle ψ based on any two adjacent waypoints d , whose expression is

[0021] ψ d =atan2(y d,m -y d,(m-1) ,x d,m -x d,(m-1) )

[0022] The position tracking error of the underactuated unmanned ship is defined as

[0023]

[0024] Where: s0 represents the longitudinal tracking error; e0 represents the lateral tracking error; R T (ψ d ) represents the transpose of the rotation matrix, and x e represents the lateral position error of the underactuated unmanned ship, and x e =xx d,m ;y e represents the longitudinal position error of the underactuated unmanned ship, and y e =yy d,m ;

[0025] According to the lateral tracking error e0 of the underactuated unmanned ship, the nominal guidance law is constructed as follows:

[0026]

[0027] Where: U r ,rr Respectively represent the nominal velocity guidance signal and the angular velocity guidance signal; u c represents the given desired speed; k represents the given gain; ψ c represents the intermediate parameter quantity, and Δ represents the set sight distance parameter;

[0028] Furthermore, in S2, an extended state observer is constructed according to the kinematic model and dynamic model of the underactuated unmanned ship, including the following steps:

[0029] The dynamic model of the underactuated unmanned ship is rewritten as:

[0030]

[0031] Where: represents the first derivative of U; represents the first-order derivative of r; χ U ,x r represents the intermediate parameter variable, and σ U ,σ r Respectively represent the nominal velocity output signal and the angular velocity output signal;

[0032] According to the rewritten dynamic model of the underactuated unmanned ship, the extended state observer is constructed as

[0033]

[0034] Where: and Respectively represent σ U With σ r An estimated value of and denote the estimated values ​​of U and r respectively; denotes a positive design parameter.

[0035] Furthermore, in S3, the safety constraints of the underactuated unmanned ship under dynamic and static obstacles are constructed, including the following steps:

[0036] Get the position coordinates p of the static obstacle S =[x S ,y S ] T ;

[0037] The position coordinates p of the dynamic obstacle D =[x D ,y D ] T ;

[0038] The position information of the underactuated unmanned ship p = [x, y] T;

[0039] The actual motion direction of the underactuated unmanned ship ψ w ;

[0040] Construct the control obstacle function h of the static obstacle S , whose expression is

[0041]

[0042] Where: Represents a positive number; Λ S represents the intermediate parameter variable, and ψ S represents the heading of the static obstacle relative to the underactuated unmanned vessel, and ψ S =atan2(y s -y,x s -x); δ S Indicates the set direction selection parameter, its specific value is:

[0043]

[0044] According to the control obstacle function of the static obstacle, the safety constraint condition of the static obstacle is obtained, and its expression is:

[0045]

[0046] Where: Indicates h S The first derivative of S represents a positive number;

[0047] According to the control obstacle function of the static obstacle and its safety constraint conditions, the navigation safety constraint is obtained as follows:

[0048] Φ S U+Ξ S r≤Π S

[0049] Where: Φ S ,Ξ S ,Π S All represent intermediate parameter variables;

[0050] and

[0051] R1, R2 represent parameter matrices, and R1 = [cosψ w ,sinψ w ] T ,R2=[sinψ w ,cosψ w ] T ;

[0052] Construct the control obstacle function h of dynamic obstacles D , whose expression is

[0053]

[0054] Where: Represents a positive number; Λ D represents the intermediate parameter variable, and ψ D represents the heading of the dynamic obstacle relative to the underactuated unmanned vessel, and ψ D =atan2(y D -y,x D -x); δ D Indicates the set direction selection parameter, its specific value is:

[0055]

[0056] According to the control obstacle function of the dynamic obstacle, the safety constraint condition of the dynamic obstacle is obtained, and its expression is:

[0057]

[0058] Where: Indicates h D The first derivative of D represents a positive number;

[0059] According to the control obstacle function of the dynamic obstacle and its safety constraint conditions, the navigation safety constraint is obtained as follows:

[0060] Φ D U+Ξ D r≤Π D

[0061] Where: Φ D ,Ξ D ,Π D All represent intermediate parameter variables;

[0062] and

[0063]

[0064] The state constraint of the underactuated unmanned ship constructed in S3 is expressed as

[0065]

[0066] Where: U min with U max Respectively represent the minimum and maximum values ​​that the nominal speed guidance signal should meet; r min With rmax They respectively represent the minimum and maximum values ​​that the angular velocity guidance signal should meet.

[0067] Furthermore, in S4, according to the navigation safety constraints and state constraints of the constructed underactuated unmanned ship, the quadratic programming model is obtained in combination with the nominal guidance law, and its expression is:

[0068]

[0069] Where: U o ,r o Represents the output signal of the optimal nominal guidance law for the underactuated unmanned vessel.

[0070] Furthermore, in S5, a velocity differential tracker is constructed according to the optimal nominal guidance law, and its expression is:

[0071]

[0072] Where: γ U ,γ r represents the design parameters; Represents r d The value after differentiation; Indicates U d The value after differentiation.

[0073] Furthermore, the anti-disturbance dynamic controller designed in S6 based on the constructed speed differential tracker and extended state observer is expressed as

[0074]

[0075] Where: φ U With φ r Both represent control gain; k U With k r represents the gain parameter; Respectively represent τ u ,τ r The first derivative of .

[0076] Furthermore, the kinematic model of the underactuated unmanned ship constructed in S1 is expressed as

[0077]

[0078] Where: x, y represent the horizontal and vertical coordinates of the underactuated unmanned ship in the earth coordinate system respectively; ψ w represents the actual motion direction of the underactuated unmanned ship, and ψ w =ψ+β; ψ represents the bow roll angle of the under-actuated unmanned ship; β represents the sideslip angle of the under-actuated unmanned ship, and β=arctan2(v / u); U represents the combined speed of the under-actuated unmanned ship, u, v, r represent the sway velocity, swing velocity and bow angular velocity of the underactuated unmanned ship respectively;

[0079] The dynamic model expression of the underactuated unmanned ship is:

[0080]

[0081] Where: m u ,m r They represent the inertia coefficients of the underactuated unmanned ship in the longitudinal and bow rolling directions respectively; f u (·),f r (·) represents the unknown uncertainties in the longitudinal and bow swing directions of the unmanned ship, which are composed of the Coriolis centripetal force, hydrodynamic damping and unmodeled dynamics; β d represents the expected sideslip angle of the underactuated unmanned ship; τ wu ,τ wr They represent the time-varying external environmental disturbances caused by wind, waves and currents in the longitudinal and bow swing directions of the unmanned ship respectively; τ u ,τ r They represent the thrust in the surge direction and the moment in the bow pitch direction of the under-actuated unmanned ship respectively.

[0082] An autonomous decision-making control system for unmanned ships under dynamic and static obstacles.

[0083] It includes a model building module of an underactuated unmanned ship, a nominal guidance law module, a quadratic programming module, a safety constraint module, a state constraint module, a velocity differential tracker module, a dynamic controller module and an extended state observer module;

[0084] The model building module is used to build a kinematic model and a dynamic model of the underactuated unmanned ship;

[0085] The nominal guidance law module is used to construct the nominal guidance law according to the kinematic model and dynamic model of the underactuated unmanned ship.

[0086] The extended state observer module is used to construct an extended state observer according to the kinematic model and dynamic model of the underactuated unmanned ship;

[0087] The safety constraint module and the state constraint module are used to construct the state constraints of the underactuated unmanned ship and the navigation safety constraints under dynamic and static obstacles;

[0088] The secondary programming module is used to obtain a secondary programming model based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship in combination with the nominal guidance law, so as to solve the secondary programming model to obtain the optimal nominal guidance law of the underactuated unmanned ship;

[0089] The velocity differential tracker module is used to construct a velocity differential tracker according to the optimal nominal guidance law;

[0090] The dynamic controller module is used to design an anti-interference dynamic controller based on the constructed velocity differential tracker and extended state observer;

[0091] And the connection relationship is that the input end of the nominal guidance law module and the input end of the dynamic controller module are respectively connected to the output end of the model construction module;

[0092] The output end of the nominal guidance law module, the output end of the safety constraint module and the output end of the state constraint module are respectively connected to the input end of the secondary planning module;

[0093] The output end of the quadratic programming module is connected to the input end of the speed differential tracker module, and the output end of the speed differential tracker module and the output end of the extended state observer module are respectively connected to the input end of the dynamic controller module;

[0094] The output end of the dynamic controller module is connected to the input end of the model building module.

[0095] Compared with the prior art, the present invention has the following beneficial effects:

[0096] First, compared with the existing unmanned ship motion control method, the state constraints of the under-actuated unmanned ship and the navigation safety constraints under dynamic and static obstacles are constructed. The obstacle avoidance and collision avoidance method based on the control obstacle function proposed in the present invention optimizes the guidance speed signal and the guidance angular velocity signal at the same time, which can avoid the collision between the under-actuated unmanned ship and static obstacles and dynamic obstacles, and can greatly improve the navigation safety of the unmanned ship.

[0097] Second, compared with the existing obstacle avoidance and collision avoidance methods based on the control obstacle function, the control obstacle function proposed in the present invention not only takes into account the position factor, but also the direction factor, and adopts different collision avoidance strategies according to the encounter situation between the unmanned ship and the obstacle, so as to avoid the influence of the obstacle on the established unmanned ship navigation mission.

[0098] Third, compared with the existing anti-interference control methods for unmanned ships, the anti-interference dynamic controller proposed in the present invention, based on the constructed velocity differential tracker and extended state observer, can achieve precise dynamic control of under-actuated unmanned ships under the premise that the model parameters of the unmanned ship are completely unknown and do not require a large amount of storage space and computing resources. BRIEF DESCRIPTION OF THE DRAWINGS

[0099] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0100] Figure 1 This is a flow chart of the autonomous decision-making control method of the unmanned ship under dynamic and static obstacles of the present invention;

[0101] Figure 2 It is a schematic diagram of the autonomous decision-making control system of the unmanned ship under dynamic and static obstacles of the present invention;

[0102] Figure 3 A schematic diagram of the navigation trajectory of the under-actuated unmanned ship provided in this embodiment;

[0103] Figure 4 A curve diagram showing the minimum distance between the unmanned ship and the obstruction provided in this embodiment;

[0104] Figure 5 A curve diagram showing the change of forward thrust and bow pitch moment of the unmanned ship provided in this embodiment;

[0105] Figure 6 This is a speed and angular velocity tracking effect diagram of the unmanned ship provided in this embodiment;

[0106] Figure 7 This is a diagram of the disturbance observation effect of the extended state observer provided in this embodiment. DETAILED DESCRIPTION

[0107] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0108] This embodiment provides an autonomous decision-making control method for an unmanned ship under dynamic and static obstacles. Figure 1 As shown, the following steps are included:

[0109] S1: Construct the kinematic and dynamic models of underactuated unmanned ship;

[0110] In a specific embodiment, the expression of the kinematic model of the underactuated unmanned ship constructed in S1 is:

[0111]

[0112] Where: x, y represent the horizontal and vertical coordinates of the underactuated unmanned ship in the earth coordinate system respectively; ψ w represents the actual motion direction of the underactuated unmanned ship, and ψ w =ψ+β; ψ represents the bow roll angle of the under-actuated unmanned ship; β represents the sideslip angle of the under-actuated unmanned ship, and β=arctan2(v / u); U represents the combined speed of the under-actuated unmanned ship, u, v, r represent the sway velocity, swing velocity and bow angular velocity of the underactuated unmanned ship respectively;

[0113] The dynamic model expression of the underactuated unmanned ship is:

[0114]

[0115] Where: m u ,m v ,m r They represent the inertia coefficients of the underactuated unmanned ship in the longitudinal, transverse and bow directions respectively; f u (·),f v (·),f r (·) represents the unknown uncertainties in the longitudinal, transverse and bow directions of the unmanned ship, which are composed of the Coriolis centripetal force, hydrodynamic damping and unmodeled dynamics; β d represents the expected sideslip angle of the underactuated unmanned ship; τ wu ,τ wr They represent the time-varying external environmental disturbances caused by wind, waves and currents in the longitudinal and bow swing directions of the unmanned ship respectively; τ u ,τ r They represent the thrust in the sway direction and the moment in the bow rolling direction of the underactuated unmanned ship respectively;

[0116] The nonlinear function is set as follows:

[0117]

[0118] In the formula

[0119]

[0120] S2: Based on the kinematic model and dynamic model of the underactuated unmanned ship, a nominal guidance law and an extended state observer for realizing path tracking of the underactuated unmanned ship are constructed, including the following steps:

[0121] S21: Constructing a nominal guidance law based on the kinematic model and dynamic model of the underactuated unmanned ship, including the following steps:

[0122] S211: Define a series of waypoints for the underactuated unmanned ship, and any two adjacent waypoints are represented as

[0123] p d,(m-1) =[x d,(m-1) ,y d,(m-1) ] T ,p d,m =[x d,m ,y d,m ] T

[0124] Where: d represents the waypoint; m represents the sequence number of the waypoint;

[0125] S212: Calculate and obtain the desired heading angle ψ based on any two adjacent waypoints d , whose expression is

[0126] ψ d =atan2(y d,m -y d,(m-1) ,x d,m -x d,(m-1) )

[0127] S213: Define the position tracking error of the underactuated unmanned ship as

[0128]

[0129] Where: s0 represents the longitudinal tracking error; e0 represents the lateral tracking error; R T (ψ d ) represents the transpose of the rotation matrix, and x e represents the lateral position error of the underactuated unmanned ship, and x e =xx d,m ;y e represents the longitudinal position error of the underactuated unmanned ship, and y e =yy d,m ;

[0130] S214: According to the lateral tracking error e0 of the underactuated unmanned ship, the nominal guidance law is constructed as follows:

[0131]

[0132] Where: U r ,r r Respectively represent the nominal velocity guidance signal and the angular velocity guidance signal; u c represents the given desired speed; k represents the given gain; ψ c represents the intermediate parameter quantity, and Δ represents the set sight distance parameter;

[0133] S22: In order to estimate the internal uncertainty and external disturbance of the unmanned ship model, an extended state observer is designed. The input signal of the extended state observer is the state information U and r of the unmanned ship, and its output signal is and Calculated by the constructed extended state observer:

[0134] That is, an extended state observer is constructed according to the kinematic model and dynamic model of the underactuated unmanned ship, including the following steps:

[0135] S221: Rewrite the dynamic model of the underactuated unmanned ship as follows:

[0136]

[0137] Where: represents the first derivative of U; represents the first-order derivative of r; χ U ,x r represents the intermediate parameter variable, and σ U ,σ r Respectively represent the nominal velocity output signal and the angular velocity output signal;

[0138] S222: According to the rewritten dynamic model of the underactuated unmanned ship, the extended state observer is constructed as

[0139]

[0140] Where: and Respectively represent σ U With σ r An estimated value of and denote the estimated values ​​of U and r respectively; represents a positive design parameter;

[0141] S3: Constructing state constraints of the under-actuated unmanned ship and navigation safety constraints under dynamic and static obstacles. In a specific embodiment, constructing safety constraints of the under-actuated unmanned ship under dynamic and static obstacles includes the following steps:

[0142] S31: Obtain the position coordinates p of the static obstruction in the earth coordinate system S =[x S ,y S ] T ;

[0143] The position coordinates p of the dynamic obstacle in the earth coordinate system D =[x D ,y D ] T ;

[0144] The position information of the underactuated unmanned ship in the earth coordinate system p = [x, y] T ;

[0145] The actual motion direction of the underactuated unmanned ship ψ w ;

[0146] S32: In order to avoid static obstacles in the environment, a control obstacle function h of static obstacles is constructed. S , whose expression is

[0147]

[0148] Where: Represents a positive number; Λ S represents the intermediate parameter variable, and ψ S represents the heading of the static obstacle relative to the underactuated unmanned vessel, and ψ S =atan2(y s -y,x s -x); δ S Indicates the set direction selection parameter, its specific value is:

[0149]

[0150] S33: According to the control obstacle function of the static obstacle, the safety constraint condition of the static obstacle is obtained, and its expression is:

[0151]

[0152] Where: Indicates h S The first derivative of S represents a positive number;

[0153] S34: According to the control obstacle function of the static obstacle and its safety constraint condition, the navigation safety constraint is obtained as follows:

[0154] Φ S U+Ξ S r≤Π S

[0155] Where: Φ S ,Ξ S ,Π S All represent intermediate parameter variables;

[0156] and

[0157] R1, R2 represent parameter matrices, and R1 = [cosψ w ,sinψw ] T ,R2=[sinψ w ,cosψ w ] T ;

[0158] S35: In order to avoid dynamic obstacles in the environment, a control obstacle function h of dynamic obstacles is constructed. D , whose expression is

[0159]

[0160] Where: Represents a positive number; Λ D represents the intermediate parameter variable, and ψ D represents the heading of the dynamic obstacle relative to the underactuated unmanned vessel, and ψ D =atan2(y D -y,x D -x); δ D Indicates the set direction selection parameter, its specific value is:

[0161]

[0162] S36: According to the control obstacle function of the dynamic obstacle, the safety constraint condition of the dynamic obstacle is obtained, and its expression is:

[0163]

[0164] Where: Indicates h D The first derivative of D represents a positive number;

[0165] S37: According to the control obstacle function of the dynamic obstacle and its safety constraint condition, the navigation safety constraint is obtained as follows:

[0166] Φ D U+Ξ D r≤Π D

[0167] Where: Φ D ,Ξ D ,Π D All represent intermediate parameter variables;

[0168] and

[0169]

[0170] S38: Considering that the maneuverability of the unmanned ship is limited, the state constraint of the underactuated unmanned ship is constructed, and its expression is:

[0171]

[0172] Where: U min with U max Respectively represent the minimum and maximum values ​​that the nominal speed guidance signal should meet; r min With r max Respectively represent the minimum and maximum values ​​that the angular velocity guidance signal should meet;

[0173] S4: Based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship, a quadratic programming model for obtaining the optimal nominal guidance law of the underactuated unmanned ship is constructed in combination with the nominal guidance law;

[0174] In a specific embodiment, according to the navigation safety constraints and state constraints of the constructed underactuated unmanned ship, a quadratic programming model is obtained in combination with the nominal guidance law, and its expression is:

[0175]

[0176] Where: U o ,r o An output signal representing the optimal nominal guidance law of the underactuated unmanned vessel;

[0177] S5: According to the optimal nominal guidance law obtained by solving the quadratic programming model, a velocity differential tracker of the underactuated unmanned ship is constructed; the input signal of the velocity differential tracker is the output signal U of the safety-constrained optimal guidance law. o 、r o ; Its output signal U d 、r d It is calculated by the following formula:

[0178] In a specific embodiment, the velocity differential tracker constructed is expressed as:

[0179]

[0180] Where: γ U ,γ r represents the design parameters; Represents r d The value after differentiation; Indicates U d The value after differentiation;

[0181] S6: Based on the constructed velocity differential tracker and extended state observer, an anti-interference dynamic controller is designed to achieve autonomous decision-making control of the underactuated unmanned ship under dynamic and static obstacles;

[0182] The input signals of the anti-disturbance dynamic controller are the output signals of the extended state observer And the tracking differentiator output signal U d 、r d , Anti-interference dynamic controller output signal τ=(τ u ,τ r ) is given by:

[0183] In a specific embodiment, according to the constructed velocity differential tracker and extended state observer, an anti-interference dynamic controller is designed, and its expression is:

[0184]

[0185] Where: φ U With φ r Both represent control gain; k U With k r represents the gain parameter; Respectively represent τ u ,τ r The first derivative of .

[0186] This embodiment also provides an autonomous decision-making control system for unmanned ships under dynamic and static obstacles. Figure 2 As shown, it includes a model building module of an underactuated unmanned ship, a nominal guidance law module, a quadratic programming module, a safety constraint module, a state constraint module, a velocity differential tracker module, a dynamic controller module and an extended state observer module;

[0187] The model building module is used to build a kinematic model and a dynamic model of the underactuated unmanned ship;

[0188] The nominal guidance law module is used to construct the nominal guidance law according to the kinematic model and dynamic model of the underactuated unmanned ship.

[0189] The extended state observer module is used to construct an extended state observer according to the kinematic model and dynamic model of the underactuated unmanned ship;

[0190] The safety constraint module and the state constraint module are used to construct the state constraints of the underactuated unmanned ship and the navigation safety constraints under dynamic and static obstacles;

[0191] The secondary programming module is used to obtain a secondary programming model based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship in combination with the nominal guidance law, so as to solve the secondary programming model to obtain the optimal nominal guidance law of the underactuated unmanned ship;

[0192] The velocity differential tracker module is used to construct a velocity differential tracker according to the optimal nominal guidance law;

[0193] The dynamic controller module is used to design an anti-interference dynamic controller based on the constructed velocity differential tracker and extended state observer;

[0194] And the connection relationship is that the input end of the nominal guidance law module and the input end of the dynamic controller module are respectively connected to the output end of the model construction module;

[0195] The output end of the nominal guidance law module, the output end of the safety constraint module and the output end of the state constraint module are respectively connected to the input end of the secondary planning module;

[0196] The output end of the quadratic programming module is connected to the input end of the speed differential tracker module, and the output end of the speed differential tracker module and the output end of the extended state observer module are respectively connected to the input end of the dynamic controller module;

[0197] The output end of the dynamic controller module is connected to the input end of the model building module.

[0198] The specific controller parameters given in this embodiment are as follows:

[0199]

[0200] φ U =25.8,φ r =10, k U =2.5, k r =5

[0201] The positions of static obstructions are set as follows:

[0202] p 1S =(-80,-50), p 2S =(0,-82)

[0203] The initial position of the dynamic obstacle is set as follows:

[0204] p 1D =(50,-25), p 2D =(76.2,46.7), p 3D =(0,87), p 4D =(-110,65)

[0205] The waypoints are set as follows:

[0206] p d,1 =(-100,0), p d,2 =(-50,-86.60254037844385), p d,3 =(50,-86.60254037844385),

[0207] pd,4 =(50,86.60254037844385), p d,5 =(-50,86.60254037844385),p d,6 =(-100,0)

[0208] The simulation results of this embodiment are as follows: Figures 3 to 7 shown. Figure 3 is the navigation track of the underactuated unmanned ship. The thin dotted line in the figure is the given expected route, the solid line is the actual navigation track of the underactuated unmanned ship, the two black circles represent static obstacles, and the four unmanned ships outside the route represent four dynamic obstacles. It can be seen from the figure that the underactuated unmanned ship tracks the given expected route and avoids dynamic and static obstacles. Figure 4 It is the minimum distance variation curve between the unmanned ship and the obstacle. The solid line in the figure represents the minimum distance between the unmanned ship and the obstacle, and the dotted line represents the set safety distance between the unmanned ship and the obstacle. It can be seen from the figure that the distance between the underactuated unmanned ship and the obstacle is always greater than the set safety distance, indicating that the unmanned ship and the obstacle have not collided. Figure 5 It is the change curve of the forward thrust and bow rolling moment of the unmanned ship. It can be seen from the figure that the forward thrust and bow rolling moment of the under-actuated unmanned ship are maintained within a certain range. Figure 6 This is the speed and angular velocity tracking effect of the unmanned ship. It can be seen that the tracking error can converge to near zero and maintain. Figure 7 This is the disturbance observation effect of the extended state observer. The solid line in the figure represents the actual disturbance, and the dotted line represents the observed disturbance. It can be seen that the observer has a good observation effect on uncertainty and disturbance.

[0209] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An autonomous decision-making control method for an unmanned ship under dynamic and static obstacles, characterized in that: include: S1: Construct the kinematic and dynamic models of underactuated unmanned ship; S2: Based on the kinematic model and dynamic model of the underactuated unmanned ship, a nominal guidance law and an extended state observer for realizing path tracking of the underactuated unmanned ship are constructed; S3: Construct the state constraints of the underactuated unmanned ship and the navigation safety constraints under dynamic and static obstacles; S4: Based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship, a quadratic programming model for obtaining the optimal nominal guidance law of the underactuated unmanned ship is constructed in combination with the nominal guidance law; S5: constructing a velocity differential tracker for the underactuated unmanned ship according to the optimal nominal guidance law obtained by solving the quadratic programming model; S6: Based on the constructed velocity differential tracker and extended state observer, an anti-interference dynamic controller is designed to achieve autonomous decision-making control of the underactuated unmanned vessel under dynamic and static obstacles.

2. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: In S2, a nominal guidance law is constructed according to the kinematic model and the dynamic model of the underactuated unmanned ship, including the following steps: Define a series of waypoints for the underactuated unmanned ship, and any two adjacent waypoints are expressed as p d,(m-1) =[x d,(m-1) ,y d,(m-1) ] T ,p d,m =[x d,m ,y d,m ] T Where: d represents the waypoint; m represents the sequence number of the waypoint; Calculate and obtain the desired heading angle ψ based on any two adjacent waypoints d , whose expression is ψ d =atan2(y d,m -y d,(m-10 ,x d,m -x d,(m-1) ) The position tracking error of the underactuated unmanned ship is defined as Where: s0 represents the longitudinal tracking error; e0 represents the lateral tracking error; R T (ψ d ) represents the transpose of the rotation matrix, and x e represents the lateral position error of the underactuated unmanned ship, and x e =xx d,m ;y e represents the longitudinal position error of the underactuated unmanned ship, and y e =yy d,m ; According to the lateral tracking error e0 of the underactuated unmanned ship, the nominal guidance law is constructed as follows: Where: U r ,r r Respectively represent the nominal velocity guidance signal and the angular velocity guidance signal; u c represents the given desired speed; k represents the given gain; ψ c represents the intermediate parameter quantity, and Δ represents the set viewing distance parameter.

3. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: In S2, an extended state observer is constructed according to the kinematic model and dynamic model of the underactuated unmanned ship, including the following steps: The dynamic model of the underactuated unmanned ship is rewritten as: Where: represents the first derivative of U; represents the first-order derivative of r; χ U ,x r represents the intermediate parameter variable, and σ U ,σ r Respectively represent the nominal velocity output signal and the angular velocity output signal; According to the rewritten dynamic model of the underactuated unmanned ship, the extended state observer is constructed as Where: and Respectively represent σ U With σ r An estimated value of and represent the estimated values ​​of U and r respectively; denotes a positive design parameter.

4. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: In S3, the safety constraints of the underactuated unmanned ship under dynamic and static obstacles are constructed, including the following steps: Get the position coordinates p of the static obstacle S =[x S ,y S ] T ; The position coordinates p of the dynamic obstacle D =[x D ,y D ] T ; The position information of the underactuated unmanned ship p = [x, y] T ; The actual motion direction of the underactuated unmanned ship ψ w ; Construct the control obstacle function h of the static obstacle S , whose expression is Where: Represents a positive number; Λ S represents the intermediate parameter variable, and ψ S represents the heading of the static obstacle relative to the underactuated unmanned vessel, and ψ S =atan2(y s -y,x s -x); δ S Indicates the set direction selection parameter, its specific value is: According to the control obstacle function of the static obstacle, the safety constraint condition of the static obstacle is obtained, and its expression is: Where: Indicates h S The first derivative of S represents a positive number; According to the control obstacle function of the static obstacle and its safety constraint conditions, the navigation safety constraint is obtained as Φ S U+Ξ S r≤Π S Where: Φ S ,Ξ S ,Π S All represent intermediate parameter variables; and R1, R2 represent parameter matrices, and R1 = [cosψ w ,sinψ w ] T ,R2=[sinψ w ,cosψ w ] T ; Construct the control obstacle function h of dynamic obstacles D , whose expression is Where: Represents a positive number; Λ D represents the intermediate parameter variable, and ψ D represents the heading of the dynamic obstacle relative to the underactuated unmanned vessel, and ψ D =atan2(y D -y,x D -x); δ D Indicates the set direction selection parameter, its specific value is: According to the control obstacle function of the dynamic obstacle, the safety constraint condition of the dynamic obstacle is obtained, and its expression is: Where: Indicates h D The first derivative of D represents a positive number; According to the control obstacle function of the dynamic obstacle and its safety constraint conditions, the navigation safety constraint is obtained as follows: F D U+Ξ D r≤Π D Where: Φ D ,Ξ D ,Π D All represent intermediate parameter variables; and The state constraint of the underactuated unmanned ship constructed in S3 is expressed as Where: U min with U max Respectively represent the minimum and maximum values ​​that the nominal speed guidance signal should meet; r min With r max They respectively represent the minimum and maximum values ​​that the angular velocity guidance signal should meet.

5. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: In S4, the quadratic programming model is obtained based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship and the nominal guidance law, and its expression is: Where: U o ,r o Represents the output signal of the optimal nominal guidance law for the underactuated unmanned vessel.

6. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: In S5, a velocity differential tracker is constructed according to the optimal nominal guidance law, and its expression is: Where: β U ,γ r represents the design parameters; Represents r d The value after differentiation; Indicates U d The value after differentiation; r d represents the expected bow angular velocity of the underactuated unmanned ship; U d represents the expected total speed of the underactuated unmanned vessel.

7. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: In S6, the anti-disturbance dynamic controller is designed based on the constructed speed differential tracker and extended state observer, and its expression is: Where: φ U With φ r Both represent control gain; k U With k r represents the gain parameter; Respectively represent τ u ,τ r The first derivative of .

8. The autonomous decision-making control method for an unmanned ship under dynamic and static obstacles according to claim 1 is characterized in that: The expression of the kinematic model of the underactuated unmanned ship constructed in S1 is: Where: x, y represent the horizontal and vertical coordinates of the underactuated unmanned ship in the earth coordinate system respectively; v w represents the actual motion direction of the underactuated unmanned ship, and ψ w =ψ+β; ψ represents the bow roll angle of the under-actuated unmanned ship; β represents the sideslip angle of the under-actuated unmanned ship, and β=arctan2(v / u); U represents the combined speed of the under-actuated unmanned ship, u, v, r represent the sway velocity, swing velocity and bow angular velocity of the underactuated unmanned ship respectively; The dynamic model expression of the underactuated unmanned ship is: Where: m u ,m r They represent the inertia coefficients of the underactuated unmanned ship in the longitudinal and bow rolling directions respectively; f u (·),f r (·) represents the unknown uncertainties in the unmanned ship's longitudinal and bow swing directions, which are composed of the Coriolis centripetal force, hydrodynamic damping and unmodeled dynamics; γ d represents the expected sideslip angle of the underactuated unmanned ship; τ wu ,τ wr They represent the time-varying external environmental disturbances caused by wind, waves and currents in the longitudinal and bow swing directions of the unmanned ship respectively; τ u ,τ r They represent the thrust in the surge direction and the moment in the bow pitch direction of the under-actuated unmanned ship respectively.

9. An unmanned ship autonomous decision-making control system implemented based on the unmanned ship autonomous decision-making control method under dynamic and static obstacles as described in any one of claims 1 to 8, characterized in that: It includes a model building module of an underactuated unmanned ship, a nominal guidance law module, a quadratic programming module, a safety constraint module, a state constraint module, a velocity differential tracker module, a dynamic controller module and an extended state observer module; The model building module is used to build a kinematic model and a dynamic model of the underactuated unmanned ship; The nominal guidance law module is used to construct the nominal guidance law according to the kinematic model and dynamic model of the underactuated unmanned ship. The extended state observer module is used to construct an extended state observer according to the kinematic model and dynamic model of the underactuated unmanned ship; The safety constraint module and the state constraint module are used to construct the state constraints of the underactuated unmanned ship and the navigation safety constraints under dynamic and static obstacles; The secondary programming module is used to obtain a secondary programming model based on the navigation safety constraints and state constraints of the constructed underactuated unmanned ship in combination with the nominal guidance law, so as to solve the secondary programming model to obtain the optimal nominal guidance law of the underactuated unmanned ship; The velocity differential tracker module is used to construct a velocity differential tracker according to the optimal nominal guidance law; The dynamic controller module is used to design an anti-interference dynamic controller based on the constructed velocity differential tracker and extended state observer; And the connection relationship is that the input end of the nominal guidance law module and the input end of the dynamic controller module are respectively connected to the output end of the model construction module; The output end of the nominal guidance law module, the output end of the safety constraint module and the output end of the state constraint module are respectively connected to the input end of the secondary planning module; The output end of the quadratic programming module is connected to the input end of the speed differential tracker module, and the output end of the speed differential tracker module and the output end of the extended state observer module are respectively connected to the input end of the dynamic controller module; The output end of the dynamic controller module is connected to the input end of the model building module.

Citation Information

Cited By

  • Method and system for formation control for unmanned surface vessel swarm via collaborative exploration deep reinforcement learning (CEDRL)

    US12735152B2