Multi-mode smooth switching flight control method and system for four-rotor unmanned aerial vehicle with carrying arms

Abstract

The invention provides a multi-mode smooth switching flight control method and system for a four-rotor unmanned aerial vehicle with a carrying arm, and belongs to the field of flight control of special unmanned aerial vehicles. In order to solve the complex dynamics problems of unmanned aerial vehicle mass center offset, rotational inertia change and extra force / torque interference caused by mechanical arm movement, a traditional single-mode controller is difficult to adapt to the stable flight control requirements of a mechanical arm at different angles. The method aims at solving the problems that when a quad-rotor unmanned aerial vehicle system with a mounted mechanical arm executes a task, the mechanical arm moves, so that the mass center and the rotational inertia of the system are changed, additional force and torque are introduced, and meanwhile an executing mechanism of the quad-rotor unmanned aerial vehicle is difficult to execute a jumping control instruction. A multi-mode smooth switching control method based on the motion state of the mechanical arm is provided, and high-performance and high-reliability flight control of the four-rotor unmanned aerial vehicle is achieved under the condition that the posture of the mechanical arm is continuously changed due to task requirements.

Description

Technical Field

[0001] The present invention relates to the technical field of special unmanned aerial vehicle flight control, and in particular to a multi-mode smooth switching flight control method and system for a four-rotor aerial vehicle with an arm-carrying arm. Background Art

[0002] A boom-carrying drone often refers to a quadrotor drone equipped with a robotic arm for aerial operations. While inheriting the high-altitude reach and maneuverability of traditional quadrotor platforms, it also gains environmentally interactive capabilities through the mounted multi-degree-of-freedom robotic arm. Its application in complex operational scenarios is growing, such as high-altitude power line inspection and insulator cleaning, fruit and vegetable picking and branch pruning in complex agricultural and forestry environments, and precise installation of aerial instruments and equipment. During operations, the robotic arm continuously adjusts its position, resulting in continuous changes in the system's center of mass and moment of inertia, while also introducing additional forces and torques into the quadrotor platform. These dynamic disturbances can significantly degrade the quadrotor's flight control performance, leading to mission failure and, in extreme cases, even structural damage. For such systems with time-varying parameters, multimodal control methods can effectively characterize these parameter variations by establishing a mode switching mechanism. Furthermore, the design of a multi-controller architecture and switching logic significantly improves the control performance of systems with varying parameters. Multimodal control methods have been successfully applied to a variety of new aircraft platforms, such as rapidly deformable reconfigurable aircraft, composite UAVs with adjustable robotic arm length, and wing-body fusion aircraft with flexible wings.

[0003] Research on flight control for arm-carrying unmanned aerial vehicles (UAVs) is rapidly developing. Existing methods are primarily based on traditional quadrotor dynamic models, treating the manipulator's motion as an external disturbance. Robust control strategies are employed for disturbance suppression, or adaptive control strategies are employed to adjust parameters online for precise control. However, when the manipulator is in rapid motion or its mass contribution is significant, fundamental differences between traditional dynamic models and the actual system occur, resulting in significant degradation in the performance of both robust and adaptive control. In fact, analysis and modeling of the manipulator's dynamics reveal that the system's moment of inertia and additional forces / torques exhibit a significant nonlinear relationship with the manipulator's angle, significantly enhancing the coupling between system states. As the manipulator's angle continuously changes, the system's dynamic characteristics exhibit typical multimodal characteristics: parameters remain unchanged at rest, change gently at low speeds, and fluctuate dramatically at rapid motion. Given the complex dynamics induced by the manipulator's motion, traditional single-modal control methods struggle to simultaneously meet the requirements for high performance and reliability. Therefore, a sophisticated multimodal flight control approach is urgently needed to achieve precise flight control of quadrotor UAVs under all conditions of manipulator motion.

[0004] To achieve optimal control performance for each mode, multimodal control methods require the design of dedicated controllers for different modes. However, this design approach is prone to control command jumps during mode switching. These jumps may not be accurately executed due to the physical constraints of the actuator. In severe cases, they can also cause damage to the mechanical structure, including motor burnout due to current surges and propeller fatigue failure due to speed steps. To obtain smooth control commands in multimodal control methods, existing research either ignores the underlying energy stability of the system and directly performs a weighted average transition on the control gains of adjacent modes after mode switching, or designs smooth controllers while ensuring system stability in discrete time. Considering that the actuators (such as propellers and motors) of quadrotors are essentially continuous-time systems, developing multimodal smooth switching control methods with continuous-time stability has become an important research direction in this field. Summary of the Invention

[0005] The technical problems to be solved by the present invention are:

[0006] In order to solve the complex dynamic problems of UAV center of mass offset, moment of inertia change and additional force / torque interference caused by the movement of the robotic arm, the traditional single-mode controller is difficult to adapt to the stable flight control requirements at different angles of the robotic arm.

