Quad-rotor unmanned aerial vehicle autonomous obstacle avoidance system for unknown environment

Abstract

The invention discloses an unknown environment-oriented four-rotor unmanned aerial vehicle autonomous obstacle avoidance system, which comprises an autonomous obstacle avoidance module and a flight control unit, the autonomous obstacle avoidance module is connected with the flight control unit, the flight control unit is also connected with a remote control unit, and the autonomous obstacle avoidance module comprises a trajectory planning unit and an environment sensing unit. The trajectory planning unit obtains output information of the environment sensing unit to generate a speed instruction to the flight control unit; the trajectory planning unit is used for constructing an inertial coordinate system and an airframe coordinate system to obtain the position and Euler angle of the unmanned aerial vehicle relative to the inertial coordinate system, performing path planning based on a potential field method, and outputting a position control instruction. Compared with the prior art, the four-rotor unmanned aerial vehicle autonomous obstacle avoidance system for the unknown environment has the advantages that the four-rotor unmanned aerial vehicle autonomous obstacle avoidance system is based on the potential field method obstacle avoidance principle, and path planning and tracking control are integrated.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous obstacle avoidance for unmanned aerial vehicles (UAVs), and in particular to an autonomous obstacle avoidance system for a quad-rotor UAV facing an unknown environment. Background Art

[0002] On June 5, 2024, DJI's FC30 transport drone completed an unprecedented high-altitude transportation test on the southern slope of Mount Everest. This not only achieved a technological breakthrough, but also proved to the world the huge potential and practical value of drone transportation in plateau transportation, emergency rescue, scientific research monitoring, environmental protection and other fields.

[0003] However, for low-altitude UAVs, no matter what the application scenario, they often need to face track constraints caused by various terrain restrictions and various environmental threats, which poses new challenges to the safe flight control of UAVs.

[0004] As the use scenarios of drones become increasingly complex, obstacle avoidance becomes particularly important, which is often the key to whether drones can successfully perform flight missions and return safely. In addition, when considering the drone flight constraints caused by environmental obstacles, the dynamics, trajectory optimization and control problems of quadrotor drones are undoubtedly more difficult.

[0005] Therefore, the inventor conducted research on obstacle avoidance flight control of quadrotor UAVs and designed an intelligent online trajectory planning and control system that integrates "perception-planning-control" to enhance the system's robust performance and improve flight safety. Summary of the invention

[0006] The technical problem to be solved by the present invention is to overcome the above technical defects and provide a quadcopter autonomous obstacle avoidance system for unknown environments based on the potential field obstacle avoidance principle and integrated path planning and tracking control.

[0007] In order to solve the above technical problems, the technical solution provided by the present invention is: an autonomous obstacle avoidance system for a quad-rotor UAV facing an unknown environment, comprising an autonomous obstacle avoidance module and a flight control unit, wherein the autonomous obstacle avoidance module is connected to the flight control unit, and the flight control unit is also connected to a remote control unit;

[0008] The autonomous obstacle avoidance module includes a trajectory planning unit and an environment perception unit. The trajectory planning unit obtains output information from the environment perception unit to generate a speed instruction to the flight control unit.

[0009] The trajectory planning unit includes constructing an inertial coordinate system and a body coordinate system to obtain the position and Euler angle of the drone relative to the inertial coordinate system, performing path planning based on a potential field method, and outputting a position control instruction.

[0010] Preferably, the environmental perception unit is a laser radar, and the flight control unit includes a motion sensor component and a power unit for measuring position and attitude information, and the power unit includes four sets of motors and an electronic speed regulator for controlling the motors.

[0011] Preferably, the remote control unit includes a receiver and a remote controller for wireless communication with the drone body, and the receiver is also communicatively connected to a ground station for obtaining the status of the drone.

[0012] Preferably, the inertial coordinate system is E={X i ,Y i ,Z i};

