USV trajectory tracking control method introducing obstacle avoidance function

Abstract

The invention relates to the technical field of unmanned surface vessel control, in particular to a USV trajectory tracking control method introducing an obstacle avoidance function, and the method comprises the steps: obtaining a nonlinear control system model according to a mathematical model of USV water surface motion; constructing a prediction model of the nonlinear control system according to the nonlinear control system model; defining an obstacle avoidance rule; setting a cost function of the prediction model; and according to the input of the nonlinear control system at the current moment, obtaining a prediction state at the next moment when the cost function is minimized in a prediction time domain range based on an obstacle avoidance rule through a prediction model. The USV trajectory tracking control method is researched on the basis of the model prediction control method, and an obstacle avoidance function is introduced, that is, when a preset trajectory encounters an obstacle, a corresponding improvement scheme is provided for the method.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned surface vehicle control, and in particular to a USV trajectory tracking control method that introduces an obstacle avoidance function. Background Art

[0002] As an important part of modern marine technology, unmanned surface vessels (USVs) have shown a wide range of application value. In terms of marine monitoring, unmanned boats can perform tasks autonomously for a long time and collect marine environmental data in real time, which is helpful for climate change research and marine ecological protection. In addition, in maritime traffic management, unmanned boats have efficient cruising capabilities, which can effectively monitor ship dynamics and prevent accidents. In search and rescue operations, surface unmanned boats can quickly reach the scene of the accident and provide timely assistance with their flexible maneuverability and rapid response characteristics. When performing tasks, USVs often need to move along preset trajectories or preset trajectory points. The accuracy of trajectory tracking not only improves the operating efficiency of unmanned boats, but also enhances their reliability in complex tasks, laying a solid foundation for the intelligent and automated development of future marine operations. Therefore, research on the trajectory tracking problem of USVs is very necessary.

[0003] During the movement of USV, it is possible to encounter obstacles on the water surface, which will cause great damage to the USV. However, in existing research, USV trajectory tracking technology is often studied as an independent problem and rarely combined with obstacle avoidance. Therefore, how to combine USV trajectory tracking and obstacle avoidance is particularly important. Summary of the invention

[0004] In order to solve the deficiencies in the above-mentioned background technology, the present invention provides a USV trajectory tracking control method introducing an obstacle avoidance function. The method studies the USV trajectory tracking control method based on a model prediction control method, introduces an obstacle avoidance function, and combines USV trajectory tracking with obstacle avoidance, so that the USV can effectively avoid obstacles during navigation and avoid collision with obstacles.

[0005] The first object of the present invention is to provide a USV trajectory tracking control method introducing an obstacle avoidance function, comprising: Construct a mathematical model of underactuated USV motion on the water surface; Obtain a nonlinear control system model based on the mathematical model of USV surface motion; Construct a prediction model of nonlinear control system based on the nonlinear control system model; Set the reference state of the target point, as well as the location and size information of the obstacle; Define obstacle avoidance rules based on the current location of the USV and the location of the obstacle; Setting the cost function of the prediction model according to the current prediction state and the reference state of the prediction model; According to the input of the nonlinear control system model at the current moment, through the prediction model, based on the obstacle avoidance rule, and when the cost function is minimized within the prediction time domain, the prediction state at the next moment is obtained; wherein the prediction time domain refers to the period between the current moment and the next moment; The USV is controlled to navigate according to the predicted state at the next moment.

[0006] Preferably, the obstacle avoidance rules are as follows:

[0007] In the formula, ( x ( t ) ，y ( t )) is the position coordinate of USV in the global coordinate system; ( x ob ( t ), y ob ( t )) is the geometric center coordinate of the obstacle; L A is the total length of the USV; L safe The safe distance that needs to be maintained between the USV and obstacles; L ob It is the longest distance between the geometric center of the obstacle and the edge of the obstacle.

[0008] Preferably, obtaining the predicted state at the next moment includes: The input of the nonlinear control system at the current moment includes the current state of the system and the control sequence within the prediction time domain obtained according to the current state; wherein the control sequence refers to the control vector of the USV at each time point in the prediction time domain; According to the current state of the system and each control vector in the control sequence, the prediction state corresponding to each control vector is obtained through the prediction model; Based on the obstacle avoidance rule, the cost function is minimized according to the deviation between the current predicted state and the reference state, and the control vector corresponding to the minimization is obtained; According to the current state of the system and the control vector corresponding to the minimization, the predicted state at the next moment is obtained through the prediction model.