[0007] The present invention is to solve the above technical problems using the following technical solutions:

[0008] The present invention provides a multi-mode smooth switching flight control method for a quad-rotor UAV, comprising the following steps:

[0009] S100, establishing an earth inertial coordinate system, a body fixed coordinate system, and a robotic arm coordinate system, and obtaining angle information of the robotic arm and position information and attitude information of the quadrotor drone;

[0010] S200, developing a tracking control framework and determining the mode, including constructing a dynamic model of the coupled system of the quadrotor drone and the robotic arm, and designing a three-loop trajectory tracking control framework comprising an outer loop for position tracking control, a middle loop for attitude angle tracking control, and an inner loop for angular velocity tracking control; based on the robotic arm angle information obtained in step S100, determining the current mode of the system using a preset modal division criterion, and calling the dynamic model of the corresponding mode;

[0011] S300, attitude tracking control of the quadrotor drone under continuously changing robotic arm angles, using the current attitude angle of the quadrotor drone obtained in step S100 as the system's reference input to determine the system state error, obtaining the desired angular velocity through a state feedback controller and passing it to the inner loop, which designs a corresponding multi-modal smooth switching controller based on the robotic arm angle to achieve angular velocity tracking;

[0012] S400, position tracking control of the quadrotor drone under continuous changes in the robotic arm angle, obtain the position information and attitude information of the quadrotor drone according to step S100 to calculate the system state error; calculate the system lift and the desired roll angle and pitch angle through the PD position controller, and pass them together with the reference yaw angle as reference input to the angle loop, and realize the position tracking control of the quadrotor drone under continuous changes in the robotic arm through step S300;

[0013] S500, multi-modal smooth switching control of the quadrotor drone during the continuous movement of the robotic arm. By giving the reference input of the system, the current position and attitude information is obtained in real time to obtain the system state error. The mode of the control system is determined according to the real-time obtained robotic arm angle. Steps S200-S400 are repeated until the quadrotor drone reaches the reference position and attitude under the continuous change of the robotic arm angle.

[0014] Furthermore, in step S100, it includes:

[0015] Establish the earth inertial space rectangular coordinate system F according to the initial position of the quadrotor drone i , whose three axes point to the east (x), north (y), and up (z) respectively; the body coordinate system F is fixed at the geometric center of the quadrotor drone b , the directions of its three axes point to the front of the quadrotor drone (x b ), left (y b ), directly above (z b ); coordinate system O fixed on the joint of the robotic arm i (i=0,1,2,3), where O0 is the base coordinate system of the manipulator system, its origin coincides with the origin of the body coordinate system, and the x0 axis coincides with the x axis of the drone. b The axis direction is consistent, the y0 axis is aligned with the drone's z axis. b The axes are in opposite directions; the other three coordinate systems fixed on the joints of the robotic arm are fixed at the three link joints, i Axis points to connecting rod, z i Axis and y b The axis direction is the same, y i The axis is determined according to the right-hand rule;

[0016] The position information (x, y, z), attitude information (φ, θ, ψ) and robotic arm angle (θ1, θ2, θ3) of the quadrotor are obtained based on the onboard sensors on the drone.

[0017] Furthermore, in step S200, including:

[0018] Based on the coordinate system established in step S100, the Newton-Euler formula is used to iteratively calculate the force and torque introduced by the robot arm movement to the UAV platform. At the same time, considering the system rotational inertia and center of mass changes caused by the robot arm movement, the dynamic model of the arm-carrying UAV is:

[0019]

[0020] Where, v x 、v y and v z are the x, y, and z axis speeds of the quadrotor drone, respectively; m b is the mass of the quadcopter; g is the acceleration due to gravity; f bx 、f bz , τ by is the force and torque brought to the drone by the movement of the robotic arm, where f bx Along x b Axis, f bz Along z b axis, τ by Around y b Axis; F T is the total lift of the quadrotor drone; M p 、M q 、M r They are the rotational torques of the three axes of the quadrotor drone; k p 、k q 、k r are the drag coefficients of the three axes of the quadrotor drone; J xx 、J yy 、J zz 、J xz is the moment of inertia of the arm-carrying quadrotor drone, which is related to the length and current angle of the robotic arm and is expressed as:

[0021]

[0022] Where a1=(l c1 s1) 2 , a2=(l1s1+l c2 s 12 ) 2 , a3=(l1s1+l2s 12 +l c3 s 123 ) 2 ; b1=(l c1 c1) 2 , b2=(l1c1+l c2 c 12 ) 2 , b3=(l1c1+l2c 12 +l c3 c123 ) 2 ; l i is the length of the robot arm link i; l ci is the length from the center of the robot link i to the joint, J bx ,J by ,J bz is the moment of inertia of the quadrotor drone; m i is the mass of connecting rod i; s1 is sin(θ1), s 12 is sin(θ 1+ θ2), s 123 is sin(θ1+θ2+θ3), c1 is cos(θ1), c 12 is cos(θ 1+ θ2), c 123 is cos(θ1+θ2+θ3);

