Autonomous navigation method, system and storage medium for underactuated unmanned boat

Abstract

The present invention discloses an autonomous navigation method, system and storage medium for an underactuated unmanned boat, which can realize autonomous navigation of an underactuated unmanned boat on a set expected track in complex waters, and effectively improve the safety and reliability of autonomous navigation of the underactuated unmanned boat. The method includes: obtaining the expected track; discretizing the expected track to obtain an expected track sequence; constructing an underactuated unmanned boat predictive control model; the underactuated unmanned boat predictive control model includes an evaluation function and constraints; inputting the current actual navigation state of the underactuated unmanned boat and the expected track sequence into the underactuated unmanned boat predictive control model, and predicting to obtain the optimal control output; determining that the underactuated unmanned boat operates within the control time domain, and controlling the actuator of the underactuated unmanned boat to execute the optimal control output.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous control of unmanned boats, and in particular to an autonomous navigation method, system and storage medium for an underactuated unmanned boat. Background Art

[0002] The basic functions of underactuated unmanned boats include sailing on the planned route and autonomously avoiding obstacles encountered during navigation. The intelligent collision avoidance function is one of the key technologies to achieve autonomous navigation of underactuated unmanned boats. Most of the intelligent collision avoidance decision algorithms in related technologies, such as neural networks, speed obstacle methods, and artificial potential field methods, are difficult to fully consider various constraints in motion control, and are also difficult to meet the actual autonomous navigation requirements of unmanned boats. Summary of the invention

[0003] In order to solve at least one of the above-mentioned technical problems, the present invention proposes an autonomous navigation method, system and storage medium for an under-actuated unmanned boat, which can realize autonomous navigation of an under-actuated unmanned boat on a set expected track in complex waters, effectively improving the safety and reliability of the autonomous navigation of the under-actuated unmanned boat.

[0004] On the one hand, an embodiment of the present invention provides an autonomous navigation method for an underactuated unmanned boat, comprising the following steps:

[0005] Get the expected track;

[0006] Discretizing the desired track to obtain a desired track sequence;

[0007] Constructing a predictive control model for an underactuated unmanned boat; the predictive control model for an underactuated unmanned boat includes an evaluation function and constraint conditions;

[0008] Inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output;

[0009] Determine that the underactuated unmanned boat operates within a control time domain, and control an actuator of the underactuated unmanned boat to execute the optimal control output.

[0010] According to an autonomous navigation method of an underactuated unmanned boat according to an embodiment of the present invention, there are at least the following beneficial effects: This embodiment obtains the expected track of the underactuated unmanned boat and discretizes the expected track to obtain the expected track sequence. At the same time, this embodiment constructs an underactuated unmanned boat prediction control model including an evaluation function and constraint conditions, and inputs the expected track sequence and the current actual navigation state of the underactuated unmanned boat into the underactuated unmanned boat prediction control model to predict the optimal control output of the underactuated unmanned boat in the control time domain, so that in the control time domain, the actuator of the underactuated unmanned boat is controlled to execute the optimal control output, and the autonomous navigation of the underactuated unmanned boat on the expected track in complex waters is realized, and the navigation of the underactuated unmanned boat is effectively constrained by the constraint conditions, which effectively improves the safety and reliability of the autonomous navigation of the underactuated unmanned boat.

[0011] According to some embodiments of the present invention, the evaluation function includes track deviation, speed change, and rule compliance;

[0012] The construction of the predictive control model of the underactuated unmanned boat comprises:

[0013] The evaluation function including the track deviation, the speed change and the rule compliance is constructed.

[0014] According to some embodiments of the present invention, the constraints include static obstacle constraints, dynamic obstacle constraints, and actuator constraints;

[0015] The constructing of the predictive control model of the underactuated unmanned boat also includes:

[0016] The constraint conditions including the static obstacle constraint conditions, the dynamic obstacle constraint conditions and the actuator constraint conditions are constructed.

[0017] According to some embodiments of the present invention, the optimal control output includes a commanded rudder angle and a commanded propeller speed;

[0018] The step of inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output includes:

[0019] The current actual navigation state of the underactuated unmanned boat and the expected track sequence are input into the underactuated unmanned boat predictive control model to predict the command rudder angle and the command propeller speed; wherein the command rudder angle and the command propeller speed minimize the evaluation function value.

[0020] According to some embodiments of the present invention, the step of inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle predictive control model to predict the command rudder angle and the command propeller speed includes:

[0021] The current actual navigation state and the expected track sequence in the prediction time domain are input into the under-actuated unmanned vehicle predictive control model to predict the command rudder angle and the command propeller speed.

