A Method for Rapid Collision Detection and Recovery of a Quadrotor UAV

Abstract

The present invention relates to a method for rapid collision detection and recovery of a quadrotor unmanned aerial vehicle. First, a non-linear disturbance observer is used to detect the external forces and torques generated by the collision. Secondly, the detected external forces and torques are combined with an admittance controller, and the position and heading angle of the unmanned aerial vehicle are adjusted in real time according to the collision external forces and torques. Finally, an inclination-rotation separation attitude controller is proposed to divide the attitude error of the unmanned aerial vehicle into a horizontal attitude error and a heading error, and a higher gain is allocated to the horizontal attitude error, thereby enhancing the horizontal attitude recovery ability of the unmanned aerial vehicle. The above proposed method can achieve rapid collision detection of the quadrotor unmanned aerial vehicle and the state recovery of the unmanned aerial vehicle after the collision, and can be used for the safety control of the quadrotor unmanned aerial vehicle in scenarios where the perception and obstacle avoidance of the unmanned aerial vehicle fail, such as visual degradation and narrow spaces.

Description

Technical Field

[0001] The present invention relates to a method for rapid collision detection and state recovery of a quadrotor unmanned aerial vehicle (UAV) based on an interference observer, which can achieve rapid collision detection of the quadrotor UAV and state recovery of the UAV after collision, and can be used for the safety control of the quadrotor UAV in scenarios where UAV perception and obstacle avoidance fail, such as visual degradation and narrow spaces. Background Art

[0002] Quadrotor UAVs have the advantages of vertical takeoff and landing, and flexibility. These characteristics have enabled UAVs to be widely used in military and civilian fields. An important problem to be addressed in the application of quadrotor UAVs is obstacle avoidance. Although current research on visual perception and planning has made some progress in UAV obstacle avoidance, as the tasks and environments faced by UAVs become more and more complex, such as unfamiliar narrow spaces, unstructured environments, or unpredictable interference during flight, UAVs are still easily affected by collisions. Collisions of UAVs cause irreversible damage to the UAV structure. Therefore, the detection and handling of collisions are crucial for the safety control of UAVs.

[0003] Currently, there are mainly two ideas for dealing with quadrotor UAV collisions. One is to conduct research on the selection of the fuselage material. Some researchers choose to use flexible materials to manufacture the UAV fuselage, relying on the fuselage to absorb part of the collision energy during the collision. However, due to the flexibility of the fuselage, it is not convenient for the UAV to carry heavy loads. The second idea is to use software algorithms for collision detection and handling. Among them, the collision detection part uses physical sensors, such as Hall sensors. This method of sensors depends on the installation position and cannot perform 360° large-range collision detection. Or, the external forces and torques generated by the collision are regarded as the state of the UAV, and filtering or optimization algorithms are used for collision detection. However, the estimation algorithms based on filtering or optimization require a certain amount of time to converge and cannot meet the real-time detection requirements. In the collision handling stage, most research has adopted strategies such as hovering immediately upon collision and flying along a certain trajectory in the direction of the collision normal upon collision. The above handling methods do not consider the unloading of collision energy and the flexible characteristics of the vertical degree of freedom of the UAV fuselage, i.e., the heading.

[0004] Flying animals in nature have the ability to detect and handle collisions. It has been observed that fruit flies will bounce back after colliding with a wall at high speed, and bees will adjust their postures and pass through narrow channels sideways when passing through narrow channels smaller than their body sizes. Inspired by flying animals, rotor UAVs can also have these behaviors after experiencing collisions, so as to perform mission tasks more intelligently. Summary of the Invention

[0005] The technical problem to be solved by the present invention is: aiming at the safety problem of the drone after collision, to solve the problems of rapid collision detection and state recovery of the drone, and to provide a method for rapid collision detection and recovery of a quadrotor drone, so as to realize the rapid collision detection of the quadrotor drone and the state recovery of the drone after collision, and to be used for the safety control of the quadrotor drone in scenarios where the perception and obstacle avoidance of the drone fail, such as visual degradation and narrow spaces.