[0009] Preferably, the cost function is as follows:

[0010] In the formula, ,t Indicates the differential symbol with respect to the time variable; Δ T represents the prediction time domain; represents the system input at time τ; Indicates the current prediction status of the prediction model and reference state The deviation between Q and R denote positive definite symmetric weight diagonal matrices respectively.

[0011] Preferably, a mathematical model of the motion of an underactuated USV on the water surface is constructed, including a kinematic model and a dynamic model of the USV on the horizontal plane.

[0012] Preferably, the nonlinear control system model is as follows:

[0013] In the formula, t 0 is the initial time of the system; d ( t ) is the system input; l ( t ) is the system status; is the first-order derivative of the system state; f () is the nonlinear system function model.

[0014] Preferably, the prediction model of the nonlinear control system is as follows:

[0015] In the formula, l ( k ) is the system status; d ( k ) is the system input; F ( ) is the prediction k+i Nonlinear functions in the system output at time instant; i represents 1, 2, ..., n ; Represents the prediction status of the prediction model output.

[0016] Preferably, after obtaining the predicted state at the next moment, the method further includes: The prediction error is obtained by detecting the current output of the USV horizontal motion model system and the predicted state at the next moment; and the predicted state of the system is corrected according to the prediction error; The USV is controlled to navigate according to the corrected predicted state.

[0017] The present invention has at least the following beneficial effects: The present invention provides a USV trajectory tracking control method with an obstacle avoidance function. The method first constructs a horizontal motion model of an underactuated USV; secondly, a control method of an underactuated USV is designed based on the model, and on this basis, an obstacle avoidance function is introduced to combine USV trajectory tracking with obstacle avoidance, so that the USV can effectively avoid obstacles during navigation and avoid collision with obstacles. The content of the present invention is simulated in a scenario where a preset trajectory encounters an obstacle, and the effectiveness of the method of the present invention is verified.

[0018] The USV trajectory tracking model prediction method designed by the present invention does not need to build an accurate nonlinear model, which effectively solves the problem of difficulty in derivation in the process of solving optimization problems. It only needs past input and output information and current system status information to predict the output information at the next moment based on the input information at the next moment.

[0019] The present invention effectively combines USV trajectory tracking with obstacle avoidance by introducing an obstacle avoidance mechanism, which is more in line with the needs of practical engineering. The method of the present invention is combined with the actual kinematic and dynamic models of USV, which is more in line with practical significance. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is a flow chart of the technical solution of the invention.

[0021] Figure 2 This is the schematic diagram of USV motion obstacle avoidance.

[0022] Figure 3 This is the simulation result diagram of USV running trajectory.

[0023] Figure 4 The actual speed and angular velocity change diagram of USV.

[0024] Figure 5 This is the actual control input change diagram of USV. DETAILED DESCRIPTION

[0025] In order to illustrate the technical means and effects adopted by the present invention to achieve the predetermined purpose of the invention, the following is a detailed description in conjunction with embodiments.

[0026] The purpose of this invention is to make up for the shortcomings of the prior art and introduce the obstacle avoidance function into the unmanned surface vehicle (USV) trajectory tracking control method. When performing a mission, the USV usually moves along a preset trajectory, so the research on the USV single body trajectory tracking is crucial. Figure 1As shown in the flowchart, the present invention studies the trajectory tracking control method of USV based on the control method of model prediction, and introduces the obstacle avoidance function, that is, when the preset trajectory encounters an obstacle, a corresponding improvement plan is proposed for the present method. First, a horizontal plane motion model of the under-actuated USV is constructed; secondly, a trajectory tracking method of the under-actuated USV is designed based on the model prediction method, and the obstacle avoidance function is introduced on this basis; finally, the content of the present invention is simulated in the scenario where the preset trajectory encounters an obstacle, and the effectiveness of the method of the present invention is verified.