[0013] The body coordinate system is B = {X b ,Y b ,Z b};

[0014] Vector ξ={x,y,z} T ,γ={θ,φ,ψ} T They respectively represent the position and Euler angle of the drone relative to the inertial coordinate system. The Euler angles include pitch angle, roll angle, and yaw angle.

[0015] Preferably, the path planning based on the potential field method includes calculating the gravitational potential field and the repulsive potential field to obtain the resultant force, wherein:

[0016] Repulsive potential field:

[0017]

[0018] where F rep is the repulsive force, k i is the repulsion coefficient, r0 is the safety distance, r i is the horizontal distance from the UAV to the i-th obstacle;

[0019] Gravitational potential field:

[0020] F att =k a r a ;

[0021] where F att is the gravitational force, k a is the gravitational coefficient, r a is the horizontal distance from the UAV to the target point;

[0022]

[0023] The next desired track point of the UAV is obtained by calculating the combined force based on the gravitational and repulsive force vectors. The flight control unit controls the UAV to track the desired track point in real time through PID control.

[0024] Preferably, the speed instruction required by the drone includes:

[0025] Determine the position of the drone (x, y), the position of the target point is (x d ,y d ), the position of the i-th obstacle is (r i ,β i )(i=1,2...n), where r i is the distance from the obstacle to the UAV, β i is the rotation angle of the line connecting the obstacle and the drone around the positive direction of the y-axis of the inertial coordinate system, then:

[0026]

[0027] Among them, k1 and k2 are controller parameters, r0 is the safety distance, and a and b are directional coefficients, whose values ​​are:

[0028]

[0029] have:

[0030]

[0031] Get the desired speed command x of the drone. d ,y. d :

[0032]

[0033] Speed ​​command required by the drone:

[0034]

[0035] |0≤|y. d |≤v0

[0036] v0 is the total expected flight speed of the UAV in the horizontal direction.

[0037] Preferably, the PID control includes taking as input to the PID controller the deviation between the input expectation r(t) and the output measurement c(t): e(t)=r(t)-c(t).

[0038] Preferably, the motion sensing component includes a gyroscope, a magnetometer, an accelerometer, an optical flow module, and a barometer.

[0039] Preferably, it includes setting a safe distance from obstacles, and the safe distance is set based on the structure radius of the drone, the response time of the obstacle avoidance control system, and the change of the airflow field caused by the obstacles:

[0040] When the distance between the drone and the obstacle is less than the safe distance, it will avoid the obstacle;

[0041] When the distance between the drone and the obstacle is greater than the safety distance, the original flight command is executed.

[0042] The advantages of the present invention compared with the prior art are:

[0043] (1) An online trajectory planning algorithm for UAVs was designed based on the potential field method. The speed constraint of the UAV was also taken into consideration during the planning process, and the flight speed of the UAV was limited to a safe range to ensure the safe flight of the UAV.

[0044] (2) An integrated “path planning-tracking control” design scheme is proposed, which directly performs trajectory planning to generate speed instructions, simplifies the control system design, and makes the obstacle avoidance algorithm easy to deploy and implement in actual engineering.

[0045] (3) Obtain key data required for obstacle avoidance algorithms based on airborne laser radar. Design an intelligent online trajectory planning and control system that integrates "perception-planning-control" to enable drones to have a higher level of autonomous decision-making capabilities when facing unknown environments, thereby improving flight safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1-1 It is an application scenario for multi-rotor drones.

[0047] Figure 2-1 It is the coordinate system of the quadrotor drone.

[0048] Figure 2-2 Schematic diagram of the obstacle avoidance problem for drones.

[0049] Figure 2-3 Schematic diagram of potential field force distribution.

[0050] Figure 2-4 Path planning and position control.

[0051] Figure 2-5 Block diagram of the obstacle avoidance controller with integrated path planning and control.

[0052] Figure 2-6 Block diagram of attitude control of UAV hanging obstacle avoidance system.

