A control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform

Abstract

The present invention discloses a method for controlling the autonomous take-off and landing of a multi-rotor UAV on a moving unmanned ship platform, the method comprising: modeling the motion of the UAV and the unmanned ship based on the model parameters of the UAV and the unmanned ship, designing a feedforward controller based on the model predictive control method according to the directed particle motion model, so that the UAV can achieve optimal control under ideal conditions during autonomous take-off and landing; introducing the optimal state of the directed particle into the back-end controller of the nonlinear geometric tracking control, and introducing error variables to adapt to the interference brought to the control system by the flight environment and the changes in the dynamic characteristics of the aircraft itself. The control method of the present invention combines the model predictive control with the geometric control of the quad-rotor UAV, so that the entire controller can accurately and robustly track the unmanned ship in the nonlinear motion state, and realize the precise autonomous take-off and landing control of the UAV on the unmanned ship.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a control method for autonomous take-off and landing of a multi-rotor unmanned aerial vehicle on an unmanned ship platform. Background Art

[0002] In recent years, unmanned systems have been widely developed due to their low cost, high flexibility, and good portability. However, in the process of unmanned system development, each system has its own advantages and shortcomings that are difficult to overcome. The coordination of heterogeneous unmanned systems can achieve the goal of complementing each other's strengths and maximizing efficiency. The demand for rotary-wing drones and unmanned ships to complete tasks in cooperation is increasing in the civil and military fields.

[0003] Research data shows that the number of failures during the recovery process of rotorcraft drones accounts for more than 80% of the total number of failures in the entire mission. At present, the control method of multi-rotor drones mostly adopts a single PID control algorithm, but the PID control method with determined parameters is difficult to adapt to the random movement of unmanned ships, resulting in failure of the controller during take-off and landing. In addition, the position of the unmanned ship changes rapidly at sea, resulting in a sharp drop in the drone's following efficiency and the shaking of the unmanned ship, making it extremely difficult for the drone to take off and land. Summary of the invention

[0004] In response to the above-mentioned technical problems, the present invention designs a control method for the autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform, which improves the unmanned control accuracy, increases the robustness of the take-off and landing process and the maneuverability of the UAV flight.

[0005] The present invention aims at a control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform, comprising the following steps:

[0006] S1: Establish a three-dimensional motion model based on directed particle motion and an unmanned ship motion model.

[0007] S2: According to the motion model of the directed particle and the unmanned ship, a model predictive control method is designed to control the directed particle and use the updated state of the particle model as the expected state of the UAV.

[0008] S3: Establish the dynamic model of the multi-rotor UAV.

[0009] S4: Design a control method based on geometric tracking control to obtain the desired thrust of each propeller of the UAV.

[0010] S5: Calculate the speed of each motor according to the propeller thrust-speed curve relationship to achieve control of the drone.

[0011] Further, the step S1 comprises the following steps:

[0012] S11: Establish a three-dimensional motion model of directed particles. The model is described as follows:

[0013] x [t] =Ax [t-1] +Bu [t] (1)

[0014]

[0015] in is the three-dimensional state of a directed particle in the world coordinate system, including position P = [x, y, z] T ,speed Acceleration Yaw angle η, yaw angular velocity A and B are related to the motion state transfer matrix and the control time Δt. is the simulation control quantity of the directed particle model.

[0016] S12: Establish a three-dimensional motion model of the unmanned boat platform. The model description is:

[0017]

[0018] Among them, P p =[x p ,y p ,z p ] T represents the position of the unmanned ship in the world coordinate system, v represents the forward speed of the unmanned ship in the world coordinate system, ψ represents the yaw angle of the unmanned ship, represents the yaw angular velocity of the unmanned ship, represents the vertical velocity of the unmanned ship in the world coordinate system, and Δt represents the sampling time of the unmanned ship sensor.

[0019] Furthermore, the step S2 includes the following steps:

[0020] S21: Design of Model Predictive Controller for Directed Mass Models:

[0021] The method used is the QP formula of the least squares problem to find the optimal control action in the future prediction range. The cost function J used is:

[0022]

[0023] in, is the simulation control quantity of the directed particle model, e = xx d , represents the current position and speed of the unmanned ship, m is the prediction step length, x L x U represents the state constraint of the simulated directed particle model, u L u UIt represents the control constraint of the simulated directed mass model. The state update model adopts the directed mass kinematic model and the unmanned ship kinematic model established by S1.