[0023] Considering the actuator dynamics of the quadrotor drone, the model is:

[0024]

[0025] Where, ζ p ,ζ q ,ζ r is the physical parameter of the actuator; M pf 、M qf 、M rf is the control input that needs to be designed;

[0026] According to the current angle of the manipulator, the angular velocity multi-modal linear variable parameter tracking control system is established as follows:

[0027]

[0028] Where, B s are the Jacobian matrices obtained after linearization of the tracking control system; w is the disturbance term of the tracking control system; z s is the system state that needs to be observed; C s is the output matrix; e s =[p,q,r,M p ,M q ,M r ] T is the system tracking error;

[0029] u s =[M pf ,M qf ,M rf ] T is the system control input; η k,σ In the same mode The polyhedral coefficient of the matrix; σ is the system mode corresponding to different robot arm angles;

[0030] According to formula (1) to formula (5), a three-loop tracking control framework of position-angle-angular velocity of the arm-carrying UAV is established to realize the flight tracking control of the UAV at different robot arm angles.

[0031] Furthermore, in step S300, it includes:

[0032] When the quadrotor UAV performs attitude tracking control, the reference input Φ d is the lateral rotation angle φ calculated by the position loop d , pitch angle θ d and a given heading angle ψ d ;

[0033] Define the angle tracking error as e Φ =Φ d -Φ=[φ d -φ,θ d -θ,ψ d -ψ] T , where Φ is the instantaneous roll angle, pitch angle, and yaw angle; the state feedback controller for angle tracking is designed as Where R is the system rotation matrix, ω d =[p d ,q d ,r d ] T ,ζ Φ =diag{ζ φ ,ζ θ ,ζ ψ};ζ Φ =diag{ζ φ ,ζ θ ,ζ ψ} are the positive real number parameters of the roll angle, pitch angle and yaw angle to be designed;

[0034] According to the current robot arm angle, switch from mode i to mode j σ(t j ), the multi-mode smooth switching controller of the angular velocity loop is designed as:

[0035]

[0036] Where, T j is the smoothing time; is the mode-dependent control gain, which is a matrix related to the robot angle B s 、C s Related; α is the smoothing coefficient related to time;

[0037] The controller of formula (7) is solved by the following inequality:

[0038]

[0039] The robust performance index of the controller in formula (7) is λ、 τ D They are the maximum value of energy increase when switching between different modes, the minimum normal number of system energy decrease under different modes, the maximum normal number of system stability under smoothing under different modes, the maximum value of smoothing time when switching between different modes, residence time, system parameters, and the system's anti-interference suppression ability, μ ij is the energy rise coefficient from mode i to mode j, μ ji is the energy switching coefficient for switching from mode j to mode j.

[0040] Furthermore, in step S400, it includes:

[0041] The position tracking control adopts PD controller. Considering the force and torque introduced by the robot arm movement, the virtual control input is designed as follows:

[0042]

[0043] Where, ζ px ,ζ vx ,ζ py ,ζ vy ,ζ pz ,ζ vz is a positive constant to be designed, so that the virtual control input of formula (9) is:

[0044]

[0045] According to formula (9) and formula (10), the controller design of position tracking control is:

[0046]

[0047] Where,

[0048] By giving the reference input x d 、y d 、z d The controller adjusts the position of the UAV in real time according to the current system position error and calculates the roll angle and pitch angle and transmits them to the attitude control loop. According to the attitude controller in step S300, the position tracking control of the UAV under different robot arm angles is realized.

[0049] A multi-modal smooth switching flight control system for an arm-carrying quad-rotor unmanned aerial vehicle (UAV) is disclosed. The system has program modules corresponding to the above steps and executes the steps in the above-mentioned multi-modal smooth switching flight control method for an arm-carrying quad-rotor unmanned aerial vehicle (UAV) during operation.

[0050] A computer-readable storage medium stores a computer program configured to implement the steps of a multi-modal smooth switching flight control method for a four-rotor unmanned aerial vehicle (UAV) when called by a processor.

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

[0052] Compared with single-mode control methods such as robust and adaptive ones, the present invention addresses the problem of changes in the system center of mass and moment of inertia caused by the movement of the robotic arm, and designs a multi-mode smooth switching controller under different robotic arm angles, ensuring high-performance and high-reliability flight control of the quadrotor drone when the robotic arm performs tasks.

[0053] In response to traditional multi-modal control methods that ignore the physical constraints of the actuator, the present invention introduces a transition period in the mode switching interval and reconstructs the system energy function based on the Lyapunov stability theory. This ensures global stability while achieving a smooth transition of control instructions and ensuring a reliable response of the actuator.

