Unmanned aerial vehicle anti-wind disturbance attitude control method and device in mine tunnel environment

Abstract

The invention provides an unmanned aerial vehicle anti-wind disturbance attitude control method and device in a mine tunnel environment, and relates to the technical field of mine tunnel unmanned aerial vehicle inspection. According to the four-rotor unmanned aerial vehicle anti-wind disturbance attitude control method suitable for the mine tunnel environment, the requirement for stable inspection of a four-rotor unmanned aerial vehicle in the mine tunnel environment is combined, firstly, through position outer ring control, a PID controller is used for controlling a position subsystem of the unmanned aerial vehicle according to an expected track, and then through attitude inner ring control, the position subsystem of the unmanned aerial vehicle is controlled; according to the method, external disturbance can be effectively inhibited, the flight safety and path tracking precision of the unmanned aerial vehicle are improved, so that the unmanned aerial vehicle can have stable routing inspection capability when facing sudden wind disturbance of a mine tunnel or a complex fluid environment, and the unmanned aerial vehicle can be applied to the field of unmanned aerial vehicle routing inspection. And a certain technical support is provided for application of the four-rotor unmanned aerial vehicle in a narrow and complex environment.

Description

Technical Field

[0001] The present invention relates to the technical field of mine tunnel unmanned aerial vehicle inspection, and in particular to a method and device for controlling the anti-wind disturbance attitude of an unmanned aerial vehicle in a mine tunnel environment. Background Art

[0002] The mine tunnel environment is a special operating environment with high risk, complexity and closedness, usually accompanied by narrow space, low illumination, slippery ground and uneven air flow. In this environment, traditional inspection and operation methods rely on manual operation or ground equipment, which is not only inefficient but also has high safety risks. Therefore, the use of quadrotor drones for autonomous inspection, environmental monitoring and safety assessment in this harsh environment has become a hot topic in recent years.

[0003] However, quadcopter drones face many technical challenges when flying in a mine tunnel environment. First, since stable GPS signals are usually not available in such environments, drones cannot rely on satellite positioning for autonomous flight control. Second, the space inside the mine tunnel is narrow and the environment is changeable. Drones are easily affected by airflow disturbances, which can cause unstable attitudes and, in severe cases, collision accidents. Therefore, improving the drone's ability to resist wind disturbances and attitude stability in a mine tunnel environment has become an important topic in the research of autonomous flight control of drones.

[0004] At present, the flight control methods of UAVs in mine tunnel environments mainly focus on data fusion based on vision, lidar or inertial measurement unit to achieve accurate estimation of attitude and position. Although such methods have achieved certain results, it is still difficult to ensure flight stability and safety when facing sudden wind disturbances or complex fluid environments. Therefore, there is a need for an anti-wind disturbance attitude control method for mine tunnel environments that can effectively suppress external disturbances and improve the flight safety and path tracking accuracy of UAVs.

[0005] The present invention is proposed based on this. Summary of the invention

[0006] The purpose of the present invention is to provide a method and device for controlling the anti-wind disturbance attitude of an unmanned aerial vehicle in a mine tunnel environment, which can effectively suppress external disturbances, improve the flight safety and path tracking accuracy of the unmanned aerial vehicle, and enable the unmanned aerial vehicle to have the ability to perform stable inspections when facing sudden wind disturbances in mine tunnels or complex fluid environments.

[0007] To achieve the above object, the present invention provides the following technical solutions:

[0008] In a first aspect, the present invention provides a method for controlling the anti-wind disturbance attitude of an unmanned aerial vehicle in a mine tunnel environment, comprising the following steps: S1 position outer loop control, using a PID controller to control the position subsystem of the unmanned aerial vehicle according to a desired trajectory; S2 attitude inner loop control, using an adaptive sliding mode controller based on an obstacle function to control the attitude subsystem of the unmanned aerial vehicle according to the desired trajectory.

[0009] In a first aspect, the present invention provides a preferred solution, in which, in the S1 position outer loop control, the expected position and expected yaw angle of the UAV are obtained according to the expected trajectory, the expected position and current position of the UAV are used as inputs of the PID controller, and the PID controller outputs the expected pitch angle and expected roll angle of the UAV in combination with the expected yaw angle, as well as outputs one of the control input quantities of the UAV, which is used to control one of the propellers of the quadrotor UAV.