[0006] The technical solution of the present invention is as follows: It is divided into the following three aspects: a collision detection method based on a nonlinear disturbance observer, an intelligent collision handling method based on an admittance controller, and a rapid attitude recovery method for the drone based on tilt-rotation separation.

[0007] In the first step, the collision suffered by the quadrotor drone is regarded as an external disturbance, and a nonlinear disturbance observer is used to estimate the collision external force and external torque, and collision detection is carried out to obtain the collision detection result.

[0008] In the second step, according to the collision external force and external torque estimated in the first step and the collision detection result, the admittance controller is used to adjust the position and heading of the quadrotor drone after collision in real time and dynamically.

[0009] In the third step, after the quadrotor drone collides, there are often large inclination errors. The torque required to adjust these inclination errors will exceed the motor saturation limit. During the attitude recovery process of the quadrotor drone, the adjustment of the heading angle has little impact on the attitude recovery of the drone, but the motor capacity required to adjust the heading angle error is very large. Therefore, under the available torque provided by the motor, giving priority to adjusting the horizontal attitude is beneficial to the state recovery of the quadrotor drone. A tilt-rotation separation attitude control method is proposed for the rapid recovery of the horizontal attitude of the quadrotor drone. The attitude error of the quadrotor drone is split into a horizontal attitude error and a heading error, and a larger gain is set for the horizontal attitude error in the design of the attitude controller, so as to realize the rapid stabilization of the horizontal attitude and avoid the instability of the quadrotor drone.

[0010] The specific implementation steps are as follows:

[0011] In the first step, the impact of the collision on the drone is regarded as an external disturbance received by the drone, and the following nonlinear disturbance observer is used to estimate the collision external force:

[0012]

[0013] In the formula is the estimated collision external force, c f is the disturbance observer gain, R is the rotation matrix from the body coordinate system of the drone to the inertial system, e w = [0 0 1] T represents the direction of the gravity vector in the inertial system, fdes is the control quantity output by the position controller, and z f is an intermediate variable.

[0014] The following nonlinear disturbance observer is used to estimate the external torque generated by the collision:

[0015]

[0016] In the formula is the estimated external collision torque, c τ is the disturbance observer gain, J is the moment of inertia of the UAV, w is the angular velocity of the UAV, τ des is the control quantity output by the attitude controller, and z τ is an intermediate variable.

[0017] During the flight of a rotor UAV, external forces and torques are usually generated on the UAV due to factors such as sensor noise and external wind disturbances. These are mixed with the external forces and torques generated by the collision. Experiments have shown that the amplitudes of the external forces and torques generated by the collision are often larger than those generated by other factors. To exclude the influence of other factors on the external forces and torques of the UAV, the following strategy is adopted in this scheme for collision detection:

[0018]

[0019] η is the collision detection flag. When η is 1, it means a collision is detected. When η is 0, it means no collision. κ f and κ τ are the collision detection thresholds for the position channel and the heading channel respectively. Among them, κ f is 0.8 N, and κ τ is 0.1 Nm.

[0020] Second step, after detecting a collision, the UAV estimates the external collision force and the external torque using the nonlinear disturbance observer and performs flexible adjustment of the position or heading. A admittance controller is constructed in the present invention as follows:

[0021]