[0027] In order to achieve the above object, the present invention provides a USV trajectory tracking control method introducing an obstacle avoidance function, comprising: Construct a mathematical model of underactuated USV motion on the water surface; Among them, a mathematical model of the under-actuated USV's surface motion is constructed, including the kinematic model and the dynamic model of the USV on the horizontal plane.

[0028] Obtain a nonlinear control system model based on the mathematical model of USV surface motion; Wherein, the nonlinear control system model is as follows:

[0029] In the formula, t 0 is the initial time of the system; d ( t ) is the system input; l ( t ) is the system status; is the first-order derivative of the system state; f () is the nonlinear system function model.

[0030] Construct a prediction model of nonlinear control system based on the nonlinear control system model; Wherein, the prediction model of the nonlinear control system is as follows:

[0031] In the formula, l ( k ) is the system status; d ( k ) is the system input; F ( ) is the prediction k+i Nonlinear functions in the system output at time instant; i represents 1, 2, ..., n ; Represents the prediction status of the prediction model output.

[0032] Set the reference state of the target point, as well as the location and size information of the obstacle; Define obstacle avoidance rules based on the current location of the USV and the location of the obstacle; The obstacle avoidance rules are as follows:

[0033] In the formula, ( x ( t ) ，y ( t )) is the position coordinate of USV in the global coordinate system; ( x ob ( t ), y ob ( t )) is the geometric center coordinate of the obstacle; L A is the total length of the USV; L safe The safe distance that needs to be maintained between the USV and obstacles; L ob It is the longest distance between the geometric center of the obstacle and the edge of the obstacle.

[0034] Setting the cost function of the prediction model according to the current prediction state and the reference state of the prediction model; The cost function is as follows:

[0035] In the formula, , t Indicates the differential symbol with respect to the time variable; Δ T represents the prediction time domain; represents the system input at time τ; Indicates the current prediction status of the prediction model and reference state The deviation between Q and R denote positive definite symmetric weight diagonal matrices respectively.

[0036] According to the input of the nonlinear control system model at the current moment, through the prediction model, based on the obstacle avoidance rule, and when the cost function is minimized within the prediction time domain, the prediction state at the next moment is obtained; wherein the prediction time domain refers to the period between the current moment and the next moment; The USV is controlled to navigate according to the predicted state at the next moment.

[0037] When obtaining the predicted status of the next moment, it includes: The input of the nonlinear control system at the current moment includes the current state of the system and the control sequence within the prediction time domain obtained according to the current state; wherein the control sequence refers to the control vector of the USV at each time point in the prediction time domain; According to the current state of the system and each control vector in the control sequence, the prediction state corresponding to each control vector is obtained through the prediction model; Based on the obstacle avoidance rule, the cost function is minimized according to the deviation between the current predicted state and the reference state, and the control vector corresponding to the minimization is obtained; According to the current state of the system and the control vector corresponding to the minimization, the predicted state at the next moment is obtained through the prediction model.

[0038] After obtaining the predicted state of the next moment, it also includes: The prediction error is obtained by detecting the current output of the USV horizontal motion model system and the predicted state at the next moment; and the predicted state of the system is corrected according to the prediction error; The USV is controlled to navigate according to the corrected predicted state.

[0039] In order to further illustrate the control method provided by the present invention, it is described in conjunction with the accompanying drawings.

[0040] See also Figure 1 As shown, a USV trajectory tracking control method introducing an obstacle avoidance function comprises: Step 1: Construct a mathematical model of the motion of an underactuated USV on the water surface A mathematical model of the underactuated USV's motion on the water surface is constructed, including the kinematic model and the dynamic model of the USV on the horizontal plane.