[0010] In a first aspect, the present invention provides a preferred solution. In the S2 attitude inner loop control, the current roll angle, pitch angle and yaw angle of the UAV, the expected yaw angle, the expected pitch angle and the expected roll angle are used as inputs of the adaptive sliding mode controller based on the obstacle function. The adaptive sliding mode controller outputs the other three control input quantities of the UAV, which are used to control the other three propellers of the quadrotor UAV.

[0011] In a second aspect, the present invention provides an anti-wind disturbance attitude control device for an unmanned aerial vehicle in a mine tunnel environment, which adopts the above method and includes: a position outer loop controller, which is used to control the position subsystem of the unmanned aerial vehicle using a PID controller according to a desired trajectory; and an attitude inner loop controller, which is used to control the attitude subsystem of the unmanned aerial vehicle using an adaptive sliding mode controller based on an obstacle function according to the desired trajectory.

[0012] Compared with the prior art, the above technical solution has the following advantages:

[0013] The present invention proposes a wind-resistant attitude control method for quad-rotor drones suitable for mine tunnel environments in combination with the demand for stable inspections of quad-rotor drones in mine tunnel environments, and designs a set of robust wind-resistant attitude control strategies in GPS-denied environments, which effectively improves the wind-resistant capability and flight stability of quad-rotor drones in mine tunnel environments, and provides certain technical support for the application of quad-rotor drones in narrow and complex environments. In short, the wind-resistant attitude control method and device for drones in mine tunnel environments of the present invention can effectively suppress external disturbances, improve the flight safety and path tracking accuracy of drones, and enable drones to have stable inspection capabilities when facing sudden wind disturbances in mine tunnels or complex fluid environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0015] Figure 1 A flowchart of a method for controlling the anti-wind disturbance attitude of a UAV in a mine tunnel environment provided by a specific embodiment of the present invention;

[0016] Figure 2 A signal flow diagram of a UAV in a method for controlling the anti-wind disturbance attitude of a UAV in a mine tunnel environment provided by a specific embodiment of the present invention;

[0017] Figure 3 A schematic diagram of an obstacle function in a method for controlling the anti-wind disturbance attitude of a UAV in a mine tunnel environment provided by a specific implementation of the present invention. DETAILED DESCRIPTION

[0018] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0019] In view of the defects or improvement needs of the prior art, this embodiment provides a method for controlling the attitude of a quad-rotor UAV in a mine tunnel environment to resist wind disturbance, so that the UAV can have the ability to perform stable inspections when facing sudden wind disturbances in the mine tunnel or complex fluid environments. It mainly includes the following contents: the UAV adaptive sliding film attitude controller designed based on the obstacle function can not only make the output converge to the predefined zero neighborhood, but also prevent the control gain from being overestimated, and does not need to use any information about the upper limit of the disturbance, nor does it need to use any low-pass filter, which reduces unnecessary parameter adjustments and greatly improves the ability of the quad-rotor UAV to cope with various wind disturbances.

[0020] Please refer to Figure 1, this embodiment provides a method for controlling the anti-wind disturbance attitude of a UAV in a mine tunnel environment, comprising the following steps: S1 position outer loop control, using a PID controller to control the position subsystem of the UAV according to the expected trajectory; S2 attitude inner loop control, using an adaptive sliding mode controller based on an obstacle function to control the attitude subsystem of the UAV according to the expected trajectory. Correspondingly, this embodiment provides a device for controlling the anti-wind disturbance attitude of a UAV in a mine tunnel environment, which adopts the above method, comprising: a position outer loop controller, used to control the position subsystem of the UAV according to the expected trajectory using a PID controller; an attitude inner loop controller, used to control the attitude subsystem of the UAV according to the expected trajectory using an adaptive sliding mode controller based on an obstacle function. This embodiment is mainly aimed at the anti-wind disturbance attitude of a quad-rotor UAV in a mine tunnel, specifically as follows:

[0021] This embodiment provides a UAV anti-wind disturbance attitude control device in a mine tunnel environment, also called a controller for a quad-rotor UAV in a mine tunnel, which is a dual closed-loop control controller. The controller design is divided into two parts: a position outer loop and an attitude inner loop. Figure 2 As shown in Figure 1, the outer loop is the position subsystem of the drone, and the inner loop is the attitude subsystem of the drone. Formula (1) represents the position loop of the system, and formula (2) represents the attitude loop of the system.