[0054] To sum up, the present invention takes the arm-carrying UAV as the application object, derives a complete dynamic model based on the consideration of the movement of the robotic arm, and considers the physical constraints of the actuator. A multi-modal smooth switching controller is designed to ensure high-performance flight control of the quadrotor UAV under the robotic arm execution task while also enabling the actuator to smoothly execute control instructions, which has high engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 This is a flow chart of a multi-mode smooth switching flight control method for a quad-rotor UAV with an arm mounted on it according to an embodiment of the present invention;

[0056] Figure 2 Figure 1 shows the continuous change of the robot arm angle and the mode partition diagram in an embodiment of the present invention, where (a) is the angle change diagram of the robot arm, (b) is the parameter space diagram of the robot arm, (c) is the modal diagram at different robot arm angles, and (d) is the partition diagram at different robot arm angles.

[0057] Figure 3 This is a diagram of the position loop tracking control framework of a quadrotor drone with continuously changing robot arm angles in an embodiment of the present invention;

[0058] Figure 4This is a diagram showing the attitude tracking effect and control instructions of a quadrotor drone with continuously changing robot arm angles in an embodiment of the present invention;

[0059] Figure 5 Figure 3 shows the position tracking effect and control command diagram of the quadrotor drone under the continuous change of the robotic arm angle in an embodiment of the present invention. (a)-(c) are the roll angle, pitch angle, and yaw angle response curves under the two controllers, respectively; (d)-(f) are the control inputs under the multimodal smoothing controller, respectively; and (g)-(i) are the control inputs under the traditional multimodal controller, respectively. DETAILED DESCRIPTION

[0060] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0061] Specific implementation plan 1: Combined Figures 1 to 4 As shown, the present invention provides a multi-mode smooth switching flight control method for a quad-rotor UAV, comprising the following steps:

[0062] S100, establishing a coordinate system and obtaining the angle information of the manipulator arm and the position and attitude information of the quadrotor drone, including establishing the earth inertial coordinate system, the body fixed coordinate system and the manipulator arm coordinate system, and obtaining the current angle of the manipulator arm and the position and attitude information of the quadrotor drone through the airborne sensor;

[0063] Specifically include:

[0064] Establish the earth inertial space rectangular coordinate system F according to the initial position of the quadrotor drone i , whose three axes point to the east (x), north (y), and sky (z) respectively; the body coordinate system F is fixed at the geometric center of the quadrotor drone b , the directions of its three axes point forward (x b ), left(y b ), up (z b ); coordinate system O fixed on the joint of the robotic arm i (i=0,1,2,3), where O0 is the base coordinate system of the manipulator system, its origin coincides with the origin of the body coordinate system, and the x0 axis coincides with the x axis of the drone. b The axis direction is consistent, the y0 axis is aligned with the drone's z axis. b The axis direction is opposite (the robotic arm mounted on the quadcopter in the present invention is only in the x b -z b Plane motion); the other three coordinate systems (three links) are fixed at the link joint (the end of the link), x i Axis points to connecting rod, z i Axis and y b The axis direction is the same, y iThe axis is determined according to the right-hand rule;

[0065] The position information (x, y, z), attitude information (φ, θ, ψ) and robotic arm angle (θ1, θ2, θ3) of the quadrotor are obtained according to the inertial measurement unit, barometer and GPS sensor installed on the drone. During the tracking control process, the position information and attitude information of the quadrotor need to be converted into the earth fixed coordinate system F i Down, easy to work;

[0066] S200: Development of a tracking control framework and modal determination. Based on the spatial coordinate system established in step S100, the system dynamics characteristics caused by the robotic arm motion are integrated into the quadcopter UAV dynamics model to construct a complete quadcopter UAV and robotic arm coupled system dynamics model. On this basis, a three-loop trajectory tracking control framework is designed: the outer loop implements position tracking control, the middle loop implements attitude angle tracking control, and the inner loop implements angular velocity tracking control. At the same time, based on the real-time robotic arm angle information obtained in step S100, the system's current mode is determined through a preset modal division criterion, and the dynamics model of the corresponding mode is called.

[0067] Specifically include:

[0068] Based on the coordinate system established in step S100, the Newton-Euler formula is used to iteratively calculate the forces and torques introduced by the robotic arm movement to the UAV platform. At the same time, the changes in the system's moment of inertia and center of mass caused by the robotic arm movement are taken into account. The dynamic model of the UAV carrying the arm is:

[0069]

[0070]

[0071] Where, v x 、v y and v z are the xyz axis speeds of the quadrotor drone, m b is the mass of the quadrotor, g is the local acceleration of gravity, and f bx 、f bz , τ by is the force and torque brought to the drone by the movement of the robotic arm (where f bx Along x b Axis, f bz Along z b axis, τ by Around y b axis), F T is the total lift of the quadrotor drone, M p 、M q 、M r They are the rotational torque of the three axes of the quadrotor drone, kp 、k q 、k r They are the drag coefficients of the three axes of the quadrotor drone, J xx 、J yy 、J zz 、J xz is the moment of inertia of the system (arm-carrying quadrotor drone), which is closely related to the length and current angle of the robotic arm and is expressed as:

[0072]

[0073] Where a1=(l c1 s1) 2 , a2=(l1s1+l c2 s 12 ) 2 , a3=(l1s1+l2s 12 +l c3 s 123 ) 2 , b1=(l c1 c1) 2 , b2=(l1c1+l c2 c 12 ) 2 , b3=(l1c1+l2c 12 +l c3 c 123 ) 2 , s1 is sin(θ1), s 12 is sin(θ 1+ θ2), s 123 is sin(θ1+θ2+θ3), c1 is cos(θ1), c 12 is cos(θ 1+ θ2), c 123 is cos(θ1+θ2+θ3), l i is the length of the robot arm link i, l ci is the length from the center of the robot link i to the end of the link, J bx ,J by ,J bz is the moment of inertia of the quadrotor drone, m i is the mass of connecting rod i;

[0074] Considering the dynamics of the quadrotor actuator (the propeller of the quadrotor), it can be modeled as:

[0075]

[0076] Where, ζ p ,ζ q ,ζ ris the physical parameter of the actuator (i.e., the inherent parameter after the hardware is produced, which is a specific number), M pf 、M qf 、M rf is the control input that needs to be designed;

[0077] Considering that the angular velocity dynamics is affected by the movement of the manipulator and has the characteristics of strong coupling, nonlinearity, and large parameter variation, a multi-modal linear variable parameter tracking control system for angular velocity is established according to the current manipulator angle:

[0078]

[0079] Where, B s is the Jacobian matrix obtained after linearization of the tracking control system, w is the disturbance term of the tracking control system, z s is the system state that needs to be observed, C s is the output matrix, e s =[p,q,r,M p ,M q ,M r ] T is the system tracking error, u s =[M pf ,M qf ,M rf ] T is the system control input, η k,σ In the same mode The polyhedral coefficient of the matrix, σ is the system mode corresponding to different robot arm angles;

[0080] Based on the above tracking control system, a three-loop tracking control framework of position, angle and angular velocity of the UAV with an arm is established to realize the flight tracking control of the UAV at different robot arm angles;

[0081] S300, attitude tracking control of the quadrotor drone under continuous changes in the robotic arm angle. The attitude tracking control is a two-loop control framework of middle-loop attitude angle tracking and inner-loop angular velocity tracking control. According to the current attitude angle of the quadrotor drone obtained in step S100, the attitude angle of the quadrotor drone is given as the reference input of the system to determine the system state error, and the desired angular velocity is obtained through the designed state feedback controller and transmitted to the inner loop. According to the system mode determined in step S200, the inner loop angular velocity control is quickly tracked by designing a corresponding multi-modal smooth switching controller, thereby realizing attitude tracking control of the quadrotor drone under continuous changes in the robotic arm angle and ensuring the smoothness of the control instructions during mode switching.

[0082] Specifically include:

[0083] When the quadrotor UAV performs attitude tracking control, its reference input Φ d is the lateral rotation angle φ calculated by the position loop d , pitch angle θ d and a given heading angle ψ d ;

[0084] Define the angle tracking error as e Φ =Φ d -Φ=[φ d -φ,θ d -θ,ψ d -ψ] T , where Φ is the instantaneous roll angle, pitch angle and yaw angle, and the state feedback controller for angle tracking is designed as Where R is the system rotation matrix, ω d =[p d ,q d ,r d ] T ,ζ Φ =diag{ζ φ ,ζ θ ,ζ ψ} are the positive real number parameters of the roll angle, pitch angle and yaw angle to be designed; if the angular velocity control loop can be tracked quickly, the angle control can be achieved; according to the current manipulator angle, the multi-mode smooth switching controller of the angular velocity loop can be designed as (switching from mode i to mode j σ(t j )):

[0085]

[0086] Where, T j is the smoothing time, The modal-dependent control gain is given by the matrix related to the robot angle B s 、C s Related, α is the time-related smoothing coefficient;

[0087] The controller can be solved by the following inequality and has a certain anti-interference ability:

[0088]

[0089] Among them, the robust performance index of the controller is λ、 τ D, c, γ0 are the maximum value of energy increase when switching between different modes, the minimum normal number to ensure the energy decrease of the system under different modes, the maximum normal number to ensure the stability of the system under smoothing under different modes, the maximum value of smoothing time when switching between different modes, the residence time, system parameters, and the anti-interference suppression ability of the system, μ ij is the energy rise coefficient from mode i to mode j, μ ji is the energy switching coefficient from mode j to mode j; the controller designed using this inequality can adjust the UAV attitude in real time according to the current system attitude error, achieve attitude tracking control and smooth transition of control input during switching, so that the UAV actuator can smoothly execute the control command;

