Environment interaction control method and system of tilting multi-rotor unmanned aerial vehicle

Abstract

The invention relates to an environment interaction control method and system for a tilting multi-rotor unmanned aerial vehicle. The environment interaction control method and system are used for a full-drive tilting six-rotor unmanned aerial vehicle in a contact type detection task. The algorithm can control the position and the force at the same time, and has accurate force tracking capability. And the active-disturbance-rejection controller is configured, so that the anti-interference function is strong, and the performance is still good under the wind disturbance along the rod direction. The method makes up the blank of underactuation and inaccurate force detection of a traditional unmanned aerial vehicle, greatly improves the control precision of the unmanned aerial vehicle, improves the capability of the unmanned aerial vehicle for performing a contact type detection task, and provides a new solution for the contact type operation unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle control, and in particular to an environmental interaction control method and system for a tiltable multi-rotor unmanned aerial vehicle. Background Art

[0002] In recent years, multi-rotor drones have been widely used in aerial photography, plant protection, and inspection industries, but these applications are limited to non-contact observation or simple payloads. In industrial scenarios such as surface cleaning and non-destructive testing, drones need to physically interact with the environment, which places higher demands on their active operating capabilities. At present, the contact operation control technology of industrial drone systems has become a cutting-edge hotspot in international academic research. Improving the contact operation efficiency and accuracy of drones and participating in environmental interaction tasks at high altitudes or other adverse environments can solve the high-risk, high-cost, and low-efficiency problems of traditional manual operations, and will lay the foundation for the widespread application of industrial drones.

[0003] For contact detection, the most commonly used is the traditional underactuated rotor UAV, whose lift direction is parallel and fixed, and cannot exert omnidirectional torque and force in three-dimensional space, which increases the requirements for control algorithms and limits the scope of achievable detection tasks. To address this problem, we aim to propose an environmental interactive control method based on a tiltable multi-rotor UAV. By adding a connecting rod as a force contact device to a fully driven thrust vectoring UAV with a special structure, a force / motion hybrid control method is designed to achieve contact force control of the body in the normal direction of the contact surface.

[0004] At present, the multi-rotor drones widely used in engineering are all based on the traditional plane configuration body design. The lift generated by each blade is perpendicular to the body, with 4D trajectory tracking capability, namely roll, pitch, yaw, lift and fall motion, and strong dynamic coupling between rotation and translation. Corresponding attitude changes need to be generated to achieve control of spatial position. Relying on the pitch / roll angle, the lift generates components in all directions to achieve spatial translation of the drone. The spatial rotation of the drone is achieved by relying on the torque of the lift on the body's center of gravity and the anti-torque of the motor.

[0005] However, existing drones have weak attitude stability when subjected to external disturbances and cannot achieve contact operations with the physical environment. In addition, drone systems often have difficulty distinguishing between contact forces, the effects of interaction with the wall, and forces in the direction of the equipped robotic arm, which makes the control of the interaction forces very complicated. Therefore, the lack of compensation for disturbances on the rigid arm will affect the performance of the aerial control platform in operation. Summary of the invention

[0006] In view of this, an object of the present invention is to provide an environmental interaction control method and system for a tiltable multi-rotor UAV to solve the problems in the background technology.

[0007] In order to achieve the above object, the present invention adopts the following technical solutions:

[0008] The environmental interactive control method of the tiltable multi-rotor UAV of the present invention comprises the steps of:

[0009] Construct rigid kinematics model, rigid body dynamics model, and control efficiency model of multi-rotor UAV;

[0010] Constructing a flight control model based on the rigid kinematics model and the rigid body dynamics model;

[0011] Simplifying the flight control model, and constructing a target control model based on the simplified flight control model, the control efficiency model and the impedance control model, wherein the target control model includes a position control part, an attitude control part, a control distribution part, an impedance control part, a motor control part and a mathematical solver part;

[0012] Obtaining expected flight information of the multi-rotor UAV, and obtaining actual feedback force and expected feedback force generated when the actuator end of the multi-rotor UAV contacts the environment;

[0013] Inputting the expected flight information, the actual feedback force and the expected feedback force into the target control model to obtain a control signal;

[0014] An active interference suppression control algorithm is introduced to filter the control signal, and the filtered control signal is fed back to the controller of the multi-rotor UAV, so that the multi-rotor UAV flies along the predicted trajectory and generates the expected feedback force to the environment.

[0015] In one embodiment of the present application, the kinematic model characterizes the relationship between the position and linear velocity of the drone, and characterizes the relationship between the angular velocity of the drone and the Euler angle.

[0016] In one embodiment of the present application, the rigid body dynamics model includes a position dynamics model and an attitude dynamics model. The position dynamics model characterizes the relationship between the linear acceleration in the world coordinate system and the propeller thrust in the world coordinate system; the attitude dynamics model characterizes the relationship between the angular acceleration and angular velocity of the UAV, the gyroscopic torque of the UAV, and the propeller torque, as well as the relationship between the gyroscopic torque of the UAV and the rotational inertia of the body and the angular velocity of the arm rotation.

[0017] In one embodiment of the present application, the flight control model is used based on the body linear velocity v, the Euler angle Body mass m, body angular acceleration The body angular velocity ω calculates the gyroscopic torque Ga, propeller thrust f and motor moment of inertia J of multiple propellers of the multi-rotor drone to control the flight state of the multi-rotor drone.

[0018] In one embodiment of the present application, the efficiency control model includes a thrust and reaction torque model and a thrust and torque model, wherein the thrust and reaction torque model characterizes the relationship between the blade thrust and the angular velocity of the propeller, and the relationship between the blade torque and the angular velocity of the propeller; the thrust and torque model characterizes the relationship between the blade thrust and the angular velocity of the propeller, and the relationship between the blade torque and the angular velocity of the propeller.