[0022] According to some embodiments of the present invention, the predictive control model of the underactuated unmanned boat is shown as follows:

[0023] min C(I)=K pd C pd (I)+K U C U (I)+K COL C COL (I)

[0024]

[0025] Wherein, C(I) is the evaluation function, st is the constraint condition, C pd (I) is the track deviation, C U (I) is the speed change, C COL (I) is the compliance degree of the rule, K pd , K U , K COL are the corresponding weight parameters, I is the input of the ship control mechanism, h s (S(I), p s , K p , r s ) is the static obstacle constraint condition, S(I) is the navigation state of the ship with I as the input, p s is the static obstacle coordinate set, r s is the radius set of static obstacle safety area, N p is the prediction time domain, h T (S(I), p T , N p , r T ) is the dynamic obstacle constraint condition, p T is the dynamic obstacle coordinate set, r T is the radius set of the dynamic obstacle safety area, h I (I) is the actuator constraint condition.

[0026] According to some embodiments of the present invention, the actuator constraints include propeller speed constraints, rudder angle constraints, propeller speed change rate constraints, and rudder angle change rate constraints.

[0027] On the other hand, an embodiment of the present invention further provides an underactuated unmanned boat autonomous navigation system, comprising:

[0028] A track acquisition module is used to obtain the desired track;

[0029] A discretization module, used for discretizing the desired track to obtain a desired track sequence;

[0030] A model building module, used to build an underactuated unmanned boat predictive control model; the underactuated unmanned boat predictive control model includes an evaluation function and constraint conditions;

[0031] A prediction module, used for inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output;

[0032] The execution module is used to determine that the under-actuated unmanned boat is operating in the control time domain, and control the actuator of the under-actuated unmanned boat to execute the optimal control output.

[0033] On the other hand, an embodiment of the present invention further provides an underactuated unmanned boat autonomous navigation system, comprising:

[0034] at least one processor;

[0035] at least one memory for storing at least one program;

[0036] When the at least one program is executed by the at least one processor, the at least one processor implements the autonomous navigation method of the under-actuated unmanned boat as described in the above embodiment.

[0037] On the other hand, an embodiment of the present invention further provides a computer storage medium, which stores a program executable by a processor. When the program executable by the processor is executed by the processor, it is used to implement the autonomous navigation method of the under-actuated unmanned boat as described in the above embodiment. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 is a flow chart of an autonomous navigation method for an underactuated unmanned boat provided by an embodiment of the present invention;

[0039] Figure 2 It is a principle block diagram of the autonomous navigation system of an underactuated unmanned boat provided by an embodiment of the present invention;

[0040] Figure 3It is a schematic diagram of the basic principle of model predictive control provided by an embodiment of the present invention;

[0041] Figure 4 It is a schematic diagram of an inertial coordinate system and an attached coordinate system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0042] The embodiments described in the embodiments of this application should not be regarded as limitations of this application. All other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0043] In the following description, reference is made to “some embodiments”, which describe a subset of all possible embodiments, but it can be understood that “some embodiments” may be the same subset or different subsets of all possible embodiments and may be combined with each other without conflict.

[0044] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of this application and are not intended to limit this application.

[0045] Before introducing the embodiments of the present application, the relevant terms involved in the present application are first explained.

[0046] The basic principle of model predictive control is as follows Figure 3 As shown in the figure, y(k) is the measured value of the system at time t=k; I is the measured value of the system at time t∈(k+1, k+N p ) input sequence; N p is called the prediction horizon; N co is called the control domain; y(k+i) is the output of the system at time t=k+i; y pd is the expected output of the system, and the shaded part is the optimization target of the model predictive control algorithm.

[0047] The basic functions of underactuated unmanned vehicles include planning to sail on the planned route and autonomously avoiding obstacles encountered during navigation. Among them, the intelligent collision avoidance function is one of the key technologies to achieve autonomous navigation of underactuated unmanned vehicles. For the autonomous collision avoidance problem of unmanned surface vehicles (USVs) during autonomous navigation, related technologies have formed intelligent collision avoidance decision algorithms based on speed obstacle method, artificial potential field method, neural network, window method and model predictive control, but these methods are difficult to solve and obtain collision avoidance decision solutions that meet the actual navigation needs of underactuated unmanned vehicles.

[0048] Based on this, an embodiment of the present invention provides an autonomous navigation method for an underactuated unmanned boat, which can realize autonomous navigation of an underactuated unmanned boat on a set desired track in complex waters, effectively improving the safety and reliability of autonomous navigation of the underactuated unmanned boat. Figure 1 The method of the embodiment of the present invention includes but is not limited to step S110, step S120, step S130, step S140 and step S150.