[0024] S22: The control quantity obtained by the model predictive controller Substituting the directed particle motion model into the simulation model, the updated state is recorded as but is the desired state vector of the UAV, including

[0025] Furthermore, the dynamic model of the multi-rotor UAV established in step S3 is described as follows:

[0026]

[0027] Where p = [x q ,y q ,z q ] T represents the position of the drone in the world coordinate system, v represents the speed of the drone in the world coordinate system, m represents the mass of the drone, and f represents the total thrust of the drone. Represents the angular velocity of the drone in the body coordinate system, The skew-symmetric matrix representing the angular velocity is J represents the inertia matrix of the drone, g represents the acceleration due to gravity, and e3 = [0 0 1] T , M=[M1 M2 M3] T represents torque, R is the rotation matrix, which is obtained by rotating the drone along the Euler angles ψ, φ, θ, where ψ, φ, θ represent the yaw angle, roll angle, and pitch angle, respectively. When the multi-rotor drone is a quad-rotor drone and the propellers are symmetrically arranged, the following relationship exists:

[0028]

[0029] Among them, f1, f2, f3, and f4 are the thrusts of the four propellers of the quadrotor drone, and d is the distance from a single propeller of the quadrotor drone to the center of mass.

[0030] Furthermore, the control method based on geometric tracking control designed in step S4 includes the following steps:

[0031] S41: Calculate the total thrust f required for control:

[0032]

[0033] Among them, e p =pp d is the position error, e v =vv dis the speed error, e3=

[001] T , is the desired acceleration, k p ,k v ,k ib ,k iw is the thrust control gain, is the internal interference error, is the external interference error.

[0034] S42: Based on the expected yaw angle ψ d And control the total vector thrust f to calculate the rotation matrix control quantity R C :

[0035] R C =[b 1,C b 2,C b 3,C ]

[0036] According to the definition of rotation matrix:

[0037] b 2,d =[-sinψ d cosψ d 0] T (11)

[0038] but:

[0039]

[0040] S43: Calculate angular velocity control amount Ω C :

[0041]

[0042]

[0043] According to the UAV dynamics formula We can get:

[0044]

[0045] According to Ω C The skew-symmetric matrix Get Ω C .

[0046] S44: Calculate the torque M required for control:

[0047]

[0048] Among them, e R is the angle error, e Ω is the angular velocity error, k R ,kΩ Control gain for angle.

[0049] S45: Calculate the expected thrust f for each propeller i :

[0050]

[0051] Furthermore, the propeller thrust-speed curve formula involved in step S5 is:

[0052]

[0053] Among them, a t ,b t is the thrust speed curve parameter, f d is the expected thrust, T d is the desired propeller speed

[0054] Furthermore, the formulas (9) and (17) are thrust calculation formulas for a quad-rotor drone, and the present invention can also be extended to other multi-rotor drones.

[0055] The beneficial effects of the present invention are:

[0056] (1) The model predictive control method based on the ideal directed particle model and the unmanned ship motion model proposed in the present invention predicts the state of the unmanned ship in the controller, controls and tracks the unmanned ship on the ideal control model, and gives the optimal solution for the control quantity of the unmanned aircraft, thereby improving the accuracy of the autonomous landing of the multi-rotor unmanned aircraft on the unmanned ship platform.

[0057] (2) The control method based on geometric tracking control proposed in the present invention has a global zero balance performance for the tracking error. It completely avoids the singularity and complexity that occurs when using local coordinates. It introduces error variables to adapt to the interference brought to the control system by changes in the flight environment and changes in the dynamic characteristics of the aircraft itself. It uses four thrust inputs to stabilize the translation and rotation degrees of freedom, and asymptotically tracks the position and heading, thereby improving the maneuverability and robustness of the vertical lift UAV for autonomous landing on the unmanned ship platform. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 A flowchart of a control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform according to an embodiment of the present invention;

[0059] Figure 2 A control block diagram of a control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform according to an embodiment of the present invention;

[0060] Figure 3A simulated unmanned ship three-axis track diagram of a control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform according to an embodiment of the present invention;

[0061] Figure 4 The MPC controller designed in the control method of the autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform in the embodiment of the present invention controls the simulated directed particle tracking unmanned ship track and error map;

[0062] Figure 5 The MPC controller designed for the control method of a multi-rotor UAV autonomous take-off and landing on an unmanned boat platform according to an embodiment of the present invention controls the simulated directed particle tracking unmanned boat control output diagram;