[0019] In one embodiment of the present application, the flight control model is simplified, including:

[0020] The flight control model is simplified to obtain a simplified flight control model, wherein the simplified flight control model is:

[0021]

[0022] Based on the simplified flight control model, the position control equation is written as:

[0023]

[0024] In the formula, and v is the linear velocity of the machine body, is the Euler angle, ω b is the body angular velocity, W is the transformation matrix, is the linear acceleration in the world coordinate system, g is the acceleration due to gravity, e3 is the z-axis unit column vector in the world coordinate system, f is the propeller thrust in the world coordinate system, m is the mass of the aircraft, τ is the torque generated by the propeller, J is the moment of inertia of the motor, is the angular acceleration on the x-axis, is the angular acceleration on the y-axis, is the angular acceleration on the z-axis, is the roll angle, θ is the pitch angle, is the yaw angle.

[0025] In one embodiment of the present application, a target control model is constructed based on the simplified flight control model, the control efficiency model and the impedance control model, including:

[0026] Building a PID-based control loop based on the simplified flight control model and the control efficiency model;

[0027] An impedance control model is embedded in the control loop to obtain a target control model, wherein the impedance control model uses the multi-rotor drone as a spring model, the input of the impedance control model is the actual feedback force and the expected feedback force, and the output of the impedance control model is the corrected displacement.

[0028] This application also provides an environmental interactive control system for a tiltable multi-rotor drone, including:

[0029] Model building module, used to build the rigid kinematics model, rigid body dynamics model, and control efficiency model of the multi-rotor UAV;

[0030] A flight control module construction module, used to construct a flight control model based on the rigid kinematics model and the rigid body dynamics model;

[0031] a target model construction module, used for simplifying the flight control model, and constructing a target control model based on the simplified flight control model, the control efficiency model and the impedance control model, wherein the target control model includes a position control part, an attitude control part, a control distribution part, an impedance control part, a motor control part and a mathematical solver part;

[0032] An acquisition module is used to acquire the expected flight information of the multi-rotor UAV, and to acquire the actual feedback force and the expected feedback force generated when the actuator end of the multi-rotor UAV contacts the environment;

[0033] a control signal calculation module, used for inputting the expected flight information, the actual feedback force and the expected feedback force into the target control model to obtain a control signal;

[0034] The filtering and control module is used to introduce an active interference suppression control algorithm to filter the control signal, and feed back the filtered control signal to the controller of the multi-rotor UAV, so that the multi-rotor UAV flies along a predicted trajectory and generates a desired feedback force on the environment.

[0035] The present application also provides an electronic device, the electronic device comprising:

[0036] one or more processors;

[0037] A storage device is used to store one or more programs. When the one or more programs are executed by the one or more processors, the electronic device implements the environmental interaction control method of the tiltable multi-rotor drone as described above.

[0038] The present application also provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor of a computer, the computer is enabled to execute the environmental interaction control method for a tiltable multi-rotor drone as described above.

[0039] The beneficial effects of the present invention are as follows: the environmental interactive control method and system of the tiltable multi-rotor UAV of the present invention are used for a fully-driven tiltable six-rotor UAV in contact detection tasks. The algorithm can control both position and force at the same time, and has accurate force tracking capabilities. It is also equipped with an anti-disturbance controller with a powerful anti-disturbance function, and still performs well under wind disturbances along the pole direction. It fills the gap of under-actuation and inaccurate force detection of traditional UAVs, greatly improves the control accuracy of UAVs, improves the ability of UAVs to perform contact detection tasks, and provides a new solution for contact operation UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The present invention will be further described below in conjunction with the accompanying drawings and embodiments:

[0041] Figure 1 is a flow chart of an environmental interactive control method for a tiltable multi-rotor drone shown in an embodiment of the present application;

[0042] Figure 2 Schematic diagram of the impedance model structure in one embodiment of the present application;

[0043] Figure 3 It is a schematic diagram of impedance with internal position loop control in this application;

[0044] Figure 4 This is a schematic diagram of a controlled force impedance control module subsystem in an embodiment of the present application;

[0045] Figure 5 A schematic diagram of a contact detection task in an embodiment of the present application;

[0046] Figure 6 It is the overall framework of the control algorithm in one embodiment of the present application;

[0047] Figure 7 A schematic diagram of the basic framework of ADRC in an embodiment of the present application;

[0048] Figure 8 This is a schematic diagram of the test results of the sinusoidal wave signal noise suppression of the auto-disturbance rejection device in this application;

[0049] Fig. 9 This is a schematic diagram of the square wave signal noise suppression test results of the auto-disturbance rejection device in this application;

[0050] Fig.10 This is a schematic diagram of the test results of the sawtooth wave signal noise suppression of the self-disturbance rejection device in this application;

[0051] Fig.11 A schematic diagram showing the position of the ADRC algorithm in the present application in the entire control algorithm system;

[0052] Fig.12 This is a schematic diagram of the force control mechanism capability test results in this application;

[0053] Fig.13 This is a schematic diagram of the anti-interference ability test results in this application;

[0054] Fig.14 This is a schematic diagram of the force tracking results under wind disturbance in this application. DETAILED DESCRIPTION

[0055] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the following embodiments and features in the embodiments can be combined with each other without conflict.

[0056] It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and thus the drawings only show the layers related to the present invention rather than being drawn according to the number, shape and size of the layers in actual implementation. In actual implementation, the type, quantity and proportion of each layer may be changed arbitrarily, and the layer layout may also be more complicated.

[0057] In the following description, numerous details are discussed to provide a more thorough explanation of embodiments of the present invention; however, it is apparent to one skilled in the art that embodiments of the present invention may be practiced without these specific details.