[0049] Specifically, the method application process of the embodiment of the present invention includes but is not limited to the following steps:

[0050] S110: Obtaining a desired track.

[0051] S120: Discretize the expected track to obtain an expected track sequence.

[0052] S130: constructing a predictive control model for an underactuated unmanned vehicle, wherein the predictive control model for an underactuated unmanned vehicle includes an evaluation function and constraint conditions.

[0053] S140: Inputting the current actual navigation state and the expected track sequence of the under-actuated unmanned vehicle into the under-actuated unmanned vehicle predictive control model to predict and obtain the optimal control output.

[0054] S150: Determine that the under-actuated unmanned boat operates within the control time domain, and control the actuator of the under-actuated unmanned boat to execute the optimal control output.

[0055] In this specific embodiment, the embodiment first obtains the expected track of the underactuated unmanned boat. Then, the expected track sequence is obtained by discretizing the expected track. Specifically, the embodiment discretizes the expected track into an expected track sequence according to a certain distance interval, such as pd 1：fin =[pd1, pd2, ..., pd fin ], pd 1：fIn is the set of expected track sequence coordinates, where the coordinates of the qth expected track sequence point are pd q =[x q ,y q ] T ,q∈[1,fin],x q is the coordinate of the qth expected track sequence on the x-axis, y qis the coordinate of the qth expected track sequence on the y-axis. Further, the present embodiment constructs a predictive control model for an underactuated unmanned boat. Specifically, the predictive control model for an underactuated unmanned boat in the present embodiment includes an evaluation function and constraints, and the optimal control output of the underactuated unmanned boat is predicted by the evaluation function and the constraints. Further, the present embodiment predicts the optimal control output by inputting the current actual navigation state of the underactuated unmanned boat and the expected track sequence into the constructed predictive control model for an underactuated unmanned boat. Specifically, the current actual navigation state of the underactuated unmanned boat can be expressed as S=[u, v, r, x, y, ψ, δ, n], where u is the longitudinal speed of the ship, v is the transverse speed of the ship, r is the bow speed of the ship, x is the horizontal coordinate of the ship in the inertial coordinate system, y is the vertical coordinate of the ship in the inertial coordinate system, ψ is the ship heading, δ is the actual rudder angle of the ship, and n is the actual rotation speed of the ship. This embodiment realizes the prediction of the optimal control output in the control time domain of the under-actuated unmanned boat by inputting the actual navigation state of the under-actuated unmanned boat at the current moment, that is, the current actual navigation state S and the expected track sequence into the predictive control model of the under-actuated unmanned boat. Furthermore, this implementation determines that when the under-actuated unmanned boat is operating in the control time domain, the actuator of the under-actuated unmanned boat is controlled to execute the optimal control output. Specifically, after the optimal control output is predicted, the unmanned boat actuator outputs the predicted optimal control output in the control time domain, so that the under-actuated unmanned boat can autonomously navigate on the expected track in complex waters, and the navigation of the under-actuated unmanned boat is effectively constrained by the constraint conditions, which effectively improves the safety and reliability of the autonomous navigation of the under-actuated unmanned boat.

[0056] It should be noted that, in some embodiments of the present invention, after the control time domain ends, this embodiment repeats the above steps S140 and S150 until the navigation time ends or the set navigation time is reached, thereby completing the entire desired navigation route and realizing autonomous navigation of the under-actuated unmanned boat on the desired track.

[0057] In some embodiments of the present invention, the evaluation function includes track deviation, speed change and rule compliance. Accordingly, the underactuated unmanned boat predictive control model is constructed, including but not limited to:

[0058] Construct an evaluation function including track deviation, speed change and rule compliance.

[0059] In this specific embodiment, the evaluation function constructed by this implementation includes track deviation, speed change and rule compliance. Specifically, in this embodiment, the track deviation C pd (I) represents the dimensionless number of the deviation between the predicted track and the expected track of the unmanned boat in the prediction time domain. pdThe smaller (I) is, the closer the predicted trajectory of the unmanned boat in the predicted time domain is to the expected track. p ) is shown in the following formula (1):

[0060]

[0061] Accordingly, the track offset C pd The calculation formula of (I) is shown in the following formula (2):

[0062]

[0063] Among them, The position of the unmanned boat at the kth moment in the prediction time domain with I as the control mechanism input, is the X coordinate of the ship in the inertial coordinate system at time t0+k when I is used as input, is the Y coordinate of the ship in the inertial coordinate system at time t0+k when I is used as input, is the coordinate of the expected track point at time t0+k, is the position coordinate of the ship at time t0, S(I) is N p ×8 matrix, indicating that I is the motion state set of the unmanned boat as the input of the control mechanism.