[0090] S400, position tracking control of the quadrotor drone under continuous changes in the robotic arm angle; according to the three-loop trajectory tracking control framework established in step S200, the position and yaw angle of the quadrotor drone are given as the reference input of the system, and the system state error is calculated according to the current position information and attitude information of the quadrotor drone obtained in step S100; first, the system lift and the desired roll angle and pitch angle are calculated by the designed PD position controller, and then the calculated roll angle and pitch angle together with the given reference yaw angle are passed to the angle loop as reference inputs, and the angle tracking is achieved using the method of step S300, and finally the position tracking control of the quadrotor drone under continuous changes in the robotic arm is achieved;

[0091] Specifically include:

[0092] When performing position tracking control on a rotorcraft, due to its underactuated characteristics, a position controller is required to calculate the roll angle and pitch angle in real time as reference inputs for angle control. Position tracking control uses a PD controller, taking into account the forces and torques introduced by the manipulator's motion. The virtual control input is designed as follows:

[0093]

[0094] Where, ζ px ,ζ vx ,ζ py ,ζ vy ,ζ pz ,ζ vz is a positive constant to be designed, so that the above virtual control input is:

[0095]

[0096] According to formula (9) and formula (10), the controller of position tracking control can be designed as:

[0097]

[0098] Where,

[0099] By giving the reference input x d 、y d 、z d The controller adjusts the position of the UAV in real time according to the current system position error and calculates the roll angle and pitch angle and transmits them to the attitude control loop. According to the attitude controller in step S300, the position tracking control of the UAV under different robot arm angles is realized;

[0100] S500, the multi-modal smooth switching control of the quadcopter drone during the continuous movement of the robotic arm; the onboard accelerometer and gyroscope are used to measure the robotic arm angle in real time to determine the current control mode of the system; at the same time, the IMU, GPS and barometer sensors are used to obtain the attitude and position information of the drone; then the system state error is calculated based on the reference input, and the onboard computer solves the control input in real time to achieve the position and attitude control of the drone, continuously updating the drone's status, and repeating this process until the drone reaches the desired trajectory.

[0101] Specific implementation scheme 2: The present invention provides a multi-modal smooth switching flight control system for an arm-carrying quad-rotor UAV. The system has a program module corresponding to the above steps, and executes the steps in the above-mentioned multi-modal smooth switching flight control method for an arm-carrying quad-rotor UAV during operation.

[0102] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0103] Specific implementation scheme three: The present invention provides a computer-readable storage medium, which stores a computer program. The computer program is configured to implement the steps of a multi-modal smooth switching flight control method for a four-rotor drone when called by a processor.

[0104] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0105] Simulation experiment

[0106] (1) Verification of the attitude control capability of the quadrotor drone under different robot arm angles. The robot arm angle changes as follows:

[0107] θ1=π / 2+π / 6sin(π / 36·t), θ2=π / 6cos(π / 36·t), θ3=π / 6sin(π / 30·t). The first and second angles are further divided, and the angle change of the robot arm is divided into four modes, namely mode 1: θ1∈[π / 3,π / 2], θ2∈[-π / 6,0], θ3∈[-π / 6,π / 6]; mode 2: θ 1∈[π / 2,2π / 3],θ2∈[-π / 6,0],θ3∈[-π / 6,π / 6];Mode three:θ1∈[π / 3,π / 2],θ2∈[0,π / 6],θ3∈[-π / 6,π / 6];Mode four:θ1∈[π / 2,2π / 3],θ2∈[0,π / 6],θ3∈[-π / 6,π / 6],The continuous change of the robot arm angle and the mode partition diagram are shown in the figure below. Figure 2 As shown. Set the attitude angle tracking value to φ d =-0.17,θ d =0.08,ψ d =0.05t, the state feedback controller parameter of the angle control loop is designed to be ζ φ =0.4,ζ θ =0.5,ζ ψ =0.2, the parameters of the multi-mode smooth switching controller of the angular velocity control loop are λ1=45, λ2=52, λ3=48, λ4=50, μ ij =1.4, c i =62,m i =2.3, b i =304,γ0=9.8,ζ p =ζ q =ζ r = 0.5, T = 0.3s, the attitude loop tracking control framework of the quadrotor drone with continuous changes in the robotic arm angle is as follows Figure 3 The obtained posture tracking curve and the control instructions output by the controller are shown as follows. Figure 5 As shown, compared with the common multi-modal control method, the multi-modal smooth switching control method designed by the present invention not only ensures high-performance and high-reliability flight control, but also realizes smooth control instructions so that the actuator can be executed smoothly.