[0058] Traditional drones have underactuated characteristics. Traditional underactuated multi-rotor drones rely on less input to control spatial multi-degree-of-freedom motion, which makes the attitude and position too closely correlated, making the system design difficult and the control effect greatly limited. Drones control their flight altitude by overcoming gravity through vertical force. However, for drones under interference, the vertical force will also be affected, which will hinder the flight altitude control of the drone and weaken the anti-interference ability of the system. For example, when a rotor drone is disturbed by external wind fields, the drone also needs to change its attitude to generate horizontal force to resist wind disturbances. Therefore, this conventional rotor drone only has passive observation capabilities, weak anti-interference capabilities, and weak attitude stability, and cannot achieve contact operations with the physical environment.

[0059] Contact inspection tasks require force control of the drone during the inspection process. In this regard, the full-motion tilt-rotor multi-rotor drone provides a solution. The total lift of the multi-rotor drone with a full-drive configuration can always produce a component in the three-axis translation direction without relying on a non-zero attitude angle, so that position control and attitude control are completely independent, the system coupling and nonlinearity are significantly reduced, and the translation and rotation dynamics are decoupled, allowing the drone to apply force in any direction.

[0060] UAV systems often have difficulty distinguishing between contact forces, the effects of interaction with the wall, and forces in the orientation of the equipped robotic arm, making the control of the interaction forces very complex. Therefore, the lack of compensation for disturbances on the rigid arm will affect the performance of the working aerial manipulator platform.

[0061] This application constructs the underlying control framework of a fully-driven six-rotor aircraft, and realizes the control of the position and force of the drone. Considering the omissions of the traditional solution, an interference observer is inserted to filter out useless signals during contact detection, avoiding undercompensation caused by noise interference in the direction of the rigid arm of the system. The final control algorithm has excellent control capabilities and accurate force tracking capabilities, which fills the gaps in the traditional solution and provides a new solution for contact detection. The specific solution is as follows.

[0062] Figure 1 FIG. 1 is a flow chart of an environmental interactive control method for a tiltable multi-rotor UAV shown in an embodiment of the present application. Figure 1 As shown: The environment interactive control method of the tiltable multi-rotor UAV of this embodiment may include steps S110 to S140:

[0063] S110, constructs the rigid kinematics model, rigid body dynamics model, and control efficiency model of the multi-rotor UAV;

[0064] Specifically, the hexacopter architecture consists of the following four parts:

[0065] (1) (Rigid body) Kinematic model (2) (Rigid body) Dynamic model

[0066] (3) Controller model (4) Control efficiency model

[0067] 1. (Rigid Body) Kinematic Model

[0068] The kinematic model focuses on the position, linear velocity, height and angular velocity of the six-rotor. Its input is linear velocity and angular velocity, and its output is attitude and position. Assumptions:

[0069] (1) The hexagonal rotor is a rigid body

[0070] (2) The system has constant mass inertia and moment inertia

[0071] (3) The center of gravity coincides with the geometric center.

[0072] First, the position vector of the six rotors on the body is p e ∈R 3 Then, the relationship between linear velocity and position is shown in equation (2.1), where p e ∈R 3 represents the linear velocity of the hexacopter. The angular velocity can be expressed by equation (2.2).

[0073]

[0074] In the formula, and v is the linear velocity of the machine body, is the Euler angle, ω b is the body angular velocity, and W is the transformation matrix.

[0075] 2. (Rigid Body) Dynamics Model

[0076] The dynamic model focuses on the motion and the associated forces. The dynamic model of the hexacopter is modeled assuming that the system has leveled the propeller disk. The dynamic model of the hexacopter consists of an attitude dynamic model and a position dynamic model.

[0077] For the position dynamics model, by analyzing the forces on the aircraft, we can obtain equation (2.3), where Fz represents the force on the z-axis and g represents the acceleration due to gravity. Fz can also be regarded as propeller thrust.

[0078]

[0079] In the formula, is the linear acceleration in the world coordinate system, g is the gravitational acceleration, e3 is the z-axis unit column vector in the world coordinate system, f is the propeller thrust in the world coordinate system, m is the body mass, b is the rotation matrix, is the rotation matrix in the world coordinate system;

[0080] f is a three-dimensional vector, including the propeller thrust Fz on the z-axis;

[0081] For the attitude dynamics model, the mathematical relationship between torque and angular velocity is shown in equation (2.4). In equation (2.4), τ = [τ x , τ y , τ z ] is the torque generated by the hexacopter propeller, J is the moment of inertia, is the gyroscopic torque. In addition, the gyroscopic torque caused by a single rotating propeller is shown in equation (2.5).

[0082]

[0083] Where J is the moment of inertia of the motor, is the angular acceleration of the aircraft body, ω is the angular velocity of the aircraft body, Ga is the gyroscopic torque, τ is the torque generated by the propeller, G a,k represents the gyroscopic torque on the kth arm, J RP is the moment of inertia of the body, w k represents the rotation angular velocity of the kth arm;

[0084] Where τ=[τ x , τ y , τ z ] is the torque generated by the propeller on the x-axis, τ y is the torque generated by the propeller on the y-axis, τ z is the torque generated by the propeller in the z-axis;

[0085] is the rolling angle gyro moment, Ga,θ is the pitch angle gyro moment, is the yaw angle gyro moment, where:

[0086]

[0087] Where n is the number of blades, W is y is the component of the motor angular velocity on the y-axis, W x is the component of the motor angular velocity on the x-axis.

[0088] S120, constructing a flight control model based on the rigid kinematics model and the rigid body dynamics model;

[0089] 3. Hexacopter Flight Control Model

[0090] By combining the hexacopter dynamics model and kinematic model, we can obtain its flight control rigidity model, as shown in (2.7).

[0091]

[0092]

[0093] In the formula, is the rotation matrix;