[0064] Furthermore, the evaluation function constructed in this embodiment also includes the speed change C U (I). Specifically, the speed change C U (I) represents the dimensionless number of the deviation between the speed of the unmanned boat during autonomous navigation and the expected speed. U The smaller (I) is, the closer the speed of the unmanned boat is to the expected speed during autonomous navigation. For example, when the expected speed of the unmanned boat on the expected track is Ud, the speed change of the unmanned boat in the predicted time domain C U The calculation formula of (I) is shown in the following formula (3):

[0065]

[0066] Where U d is the expected speed of the unmanned boat, The output is the ship speed of the t0+kth data, and its calculation formula is shown in the following formula (4):

[0067]

[0068] Among them, The input is I, which is the ship's sway speed at the moment t0+k. The input is I, which is the ship's swaying speed at the moment t0+k.

[0069] At the same time, the evaluation function constructed in this embodiment also includes the rule compliance C COL (I). Specifically, the rule compliance C COL (I) A dimensionless number indicating whether the unmanned boat meets relevant standards or rules during autonomous collision avoidance. For example, the relevant standards or rules met by this embodiment may be relevant standards, rules or regulations for ship collision avoidance at sea. COL (I) The calculation formula is shown in the following formula (5):

[0070]

[0071] Among them, μ is a Boolean variable. When two unmanned boats are in a situation of meeting or crossing from the right, μ = 1. At this time, the ship is a ship that gives way and must take evasive action by turning right. When two unmanned boats are in a situation of overtaking or crossing from the left, μ = 0. In this case, the collision avoidance rules do not stipulate the evasive measures of the ship. At this time, it is only necessary to maintain a certain safe distance from the other ship. R is the collision risk of the ship. When the ship is in danger of collision with multiple ships, the value of R is the maximum value of multiple collision risks, B k (I) is the projection coordinate of the ship's position at time t0+k on the y-axis of the attached coordinate system at time t0. k (I)>0 indicates that the unmanned boat takes the right turn to avoid the situation within the prediction time domain. k When (I) < 0, it indicates that the unmanned boat takes the evasive measure of turning left within the predicted time domain.

[0072] It should be noted that, in this embodiment, two factors, namely, the distance to closest approach (DCPA) and the time to closest approach (TCPA), are considered through fuzzy set theory to calculate the risk of ship collision, and the calculation formula is shown in the following formula (6):

[0073] R=α1R DCPA +α2R TCPA (6)

[0074] Among them, R DCPA is the membership function of the closest encounter distance, R TCPA is the membership function of the closest encounter time, α1 and α2 are R DCPA and R TCPA Exemplarily, the weight may be set to α1=α2=0.5.

[0075] This embodiment constructs an evaluation function including track deviation, speed change and rule compliance. By combining the navigation deviation of the expected trajectory, the speed change for the expected speed, the collision risk and the compliance with relevant collision rules during the autonomous navigation of the unmanned boat, an evaluation function of the predictive control model of the under-actuated unmanned boat is constructed, which can effectively improve the safety and reliability of the autonomous navigation of the under-actuated unmanned boat.

[0076] In some embodiments of the present invention, the constraints include static obstacle constraints, dynamic obstacle constraints and actuator constraints. Accordingly, constructing an underactuated unmanned boat predictive control model also includes but is not limited to:

[0077] Construct constraints including static obstacle constraints, dynamic obstacle constraints, and actuator constraints.

[0078] In this specific embodiment, the constraints constructed in this embodiment include static obstacle constraints, dynamic obstacle constraints, and actuator constraints. Specifically, at the initial moment, there are e static obstacles on the expected track of the unmanned boat. The position coordinates of these static obstacles are in, is the position coordinate of the i-th static obstacle, x i is the x-axis coordinate of the i-th static obstacle position in the inertial coordinate system, y i is the y-axis coordinate of the position of the i-th static obstacle in the inertial coordinate system. At the same time, the circular safety area radius of each static obstacle is Therefore, the constraint condition of the static obstacle on the unmanned boat is constructed in the prediction time domain as shown in the following formula (7):

[0079] h s (S(I), p s , N p , r s )≤0 (7)

[0080] Specifically, the calculation process of the static obstacle constraint condition in this embodiment is shown in the following equations (8), (9) and (10):

[0081]

[0082]

[0083]

[0084] Among them, k is the kth moment in the prediction time domain, k∈[1, N p ].

[0085] In addition, when there are d opposing ships posing a collision risk with the unmanned boat at time t0, the position coordinates of the opposing ships are Among them, d is the number of enemy ships, j is the jth enemy ship. At the same time, the radius set of the circular safety area corresponding to the enemy ship is Therefore, in the prediction time domain, the constraints of dynamic obstacles on the unmanned boat, that is, the constructed dynamic obstacle constraints, are shown in the following formula (11):