[0022]

[0023] Where P = [x, y, z] represents the current position of the drone, m represents the mass of the drone, g represents gravity, Θ = (φ, θ, ψ) represents the attitude angle, and the three angles φ, θ, and ψ represent the current roll angle, pitch angle, and yaw angle of the drone’s quadrotor, respectively. xx ,J yy ,J zz Represent the moment of inertia of the x, y, and z axes respectively, They represent the second-order derivatives of the corresponding Euler angles, They represent the first derivative of the corresponding Euler angles, Represent the second-order derivative of the current position of the UAV, τ dφ ,τ dθ ,τ dψ They represent the external disturbances of the roll angle channel, the pitch angle channel, and the yaw angle channel, respectively. [u 1 ,u 2 ,u 3 ,u 4 ] is the control input of the UAV, which is used to control the four propellers of the UAV.

[0024] By dividing the control system into inner and outer loops, the design of the controller can be customized for each loop to meet different control requirements. In the mine wind farm, the quadcopter will be subject to additional interference from the external airflow. The outer loop uses a PID controller, which has good stability and adaptability and can handle a certain degree of system disturbance and uncertainty. However, in the presence of strong interference, the performance of the PID controller may be limited.

[0025] In the S1 position outer loop control, the expected position and expected yaw angle of the drone are obtained according to the expected trajectory. The expected position and current position of the drone are used as the input of the PID controller. The PID controller outputs the expected pitch angle and expected roll angle of the drone in combination with the expected yaw angle, and outputs one of the control inputs of the drone, which is used to control one of the propellers of the quadcopter drone. The details are as follows:

[0026] As the inner loop of the quadrotor, the attitude controller needs to directly handle external interference and respond quickly.

[0027] In order to meet these requirements, the present invention proposes an outer loop PID control to track the position of the UAV, and an inner loop barrier function based adaptive sliding mode controller (BFASMC) to enable the UAV to have good anti-wind disturbance capability.

[0028] The outer loop of the drone uses PID control to define the desired position P d =[x d y d z d ], then the outer ring error E pos =[e x e y e z ]:

[0029]

[0030] Thus the outer loop control output U pos =[u x u y u z ]:

[0031]

[0032] where k xp ,k yp ,k zp ,k xi ,k yi ,k zi is a constant, which is set and adjusted according to actual flight needs. The desired yaw angle is given by the control output of the outer loop. The desired pitch angle θ can be obtained by inverse solutiond and roll angle φ d :

[0033] The expected attitude angle of the inner loop is obtained by inverse solution:

[0034]

[0035] In the S2 attitude inner loop control, the current roll angle, pitch angle and yaw angle of the drone, the expected yaw angle, the expected pitch angle and the expected roll angle are used as the input of the adaptive sliding mode controller based on the obstacle function, and the adaptive sliding mode controller outputs the other three control inputs of the drone to control the other three propellers of the quadcopter drone. The details are as follows:

[0036] The inner loop of the UAV adopts an adaptive sliding mode controller based on the obstacle function. The obstacle function is obtained as follows: First, for any given ε>0, there exists a continuous even function K b (x), for x∈[-ε,ε], K b ∈[b,∞],K b (x) is strictly increasing on x∈[0,ε], ε, x, b are all positive integers, and satisfy:

[0037]

[0038] (2) K b (x) has a unique minimum value at zero, K b (0) = b ≤ 0;

[0039] Then it is called K b (x) is the barrier function, and the barrier function is taken as:

[0040]

[0041] Where ε>0, the function has a minimum value K(0)=0 at the zero point.

[0042] Next, for the attitude subsystem of the quadrotor drone, a sliding mode controller based on the obstacle function is designed. Take the roll angle channel as an example, and the pitch and yaw channels are similar. The tracking error of the roll channel is defined as:

[0043] e φ =φ-φ d

[0044] Design the synovial surface as follows:

[0045] s φ =e φ +ce φ

[0046] We can get:

[0047]

[0048] Thus, the design control input u is obtained 2 .

[0049] Where K(t,s φ ) is the adaptive gain, and its adaptive rate is:

[0050]

[0051] in, are all positive integers, and t is time; following the same design method, the controllers for the pitch and yaw channels of the UAV can be designed, thereby giving the BF-based Adaptive Sliding Mode Controller (BFASMC) based on the obstacle function as follows:

[0052]