[0094] The flight control model is used to control the flight speed v and the Euler angle based on the body linear velocity v Body mass m, body angular acceleration The body angular velocity ω calculates the gyroscopic torque Ga, propeller thrust f and motor moment of inertia J of multiple propellers of the multi-rotor drone to control the flight state of the multi-rotor drone.

[0095] 4. Control Efficiency Model

[0096] The input of the control efficiency model is the angular velocity of the propeller, and the output is torque and thrust. The output torque and force are both 6-dimensional, which means that the system achieves full drive.

[0097] The efficiency control model includes a thrust and reaction torque model and a thrust and moment model.

[0098] Thrust and reaction torque models

[0099] The six-rotor drone is hovering in the air, the propeller thrust is shown in (2.8), and the reaction torque is shown in (2.9). (2.9) represents the static model about the reaction torque, and its corresponding dynamic model is expressed in (2.10).

[0100]

[0101] Where Ti is the blade tension, c T is the blade thrust function, M i is the blade torque, w i is the angular velocity of the i-th propeller, c M is the blade torque function, that is, the torque on the i-th propeller, τ i represents the torque on the i-th propeller, are the components of the propeller blade speed in three directions.

[0102] Where, the blade force function c T The mathematical expression is:

[0103]

[0104] Where ρ is the air density, D p is the propeller diameter, C T is the tension coefficient;

[0105] Blade torque function c M for:

[0106]

[0107] In the formula, C M is the torque coefficient.

[0108] Thrust and torque model

[0109] The force and torque of the rotor are achieved by the motor driving the propeller. The angular velocity of each propeller is w. i , then the total force and torque in the three directions can be expressed by (2.12).

[0110]

[0111] Where A is the allocation matrix, Ω is the square of the angular velocity, γ represents the angle between the arm i and the x-axis of the body in the xy plane, β represents the angle between the arm i and the xoy plane of the body, and C f is the thrust coefficient of the rotor, C d represents the rotor drag coefficient, α is the tilt angle, i is the arm number, s is the abbreviation of sin, and c is the abbreviation of cos.

[0112] f and τ are the force and torque required for the multirotor to reach a specified position and attitude state.

[0113] S130, simplifying the flight control model, and constructing a target control model based on the simplified flight control model, the control efficiency model, and the impedance control model, wherein the target control model includes a position control part, an attitude control part, a control allocation part, an impedance control part, a motor control part, and a mathematical solver part;

[0114] First, a low-level flight control framework is established, which establishes a reasonable connection between the UAV's position, attitude, control distribution, and motor control.

[0115] The motor control part receives w dk , (k=1,2,…,nr), we need to obtain the throttle command σ required for each motor dk The position control part receives the desired position trajectory and needs to calculate the required total thrust and the two required Euler angles, roll and pitch. The attitude control part will first receive the required Euler angles and then get the desired torque. The control distribution part distributes the required propeller angular velocity to each motor in the hexacopter so that the model produces the required torque and force.

[0116] In fact, this control framework is designed for underactuated multirotors. Therefore, the final control framework will be adjusted according to the actual airframe model design.

[0117] To simplify the controller design, the previously established nonlinear model is adjusted. The adjusted simplified model can be expressed as (3.1).

[0118]

[0119] Based on the simplified flight control model, the position control equation is written as:

[0120]

[0121] In the formula, is the angular acceleration on the x-axis, is the angular acceleration on the y-axis, is the angular acceleration on the z-axis, is the roll angle, θ is the pitch angle, is the yaw angle;

[0122] For the horizontal position channel model, ignoring the high-order terms, we get:

[0123]

[0124] In the formula, is the height position, v h is the linear velocity in the height direction, is the linear acceleration in the height direction in the world coordinate system, is the distribution matrix of yaw angle, θ h is the set calculation matrix, is the transformation matrix, P x is the x-axis position, P y is the y-axis position;

[0125] The gesture channel model is simplified as:

[0126]

[0127] In the formula, and v z is the speed on the z axis, is the acceleration on the z-axis, is the angular acceleration;

[0128] The motor control equation is:

[0129] w k =C R σ+w b (3.7)

[0130]

[0131] σ is the motor input throttle, C R and w b is a static parameter representing the relationship between the throttle and the motor speed, w kd is the desired angular velocity, σ d is the motor throttle output of the controller.

[0132] 5. Impedance Control Model

[0133] The impedance control model can solve the shortcomings of traditional motion control when faced with high control accuracy requirements and large environmental stiffness environments. It has strong stability and provides a feasible strategy for solving position uncertainty by maintaining the relationship between external force and displacement.

[0134] Figure 2FIG. 1 is a schematic diagram of the impedance model structure in an embodiment of the present application. The impedance model in the present application is as follows: Figure 2 As shown; Figure 2 Where M is mass, x is displacement, S is the complex frequency variable in Laplace transform, B is damping, and K is stiffness;

[0135] The basic principle of variable impedance control is to conceptualize the robot through a spring-mass physics model, where the manipulator can dynamically adjust the virtual mass, damping, and stiffness parameters to modulate the system characteristics between a compliant state resisting external forces and a stable state.

[0136] Figure 3 Schematic diagram of impedance with internal position loop control in this application. The impedance control of this application embeds the position control loop into the force control loop, such as Figure 3 As shown, M d is the expected quality, B d is the expected damping, K e is the error stiffness, K d is the desired stiffness, X r is the real impedance, X e is the error impedance, X d is the expected impedance, S is the complex frequency variable of the Laplace transform, q d is the expected intermediate quantity of the Laplace transform, q e is the error intermediate quantity of the Laplace transform, F is the force, F r The contact force between the environment and the manipulator end effector can be converted into a position signal for the actual force. By superimposing this correction signal on the desired position signal and then subjecting it to PID control, simultaneous adjustment of contact force and position can be achieved.