[0053] Figure 2-7 The overall architecture of the drone hanging obstacle avoidance system.

[0054] Figure 3-1 Overall block diagram of system hardware and software design.

[0055] Figure 4-1 Quadrotor drone obstacle avoidance flight.

[0056] Figure 4-2Drone position tracking curve.

[0057] Figure 4-3 Speed ​​tracking curve.

[0058] Figure 4-4 Obstacle avoidance test of a drone during fast flight.

[0059] Figure 4-5 Fixed-point drone avoidance test of incoming objects.

[0060] Figure 4-6 Test on avoiding wall and cylindrical obstacles during suspended flight.

[0061] Figure 4-7 Suspended flight obstacle avoidance test in a forest scene. DETAILED DESCRIPTION

[0062] The present invention is further described in detail below in conjunction with the accompanying drawings.

[0063] UAV System Modeling:

[0064] The quadcopter can be represented as a rigid body with a mass of m, which is subject to gravity and aerodynamic forces. An inertial coordinate system E = {X i ,Y i ,Z i} and a body coordinate system B = {X b ,Y b ,Z b}, vector ξ={x,y,z} T ,γ={θ,φ,ψ} T They respectively represent the position of the aircraft relative to the inertial system E and the Euler angles (pitch angle, roll angle, yaw angle).

[0065] Using an X-shaped quadrotor drone, the four virtual control quantities of the system are shown in formula (2-1), and the conversion relationship between the virtual control quantity and the motor speed is shown in formula (2-2):

[0066]

[0067] Where U i (i=1,2,3,4) is the virtual control quantity, F i (i=1,2,3,4) is the lift generated by each propeller, M i (i=1,2,3,4) is the torque generated by each propeller, l is the distance between the center of mass of the aircraft and the axis of rotation of the quadrotor, b is the lift coefficient of the quadrotor, d is the drag coefficient, g is the acceleration due to gravity, and w is the acceleration due to gravity. i (i=1,2,3,4) is the motor angular velocity, I x ,I y,I z are the moments of inertia of the x, y, and z axes respectively, then the equation of motion is:

[0068]

[0069] Drone obstacle avoidance analysis:

[0070] like Figure 2-2 As shown in the figure, assuming that the horizontal position of the drone is (x, y) and the horizontal position of the target point is (x d ,y d ), the horizontal position of the ith obstacle is (x i ,y i )(i=1,2...n), the distance from the drone to the target point is r a , the distance from the drone to the obstacle is r i ;

[0071] Control target: The drone reaches the target position, that is, x d -x=0、y d -y=0、r a = 0, and at the same time avoid obstacles so that the drone does not collide with obstacles at any time, that is, r i > 0. This means that the drone needs to be controlled to successfully bypass all obstacles and reach the target point;

[0072] There are two main threats from obstacles: (1) they block the normal flight path of the drone, causing a collision; and (2) during the flight of a rotorcraft drone, the rapid rotation of its blades will generate strong airflow in the drone's surrounding environment, and obstacles will change the airflow field, which will affect the drone's flight stability.

[0073] Therefore, in the present invention:

[0074] In the obstacle avoidance problem of drones, the safe distance r0 from obstacles is set. The determination of r0 needs to comprehensively consider the structure radius r of the drone. uav , the response time of the obstacle avoidance control system, the change of the airflow field caused by obstacles, and the smoothness of the track. i When <r0, it means that the UAV is within the influence range of the obstacle and performs obstacle avoidance operations. After the obstacle avoidance is completed, the original flight mission will continue. Otherwise, it means that the obstacle does not pose a threat temporarily and the original flight mission will continue.

[0075] Path planning and position control method based on potential field method:

[0076] The artificial potential field consists of two parts, namely the gravitational potential field and the repulsive potential field. The drone is subject to the gravitational force F of the target point. att The repulsive force F of the action and obstacle repThe resultant force F total Under the action, it moves toward the target point, and the potential field force distribution is as follows Figure 2-3 shown.