[0053] Among them, Ω is the angular velocity vector and J is the moment of inertia matrix.

[0054] It should be noted that u 3 ,u 4 Can be merged into Solve in, and get u through this formula 3 ,u 4 .

[0055] In addition, this embodiment also provides a four-rotor UAV hardware system for resisting wind disturbance in a mine tunnel environment, which mainly includes six parts: a host computer, a lower computer, a sensor module, a power module, an actuator, and an external safety assurance module. The sensor module provides the lower computer with the posture information of the UAV in the mine tunnel, including the three-dimensional position and the quaternion information describing the attitude angle. The lower computer transmits the status data of the UAV to the host computer. The host computer calculates the desired control quantity according to the control requirements and transmits it to the lower computer for execution, ensuring the stability of the UAV under wind disturbance conditions. The actuator provides power support for the UAV flight, and the power module provides power guarantee for the upper computer, the lower computer, the sensor module and the actuator and other airborne systems. The upper computer, the lower computer and the sensor module in the present invention are introduced in detail below.

[0056] (1) Sensor module: According to the application and performance requirements, the sensor modules directly connected to the flight control used in the present invention include: gyroscope, accelerometer and remote control receiver. The gyroscope and accelerometer are integrated sensors built into the flight control board. Since the magnetometer in the mine tunnel is interfered with, the data obtained is not accurate, and the magnetometer built into the flight control is shielded. The fusion of gyroscope and accelerometer data provides attitude angle and three-axis acceleration information for the UAV system. The remote control receiver and the remote control in the external safety assurance module are a set of signal receiving and sending devices. The pilot can force the UAV to jump from the program control mode to the manual control mode by tossing the control stick in the remote control, thereby ensuring the flight safety of the UAV system.

[0057] (2) Lower computer: According to the application and performance requirements, the present invention uses PIXHWAK mini6 c as the lower computer, that is, the flight control board. The module runs a sensor data fusion algorithm, which obtains accurate position, speed, acceleration, attitude and other status information of the drone system by fusing the data of built-in sensors such as gyroscopes and accelerometers and external sensors such as mid360 three-dimensional laser radar, and combines the control instructions received from the upper computer with the status information to generate a PWM signal for controlling the motor speed. The PWM signal is then transmitted to the electronic speed regulator to control the motor speed.

[0058] (3) Host computer: According to the application and performance requirements, the present invention uses NVIDIA Jetson TX2 NX as the host computer. The host computer is an integrated module, and a state machine system runs inside the board. It will receive information sent from the lower computer, external sensors and other modules. The data of the external sensor mid360 laser radar will run the FAST-LIO positioning algorithm in the host computer to obtain the real-time position and attitude of the drone in the inertial coordinate system, and the position and attitude information will be integrated to the lower computer through MAVROS. In addition, the host computer can also generate the desired control instructions in combination with the current flight mission and pass them to the flight control board for execution. The control instructions in the present invention are the desired attitude angle and desired thrust of the drone, on this basis, the drone system can be controlled in the most flexible and safe way.

[0059] (4) External sensor module: This part mainly includes the mid360 laser radar directly connected to the host computer. The mid360 laser radar is a medium-wavelength laser radar selected according to the mine environment. It has a certain degree of penetration into the dust inside the mine, thereby ensuring that the laser data is the real distance to obstacles or walls. The laser radar can scan 360 degrees in the horizontal direction and 59 degrees in the vertical direction, so that it can better scan the obstacle information in the three-dimensional space. At the same time, the sensor itself has an IMU to achieve more accurate motion estimation.

[0060] The above modules are implemented as follows Figure 2 Information is transmitted in a certain way, which jointly ensures the basic flight capability of the drone and its ability to interact with the outside world.

[0061] This embodiment aims at the complex, narrow and GPS-restricted environment of mines and tunnels. By fusing multi-sensor data including inertial measurement unit and laser mid360, a robust anti-wind disturbance attitude control algorithm is designed to achieve stable flight and high-precision attitude control of the quadcopter UAV in the mine environment, effectively improving the UAV's anti-wind disturbance capability and path tracking accuracy.

[0062] A person skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the processes of the above-mentioned embodiments of the methods. The storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0063] The technical features of the above-described embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above-described embodiments are not described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification. The above-described embodiments only express several implementation methods of the present invention, and their descriptions are relatively specific and detailed, but they cannot be understood as limiting the scope of the present invention. It should be pointed out that for those of ordinary skill in the art, without departing from the concept of the present invention, several variations and improvements can be made, which all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the attached claims.