[0086] h T (S(I), p T , N p , r T )≤0 (11)

[0087] Specifically, in the formula h T (S(I), p T , N p , r T ) is an m×N p The calculation process of the matrix is ​​shown in the following equations (12), (13) and (14):

[0088]

[0089]

[0090]

[0091] Among them, h T (S(I), p T , N p , r T ) is a dynamic obstacle safety area constraint mainly based on the other ship.

[0092] In addition, the constraints constructed in this embodiment also take into account the dynamic characteristics of the underactuated unmanned boat, and by constructing the actuator constraints, the predicted optimal control output meets the dynamic characteristics requirements of the actuator of the underactuated unmanned boat. In this embodiment, by constructing static obstacle constraints, dynamic obstacle constraints and actuator constraints, the probability of the underactuated unmanned boat colliding with static obstacles or dynamic obstacles during autonomous navigation is reduced. At the same time, by constructing actuator constraints to alleviate the problem that the optimal control output calculated by the model does not meet the internal mechanical characteristics constraints of the actuator of the underactuated unmanned boat, the safety and reliability of the autonomous navigation of the underactuated unmanned boat are effectively improved.

[0093] In some embodiments of the present invention, the optimal control output includes a commanded rudder angle and a commanded propeller speed. Accordingly, the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence are input into the underactuated unmanned vehicle predictive control model, and the optimal control output is predicted, including but not limited to:

[0094] The current actual navigation state and the expected track sequence of the underactuated unmanned vehicle are input into the predictive control model of the underactuated unmanned vehicle to predict the command rudder angle and the command propeller speed, which minimize the evaluation function value.

[0095] In this specific embodiment, the optimal control output of the underactuated unmanned boat in the control time domain predicted by the predictive control model of the underactuated unmanned boat includes the command rudder angle and the command propeller speed of the ship. Specifically, according to the design requirements of conventional surface ships, this embodiment only considers the three degrees of freedom of the ship's surge, sway and bow pitch, and describes the characteristics of multiple engines and propellers through first-order inertia relief, so as to obtain the mathematical motion model of the underactuated unmanned boat as shown in the following formula (15):

[0096]

[0097] Where (x, y) represents the position of the unmanned boat in the inertial coordinate system, ψ represents the heading of the unmanned boat in the inertial coordinate system; u represents the longitudinal speed of the unmanned boat; v represents the transverse speed of the unmanned boat; r represents the bow speed of the unmanned boat; m x is the additional mass of the unmanned boat on the x-axis; m y is the additional mass of the unmanned boat on the y-axis; I ZZ is the moment of inertia of the unmanned boat around the z axis; J ZZ is the additional mass moment of inertia of the unmanned boat; δ c is the command rudder angle; δ is the actual rudder angle; T R is the servo time constant; n c is the command speed; n is the actual speed, T P is the propeller characteristic constant; X is the resultant force on the ship on the x-axis; Y is the resultant force on the ship on the y-axis; N is the resultant moment on the ship on the z-axis. Figure 4 In this embodiment, the calculation formula of X, Y, and N is shown in the following formula (16):

[0098]

[0099] Among them, (X H , Y H , N H ) represent the forces and moments acting on the hull on the x, y, and z axes respectively; (X P , YP , N P ) represent the force and torque generated by the propeller on the x, y, and z axes respectively; (X R , Y R , N R ) represent the fluid power and torque generated by the servo on the x, y, and z axes respectively; (X E , Y E , N E ) represent the torque of external environment interference on x, y, and z axes respectively; x ce Indicates the coordinate of the ship center on the x-axis in the attached coordinate system.

[0100] At the same time, the external interferences to ships sailing at sea are mainly divided into wind, waves and currents. According to ship motion control, the disturbances of wind and waves to ship motion have basically the same characteristics. Therefore, in this embodiment (X E , Y E , N E ) is calculated as follows (17):

[0101]

[0102] Among them, [X W , Y W , N W ]′ and [X W , Y W , N W ]′ are the forces and moments generated by wind and flow on the x, y, and z axes respectively. W , Y W , N W ]′ and [X W , Y W , N W The calculation process of ]′ is shown in the following equations (18) and (19):

[0103]

[0104]