[0108] (2) Verification of the position control capability of the quadrotor drone under different manipulator angles. In position tracking, the controllers and parameters of the angle control loop and angular velocity control loop are the same as those of the attitude tracking control. The position loop tracking control framework of the quadrotor drone under continuous changes in the manipulator angle is as follows: Figure 4 As shown. Set the position reference trajectory to x d =0.8sin(0.1t),y d =0.8cos(0.1t), z d =0.06t,ψd =0.1. For the convenience of comparison, two types of position loops are designed: PD controller and sliding mode controller. The PD controller parameter is ζ px =2.7,ζ vx =0.8,ζ py =3.4,ζ vy =0.4,ζ pz =4.2,ζ vz =5.5, the sliding surface in the sliding mode controller is set to The approach rate is set to The obtained position tracking curve and the control instructions output by the controller are as follows: Figure 5 As shown in the figure, under the PD controller, the system state can track the reference trajectory very well, and the output control command does not jump. However, the control command output by the sliding mode controller has been shaking up and down, causing the system state to also shake near the reference trajectory.

[0109] Although the present invention is disclosed as above, the scope of protection disclosed by the present invention is not limited thereto. Those skilled in the art of the present invention may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A multi-mode smooth switching flight control method for a quad-rotor UAV with an arm, characterized in that: The following steps are involved: S100, establishing an earth inertial coordinate system, a body fixed coordinate system, and a robotic arm coordinate system, and obtaining angle information of the robotic arm and position information and attitude information of the quadrotor drone; S200, developing a tracking control framework and determining the mode, including constructing a dynamic model of the coupled system of the quadrotor drone and the robotic arm, and designing a three-loop trajectory tracking control framework comprising an outer loop for position tracking control, a middle loop for attitude angle tracking control, and an inner loop for angular velocity tracking control; based on the robotic arm angle information obtained in step S100, determining the current mode of the system using a preset modal division criterion, and calling the dynamic model of the corresponding mode; S300, attitude tracking control of the quadrotor drone under continuously changing robotic arm angles, using the current attitude angle of the quadrotor drone obtained in step S100 as the system's reference input to determine the system state error, obtaining the desired angular velocity through a state feedback controller and passing it to the inner loop, which designs a corresponding multi-modal smooth switching controller based on the robotic arm angle to achieve angular velocity tracking; S400, position tracking control of the quadrotor drone under continuous changes in the robotic arm angle, obtain the position information and attitude information of the quadrotor drone according to step S100 to calculate the system state error; calculate the system lift and the desired roll angle and pitch angle through the PD position controller, and pass them together with the reference yaw angle as reference input to the angle loop, and realize the position tracking control of the quadrotor drone under continuous changes in the robotic arm through step S300; S500, multi-modal smooth switching control of the quadrotor drone during the continuous movement of the robotic arm. By giving the reference input of the system, the current position and attitude information is obtained in real time to obtain the system state error. The mode of the control system is determined according to the real-time obtained robotic arm angle. Steps S200-S400 are repeated until the quadrotor drone reaches the reference position and attitude under the continuous change of the robotic arm angle.

2. The multi-mode smooth switching flight control method for a quad-rotor UAV with an arm-carrying structure according to claim 1, characterized in that: In step S100, it includes: Establish the earth inertial space rectangular coordinate system F according to the initial position of the quadrotor drone i , whose three axes point to the east (x), north (y), and up (z) respectively; the body coordinate system F is fixed at the geometric center of the quadrotor drone b , the directions of its three axes point to the front of the quadrotor drone (x b ), left (y b ), directly above (z b ); coordinate system O fixed on the joint of the robotic arm i (i=0,1,2,3), where O0 is the base coordinate system of the manipulator system, its origin coincides with the origin of the body coordinate system, and the x0 axis coincides with the x axis of the drone. b The axis direction is consistent, the y0 axis is aligned with the drone's z axis. b The axes are in opposite directions; the other three coordinate systems fixed on the joints of the robotic arm are fixed at the three link joints, i Axis points to connecting rod, z i Axis and y b The y axis has the same direction. i The axis is determined according to the right-hand rule; The position information (x, y, z), attitude information (φ, θ, ψ) and robotic arm angle (θ1, θ2, θ3) of the quadrotor are obtained based on the onboard sensors on the drone.

