Anti-wind disturbance attitude control method and device for unmanned aerial vehicle in mine environment

Abstract

The present invention provides a method and device for controlling the attitude of a UAV in a mine tunnel environment, and relates to the technical field of mine tunnel UAV inspection. In combination with the demand for stable inspection of a quadcopter UAV in a mine tunnel environment, the present invention proposes a method for controlling the attitude of a quadcopter UAV in a mine tunnel environment, which is suitable for a mine tunnel environment. First, through the position outer loop control, the position subsystem of the UAV is controlled by a PID controller according to the desired trajectory. Then, through the attitude inner loop control, the attitude subsystem of the UAV is controlled by an adaptive sliding mode controller based on an obstacle function according to the desired trajectory. The present invention can effectively suppress external disturbances, improve the flight safety and path tracking accuracy of the UAV, and enable the UAV to have the ability to conduct stable inspections when facing sudden wind disturbances in mine tunnels or complex fluid environments, providing certain technical support for the application of quadcopter UAVs in narrow and complex environments.

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 a UAV in a mine tunnel environment. Background Art

[0002] Mine tunnels are a unique operating environment characterized by high risk, complexity, and confinement, often characterized by cramped spaces, low illumination, slippery surfaces, and uneven airflow. Traditional inspection and operation methods in these environments rely on manual operation or ground-based equipment, which is not only inefficient but also poses significant safety risks. Therefore, the use of quadrotor drones for autonomous inspection, environmental monitoring, and safety assessment in these harsh environments has become a research hotspot in recent years.

[0003] However, quadcopter drones face numerous technical challenges when flying in mine tunnels. First, since stable GPS signals are often unavailable in these environments, drones cannot rely on satellite positioning for autonomous flight control. Second, the confined space and volatile environment of mine tunnels make drones susceptible to airflow disturbances, causing instability and, in severe cases, collisions. Therefore, improving drones' wind resistance and attitude stability in mine tunnels has become a key research topic in autonomous drone flight control.

[0004] Currently, UAV flight control methods for mine tunnels primarily focus on data fusion based on vision, lidar, or inertial measurement units to accurately estimate attitude and position. While these methods have achieved some success, they still struggle to ensure flight stability and safety in the face of sudden wind disturbances or complex fluid environments. Therefore, a wind-resistant attitude control method for mine tunnels is needed that can effectively suppress external disturbances and improve UAV flight safety and path tracking accuracy.