[0022] In the formula, X r = [p ref ψ ref T is the reference trajectory generated according to the external collision force, X d = [p des ψ des T ​​It represents the path and heading angle that the drone refers to before the collision. M represents the virtual inertia coefficient matrix, D represents the elastic coefficient matrix, and K represents the rigid coefficient matrix. It is the external force and external torque estimated by the nonlinear disturbance observer in the second step. As can be seen from the above formula, when a collision occurs and the collision external force and external torque are generated, the expected position and heading angle of the drone can be adjusted in real time according to the external torque. Thus, the continuous damage caused by the collision impact energy to the drone can be avoided.

[0023] In the third step, after the collision occurs, the rotor drone will generate large inclination angles in the three attitude angles of pitch, roll, and heading angle due to the collision impact. The pitch and roll angles in the drone attitude play a relatively important role in attitude stability, while the heading angle has little impact on the stability of the drone. The torque required for the drone to adjust its attitude under large inclination angles often exceeds the limit value of the motor output. In the case of motor saturation, preferentially using the limited resources (controller torque) for the horizontal attitude adjustment of the drone will improve the attitude recovery ability of the drone. The present invention proposes an attitude control method of tilt-rotation separation for the rapid recovery of the drone's horizontal attitude. Specifically: the attitude error of the drone is divided into horizontal attitude error and heading error, and a larger gain is set for the horizontal attitude error (pitch-roll angle error) when designing the attitude controller, so as to achieve the rapid stabilization of the horizontal attitude and avoid the instability of the drone. The adjustment of the pitch and roll angles is in the z-axis direction of the drone in the inertial system and the expected z-axis direction generated by the drone in the position loop. When the expected acceleration a generated by the position loop is known, z can be calculated as: w axis direction and the expected z w,des axis direction generated by the drone in the position loop. When the expected acceleration a generated by the position loop is known, z des can be calculated as: w,des can be calculated as:

[0024]

[0025] z w can be calculated as:

[0026] z w = Re w

[0027] The horizontal attitude error between z w and z w,des can be represented by the rotation axis and the rotation angle α, where:

[0028] α = cos -1 (z w T z w,des )

[0029]

[0030] Therefore, the horizontal attitude error represented by quaternion is:

[0031]

[0032] To align z as quickly as possible w and z w,des , the last element of q e,xy can be set to 0. Therefore, the horizontal attitude error can be expressed as:

[0033]

[0034] After aligning z w and z w,des , there is still a heading angle error that needs to be corrected. The heading angle error can be expressed as:

[0035]

[0036] where q e is the expected total attitude error;

[0037] After separating the horizontal attitude error and the heading angle error, the expected torque of the motor can be expressed as:

[0038]

[0039] where and are the imaginary parts of the horizontal attitude quaternion error q e,xy and the heading error q e,z respectively, k q,xy , k q,z and k w are the controller gains, w e and are the angular velocity error and the angular velocity differential error respectively, and they can be calculated by the following formulas:

[0040]

[0041]

[0042] In the formulas, R des is the expected attitude angle error, is the derivative of the expected attitude angle error.

[0043] The advantages of the present invention compared with the prior art are as follows:

[0044] (1) The present invention regards the collision external force and external torque as a kind of interference, and detects the collision external force and external torque based on the disturbance observer, which can expand the collision detection range and improve the collision detection speed at the same time.

[0045] (2) The present invention proposes to use an admittance controller for UAV collision handling, which can adjust the position or heading of the UAV in real time according to the collision external force or external torque, avoiding the crash of the UAV.

[0046] (3) The present invention designs an attitude controller based on the idea of tilt-rotation separation, enabling the UAV to preferentially output reasonable torques to ensure the stability of the horizontal attitude under the condition of motor saturation, and for the rapid recovery of the attitude of the UAV in the case of instability. Description of the Drawings

[0047] Figure 1 is the flowchart of a method for rapid collision detection and recovery of a quadrotor UAV according to the present invention;

[0048] Figure 2 is the schematic diagram of the flexible adjustment of the position channel of the UAV after collision;

[0049] Figure 3 is the schematic diagram of the flexible adjustment of the heading channel of the UAV after collision. Detailed Embodiment

[0050] The present invention will be further described in detail below with reference to the drawings.

[0051] As Figure 1 shown, the specific implementation steps of the present invention are as follows:

[0052] In the first step, the influence of the collision on the UAV is regarded as an external disturbance received by the UAV, and the following nonlinear disturbance observer is used to estimate the collision external force:

[0053]

[0054] In the formula is the estimated collision external force, c f is the disturbance observer gain, R is the rotation matrix from the UAV body coordinate system to the inertial system, e w = [0 0 1] T represents the direction of the gravity vector in the inertial system, v is the UAV speed, f des is the output control quantity of the position controller, z f is an intermediate variable.

[0055] The following nonlinear disturbance observer is used to estimate the external torque generated by the collision:

[0056]

[0057] In the formula is the estimated collision external torque, c τ is the disturbance observer gain, J is the moment of inertia of the UAV, w is the angular velocity of the UAV, τdes is the control quantity output by the attitude controller, and z τ is an intermediate variable.

[0058] Generally, during the flight of a rotor UAV, external forces and torques will affect the UAV due to factors such as sensor noise and external wind disturbances. These are mixed with the external forces and torques generated by collisions. Experiments have shown that the amplitudes of the external forces and torques generated by collisions are often larger than those generated by other factors. To exclude the influence of other factors on the external forces and torques of the UAV, the following strategy is adopted for collision detection in this scheme:

[0059]

[0060] η is the collision detection flag. When η is 1, it means a collision is detected. When η is 0, it means no collision. κ f and κ τ are the collision detection thresholds for the position channel and the heading channel respectively.

[0061] In the second step, after detecting a collision, the UAV estimates the collision external force and the external torque using a non - linear disturbance observer and performs flexible adjustment of the position or heading. The admittance controller designed in this scheme is as follows:

[0062]

[0063] In the formula, X r = [p ref ψ ref T is the reference trajectory generated according to the collision external force. X d = [p des ψ des T represents the path and heading angle that the UAV referred to before the collision. M represents the virtual inertia coefficient matrix, D represents the elastic coefficient matrix, and K represents the rigid coefficient matrix. is the external force and external torque estimated by the non - linear disturbance observer in the second step. It can be seen from the above formula that when a collision generates collision external force and external torque, the desired position and heading angle of the UAV can be adjusted in real time according to the external torque. Thus, the continuous damage caused by the collision impact energy to the UAV can be avoided.

[0064] ​​Step 3: After the collision occurs, the rotor UAV will generate large tilting angles in three attitude angles, namely pitch, roll, and yaw angle, due to the collision impact. The pitch and roll angles in the UAV attitude play a relatively important role in attitude stability, while the yaw angle has little impact on the UAV's stability. The torque required for attitude adjustment of the UAV at large tilting angles often exceeds the limit value of the motor output. In the case of motor saturation, preferentially using limited resources (controller torque) for the horizontal attitude adjustment of the UAV will improve the UAV's attitude recovery ability. The present invention proposes an attitude control method of tilt-rotation separation for the rapid recovery of the UAV's horizontal attitude, specifically: dividing the attitude error of the UAV into horizontal attitude error and heading error, and setting a larger gain for the horizontal attitude error (pitch-roll angle error) when designing the attitude controller, so as to achieve rapid stabilization of the horizontal attitude and avoid the instability of the UAV. The adjustment of the pitch and roll angles is the z-axis direction of the UAV in the inertial system of adjustment and the expected z-axis direction generated by the UAV in the position loop. When the expected acceleration a generated by the position loop is known, z can be calculated as follows: w The z-axis direction of the UAV in the inertial system of adjustment and the expected z-axis direction generated by the UAV in the position loop w,des When the expected acceleration a generated by the position loop is known des After that, z w,des Can be calculated as:

[0065]

[0066] z w Can be calculated as:

[0067] z w = Re w

[0068] The horizontal attitude error between z w and z w,des Can be represented by the rotation axis And the rotation angle α, where:

[0069] α = cos -1 (z w T z w,des )

[0070]

[0071] Therefore, the horizontal attitude error represented by quaternion is:

[0072]

[0073] In order to align z w and z w,des as quickly as possible, the last element of q e,xy Can be set to 0. Therefore, the horizontal attitude error can be represented as:

[0074]

[0075] Align z w and z w,des After that, there is still a heading angle error that needs to be corrected, and the heading angle error can be expressed as:

[0076]

[0077] where q e is the desired total attitude error;

[0078] After separating the horizontal attitude error and the heading angle error, the desired torque output by the motor can be expressed as:

[0079]

[0080] where and are the imaginary parts of the horizontal attitude quaternion error q e,xy and the heading error q e,z respectively, k q,xy , k q,z and k w are controller gains, w e and are the angular velocity error and the angular velocity differential error respectively, and they can be calculated by the following formulas:

[0081]

[0082]

[0083] In the formulas, R des is the desired attitude angle error, is the differential of the desired attitude angle error.

[0084] As Figure 2 shown, when the UAV encounters an object larger than its own size, the UAV will re-plan a path according to the external force estimated by the non-linear observer, and the UAV will track this path and quickly adjust its attitude.

[0085] As Figure 3 shown, when the UAV encounters an object smaller than its own size, the UAV will real-time plan a new heading angle according to the external torque estimated by the non-linear observer, and the UAV will adjust the heading angle and flexibly adjust its attitude to pass through the obstacle.

[0086] Combining Figure 2 and 3 , it can be seen that the collision detection and processing method proposed by the present invention can well detect collisions and adjust the behavior of the UAV in real time, avoiding damage to the UAV.

[0087] The content not described in detail in the specification of the present invention belongs to the prior art well-known to those skilled in the art.

Claims

1. A method for rapid collision detection and recovery of a quadrotor UAV, characterized in that, It includes the following steps: In the first step, regarding the collision suffered by the quadrotor UAV as an external disturbance, a nonlinear disturbance observer is used to estimate the collision external force and external torque, and collision detection is carried out to obtain the collision detection result; the following strategy is adopted for collision detection: ； is the estimated external collision moment is the estimated external collision force is the collision detection flag. When is 1, it means a collision is detected. When is 0, it means no collision and are the collision detection thresholds on the position channel and the heading channel respectively; In the second step, according to the collision external force and external torque estimated in the first step and the collision detection result, a admittance controller is used to perform real-time dynamic adjustment on the position and heading of the quadrotor UAV after the collision; the admittance controller is as follows: ； In the formula is to generate a reference trajectory according to the collision external force, represents the path and heading angle that the drone refers to before the collision, represents the virtual inertia coefficient matrix, represents the elastic coefficient matrix, is the external force and external torque estimated by the nonlinear disturbance observer; In the third step, a tilt-rotation separation attitude control method is proposed for the rapid recovery of the horizontal attitude of the quadrotor UAV. The attitude error of the quadrotor UAV is split into a horizontal attitude error and a heading error, and a larger gain is set for the horizontal attitude error in the design process of the attitude controller, so as to achieve the rapid stabilization of the horizontal attitude and avoid the instability of the quadrotor UAV; After separating the horizontal attitude error and the heading angle error, the desired torque output by the motor is expressed as: ； where is the output control quantity of the attitude controller, and are the imaginary parts of the horizontal attitude quaternion error and the heading error respectively, , and are the controller gains, and are the angular velocity error and the angular velocity differential error respectively, and they are calculated by the following formulas: ； ； In the formula is the desired attitude angle error, is the differential of the desired attitude angle error.

2. The method for rapid collision detection and recovery of a quadrotor UAV according to claim 1, characterized in that: the specific implementation of the first step is as follows: (1) The following nonlinear disturbance observer is used to estimate the collision external force: ； In the formula is the estimated collision external force, is the disturbance observer gain, is the rotation matrix from the UAV body coordinate system to the inertial system, represents the direction of the gravity vector in the inertial system, is the UAV velocity, is the output control quantity of the position controller, is an intermediate variable; (2) The following nonlinear disturbance observer is used to estimate the external torque generated by the collision: ； In the formula is the estimated external collision torque, is the disturbance observer gain, is the moment of inertia of the UAV, is the angular velocity of the UAV, is an intermediate variable.

3. The rapid collision detection and recovery method for a quadrotor UAV according to claim 1, characterized in that: In the second step, when a collision generates a collision external force and external torque, the desired position and heading angle of the UAV are adjusted in real time according to the external torque to avoid continuous damage to the UAV by the collision impact energy.

4. The rapid collision detection and recovery method for a quadrotor UAV according to claim 1, characterized in that: In the third step, the attitude control method for tilt-rotation separation is as follows: the axis direction of the drone under the adjustment of the pitch and roll angles in the inertial system and the expected axis direction generated by the drone under the position loop. When the expected acceleration generated by the position loop is determined , the calculation is as follows: ； Calculated as: ； Between and The horizontal attitude error is represented by the rotation axis and the rotation angle as follows: ； ； The horizontal attitude error represented by quaternion is: ； To align as quickly as possible and , The last element of is set to 0, and the horizontal attitude error is expressed as: ； Alignment and after that, the heading angle error needs to be corrected, and the heading angle error is expressed as: ； wherein is the desired total attitude error.

Images