[0063] Figure 6 A control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform according to an embodiment of the present invention simulates a directed particle tracking diagram of the position, velocity, and acceleration state changes of the unmanned ship;

[0064] Figure 7 A comparison diagram of the position trajectory of a real UAV controlled by a geometric control method and the position trajectory of an unmanned ship tracked by a directed particle in an embodiment of the present invention;

[0065] Figure 8 A comparison diagram of the speed state of a real drone and the speed state of a directed particle tracking unmanned ship, using a geometric control method designed for a control method of autonomous takeoff and landing of a multi-rotor drone on an unmanned ship platform in an embodiment of the present invention;

[0066] Fig. 9 An unmanned boat, a simulated directed particle, and a three-dimensional track diagram of a multi-rotor unmanned aircraft in a control method for autonomous take-off and landing of a multi-rotor unmanned aircraft on an unmanned boat platform according to an embodiment of the present invention;

[0067] Fig.10 A propeller thrust curve diagram of a quadrotor drone in a method for controlling the autonomous take-off and landing of a multi-rotor drone on an unmanned ship platform according to an embodiment of the present invention;

[0068] Fig.11 A quad-rotor drone propeller speed curve diagram of a control method for autonomous take-off and landing of a multi-rotor drone on an unmanned ship platform according to an embodiment of the present invention. DETAILED DESCRIPTION

[0069] The present invention is further described in detail below through the accompanying drawings and preferred embodiments.

[0070] refer to Figure 1 , Figure 2 , Figure 1 A flow chart of a control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform provided by the present invention, Figure 2 The control block diagram includes the following steps:

[0071] S1: Establish a three-dimensional motion model based on directed particle motion and an unmanned ship motion model.

[0072] S2: According to the motion model of the directed particle and the unmanned ship, a model predictive control method is designed to control the directed particle and use the updated state of the particle model as the expected state of the UAV.

[0073] S3: Establish the dynamic model of the multi-rotor UAV.

[0074] S4: Design a control method based on geometric tracking control to obtain the desired thrust of each propeller of the UAV.

[0075] S5: Calculate the speed of each motor according to the propeller thrust-speed curve relationship to achieve control of the drone.

[0076] Furthermore, the step S1 includes the following steps:

[0077] S11: Establish a three-dimensional kinematic model of directed particle motion. The model is described as follows:

[0078] x [t] =Ax [t-1] +Bu [t] (1)

[0079]

[0080] in is the three-dimensional state of a directed particle in the world coordinate system, including position P = [x, y, z] T ,speed Acceleration Yaw angle η, yaw angular velocity A and B are kinematic state transfer matrices related to the control time Δt. is the simulation control quantity of the directed mass model, and the initial state of the drone is set to x0 = [-20, 0, 0, 20, 0, 0, 10, 0, 0, 0, 0] T , control time Δt is 0.01s, u0=[0,0,0,0] T .

[0081] S12: Establish a three-dimensional motion model of the unmanned boat platform. The model description is:

[0082]

[0083] Among them, P p =[x p ,y p ,zp ] T represents the position of the unmanned ship in the world coordinate system, v represents the forward speed of the unmanned ship in the world coordinate system, ψ represents the yaw angle of the unmanned ship, represents the yaw angular velocity of the unmanned ship, represents the vertical velocity of the unmanned ship in the world coordinate system, Δt represents the iteration time of the unmanned ship. Assuming that the unmanned ship is affected by the waves and has a relatively strong nonlinear motion in the Y and Z directions, the navigation trajectory is as follows: Figure 3 shown.

[0084] Furthermore, the step S2 includes the following steps:

[0085] S21: Design of Model Predictive Controller for Directed Mass Models:

[0086] The method used is the QP formula of the least squares problem to find the optimal control action in the future prediction range. The cost function J used is:

[0087]

[0088] in, is the simulation control quantity of the directed particle model, e = xx d , represents the current position and speed of the unmanned ship, m is the prediction step length, x L x U represents the state constraint of the simulated directed particle model, u L u U represents the control constraints of the simulated directed mass model. The state update model adopts the directed mass kinematic model and the unmanned ship kinematic model established by S1, where u L =[-5,-5,-5,-1.5]u U =[5,5,5,1.5], Q=diag[5,5,3,2,2,2,2,2,2,1,1], P=diag[0.1,0.1,0.1,0.1].