[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 a UAV in a mine tunnel environment, which can effectively suppress external disturbances, improve the flight safety and path tracking accuracy of the UAV, and enable the UAV to have the ability to conduct 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 the 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, which uses a PID controller to control the position subsystem of the unmanned aerial vehicle according to a 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 unmanned aerial vehicle according to the desired trajectory.

[0009] In the first aspect, the present invention provides a preferred solution. 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 inputs 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, as well as one of the control input quantities of the drone, which is used to control one of the propellers of the quadrotor drone.

[0010] In the 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 drone, 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 drone, which are used to control the other three propellers of the quadrotor drone.

[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 existing technology, the above technical solution has the following advantages:

[0013] The present invention combines the needs of stable inspection of quadcopter UAVs in mine tunnel environments to propose a wind-resistant attitude control method for quadcopter UAVs suitable for mine tunnel environments, and combines the characteristics of quadcopter UAVs to design a set of robust wind-resistant attitude control strategies in GPS-denied environments, which effectively improves the wind-resistant capability and flight stability of quadcopter UAVs in mine tunnel environments, and provides certain technical support for the application of quadcopter UAVs in narrow and complex environments. In short, the wind-resistant attitude control method and device for UAVs in mine tunnel environments of the present invention can effectively suppress external disturbances, improve the flight safety and path tracking accuracy of UAVs, and enable UAVs to have the ability to conduct stable inspections 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 following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any 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 embodiment of the present invention. DETAILED DESCRIPTION

[0018] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0019] In response to the shortcomings of existing technologies or the need for improvement, this embodiment provides a method for controlling the attitude of a quadrotor drone in a mine tunnel environment to resist wind disturbances, enabling the drone to maintain stable inspection capabilities in the face of sudden wind disturbances or complex fluid environments in the mine tunnel. The method primarily includes the following: an adaptive sliding membrane attitude controller for the drone designed based on an obstacle function, which not only converges the output to a predefined zero neighborhood but also prevents the control gain from being overestimated. This method eliminates the need for any information about the upper limit of the disturbance and any low-pass filter, reducing unnecessary parameter adjustments and significantly improving the quadrotor drone's ability 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 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 UAV according to the desired 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 desired trajectory using a PID controller; an attitude inner loop controller, used to control the attitude subsystem of the UAV according to the desired 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 quadcopter 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 known as a controller for a quadcopter UAV in a mine tunnel. It is a dual closed-loop control controller that divides the controller design into two parts: the position outer loop and the attitude inner loop. Figure 2 As shown in Figure 2, the outer loop is the position subsystem of the UAV, and the inner loop is the attitude subsystem of the UAV. Equation (1) represents the position loop of the system, and Equation (2) represents the attitude loop of the system.

[0022]

[0023] Among them, P = [x, y, z] represents the current position of the drone, m represents the mass of the drone, g is gravity, Θ = (φ, θ, ψ) is 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 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 angle, 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.

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

[0025] In the S1 position outer loop control, the desired position and desired yaw angle of the drone are obtained based on the desired trajectory. The desired position and current position of the drone are used as inputs to the PID controller. The PID controller combines the desired yaw angle to output the desired pitch angle and desired roll angle of the drone, as well as one of the drone's control inputs, which is used to control one of the quadcopter's propellers. 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 for UAV tracking position and an inner loop barrier function based adaptive sliding mode controller (BFASMC) to enable the UAV to have good anti-wind interference 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 the 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 inverse solution is used to obtain the expected attitude angle of the inner loop:

[0034]

[0035] In the S2 attitude inner loop control, the drone's current roll, pitch, and yaw angles, as well as the desired yaw, pitch, and roll angles, are used as inputs to the adaptive sliding mode controller based on the obstacle function. The adaptive sliding mode controller then outputs three other control inputs for controlling the quadcopter's other three propellers. Specifically, the following are the outputs:

[0036] The inner loop of the UAV adopts an adaptive sliding mode controller based on the barrier function. The barrier function is obtained as follows: First, for any given ε>0, there exists a continuous even function K b (x), for x∈[-ε,ε], there are K b ∈[b,∞],K b (x) is strictly increasing on x∈[0,ε], ε, x, and 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, which is:

[0040]

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

[0042] Next, we design a sliding mode controller based on an obstacle function for the attitude subsystem of the quadrotor drone. Taking the roll angle channel as an example, and the same principles for the pitch and yaw channels, we define the tracking error of the roll channel as:

[0043] e φ =φ-φ d

[0044] Design the synovial surface as follows:

[0045] s φ =e φ +ce φ

[0046] We can get:

[0047]

[0048] Thus, the designed control input u2 is obtained.

[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] Where Ω is the angular velocity vector and J is the moment of inertia matrix.

[0054] It should be noted that u3 and u4 can be merged into Solve in and get u3,u4 through this formula.

[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 slave computer, a sensor module, a power module, an actuator, and an external safety assurance module. The sensor module provides the slave computer with the posture information of the UAV in the mine tunnel, including three-dimensional position and quaternion information describing the attitude angle. The slave 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 slave computer for execution, thereby 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 airborne systems such as the host computer, the slave computer, the sensor module and the actuator. The host computer, the slave computer and the sensor module in the present invention are introduced in detail below.

[0056] (1) Sensor module: Based on 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. Among them, 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 transmitting devices. By the pilot toggling the control stick in the remote control, the UAV can be forced to jump from the program control mode to the manual control mode, thereby ensuring the flight safety of the UAV system.

[0057] (2) Lower computer: Based on the application and performance requirements, the present invention uses the PIXHWAK mini6 c as the lower computer, also known as the flight control board. This module runs a sensor data fusion algorithm, which obtains accurate position, speed, acceleration, attitude and other status information of the drone system by fusing data from built-in sensors such as gyroscopes and accelerometers and external sensors such as the mid360 three-dimensional laser radar. The control instructions received from the upper computer are combined with the status information to generate a PWM signal to control 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 on the board. It will receive information sent from the lower computer, external sensors and other modules. Among them, the external sensor mid360 lidar data will run the FAST-LIO positioning algorithm on 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 into 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 most flexibly and safely.

[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 based on the mine environment. It has a certain degree of penetration into the dust inside the mine, thus ensuring that the laser data is the actual distance to the obstacle or wall. The laser radar can scan 360 degrees horizontally and 59 degrees vertically, thereby better scanning 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 targets complex, narrow, and GPS-restricted environments such as mines and tunnels. By fusing multi-sensor data, including an 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 in mine tunnel environments, effectively improving the drone's anti-wind disturbance capability and path tracking accuracy.

[0062] Those skilled in the art will appreciate that all or part of the processes in the above-described method embodiments can be implemented by instructing related hardware through a computer program. The program can be stored in a computer-readable storage medium, and when executed, the program can include the processes in the above-described method embodiments. The storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0063] The technical features of the above-described embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above-described embodiments are 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, several variations and improvements can be made without departing from the concept of the present invention, and these all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be based on the appended claims.

Claims

1. A method for controlling the attitude of a UAV against wind disturbance in a mine tunnel environment, characterized in that: The method includes 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; In the adaptive sliding mode controller based on the barrier function, the barrier function is as follows: First, for any given , there exists a continuous even function ,for ,have , exist Strictly increasing, 、 are all positive integers and satisfy: ； There is a unique minimum at zero, ; Then it is called Is the barrier function, take the barrier function as: ; in , the function has a minimum value at zero ; 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: ； Design the synovial surface as follows: ； We can get: ； Thus, the control input is designed for: ； in is the adaptive gain, and its adaptive rate is: ； in, is a positive integer; the same design method is used to design the controllers for the pitch and yaw channels of the UAV, so the gain adaptive UAV attitude sliding mode controller based on the obstacle function is given as follows: ； in, is the angular velocity vector, is the moment of inertia matrix.

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): （1） （2） in, Indicates the current location of the drone. Indicates the quality of the drone, is gravity, is the attitude angle, The three angles represent the current roll angle, pitch angle and yaw angle of the drone's quadrotor. Represent the moment of inertia of 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 angle, Represent the second-order derivative of the current position of the UAV, They represent the external disturbances of the roll angle channel, the pitch angle channel, and the yaw angle channel, respectively. It is the control input of the UAV, 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 desired position and desired yaw angle of the drone are obtained according to the desired trajectory. The desired position and current position of the drone are used as inputs of the PID controller. The PID controller outputs the desired pitch angle and desired roll angle of the drone in combination with the desired yaw angle, as well as one of the control input quantities of the drone, which is used to control one of the propellers of the quadrotor drone.

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 desired yaw angle, the desired pitch angle and the desired 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 inputs of the UAV for controlling the other three propellers of the quadrotor UAV.

5. A UAV anti-wind disturbance attitude control device in a mine tunnel environment, using the method described in any one of claims 1 to 4 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