[0041] Step 1-1: Establish the kinematic and dynamic model of the underactuated USV on the horizontal plane; USV is a surface vessel that can move in a two-dimensional plane. In order to study the motion law of USV with three degrees of freedom on the horizontal plane, the present invention uses longitudinal velocity, lateral velocity and yaw angular velocity to describe the horizontal motion of USV. Then the kinematic model of USV on the horizontal plane is: (1) In the formula, x and y is the coordinate of USV in the global coordinate system; ψ is the heading angle, u , v , r are the longitudinal velocity, lateral velocity and yaw angular velocity of the USV respectively; the dynamic model of the USV on the horizontal plane is: (2) In the formula, X is the forward thrust of the USV drive unit; N is the yaw moment; by adjusting these two variables, the USV's motion on the horizontal plane is controlled. Since the USV is an under-actuated surface vessel, the lateral thrust Y =0; m 11 , m 22 , m 33 represents the generalized mass of the USV including the additional mass; d 11 =- X u - X u|u| | u |, d 22 =- Y v - Y v|v| | v |,d 33 =- N r - N r|r| | r |; X u , Y v , N r is the linear damping coefficient, X u|u| , Y v|v| , N r|r| is the secondary damping coefficient.

[0042] Step 1-2: Obtain a nonlinear control system model based on the mathematical model of USV surface motion; Specifically, the horizontal plane model of USV motion is rewritten into a nonlinear control system model. l (t)=[ x , y , ψ , u , v , r ] T For USV t The state vector at time, d (t)=[ X , N ] Tis the control input vector. Since the horizontal plane model of USV motion is nonlinear control, the nonlinear control system model is as follows: (3) In the formula, t 0 is the initial time of the system; d ( t ) is the system input; l ( t ) is the system status; is the first-order derivative of the system state; f () is the nonlinear system function model.

[0043] Step 2: Model Predictive Controller Design Step 2-1: Prediction Model Construct a prediction model of nonlinear control system based on the nonlinear control system model; In this embodiment, the nonlinear control system model is k The state equation at the moment is as follows: (4) In the formula, d ( k ) is the system input; Y ( k ) is the system output; l ( k ) is the system status; is the mapping function from system state to system output.

[0044] According to the model predictive control theory, k The initial state of the nonlinear control system at time l ( k ) and the current and future system inputs d ( k ), d ( k +1), ..., to construct the prediction model of nonlinear control system as follows: (5) According to formula (5), i =1 starts recursion, and thus predicts k + i The system output at this moment is: (6) It should be noted that when solving the optimization target problem, it is often necessary to find the derivative solution, and the prediction k+i Nonlinear function in the system output at time F() is composed of two nonlinear functions. It is often difficult or even impossible to find the exact derivative form when calculating the derivative. However, this prediction model often does not require an exact form. It only needs the past input and output information and the current system state information to predict the output information at the next moment based on the input information at the next moment.

[0045] In the formula, F () is the function f () and functions g () is a nonlinear function. It can be seen from formula (6) that the system output of the nonlinear system at any time in the future is only related to the current state and input of the system and the control input at the future time.

[0046] Step 2-2: Define obstacle avoidance rules Set the reference state of the target point, as well as the location and size information of the obstacle; Define obstacle avoidance rules based on the current location of the USV and the location of the obstacle; like Figure 2 As shown in the figure, in view of the situation that the USV may encounter obstacles during trajectory tracking, the USV obstacle avoidance problem is described as follows: Assuming that the geometric center of the obstacle is known, it is necessary to ensure that each point on the USV's running trajectory maintains a safe distance from the obstacle. Therefore, this problem is converted into an inequality constraint form in the optimization problem in the prediction model. The specific obstacle avoidance rules are as follows: (7) In the formula, ( x ( t ) ，y ( t )) is the position coordinate of USV in the global coordinate system; ( x ob ( t ), y ob ( t )) is the geometric center coordinate of the obstacle; L A is the total length of the USV; L safe The safe distance that needs to be maintained between the USV and obstacles; L ob It is the longest distance between the geometric center of the obstacle and the edge of the obstacle.

[0047] Step 2-3: Rolling Optimization Setting the cost function of the prediction model according to the current prediction state and the reference state of the prediction model; It should be noted that the prediction model of nonlinear control system, namely trajectory tracking prediction model, is to solve an optimal control problem in the backward time domain under constraints, with the purpose of determining the next acceptable control behavior. t At this moment, the state of the system l (t) is measurable, based on l (t) can calculate the control sequence (δ*(t),δ*(t+1),…,δ*(t+P-1)) within the prediction time domain P, and then apply the first element δ*(t) of the control vector in the system. The above process is repeated at all future moments. Among them, a predicted control vector is output at each time point. The goal of this control algorithm is to minimize the following cost function, where the cost function is as follows: (8) In the formula, , τ represents the differential symbol with respect to the time variable, ΔT represents the prediction time domain, represents the system input at time τ, Represents the current prediction output of the prediction model and reference output The deviation between W×W and R N×N denote positive definite symmetric weight diagonal matrices respectively.