[0105] Among them, ρ in formula (18) W is the air density; V W is the relative wind speed; θ W is the relative wind direction angle; A f A is the orthographic projection area above the waterline of the ship; s C is the side projection area above the waterline of the ship; WX (θ W ) and C WY (θ W ) are the wind pressure coefficients on the x and y axes respectively; C WN (θW ) is the wind pressure moment coefficient around the z axis; in formula (19), ρ C is the fluid density; V C is the relative flow velocity; θ C is the relative flow angle; A fc A is the orthographic projection area of ​​the underwater part of the ship; sc C is the side projection area of ​​the underwater part of the ship; CX (θ C ) and C CY (θ C ) are the flow load coefficients on the x and y axes respectively; C CN (θ C ) is the flow load torque coefficient about the z-axis.

[0106] In this embodiment, the current actual navigation state S = [u, v, r, x, y, ψ, δ, n] of the underactuated unmanned boat and the expected track sequence are input into the underactuated unmanned boat predictive control model, so as to predict the command rudder angle and command propeller speed that minimize the evaluation function value, that is, the optimal control output. Thus, the actuator of the unmanned boat is controlled to output the corresponding command rudder angle and command propeller speed in the control time domain, so as to realize the autonomous navigation of the underactuated unmanned boat on the set expected track in complex waters.

[0107] In some embodiments of the present invention, the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence are input into the underactuated unmanned vehicle predictive control model to predict the command rudder angle and the command propeller speed, including but not limited to:

[0108] The current actual navigation state and the expected track sequence in the prediction time domain are input into the predictive control model of the underactuated unmanned vehicle, and the command rudder angle and command propeller speed are predicted.

[0109] In this specific embodiment, this embodiment first selects part of the expected track sequence in the prediction time domain, and then inputs the current actual navigation state and the selected expected track sequence into the predictive control model of the underactuated unmanned boat to predict the command rudder angle and the model propeller speed. Specifically, in this embodiment, the expected track sequence is selected in the prediction time domain according to the simple Euler integral method. Furthermore, the selected expected track sequence and the current actual navigation state of the unmanned boat are input into the predictive control model of the underactuated unmanned boat, and the optimal control output of the unmanned boat in the control time domain is predicted, so as to realize the autonomous navigation of the underactuated unmanned boat on the set expected track.

[0110] In some embodiments of the present invention, the predictive control model of the underactuated unmanned vehicle is shown in the following equations (20) and (21):

[0111] min C(I)=K pd Cpd (I)+K U C U (I)+K CoL C COL (I) (20)

[0112]

[0113] Among them, C(I) is the evaluation function, st is the constraint condition, C pd (I) is the track deviation, C U (I) is the change in speed, C COL (I) is the rule compliance, K pd , K U , K COL are the corresponding weight parameters, I is the input of the ship control mechanism, h s (S(I), p s , N p , r s ) is the static obstacle constraint condition, S(I) is the navigation state of the ship with I as input, and p s is the static obstacle coordinate set, r s is the radius set of static obstacle safety area, N p is the prediction time domain, h T (S(I), p T , N p , r T ) is the dynamic obstacle constraint condition, p T is the dynamic obstacle coordinate set, r T is the radius set of the dynamic obstacle safety area, h I (I) is the actuator constraint. It should be noted that in some embodiments of the present invention, when the expected speed of the unmanned boat during autonomous navigation is not limited to the specified value, the weight K of the speed change can be U Set to 0.

[0114] In some embodiments of the present invention, the actuator constraints include propeller speed constraints, rudder angle constraints, propeller speed change rate constraints, and rudder angle change rate constraints.

[0115] In this specific embodiment, the actuator of the underactuated unmanned boat is composed of a propeller and a steering gear. Therefore, the dynamic characteristics of the propeller and the steering gear are the constraints that need to be considered in the predictive control model of the underactuated unmanned boat. Specifically, the actuator constraints constructed in this embodiment include the constraints on the output size of the actuator and the constraints on the rate of change of the actuator. Among them, the output size constraint of the actuator [I min , I max ] TIncluding propeller speed n and rudder angle δ range constraints, that is, propeller speed constraints and rudder angle constraints. When the unmanned boat and a ship are in danger of collision and the encounter situation is right crossing or head-on encounter, the ship is the give-way ship, considering the sea commission to change the course to avoid the other ship, the corresponding I min ,I max The calculation formula is shown in formula (22):

[0116]

[0117] Among them, δ min is the minimum rudder angle, δ max is the maximum rudder angle, n min is the minimum propeller speed, n max is the maximum propeller speed, The input is the propeller speed of the ship at time t0, and sign(R) is the sign function of the collision risk R. This embodiment constructs propeller speed constraints and rudder angle constraints to ensure that the unmanned boat changes the rudder angle to complete the avoidance behavior in the encounter situation between the two ships without changing the propeller speed.