[0077] Assume the horizontal position of the drone is (x, y), and the horizontal position of the target point is (x d ,y d ), the horizontal position of the ith obstacle is (x i ,y i )(i=1,2...n), then the repulsive force of the ith obstacle is directed from the obstacle to the drone, and its magnitude can be expressed as:

[0078]

[0079] Among them, k i is the repulsion coefficient, r0 is the safety distance, r i is the horizontal distance from the UAV to the i-th obstacle:

[0080]

[0081] The gravitational direction of the target point is from the drone to the target point, and its magnitude is:

[0082] F att =k a r a (2-6)

[0083] Among them, k a is the gravitational coefficient, r a is the horizontal distance from the drone to the target point:

[0084]

[0085] Through the above-obtained gravitational and repulsive force vectors, the resultant force can be calculated to obtain the next desired track point of the UAV. Then, the position controller can realize the real-time tracking of the desired track point by the UAV, thereby realizing the obstacle avoidance path planning of the UAV, such as Figure 2-4 shown.

[0086] Integrated design of path planning and position control based on potential field method:

[0087] The position of the drone is (x, y), and the position of the target point is (x d ,y d ), the position of the i-th obstacle is (r i ,β i )(i=1,2...n), where r i is the distance from the obstacle to the UAV, β i is the rotation angle of the line connecting the obstacle and the drone around the positive direction of the y-axis of the inertial coordinate system, then:

[0088]

[0089] Among them, k1 and k2 are controller parameters, r0 is the safety distance, and a and b are directional coefficients, whose values ​​are:

[0090]

[0091] So we have:

[0092]

[0093] Finally, the desired speed command x of the drone is obtained. d ,y. d :

[0094]

[0095] Among them, v0 is the total expected flight speed of the UAV in the horizontal direction, which can be set to a fixed value to allow the UAV to fly at a constant speed, or it can be dynamically adjusted according to the actual flight mission requirements.

[0096] The speed constraint relationship of the designed horizontal position controller is shown in formula (2-15). The horizontal flight speed V of the UAV is always constrained to be below the set value v0, that is, V≤v0. The expected speed command constraint range of the UAV along the x and y axes is [0,v0].

[0097] The position controller of the quadrotor suspension system is as follows Figure 2-5 As shown, compared to Figure 2-4 In the traditional obstacle avoidance path planning and position tracking design, this paper adopts the design idea of ​​integrated planning and control, and directly calculates the speed command required by the drone through trajectory planning:

[0098]

[0099] v0 is the total expected flight speed of the UAV in the horizontal direction.

[0100] The horizontal position controller of the UAV suspension system with obstacle avoidance function is obtained through the above, and the controller design of the inner loops such as altitude loop, speed loop, attitude loop is continued. The PID control algorithm is used to achieve the stability of the inner loop of the UAV to ensure the stable flight of the UAV suspension system and effective obstacle avoidance. The UAV attitude control block diagram is shown in the figure. Figure 2-6 shown.

[0101] The PID controller is based on the deviation between the expected input r(t) and the measured output value c(t): e(t) = r(t)-c(t). The deviation is given as input to the PID controller, and then the output of the controller is used as the control input of the controlled object. The purpose of control is to make the output value equal to the given input value. The mathematical expression is:

[0102]

[0103] The transfer function is:

[0104]

[0105] Where: K p is the proportionality coefficient, T i is the integration time constant, T d is the differential time constant; K i =K p / T i , is the integral coefficient; K d =K p ×T d is the differential coefficient.

[0106] After discretizing equation (2-17), we get:

[0107]

[0108] Depend on Figure 2-6 It can be seen that in the conventional potential field obstacle avoidance method, a feasible path is first planned based on the relative motion relationship between the drone and the target point and the obstacle, and then the drone's position controller is used to track the path to achieve the purpose of obstacle avoidance. This method consists of two parts: path planning loop and position control loop. The integrated method directly performs trajectory planning, generates speed instructions, and realizes the functions of path planning and position control at the same time.