[0048] As described in step 1, for the horizontal motion model of the underactuated USV, the dimension of the state variables is 6, so W =6; the dimension of the control variable is 2, so N =2.

[0049] According to the input of the nonlinear control system at the current moment, through the prediction model, based on the obstacle avoidance rule, and when the cost function is minimized within the prediction time domain, the prediction state at the next moment is obtained; wherein the prediction time domain refers to the period between the current moment and the next moment; When obtaining the predicted status of the next moment, it includes: The input of the nonlinear control system at the current moment includes the current state of the system and the control sequence within the prediction time domain obtained according to the current state; wherein the control sequence refers to the control vector of the USV at each time point in the prediction time domain; According to the current state of the system and each control vector in the control sequence, the prediction state corresponding to each control vector is obtained through the prediction model; Based on the obstacle avoidance rule, the cost function is minimized according to the deviation between the current predicted state and the reference state, and the control vector corresponding to the minimization is obtained; According to the current state of the system and the control vector corresponding to the minimization, the predicted state at the next moment is obtained through the prediction model.

[0050] Specifically, the obstacle avoidance constraint (7) in step 2-2 is added to the cost function (8), thus completing the design of the USV trajectory tracking model predictive controller with the obstacle avoidance function.

[0051] Step 3: Feedback Correction According to the present invention, after obtaining the predicted state at the next moment, the method further includes: The prediction error is obtained by detecting the current output of the USV horizontal motion model system and the predicted state at the next moment; and the predicted state of the system is corrected according to the prediction error; The USV is controlled to navigate according to the corrected predicted state.

[0052] In the feedback correction process, the current output of the USV horizontal motion model system is detected and fed back to the predicted output for correction, thereby obtaining the current k The prediction error at time (9) In the formula, e ( k ) is the output prediction error at the current moment, Y ( k ) is the actual output of the system at the current moment, ( k | k -1) is the k The predicted output at time.

[0053] According to the k The error information at the moment is used to predict k+i The prediction error can be expressed as (10) In the formula, E i () is the k+i The error function at time, l is the length of the historical error moment.

[0054] Then, the prediction error formula (10) is used to correct the system's prediction output formula (6), and the output feedback prediction is obtained as follows: (11) Will Substitute into formula (8) , feedback correction can be achieved.

[0055] In order to further illustrate a USV trajectory tracking control method with obstacle avoidance function provided by the present invention, a specific embodiment is given to verify the effect of the technical solution of the present invention: Now assume that the USV starts from the starting point and the initial state is l 0(t)=[ x 0, y 0, ψ 0, u 0, v 0, r 0] T =[0,0,0,0,0,0] T , the reference state of the target point to be reached Y d (t)=[ x d , y d , ψ d , u d , v d , r d ] T =[70,70,pi / 4,1.5,0,0] T The obstacle is assumed to be circular, with its geometric center as its center and coordinates ( x ob (t), y ob (t)) is set to (30,30), and the radius L ob Set to 2m, the safe distance that the USV needs to maintain from obstacles L safe =3m. Assume that the total length of USV is L A The thrust of the propeller is 3m. X and yaw moment Z There are upper and lower limits, and the range is set to ±20N(m).

[0056] The sampling period of the algorithm optimization parameter is 0.2s, and the prediction time domain length P =12, which means the predicted time is 2.4s. Diagonal matrix Q 6×6 =diag(0.001,0.001,0.001,100,100,10), R 2×2 =diag(0.001,0.001).