[0118] In addition, the actuator constraint condition of this embodiment also includes an actuator change rate constraint. Specifically, the actuator change rate constraint [I′ min , I′ max ] T Including propeller speed change rate n' and rudder angle change rate δ' range constraints, that is, propeller speed change rate constraints and rudder angle change rate constraints. Among them, the corresponding I' min , I′ max The calculation formula is shown in formula (23):

[0119]

[0120] It should be noted that, in the prediction time domain, the constraints imposed by the actuator of the unmanned boat in this embodiment on the unmanned boat's autonomous navigation are as shown in the following formula (24):

[0121]

[0122] Among them, h in the formula I (I) is a matrix of Np×4, and its calculation formula is shown in the following formula (25):

[0123]

[0124] In this embodiment, the change rate constraint of the actuator of the unmanned boat in the above formula (25) is solved by a difference equation, and the calculation is shown in the following formula (26):

[0125]

[0126] Among them, is the rudder angle value at time t0+k when I is used as input, is the rudder angle value at time t0+k-1 when I is used as input, is the propeller speed value at time t0+k when I is used as input, It is the propeller speed value at the moment t0+k-1 when I is used as input.

[0127] An embodiment of the present invention further provides an underactuated unmanned boat autonomous navigation system, comprising:

[0128] The track acquisition module is used to obtain the desired track.

[0129] The discretization module is used to discretize the expected track to obtain the expected track sequence.

[0130] The model building module is used to build a predictive control model for an underactuated unmanned boat, wherein the predictive control model for an underactuated unmanned boat includes an evaluation function and constraint conditions.

[0131] The prediction module is used to input the current actual navigation state and expected track sequence of the under-actuated unmanned vehicle into the under-actuated unmanned vehicle prediction control module to predict and obtain the optimal control output.

[0132] The execution module is used to determine that the under-actuated unmanned boat operates within the control time domain and control the actuator of the under-actuated unmanned boat to execute the optimal control output.

[0133] Reference Figure 2 An embodiment of the present invention further provides an underactuated unmanned boat autonomous navigation system, comprising:

[0134] At least one processor 210 .

[0135] At least one memory 220 is used to store at least one program.

[0136] When at least one program is executed by at least one processor 210, the at least one processor 210 implements the autonomous navigation method of the under-actuated unmanned boat as described in the above embodiment.

[0137] An embodiment of the present invention further provides a computer-readable storage medium, which stores computer-executable instructions. The computer-executable instructions are executed by one or more control processors, for example, to execute the steps described in the above embodiment.

[0138] It will be appreciated by those skilled in the art that all or some of the steps and systems in the disclosed method above may be implemented as software, firmware, hardware and appropriate combinations thereof. Some physical components or all physical components may be implemented as software executed by a processor, such as a central processing unit, a digital signal processor or a microprocessor, or may be implemented as hardware, or may be implemented as an integrated circuit, such as an application specific integrated circuit. Such software may be distributed on a computer-readable medium, which may include a computer storage medium (or a non-transitory medium) and a communication medium (or a temporary medium). As known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technologies, CD-ROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tapes, disk storage or other magnetic storage devices, or any other medium that may be used to store desired information and may be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically embodies computer readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transport mechanism, and may include any information delivery media.

[0139] The above is a specific description of the preferred implementation of the present invention, but the present invention is not limited to the above-mentioned implementation mode. Technical personnel familiar with the field can also make various equivalent deformations or substitutions without violating the spirit of the present invention. These equivalent deformations or substitutions are all included in the scope defined by the claims of the present invention.

Claims

1. An autonomous navigation method for an underactuated unmanned boat, characterized in that: The following steps are involved: Get the expected track; Discretizing the desired track to obtain a desired track sequence; Constructing a predictive control model for an underactuated unmanned boat; the predictive control model for an underactuated unmanned boat includes an evaluation function and constraint conditions; Inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output; Determining that the underactuated unmanned boat operates within a control time domain, and controlling an actuator of the underactuated unmanned boat to execute the optimal control output; The evaluation function includes track deviation, speed change and rule compliance; The construction of the predictive control model of the underactuated unmanned boat comprises: Constructing the evaluation function including the track deviation, the speed change and the rule compliance; The constraints include static obstacle constraints, dynamic obstacle constraints and actuator constraints; The constructing of the predictive control model of the underactuated unmanned boat also includes: Constructing the constraint conditions including the static obstacle constraint conditions, the dynamic obstacle constraint conditions and the actuator constraint conditions; Wherein, the optimal control output includes a commanded rudder angle and a commanded propeller speed; The step of inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output includes: Inputting the current actual navigation state of the underactuated unmanned boat and the expected track sequence into the underactuated unmanned boat prediction control model to predict the command rudder angle and the command propeller speed; wherein the command rudder angle and the command propeller speed minimize the evaluation function value; The predictive control model of the underactuated unmanned boat is as follows: minC(I)=K pd C pd (I)+K U C U (I)+K COL C COL (I) Wherein, C(I) is the evaluation function, st is the constraint condition, C pd (I) is the track deviation, C U (I) is the speed change, C COL (I) is the compliance degree of the rule, K pd , K U , K COL are the corresponding weight parameters, I is the input of the ship control mechanism, h s (S(I),p s ,N p ,r s ) is the static obstacle constraint condition, S(I) is the navigation state of the ship with I as the input, p s is the static obstacle coordinate set, r s is the radius set of static obstacle safety area, N p is the prediction time domain, h T (S(I),p T ,N p ,r T ) is the dynamic obstacle constraint condition, p T is the dynamic obstacle coordinate set, r T is the radius set of the dynamic obstacle safety area, h I (I) is the actuator constraint condition; The rule compliance is a dimensionless number that represents whether the unmanned boat meets relevant standards or rules during autonomous collision avoidance.