[0137] When building the simulation model, two simplification methods were used. The first one was to replace the complex model of UVA with a six-degree-of-freedom model. In this case, the drone itself can be regarded as a mass point. The other one was to present the contact wall in the form of a spring model with a stiffness of 5000N / m.

[0138] In this impedance control model, the force exerted by the wall is calculated by multiplying the difference between the expected and actual displacements in a Cartesian coordinate system by the stiffness coefficient. The expected and actual forces from the wall are then input into the impedance control module, which then models their difference in the context of a spring-damper mass system, generates a displacement correction, and then combines this correction with the actual displacement of the system to obtain the corrected displacement. The drone is adaptive in this regard by adjusting for the error between the expected and actual displacements. Therefore, even when encountering a more elastic wall, the controller can adjust the expected displacement through external forces to avoid damage to the fuselage.

[0139] This combination of compensating displacement is the core of impedance control. To improve the control process, an impedance control subsystem is also introduced to mitigate the impact of force feedback. Figure 4 FIG. 1 is a schematic diagram of a controlled force impedance control module subsystem in an embodiment of the present application. Figure 4 As shown in Figure 1, this subsystem operates by processing the difference between the desired contact force and the actual contact force. The force error is input into the spring-damper mass system, and the resulting output is a displacement error. Although this displacement error lacks tangible physical meaning, it is different from the compensation displacement mentioned earlier. It only serves as an input signal to the PID controller to facilitate the adjustment of force control.

[0140] Now, we will combine the low-level flight control framework and the impedance model together. In the contact inspection task, the hexacopter needs to first move along the z-axis to reach a specified height, then move along the wall and use a rigid rod to detect defects on the wall.

[0141] Figure 5 is a schematic diagram of a contact detection task in an embodiment of the present application. The specific performance of the contact detection task is as follows Figure 5 shown.

[0142] In the y-axis and z-axis, the logic of our control algorithm is similar to that of the underactuated hexacopter. In the x-axis, there is no significant movement, which means that position control is not required. The contact force is along the x-axis, so the impedance model is applied in the x-axis.

[0143] Figure 6 The overall framework of the control algorithm in one embodiment of the present application is as follows: Figure 6 shown.

[0144] The overall control framework includes position control part, attitude control part, control distribution part, motor control part and mathematical solver part.

[0145] Position control part: The horizontal controller is related to the position of the six-rotor drone on the x and y axes, and the vertical height controller is related to the position of the six-rotor drone on the z axis. The position control part inputs the desired trajectory and yaw angle, and outputs the remaining two Euler angles and thrust, that is, the force on the z axis.

[0146] Attitude control part: describes the relationship between Euler angle and three-dimensional torque.

[0147] Impedance control part: The input of the impedance model is the actual contact force and the expected contact force, and the output is the force exerted by the hexagonal rotor on the x-axis. This project assumes that the six-rotor drone is a mass damper-spring system. However, as mentioned earlier, the mass damper and spring parameters are fictitious and need to be adjusted.

[0148] Control Distribution: The control distribution describes the relationship between the three-dimensional force, torque and the speed of the hexacopter propeller. The input is the applied force and torque, and the output is the three-dimensional propeller speed. If the hexacopter is underdriven, the input will produce torque in 3 dimensions, and thrust is the force on the z-axis.

[0149] Mathematical solver part: This part is responsible for solving the state of the system according to the output of the motor control part. Its output is the actual position, Euler angle, etc., which will be transmitted to the previous control part.

[0150] S140, obtaining expected flight information of the multi-rotor UAV, and obtaining actual feedback force and expected feedback force generated when the actuator end of the multi-rotor UAV contacts the environment;

[0151] S150, inputting the expected flight information, the actual feedback force and the expected feedback force into the target control model to obtain a control signal;

[0152] S160, introducing an active interference suppression control algorithm to filter the control signal, and feeding back the filtered control signal to the controller of the multi-rotor UAV, so that the multi-rotor UAV flies along a predicted trajectory and generates a desired feedback force on the environment.

[0153] In this application, the role of the disturbance filter module is to estimate the external disturbance and automatically adjust the control input variable in the required direction according to the force or displacement input required by the drone and the contact force feedback from the sensor. Based on this requirement, the active disturbance rejection control algorithm is selected as the external disturbance observer and filter.

[0154] The ADRC (Active Disturbance Rejection Control) algorithm uses three different algorithmic units: TD (Tracking Differentiator), NLSEF (Nonlinear State Error Feedback), and ESO (Extended State Observer). TD is used to provide real-time estimation of the system state, NLSEF introduces nonlinear feedback to improve control performance, and ESO is used to estimate and compensate for disturbances and unmodeled dynamics within the system. The following are the relevant equations:

[0155] Controlled objects:

[0156]

[0157] TD:

[0158] v1(k+1)=v1(k)+hv2(k) (4.7)

[0159] v2(k+1)=v2(k)+hfst(v1(k)-v0,v2(k),r,h0) (4.8)

[0160] ESO:

[0161] ε1=z1(k)-y(k) (4.9)

[0162] z1(k+1)=z1(k)+h(z2(k)-β 01 ε1) (4.10)

[0163] z2(k+1)=z2(k)+h(z3(k)-β02fal(ε1,α1,δ)=bu(k)) (4.11)

[0164] z2(k+1)=z2(k)-hβ 03 fal(ε1,α2,δ) (4.12)

[0165] NLSEF:

[0166] e1=v1(k)-z1(k) (4.13)

[0167] e2=v2(k)-z2(k) (4.14)

[0168] u0=β 01 fal(e1,α1,δ)+β 02 fal(e1,α2,δ) (4.15)

[0169] u(k)=u0-z3(k) / b (4.16)

[0170] Here are the meanings of the symbols in the ADRC equation:

[0171] x: state vector of the controlled object;

[0172] The derivative of the state vector with respect to time;

[0173] w: process noise;

[0174] t: time;

[0175] b: control input coefficient;

[0176] y: output of the controlled object;

[0177] v1: Tracking state variable 1 in the differentiator;

[0178] v2: Tracking state variable 2 in the differentiator;

[0179] h: time step;

[0180] r: reference value;

[0181] h0: initial value of hst function;

[0182] ε1: estimated error or difference between measured output and actual output;

[0183] z1: state variable 1 in the extended state observer;

[0184] z2: state variable 2 in the extended state observer;

[0185] z3: state variable 3 in the extended state observer;

[0186] β 01 , β 02 , β 03 : observer gain or coefficient;

[0187] fal: nonlinear function or nonlinear approximate function;

[0188] α1, α2: parameters of the fal function;

[0189] δ: tolerance parameter of fal function;

[0190] e1: Error between v1 and z1;

[0191] e2: Error between v1 and z2;

[0192] u0: control signal generated by nonlinear state error feedback;

[0193] u(k): final control signal at time step k;

[0194] Figure 7 FIG. 1 is a schematic diagram of the basic framework of ADRC in an embodiment of the present application. The ADRC framework in the present application is as follows: Figure 7 shown.

[0195] Based on this framework and principle, a set of ADRC algorithm modules were built in Simulink, specifically designed for multi-rotor tilt-rotor UAVs. By considering 28 expected parameters of the UAV, ESO automatically identifies interference factors and calculates real-time compensation. This compensation is then fed back into the UAV control system, effectively eliminating internal and external interference.

[0196] The testing process is as follows:

[0197] This application builds a separate test platform for the ADRC module. In order to simulate the filtering effect that is subsequently loaded into the UAV force control algorithm, a signal generator is used to simulate the expected force input in the direction of interest and used as a simulated force sensor signal after adding Gaussian distributed noise.

[0198] Figure 8This is a schematic diagram of the test results of the sine wave signal noise suppression of the self-disturbance rejection device in this application. First, the expected sine wave signal is input for testing. The test results are as follows: Figure 8 As shown in the figure, the filtering effect of the test is basically in line with expectations, with obvious filtering effect. Although there is some signal distortion in the front section, the ADRC still has a very good suppression effect on the overall noise, almost completely filtering out the interference.

[0199] Fig. 9 This is a schematic diagram of the square wave signal noise suppression test results of the self-disturbance rejection device in this application. Fig.10 Schematic diagram of the noise suppression test results of the sawtooth wave signal of the self-anti-disturbance device in this application. Figure 9-10 As shown, the present application also has a good filtering effect on square waves and sawtooth waves.

[0200] ADRC has excellent processing capabilities for all three signals. For square wave and sawtooth wave signals, ADRC processing will cause some unnecessary fluctuations and distortion due to the non-conductive nature of the signals. For most continuous signals, ADRC can meet the requirements of filtering effects. In general, ADRC has very strong calculation speed, signal processing continuity and filtering capabilities.

[0201] This project requires measuring the forces from the walls and other states of the drone. These measurements may not be completely accurate due to unexpected errors and other external interference. In order to improve the accuracy of the measurement data and eliminate errors, ADRC is introduced in the control algorithm to filter these signals. For the force signal of the wall of the contact detection task, the sensor measurement can be input, or the feedback signal given by the drone control model can be used as the observation of ADRC. For the position, velocity and other state signals of the drone, ADRC is used to reduce the impact of measurement errors.

[0202] Considering the coupling of virtual displacement and force after applying the wall model, we choose the position variable in the x direction that this project is concerned about as the input variable of ADRC, which is actually calculated by the dynamic model and the impedance correction model. The required displacement is added as another necessary input of ADRC. The corrected displacement control signal will directly enter the drone position controller as feedback to correct the input signal. The position of the ADRC algorithm in the entire control algorithm system is shown in Figure 2. Fig.11 shown.

[0203] The overall algorithm framework includes the dynamic model, impedance control algorithm, interference filtering algorithm and wall model of the six-rotor tiltable UAV. The main control algorithms are encapsulated into subsystems, namely position controller, attitude controller, control distributor, signal converter, motion information solver, signal distributor, impedance correction model, wall model, x-axis specific impedance module and anti-disturbance controller, as well as simple module signal generator, noise generator and oscilloscope.

[0204] The signal generator generates the required signals and inputs them into the UAV controller. The control variables of the UAV are calculated through the UAV dynamics model and information solver, and the signals are respectively input into the position and attitude controllers through the 36 signal distributor to complete the control closed loop. At the same time, the output position signal of the signal distributor is input into the impedance correction algorithm and corrected in combination with the expected signal to achieve accurate position tracking, which is converted into a force signal through the PID and wall model. The algorithm adds force signals and noise to simulate external disturbances in actual situations, and feeds into the control distributor through an impedance module dedicated to the x-axis force signal. In order to deal with the impact of external disturbances, an ADRC is placed after the impedance correction algorithm to filter out noise from the position signal, which is corrected and re-input into the position controller.

[0205] Experiment 1: Algorithm’s force control capabilities

[0206] The first experiment is to test the algorithm's force control performance for the required force in the rod direction (i.e., the x-axis direction). The complete algorithm minus the filtering algorithm is used as a test platform.

[0207] Table 1 Parameter values ​​of Experiment 1

[0208]

[0209]

[0210] Experimental selection of variable step size and automatic selection of solver The minimum step size was set to 1e-7, and all other parameters were automatic. The solution time was set to approximately 40 seconds. Fig.12 The simulation results are shown. Even though the generator signal changes dramatically, the force control system still completes fast and accurate force tracking with good convergence effect and convergence time of about 4 seconds.