Claims

1. A method for controlling the anti-wind disturbance attitude of a UAV in a mine tunnel environment, characterized in that: The method comprises the following steps: S1 position outer loop control, which uses a PID controller to control the position subsystem of the UAV according to the desired trajectory; S2 attitude inner loop control, which uses an adaptive sliding mode controller based on an obstacle function to control the attitude subsystem of the UAV according to the desired trajectory.

2. The anti-wind disturbance attitude control method for UAV in a mine tunnel environment according to claim 1 is characterized in that: The position loop of the position subsystem is expressed as formula (1), and the attitude loop of the attitude subsystem is expressed as formula (2): Where P = [x, y, z] represents the current position of the drone, m represents the mass of the drone, g represents gravity, Θ = (φ, θ, ψ) represents the attitude angle, and the three angles φ, θ, and ψ represent the current roll angle, pitch angle, and yaw angle of the drone’s quadrotor, respectively. xx ,J yy ,J zz Represent the moment of inertia of the x, y, and z axes respectively, They represent the second-order derivatives of the corresponding Euler angles, They represent the first derivative of the corresponding Euler angles, Represent the second-order derivative of the current position of the UAV, τ dφ ,τ dθ ,τ dψ They represent the external disturbances of the roll angle channel, the pitch angle channel and the yaw angle channel respectively. [u1,u2,u3,u4] are the control inputs of the UAV, which are used to control the four propellers of the UAV.

3. The anti-wind disturbance attitude control method for UAV in a mine tunnel environment according to claim 2 is characterized in that: In the S1 position outer loop control, the expected position and expected yaw angle of the UAV are obtained according to the expected trajectory, and the expected position and current position of the UAV are used as inputs of the PID controller. The PID controller outputs the expected pitch angle and expected roll angle of the UAV in combination with the expected yaw angle, and outputs one of the control input quantities of the UAV, which is used to control one of the propellers of the quadrotor UAV.

4. The anti-wind disturbance attitude control method for UAV in a mine tunnel environment according to claim 3 is characterized in that: In the S2 attitude inner loop control, the current roll angle, pitch angle and yaw angle of the UAV, the expected yaw angle, the expected pitch angle and the expected roll angle are used as inputs of the adaptive sliding mode controller based on the obstacle function, and the adaptive sliding mode controller outputs the other three control input quantities of the UAV for controlling the other three propellers of the quadrotor UAV.

5. The anti-wind disturbance attitude control method for UAV in a mine tunnel environment according to claim 4 is characterized in that: In the adaptive sliding mode controller based on the barrier function, the barrier function is as follows: First, for any given ε>0, there exists a continuous even function K b (x), for x∈[-ε,ε], K b ∈[b,∞],K b (x) is strictly increasing on x∈[0,ε], ε, x, b are all positive integers, and satisfy: (2) K b (x) has a unique minimum value at zero, K b (0) = b ≤ 0; Then it is called K b (x) is the barrier function, and the barrier function is taken as: Where ε>0, the function has a minimum value K(0)=0 at the zero point.

6. The anti-wind disturbance attitude control method for UAV in a mine tunnel environment according to claim 5 is characterized in that: For the roll angle channel of the attitude subsystem of the UAV, the adaptive sliding mode controller based on the obstacle function is designed as follows: The tracking error of the roll channel is defined as: e φ =φ-φ d Design the synovial surface as follows: S φ =e φ +what φ We can get: Therefore, the control input u2 is designed as: Where K(t,s φ ) is the adaptive gain, and its adaptive rate is: in, is a positive integer; following the same design method, the controller design for the pitch and yaw channels of the UAV can be performed, thereby giving the form of the gain adaptive UAV attitude sliding mode controller based on the obstacle function as follows: Among them, Ω is the angular velocity vector and J is the moment of inertia matrix.

7. A UAV anti-wind disturbance attitude control device in a mine tunnel environment, using the method described in any one of claims 1 to 6 above, characterized in that: include: The position outer loop controller is used to control the position subsystem of the UAV using a PID controller according to the desired trajectory; The attitude inner loop controller is used to control the attitude subsystem of the UAV according to the desired trajectory using an adaptive sliding mode controller based on the obstacle function.

Images