2. The autonomous navigation method of an underactuated unmanned boat according to claim 1, characterized in that: The step of inputting the current actual navigation state of the underactuated unmanned boat and the expected track sequence into the underactuated unmanned boat prediction control model to predict the command rudder angle and the command propeller speed includes: The current actual navigation state and the expected track sequence in the prediction time domain are input into the under-actuated unmanned vehicle predictive control model to predict the command rudder angle and the command propeller speed.

3. The autonomous navigation method of an underactuated unmanned boat according to claim 1, characterized in that: The actuator constraints include propeller speed constraints, rudder angle constraints, propeller speed change rate constraints, and rudder angle change rate constraints.

4. An underactuated unmanned boat autonomous navigation system, characterized in that: include: A track acquisition module is used to obtain the desired track; A discretization module, used for discretizing the desired track to obtain a desired track sequence; A model building module, used to build an underactuated unmanned boat predictive control model; the underactuated unmanned boat predictive control model includes an evaluation function and constraint conditions; A prediction module, used for inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output; An execution module, used for determining that the underactuated unmanned boat operates within a control time domain, and controlling an actuator of the underactuated unmanned boat to execute the optimal control output; The evaluation function includes track deviation, speed change and rule compliance; The construction of the predictive control model of the underactuated unmanned boat comprises: Constructing the evaluation function including the track deviation, the speed change and the rule compliance; The constraints include static obstacle constraints, dynamic obstacle constraints and actuator constraints; The constructing of the predictive control model of the underactuated unmanned boat also includes: Constructing the constraint conditions including the static obstacle constraint conditions, the dynamic obstacle constraint conditions and the actuator constraint conditions; Wherein, the optimal control output includes a commanded rudder angle and a commanded propeller speed; The step of inputting the current actual navigation state of the underactuated unmanned vehicle and the expected track sequence into the underactuated unmanned vehicle prediction control model to predict and obtain an optimal control output includes: Inputting the current actual navigation state of the underactuated unmanned boat and the expected track sequence into the underactuated unmanned boat predictive control model to predict the command rudder angle and the command propeller speed; wherein the command rudder angle and the command propeller speed minimize the evaluation function value; The predictive control model of the underactuated unmanned boat is as follows: minC(I)=K pd C pd (I)+K U C U (I)+K COL C COL (I) Wherein, C(I) is the evaluation function, st is the constraint condition, C pd (I) is the track deviation, C U (I) is the speed change, C cOL (I) is the compliance degree of the rule, K pd , K U , K COL are the corresponding weight parameters, I is the input of the ship control mechanism, h s (S(I),p s ,N p ,r s ) is the static obstacle constraint condition, S(I) is the navigation state of the ship with I as the input, p s is the static obstacle coordinate set, r s is the radius set of static obstacle safety area, N p is the prediction time domain, h T (S(I),p T ,N p ,r T ) is the dynamic obstacle constraint condition, p T is the dynamic obstacle coordinate set, r T is the radius set of the dynamic obstacle safety area, h I (I) is the actuator constraint condition; The rule compliance is a dimensionless number that represents whether the unmanned boat meets relevant standards or rules during autonomous collision avoidance.

5. An underactuated unmanned boat autonomous navigation system, characterized in that: include: at least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the autonomous navigation method of the under-actuated unmanned boat as described in any one of claims 1 to 3.

6. A computer storage medium storing a program executable by a processor, characterized in that: The program executable by the processor is used to implement the autonomous navigation method of the under-actuated unmanned boat as described in any one of claims 1 to 3 when executed by the processor.

Images