[0109] The integrated method in the present invention not only reduces the number of system loops, simplifies the structure of the control system, and avoids the inherent time lag problem in the multi-loop system, but also requires only two control parameters to be adjusted compared to the five parameters of the traditional method, thus reducing the difficulty of parameter adjustment in actual engineering applications.

[0110] The hardware system block diagram is as follows Figure 3-1 As shown, the flight control unit uses an open source anonymous flight control, and the main control chip is STM32F407ZGT6. The flight control integrates sensors such as accelerometer, angular velocity meter, magnetometer, barometer, and an external optical flow module to measure basic position, attitude and other information. The flight control has a wealth of expansion interfaces (SPI, IIC, UART, etc.) to install external devices to expand the functions of the drone and verify the designed control algorithm;

[0111] The obstacle detection unit is composed of two-dimensional laser radar and 32 single-chip microcomputer, which can complete the functions of real-time environment scanning, detection data analysis and two-dimensional map construction, obtain obstacle information around the drone, and send the distance and angle data of the obstacle to the flight control through the serial port. UWB / GPS is used to realize the positioning of the drone.

[0112] The remote control unit, which is composed of a receiver and a remote controller, can issue simple control commands to the drone. It can also take over the control of the drone in time when the drone is out of control or in other emergency situations, so as to avoid serious safety accidents or property losses as much as possible. The auxiliary debugging unit is composed of a data transmission module and a ground station. The ground station can obtain some basic data information of the drone, analyze flight test data, and also issue simple flight commands to the drone through the ground station.

[0113] The UAV power system consists of batteries, electronic speed regulators, brushless motors, and propellers. The flight control calculates the PWM wave signal through the control algorithm to adjust the speed of the brushless motor, change the force and torque applied to the UAV, and thus achieve attitude and position control to complete the preset flight mission.

[0114] test:

[0115] Before performing algorithm flight verification, an algorithm simulation experiment is performed on Matlab / Simulink. The simulation experiment parameters are shown in Table 4-1.

[0116] Table 4-1 Simulation experiment parameters

[0117]

[0118] The simulation results are as follows Figure 4-1 , 4-2 , as shown in 4-3. Figure 4-1 is the horizontal projection of the UAV flight path, Figure 4-2 These are the position tracking curves of the UAV in the x-axis and y-axis directions. From the simulation results, we can see that the UAV can accurately reach the target position while avoiding three obstacles.

[0119] Figure 4-3 This is the speed tracking curve of the UAV. The simulation results show that the flight speed of the UAV is always limited within the constraint range, and the inner loop PID controller can also track the expected speed well.

[0120] Embodiment 1:

[0121] Let the drone fly forward at a speed of 2m / s. When the distance to the cylindrical obstacle in front is close, the drone will automatically slow down and brake, then turn right to avoid the obstacle, and continue to fly forward after avoiding the obstacle. Figure 4-4 shown.

[0122] Embodiment 2:

[0123] Control the drone to fly at the current location, then hold obstacles and approach the drone from different directions. The drone can successfully avoid the obstacles autonomously. After removing the obstacles, the drone will fly back to the initial location and continue to fly. Figure 4-5 shown.

[0124] Embodiment three:

[0125] The UAV was tested for flight obstacle avoidance. The load was 100g and the rope was 60cm long. Both the UAV and the load were able to successfully avoid obstacles such as walls and cylinders. Figure 4-6 shown.

[0126] Embodiment 4:

[0127] The obstacle avoidance effect of the suspended UAV flight was tested in a forest scene. The test results show that both the UAV and the suspended object can effectively avoid obstacles in the forest scene, such as Figure 4-7 shown.