3. The multi-mode smooth switching flight control method for a quad-rotor UAV with an arm-carrying structure according to claim 2, characterized in that: In step S200, it includes: Based on the coordinate system established in step S100, the Newton-Euler formula is used to iteratively calculate the force and torque introduced by the robot arm movement to the UAV platform. At the same time, considering the system rotational inertia and center of mass changes caused by the robot arm movement, the dynamic model of the arm-carrying UAV is: Where, v x 、v y and v z are the xyz axis speeds of the quadrotor drone, m b is the mass of the quadcopter; g is the acceleration due to gravity; f bx 、f bz , τ by is the force and torque brought to the drone by the movement of the robotic arm, where f bx Along x b Axis, f bz Along z b axis, τ by Around y b Axis; F T is the total lift of the quadrotor drone; M p 、M q 、M r They are the rotational torques of the three axes of the quadrotor drone; k p 、k q 、k r are the drag coefficients of the three axes of the quadrotor drone; J xx 、J yy 、J zz 、J xz is the moment of inertia of the arm-carrying quadrotor drone, which is related to the length and current angle of the robotic arm and is expressed as: Where a1=(l c1 s1) 2 , a2=(l1s1+l c2 s 12 ) 2 , a3=(l1s1+l2s 12 +l c3 s 123 ) 2 ; b1=(l c1 c1) 2 , b2=(l1c1+l c2 c 12 ) 2 , b3=(l1c1+l2c 12 +l c3 c 123 ) 2 ; l i is the length of the robot arm link i; l ci is the length from the center of the robot link i to the joint, J bx ,J by ,J bz is the moment of inertia of the quadrotor drone; m i is the mass of connecting rod i; s1 is sin(θ1), s 12 is sin(θ1+θ2), s 123 is sin(θ1+θ2+θ3), c1 is cos(θ1), c 12 is cos(θ 1+ θ2), c 123 is cos(θ1+θ2+θ3); Considering the actuator dynamics of the quadrotor drone, the model is: Where, ζ p ,ζ q ,ζ r is the physical parameter of the actuator; M pf 、M qf 、M rf is the control input that needs to be designed; According to the current angle of the manipulator, the angular velocity multi-modal linear variable parameter tracking control system is established as follows: Where, B s are the Jacobian matrices obtained after linearization of the tracking control system; w is the disturbance term of the tracking control system; z s is the system state that needs to be observed; C s is the output matrix; e s =[p,q,r,M p ,M q ,M r ] T is the system tracking error; u s =[M pf ,M qf ,M rf ] T is the system control input; η k,σ In the same mode The polyhedral coefficient of the matrix; σ is the system mode corresponding to different robot arm angles; According to formula (1) to formula (5), a three-loop tracking control framework of position-angle-angular velocity of the arm-carrying UAV is established to realize the flight tracking control of the UAV at different robot arm angles.

4. The multi-mode smooth switching flight control method for a quad-rotor UAV with an arm-carrying structure according to claim 3 is characterized in that: In step S300, it includes: When the quadrotor UAV performs attitude tracking control, the reference input Φ d is the lateral rotation angle φ calculated by the position loop d , pitch angle θ d and a given heading angle ψ d ; Define the angle tracking error as e Φ =Φ d -Φ=[φ d -φ,θ d -θ,ψ d -ψ] T , where Φ is the instantaneous roll angle, pitch angle, and yaw angle; the state feedback controller for angle tracking is designed as Where R is the system rotation matrix, ω d =[p d ,q d ,r d ] T ,ζ Φ =diag{ζ φ ,ζ θ ,ζ ψ };ζ Φ =diag{ζ φ ,ζ θ ,ζ ψ } are the positive real number parameters of the roll angle, pitch angle and yaw angle to be designed; According to the current robot arm angle, switch from mode i to mode j σ(t j ), the multi-mode smooth switching controller of the angular velocity loop is designed as: Where, T j is the smoothing time; is the mode-dependent control gain, which is a matrix related to the robot angle B s 、C s Related; α is the smoothing coefficient related to time; The controller of formula (7) is solved by the following inequality: The robust performance index of the controller in formula (7) is τ D They are the maximum value of energy increase when switching between different modes, the minimum normal number of system energy decrease under different modes, the maximum normal number of system stability under smoothing under different modes, the maximum value of smoothing time when switching between different modes, residence time, system parameters, and the system's anti-interference suppression ability, μ ij is the energy rise coefficient from mode i to mode j, μ ji is the energy switching coefficient for switching from mode j to mode j.

5. The multi-mode smooth switching flight control method for a quad-rotor UAV with an arm-carrying structure according to claim 4, characterized in that: In step S400, it includes: The position tracking control adopts PD controller. Considering the force and torque introduced by the robot arm movement, the virtual control input is designed as follows: Where, ζ px ,ζ vx ,ζ py ,ζ vy ,ζ pz ,ζ vz is a positive constant to be designed, so that the virtual control input of formula (9) is: According to formula (9) and formula (10), the controller design of position tracking control is: Where, By giving the reference input x d 、y d 、z d The controller adjusts the position of the UAV in real time according to the current system position error and calculates the roll angle and pitch angle and transmits them to the attitude control loop. According to the attitude controller in step S300, the position tracking control of the UAV under different robot arm angles is realized.

6. A multi-mode smooth switching flight control system for a quad-rotor UAV with an arm, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 5 above, and executes the steps in the above-mentioned multi-modal smooth switching flight control method of a four-rotor drone with an arm when running.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a multi-modal smooth switching flight control method for a four-rotor drone with an arm mounted thereon according to any one of claims 1 to 5 when called by a processor.

Images