[0211] Experiment 2: Interference Observer's Anti-interference Capability

[0212] The second experiment will test the filtering performance of the disturbance observer to see if it can make the tracking force signal closer to the true value in the presence of noise. The testbed is the complete algorithm with the filtering module.

[0213] Its relevant parameters are shown in Table 2. Most of the parameters are the same as those of the first experiment, only the signal from the generator is different, and the parameters of the impedance control have been fine-tuned to include the parameters of the active disturbance rejection controller.

[0214] Table 2 Overall algorithm parameter values

[0215]

[0216]

[0217] The solver is still set to variable step size, the solver mode is automatically selected, and the minimum step size is changed to 1e-6 to reduce the simulation time. Due to some unexplained non-convergence characteristics of the initial value calculation, the ADRC settings are changed to discrete time to simulate the dynamics for 15 seconds. The final result is shown in Fig.13 As shown. Red is the input desired force, blue is the desired force after adding noise, and white is the filtered force signal. It can be seen that the force is tracked very clearly and converges faster. ADRC filters the random noise almost perfectly. The corrected force signal is very smooth.

[0218] Experiment 3: Force tracking capability under wind disturbance

[0219] For the third experiment, we will test the control algorithm’s force tracking capability in the presence of wind. Wind disturbances can affect the actual velocity of the drone relative to the ground. Therefore, we introduce a wind modeling section and use the wind speed in (5.1) to modify the actual velocity of our hexacopter.

[0220]

[0221] In Experiment 3, the parameters were the same as those in Table 2. The force tracking results are shown in Fig.14 The orange curve is the combination of white noise and the desired contact force, and the white curve is the actual force applied by the drone under wind disturbance. Fig.14 It can be seen that the output contact eventually reaches a steady-state value that is approximately the same as the desired force. It can therefore be concluded that the control algorithm can achieve high-precision force tracking in the presence of wind disturbances.

[0222] Therefore, this application proposes an environmental interactive control method for a tiltable multi-rotor UAV, which is used for a fully-driven tiltable six-rotor UAV in contact detection tasks. The algorithm can control both position and force at the same time, and has accurate force tracking capabilities. It is also equipped with an anti-disturbance controller with powerful anti-interference capabilities, and still performs well under wind disturbances along the pole direction. It fills the gap of under-actuation and inaccurate force detection of traditional UAVs, greatly improves the control accuracy of UAVs, improves the ability of UAVs to perform contact detection tasks, and provides a new solution for contact operation UAVs.

[0223] This application also provides an environmental interactive control system for a tiltable multi-rotor drone, including:

[0224] Model building module, used to build the rigid kinematics model, rigid body dynamics model, and control efficiency model of the multi-rotor UAV;

[0225] A flight control module construction module, used to construct a flight control model based on the rigid kinematics model and the rigid body dynamics model;

[0226] a target model construction module, used for simplifying the flight control model, and constructing a target control model based on the simplified flight control model, the control efficiency model and the impedance control model, wherein the target control model includes a position control part, an attitude control part, a control distribution part, an impedance control part, a motor control part and a mathematical solver part;

[0227] An acquisition module is used to acquire the expected flight information of the multi-rotor UAV, and to acquire the actual feedback force and the expected feedback force generated when the actuator end of the multi-rotor UAV contacts the environment;

[0228] a control signal calculation module, used for inputting the expected flight information, the actual feedback force and the expected feedback force into the target control model to obtain a control signal;

[0229] The filtering and control module is used to introduce an active interference suppression control algorithm to filter the control signal, and feed back the filtered control signal to the controller of the multi-rotor UAV, so that the multi-rotor UAV flies along a predicted trajectory and generates a desired feedback force on the environment.

[0230] This application proposes an environmental interactive control system for a tiltable multi-rotor UAV, which is used for a fully-driven tiltable six-rotor UAV in contact detection tasks. The algorithm can control both position and force at the same time, and has accurate force tracking capabilities. It is also equipped with an anti-disturbance controller with powerful anti-interference capabilities, and still performs well under wind disturbances along the pole direction. It fills the gap of under-actuation and inaccurate force detection of traditional UAVs, greatly improves the control accuracy of UAVs, improves the ability of UAVs to perform contact detection tasks, and provides a new solution for contact operation UAVs.

[0231] This embodiment further provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, any one of the methods in this embodiment is implemented, wherein the method is the execution logic of this system.

[0232] This embodiment also provides an electronic terminal, including: a processor and a memory;

[0233] The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the terminal executes any one of the methods in this embodiment.

[0234] The computer-readable storage medium in this embodiment can be understood by ordinary technicians in this field: all or part of the steps of implementing the above-mentioned method embodiments can be completed by hardware related to the computer program. The aforementioned computer program can be stored in a computer-readable storage medium. When the program is executed, the steps of the above-mentioned method embodiments are executed; and the aforementioned storage medium includes: ROM, RAM, magnetic disk or optical disk and other media that can store program codes.

[0235] The electronic terminal provided in this embodiment includes a processor, a memory, a transceiver and a communication interface. The memory and the communication interface are connected to the processor and the transceiver and complete communication with each other. The memory is used to store computer programs, the communication interface is used to communicate, and the processor and the transceiver are used to run computer programs so that the electronic terminal executes each step of the above method.

[0236] In this embodiment, the memory may include a random access memory (RAM), and may also include a non-volatile memory (non-volatile memory), such as at least one disk memory.

[0237] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0238] In the above-mentioned embodiments, although the present invention has been described in conjunction with the specific embodiments of the present invention, many replacements, modifications and variations of these embodiments will be apparent to those of ordinary skill in the art based on the foregoing description. The embodiments of the present invention are intended to cover all such replacements, modifications and variations falling within the broad scope of the appended claims.