[0089] S22: The control quantity obtained by the model predictive controller Substituting the directed particle motion model into the simulation model, the updated state is recorded as but is the desired state vector of the UAV, including

[0090] refer to Figure 4 , shows the particle track diagram and error diagram of the directed particle tracking unmanned ship controlled by the above control method, which shows the steady-state error of the three axes. It can be seen that the steady-state error fluctuates around 0 in a very small range, achieving a good control effect. Figure 5, it can be clearly observed that the control output is limited within the control constraints, Figure 6 Gives the state required by the backend geometry controller.

[0091] Furthermore, the dynamic model of the multi-rotor UAV established in step S3 is described as follows:

[0092]

[0093] Where p = [x q ,y q ,z q ] T represents the position of the drone in the world coordinate system, v represents the speed of the drone in the world coordinate system, m represents the mass of the drone, and f represents the total thrust of the drone. Represents the angular velocity of the drone in the body coordinate system, The skew-symmetric matrix representing the angular velocity is J represents the inertia matrix of the drone, g represents the acceleration due to gravity, and e3 = [0 0 1] T , M=[M1 M2 M3] T represents torque, set m = 4.38, J = diag [0.0820, 0.0845, 0.1377], the initial state is the same as the initial state of the directed particle, R is the rotation matrix, obtained by rotating the drone along the Euler angles ψ, φ, θ, ψ, φ, θ represent the yaw angle, roll angle and pitch angle respectively. When the multi-rotor drone is a quad-rotor drone and the propellers are symmetrically arranged, the following relationship exists:

[0094]

[0095] Among them, f1, f2, f3, and f4 are the thrusts of the four propellers of the quadrotor drone respectively, and d is the distance from a single propeller of the quadrotor drone to the center of mass, and d is set to 0.315.

[0096] Furthermore, the control method based on geometric tracking control designed in step S4 includes the following steps:

[0097] S41: Calculate the total thrust f required for control:

[0098]

[0099] Among them, e p =pp d is the position error, e v =vv d is the speed error, e3=

[001] T , is the desired acceleration, and the thrust control gain k is taken p=80,k v =24,k ib =2,k iw =5, is the internal disturbance error, such as the drone imbalance, is the external disturbance error, such as wind.

[0100] S42: Based on the expected yaw angle ψ d And control the total vector thrust f to calculate the rotation matrix control quantity R C :

[0101] R C =[b 1,C b 2,C b 3,C ]

[0102] According to the definition of rotation matrix:

[0103] b 2,d =[-sinψ d cosψ d 0] T (11)

[0104] but:

[0105]

[0106] S43: Calculate angular velocity control amount Ω C :

[0107]

[0108] According to the UAV dynamics formula We can get:

[0109]

[0110] According to Ω C The skew-symmetric matrix Get Ω C .

[0111] S44: Calculate the torque M required for control:

[0112]

[0113] Among them, e R is the angle error, e Ω is the angular velocity error, and the angle control gain k is taken R =8.81,k Ω =2.54.

[0114] S45: Calculate the expected thrust f for each propeller i :

[0115]

[0116] Furthermore, the propeller thrust-speed curve formula involved in step S5 is:

[0117]

[0118] Among them, take the thrust speed curve parameter a t =0.15,b t =0.04, f d is the expected thrust, T d is the desired propeller speed.

[0119] Furthermore, the formulas (9) and (17) are thrust calculation formulas for a quad-rotor drone, and the present invention can also be extended to other multi-rotor drones.

[0120] refer to Figure 7 , Figure 8 , Fig. 9 The state comparison diagram, error diagram and three-dimensional trajectory diagram obtained by controlling the quadrotor UAV tracking based on MPC using the above control method are shown. It can be observed that the geometric control method can track the desired state almost without error, reflecting the robustness, accuracy and maneuverability of the controller. From the three-dimensional trajectory diagram, it can be observed that the use of this control method for autonomous take-off and landing on a nonlinear motion unmanned ship can achieve more ideal performance.

[0121] refer to Fig.10 , Fig.11 The final hardware control results are displayed, indicating that this controller can be applied to actual hardware systems.

[0122] The invention is described above by way of example in conjunction with the accompanying drawings. It is obvious that the implementation of the present invention is not limited to the above-mentioned method. As long as various improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the protection scope of the present invention.

Claims