[0057] The simulation results of USV obstacle avoidance are shown in the figure below. Figure 3As shown in the figure, the speed and angular velocity of the USV change during the simulation. Figure 4 As shown, Figure 4 a is the change of USV longitudinal speed, Figure 4 b in the middle is the lateral velocity change of USV. Figure 4 In the figure, c is the change of the USV yaw angular velocity. It can be seen that when the USV approaches an obstacle, the speed and angular velocity change to avoid the obstacle and then move towards the target point. Finally, the position of the USV coincides with the target point. Figure 5 As shown, Figure 5 a in the middle is the change of USV forward thrust, Figure 5 b is the change of the yaw moment of the USV. The position change of the USV is achieved by adjusting the two-dimensional control variables, and the simulation results verify the effectiveness and correctness of the present invention.

[0058] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A USV trajectory tracking control method introducing an obstacle avoidance function, characterized in that: include: Construct a mathematical model of underactuated USV motion on the water surface; Obtain a nonlinear control system model based on the mathematical model of USV surface motion; Construct a prediction model of nonlinear control system based on the nonlinear control system model; Set the reference state of the target point, as well as the location and size information of the obstacle; Define obstacle avoidance rules based on the current location of the USV and the location of the obstacle; Setting the cost function of the prediction model according to the current prediction state and the reference state of the prediction model; According to the input of the nonlinear control system model at the current moment, through the prediction model, based on the obstacle avoidance rule, and when the cost function is minimized within the prediction time domain, the prediction state at the next moment is obtained; wherein the prediction time domain refers to the period between the current moment and the next moment; The USV is controlled to navigate according to the predicted state at the next moment.

2. The USV trajectory tracking control method with obstacle avoidance function according to claim 1 is characterized in that: The obstacle avoidance rules are as follows: In the formula, ( x ( t ) ，y ( t )) is the position coordinate of USV in the global coordinate system; ( x ob ( t ), y ob ( t )) is the geometric center coordinate of the obstacle; L A is the total length of the USV; L safe The safe distance that needs to be maintained between the USV and obstacles; L ob It is the longest distance between the geometric center of the obstacle and the edge of the obstacle.

3. The USV trajectory tracking control method with obstacle avoidance function introduced in claim 1 is characterized in that: When obtaining the predicted status of the next moment, it includes: The input of the nonlinear control system at the current moment includes the current state of the system and the control sequence within the prediction time domain obtained according to the current state; wherein the control sequence refers to the control vector of the USV at each time point in the prediction time domain; According to the current state of the system and each control vector in the control sequence, the prediction state corresponding to each control vector is obtained through the prediction model; Based on the obstacle avoidance rule, the cost function is minimized according to the deviation between the current predicted state and the reference state, and the control vector corresponding to the minimization is obtained; According to the current state of the system and the control vector corresponding to the minimization, the predicted state at the next moment is obtained through the prediction model.

4. The USV trajectory tracking control method with obstacle avoidance function according to claim 3 is characterized in that: The cost function is as follows: In the formula, , τ Indicates the differential symbol with respect to the time variable; Δ T represents the prediction time domain; represents the system input at time τ; Indicates the current prediction status of the prediction model and reference state The deviation between Q and R denote positive definite symmetric weight diagonal matrices respectively.

5. The USV trajectory tracking control method with obstacle avoidance function introduced in claim 1 is characterized in that: A mathematical model of the underactuated USV's motion on the water surface is constructed, including the kinematic model and the dynamic model of the USV on the horizontal plane.

6. The USV trajectory tracking control method with obstacle avoidance function introduced in claim 1 is characterized in that: The nonlinear control system model is as follows: In the formula, t 0 is the initial time of the system; δ ( t ) is the system input; λ ( t ) is the system status; is the first-order derivative of the system state; f () is the nonlinear system function model.

7. The USV trajectory tracking control method with obstacle avoidance function according to claim 1 is characterized in that: The prediction model of the nonlinear control system is as follows: In the formula, λ ( k ) is the system status; δ ( k ) is the system input; F ( ) is the prediction k+i Nonlinear functions in the system output at time instant; i represents 1, 2, ..., n ; Represents the prediction status of the prediction model output.

8. The USV trajectory tracking control method with obstacle avoidance function introduced in claim 1 is characterized in that: After obtaining the predicted state of the next moment, it also includes: The prediction error is obtained by detecting the current output of the USV horizontal motion model system and the predicted state at the next moment; and the predicted state of the system is corrected according to the prediction error; The USV is controlled to navigate according to the corrected predicted state.

Images