[0239] The above embodiments are merely illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Anyone familiar with the art may modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by a person of ordinary skill in the art without departing from the spirit and technical concept disclosed by the present invention shall still be covered by the claims of the present invention.

Claims

1. An environmental interactive control method for a tiltable multi-rotor UAV, characterized in that: Includes steps: Construct rigid kinematics model, rigid body dynamics model, and control efficiency model of multi-rotor UAV; Constructing a flight control model based on the rigid kinematics model and the rigid body dynamics model; Simplifying the flight control model, and constructing a target control model based on the simplified flight control model, the control efficiency model and the impedance control model, wherein the target control model includes a position control part, an attitude control part, a control distribution part, an impedance control part, a motor control part and a mathematical solver part; Obtaining expected flight information of the multi-rotor UAV, and obtaining actual feedback force and expected feedback force generated when the actuator end of the multi-rotor UAV contacts the environment; Inputting the expected flight information, the actual feedback force and the expected feedback force into the target control model to obtain a control signal; An active interference suppression control algorithm is introduced to filter the control signal, and the filtered control signal is fed back to the controller of the multi-rotor UAV, so that the multi-rotor UAV flies along the predicted trajectory and generates the expected feedback force to the environment.

2. The environmental interactive control method of a tiltable multi-rotor UAV according to claim 1, characterized in that: The kinematic model describes the relationship between the position and linear velocity of the drone, and the relationship between the angular velocity of the drone and the Euler angle.

3. The environmental interactive control method of a tiltable multi-rotor UAV according to claim 2, characterized in that: The rigid body dynamics model includes a position dynamics model and an attitude dynamics model. The position dynamics model characterizes the relationship between the linear acceleration in the world coordinate system and the propeller thrust in the world coordinate system; the attitude dynamics model characterizes the relationship between the angular acceleration and angular velocity of the UAV, the gyroscopic torque of the UAV, and the propeller torque, as well as the relationship between the gyroscopic torque of the UAV and the rotational inertia of the body and the angular velocity of the arm rotation.

4. The environmental interactive control method of a tiltable multi-rotor UAV according to claim 1, characterized in that: The flight control model is used to calculate the flight speed v, Euler angle θ, body mass m, and body angular acceleration based on the body linear velocity v, Euler angle θ, body mass m, and body angular acceleration. The body angular velocity ω calculates the gyroscopic torque Ga, propeller thrust f and motor moment of inertia J of multiple propellers of the multi-rotor drone to control the flight state of the multi-rotor drone.

5. The environmental interactive control method of a tiltable multi-rotor UAV according to claim 1, characterized in that: The efficiency control model includes a thrust and reaction torque model and a thrust and torque model, wherein the thrust and reaction torque model characterizes the relationship between the blade pull and the angular velocity of the propeller, and the relationship between the blade torque and the angular velocity of the propeller; the thrust and torque model characterizes the relationship between the blade thrust and the angular velocity of the propeller, and the relationship between the blade torque and the angular velocity of the propeller.

6. The environmental interactive control method of a tiltable multi-rotor UAV according to claim 1, characterized in that: The flight control model is simplified, including: The flight control model is simplified to obtain a simplified flight control model, wherein the simplified flight control model is: Based on the simplified flight control model, the position control equation is written as: In the formula, and v is the linear velocity of the machine body, is the Euler angle, ω b is the body angular velocity, W is the transformation matrix, is the linear acceleration in the world coordinate system, g is the acceleration due to gravity, e3 is the unit column vector of the z-axis in the world coordinate system, f is the propeller thrust in the world coordinate system, m is the mass of the aircraft, τ is the torque generated by the propeller, J is the moment of inertia of the motor, is the angular acceleration on the x-axis, is the angular acceleration on the y-axis, is the angular acceleration on the z-axis, is the roll angle, θ is the pitch angle, is the yaw angle.

7. The environmental interactive control method of a tiltable multi-rotor UAV according to claim 6, characterized in that: A target control model is constructed based on the simplified flight control model, the control efficiency model and the impedance control model, including: Building a PID-based control loop based on the simplified flight control model and the control efficiency model; An impedance control model is embedded in the control loop to obtain a target control model, wherein the impedance control model uses the multi-rotor drone as a spring model, the input of the impedance control model is the actual feedback force and the expected feedback force, and the output of the impedance control model is the corrected displacement.

8. An environmental interactive control system for a tiltable multi-rotor UAV, characterized in that: include: Model building module, used to build the rigid kinematics model, rigid body dynamics model, and control efficiency model of the multi-rotor UAV; A flight control module construction module, used to construct a flight control model based on the rigid kinematics model and the rigid body dynamics model; a target model construction module, used for simplifying the flight control model, and constructing a target control model based on the simplified flight control model, the control efficiency model and the impedance control model, wherein the target control model includes a position control part, an attitude control part, a control distribution part, an impedance control part, a motor control part and a mathematical solver part; An acquisition module is used to acquire the expected flight information of the multi-rotor UAV, and to acquire the actual feedback force and the expected feedback force generated when the actuator end of the multi-rotor UAV contacts the environment; a control signal calculation module, used for inputting the expected flight information, the actual feedback force and the expected feedback force into the target control model to obtain a control signal; The filtering and control module is used to introduce an active interference suppression control algorithm to filter the control signal, and feed back the filtered control signal to the controller of the multi-rotor UAV, so that the multi-rotor UAV flies along a predicted trajectory and generates a desired feedback force on the environment.

9. An electronic device, characterized in that: The electronic device comprises: one or more processors; A storage device for storing one or more programs, which, when executed by the one or more processors, enables the electronic device to implement the environmental interaction control method of the tiltable multi-rotor drone as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed by a processor of a computer, the computer is caused to execute the environmental interactive control method of a tiltable multi-rotor drone according to any one of claims 1 to 7.

Images