[0128] The present invention and its embodiments are described above, and such description is not restrictive. The drawings show only one embodiment of the present invention, and the actual structure is not limited thereto. In short, if ordinary technicians in the field are inspired by it, without departing from the purpose of the invention, they can design a structure and embodiment similar to the technical solution without creativity, which should belong to the protection scope of the present invention.

Claims

1. The autonomous obstacle avoidance system for quad-rotor drones in unknown environments is characterized by: It includes an autonomous obstacle avoidance module and a flight control unit, wherein the autonomous obstacle avoidance module is connected to the flight control unit, and the flight control unit is also connected to a remote control unit; The autonomous obstacle avoidance module includes a trajectory planning unit and an environment perception unit. The trajectory planning unit obtains output information from the environment perception unit to generate a speed instruction to the flight control unit. The trajectory planning unit includes constructing an inertial coordinate system and a body coordinate system to obtain the position and Euler angle of the drone relative to the inertial coordinate system, performing path planning based on a potential field method, and outputting a position control instruction.

2. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 1, characterized in that: The environmental perception unit is a laser radar, and the flight control unit includes a motion sensor component and a power unit for measuring position and attitude information. The power unit includes four sets of motors and an electronic speed regulator for controlling the motors.

3. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 1, characterized in that: The remote control unit includes a receiver and a remote controller for wireless communication with the drone body, and the receiver is also communicatively connected to a ground station for obtaining the status of the drone.

4. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 1, characterized in that: The inertial coordinate system is E={X i ,Y i ,Z i }; The body coordinate system is B = {X b ,Y b ,Z b }; Vector ξ={x,y,z} T ,γ={θ,φ,ψ} T They respectively represent the position and Euler angle of the drone relative to the inertial coordinate system. The Euler angles include pitch angle, roll angle, and yaw angle.

5. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 4 is characterized in that: The path planning based on the potential field method includes calculating the gravitational potential field and the repulsive potential field to obtain the resultant force, wherein: Repulsive potential field: Among them, F rep is the repulsive force, k i is the repulsion coefficient, r0 is the safety distance, r i is the horizontal distance from the UAV to the i-th obstacle; Gravitational potential field: F att =k a r a ; Among them, F att is the gravitational force, k a is the gravitational coefficient, r a is the horizontal distance from the UAV to the target point; The next desired track point of the UAV is obtained by calculating the combined force based on the gravitational and repulsive force vectors. The flight control unit controls the UAV to track the desired track point in real time through PID control.

6. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 5, characterized in that: The speed instructions required by the drone include: Determine the position of the drone (x, y), the position of the target point is (x d ,y d ), the position of the i-th obstacle is (r i ,β i )(i=1,2...n), where r i is the distance from the obstacle to the UAV, β i is the rotation angle of the line connecting the obstacle and the drone around the positive direction of the y-axis of the inertial coordinate system, then: Among them, k1 and k2 are controller parameters, r0 is the safety distance, and a and b are directional coefficients, whose values ​​are: have: Get the desired speed command x of the drone. d , y. d : Speed ​​command required by the drone: |0≤|y. d |≤v0 v0 is the total expected flight speed of the UAV in the horizontal direction.

7. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 6, characterized in that: The PID control includes taking as input to the PID controller the deviation between the input expectation r(t) and the output measurement c(t): e(t)=r(t)-c(t).

8. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to claim 2, characterized in that: The motion sensing component includes a gyroscope, a magnetometer, an accelerometer, an optical flow module, and a barometer.

9. The autonomous obstacle avoidance system for a quad-rotor drone facing an unknown environment according to any one of claims 1 to 8, characterized in that: Including setting the safe distance from obstacles. The safe distance is set based on the structure radius of the drone, the response time of the obstacle avoidance control system, and the changes in the airflow field caused by obstacles: When the distance between the drone and the obstacle is less than the safe distance, it will avoid the obstacle; When the distance between the drone and the obstacle is greater than the safety distance, the original flight command is executed.

Images