1. A control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform, characterized in that: The following steps are involved: S1: Establish a three-dimensional motion model based on directed particle motion and an unmanned ship motion model; S2: Design a model-based predictive control method based on the motion model of the directed mass point and the unmanned ship to control the directed mass point, and use the updated state of the mass point model as the expected state of the UAV; S3: Establish the dynamic model of the multi-rotor UAV; S4: Design a control method based on geometric tracking control to obtain the desired thrust of each propeller of the UAV; S5: Calculate the speed of each motor according to the propeller thrust-speed curve relationship to achieve control of the drone; The step S1 comprises the following steps: S11: Establish a three-dimensional motion model of directed particles. The model is described as follows: x [t] =Ax [t-1] +Bu [t] (1) in is the three-dimensional state of a directed particle in the world coordinate system, including position P = [x, y, z] T ,speed Acceleration Yaw angle η, yaw angular velocity A and B are related to the motion state transfer matrix and the control time Δt. is the simulation control quantity of the directed particle model; S12: Establish a three-dimensional motion model of the unmanned boat platform. The model description is: Among them, P p =[x p ,y p ,z p ] T represents the position of the unmanned ship in the world coordinate system, v represents the forward speed of the unmanned ship in the world coordinate system, ψ represents the yaw angle of the unmanned ship, represents the yaw angular velocity of the unmanned ship, represents the vertical speed of the unmanned ship in the world coordinate system, and Δt represents the sampling time of the unmanned ship sensor; the step S2 comprises the following steps: S21: Design of Model Predictive Controller for Directed Mass Models: The QP formula of the least squares problem is used to find the optimal control action in the future prediction range, and the designed cost function J is: in, is the simulation control quantity of the directed particle model, e = xx d , represents the current position and speed of the unmanned ship, m is the prediction step length, x L x U represents the state constraint of the simulated directed particle model, u L u U It represents the control constraints of the simulated directed particle model; the state update model adopts the directed particle kinematic model and the unmanned ship kinematic model established by S1; S22: The control quantity obtained by the model predictive controller Substituting the directed particle motion model into the simulation model, the updated state is recorded as but is the desired state vector of the UAV, including The dynamic model of the multi-rotor UAV established in step S3 is described as follows: Where p = [x q ,y q ,z q ] T represents the position of the drone in the world coordinate system, v represents the speed of the drone in the world coordinate system, m represents the mass of the drone, and f represents the total thrust of the drone. Represents the angular velocity of the drone in the body coordinate system, The skew-symmetric matrix representing the angular velocity is J represents the inertia matrix of the drone, g represents the acceleration due to gravity, and e3 = [0 0 1] T , M=[M1 M2 M3] T represents torque, R is the rotation matrix, which is obtained by rotating the drone along the Euler angles ψ, φ, θ, where ψ, φ, θ represent the yaw angle, roll angle, and pitch angle, respectively; when the multi-rotor drone is a quad-rotor drone and the propellers are symmetrically arranged, the following relationship exists: Among them, f1, f2, f3, and f4 are the thrusts of the four propellers of the quadrotor drone, and d is the distance from a single propeller of the quadrotor drone to the center of mass; The control method based on geometric tracking control involved in step S4 includes the following steps: S41: Calculate the total thrust f required for control: Among them, e p =pp d is the position error, e v =vv d is the speed error, e3=[001] T , is the desired acceleration, k p ,k v ,k ib ,k iw is the thrust control gain, is the internal interference error, is the external interference error; S42: Based on the expected yaw angle ψ d And control the total vector thrust f to calculate the rotation matrix control quantity R C : R C =[b 1,C b 2,C b 3,C ] According to the definition of rotation matrix: b 2,d =[-sinψ d cosψ d 0] T (11) but: S43: Calculate angular velocity control amount Ω C : According to the UAV dynamics formula We can get: According to Ω C The skew-symmetric matrix Get Ω C ; S44: Calculate the torque M required for control: Among them, e R is the angle error, e Ω is the angular velocity error, k R ,k Ω Control gain for angle; S45: Calculate the expected thrust f for each propeller i : 。 2. The control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform according to claim 1, characterized in that: The propeller thrust-speed curve formula involved in step S5 is: Among them, a t ,b t is the thrust speed curve parameter, f d is the expected thrust, T d is the desired propeller speed.

3. According to the control method for autonomous take-off and landing of a multi-rotor UAV on an unmanned ship platform as described in claim 2, formula (9) and formula (17) are thrust calculation formulas for a quad-rotor UAV.

Images