An unmanned aerial vehicle reactive obstacle avoidance method based on ultrasonic sensors

Abstract

The application discloses an unmanned aerial vehicle reactive obstacle avoidance method based on an ultrasonic sensor and belongs to the technical field of unmanned aerial vehicles.The application comprises the following steps: S1, real-time monitoring and recording obstacles by means of an ultrasonic sensor; S2, modeling the obstacles to obtain an obstacle surface model; S3, judging whether to perform obstacle plane fitting; S4, performing obstacle surface model deduplication and danger removal judgment; S5, judging the position relationship between the current movement direction of the unmanned aerial vehicle and the obstacle surface; and S6, calculating corresponding obstacle avoidance trajectories and avoiding obstacles according to obstacle planning.The application provides an unmanned aerial vehicle reactive obstacle avoidance algorithm based on an ultrasonic sensor, solves the problems of insufficient utilization of ultrasonic sensor data, inaccurate obstacle modeling, non-smooth subsequent obstacle avoidance strategy and insufficient safety of existing algorithms, and efficiently and safely realizes the avoidance of static obstacles by the unmanned aerial vehicle.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) technology, and more specifically to a reactive obstacle avoidance method for UAVs based on ultrasonic sensors. Background Technology

[0002] With the development of technology, drones are now widely used in various fields. In these applications, drones often need to operate in complex environments, making it crucial to ensure flight safety and prevent collisions with obstacles. Furthermore, due to the limited payload, onboard computing resources, and battery capacity of drones, it is essential to deploy lightweight, simple, reliable, and low-power sensors. Ultrasonic sensors, compared to lidar and binocular cameras, effectively meet this requirement. Additionally, the relatively high speed of drone movement necessitates real-time and fast obstacle avoidance algorithms. Considering these requirements, researching reactive obstacle avoidance algorithms for drones based on lightweight ultrasonic sensors is of significant importance.

[0003] Many existing technologies only use the basic ranging function of ultrasonic sensors. Reference [1] uses an ultrasonic sensor for ranging and stops the robot when an obstacle is detected. This algorithm is too simple. The strategy of stopping when an obstacle is encountered makes the robot's movement efficiency low, the movement trajectory is not smooth, and the energy consumption and time consumption are high. References [2][3] use multiple ultrasonic sensors to measure the distance between the robot and the wall and adjust the robot's movement direction according to the ranging results to realize the robot's wall-walking function. This type of algorithm mainly keeps the robot at a small fixed distance from the obstacle and does not achieve obstacle avoidance. Moreover, this approach reduces the robot's movement space and is not conducive to subsequent movement planning. In addition, this type of algorithm uses a large number of ultrasonic sensors, which often results in redundant waste of sensor observation data. Another type of algorithm collects the distance measurement results of the ultrasonic sensor over a period of time, performs simple processing on the collected distance data and takes obstacle avoidance operation. Reference [4] maps the observation data of the ultrasonic sensor onto the grid map. This method requires a large amount of grid map data to be stored, which consumes a lot of memory resources. Reference [5] obtains the obstacle boundary by judging the changing trend of the collected measurement data. This method has a large amount of sampling data and requires a long sampling time, so it cannot run in real time. Moreover, the subsequent planned trajectory is a simple broken line segment, which does not make full use of the kinematic characteristics of the UAV and has low motion efficiency. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention provides a reactive obstacle avoidance method for unmanned aerial vehicles (UAVs) based on ultrasonic sensors. The purpose is to provide a reactive obstacle avoidance algorithm for UAVs based on ultrasonic sensors, which solves the problems of insufficient utilization of ultrasonic sensor data, inaccurate obstacle modeling, unsmooth subsequent obstacle avoidance strategies, and insufficient safety in existing algorithms, and achieves efficient and safe avoidance of static obstacles by UAVs.

[0005] The technical solution adopted in this invention is as follows:

[0006] A reactive obstacle avoidance method for unmanned aerial vehicles (UAVs) based on ultrasonic sensors includes the following steps:

[0007] S1: Set alarm threshold Mrange and flag value avaiFlag for the ultrasonic sensor of the drone. Monitor and record obstacles in real time through the ultrasonic sensor, and store the real-time obstacle data information in the cache.

[0008] S2: Use the dataset in the buffer of S1 to model the obstacle and obtain the obstacle surface model with spatial point coordinates;

[0009] S3: Based on the alarm threshold Mrange set in S1 and the corresponding data information of the obstacle surface model in S2, determine whether to perform obstacle plane fitting.

[0010] S4: Add the obstacle surface model to the WallQueue queue, and perform obstacle surface model deduplication and hazard removal judgment;

[0011] S5: Obtain the obstacle surface to be avoided from the WallQueue queue, and determine the positional relationship between the drone's current movement direction and the obstacle surface;

[0012] S6: Based on obstacle data detected by ultrasonic sensors and the current flight status of the drone, the corresponding obstacle avoidance trajectory is calculated, and the drone avoids obstacles according to single obstacle planning or multiple obstacle planning.

[0013] Preferably, the UAV is equipped with at least three ultrasonic sensors. Each ultrasonic sensor is equipped with a corresponding alarm threshold Mrange and a flag value avaiFlag. The flag value avaiFlag reflects the usage status of the ultrasonic sensor. The flag value avaiFlag is initially set to avaiFlag = 1, indicating that the corresponding ultrasonic sensor is in use. When the ultrasonic sensor measurement data is less than the alarm threshold Mrange and avaiFlag = 1, an alarm is triggered and an obstacle avoidance trajectory is calculated. At the same time, the value of avaiFlag is set to 0.

[0014] Preferably, the calculation principle of the alarm threshold Mrange is as follows:

[0015] Assuming the drone's camera field of view is θ, when the boundary of the field of view is perpendicular to the obstacle surface, if the measurement data from the ultrasonic sensor is less than the alarm threshold Mrange, it indicates that the drone is too close to the obstacle and needs to avoid it. According to geometric relationships, Mrange = h / sinθ, where h represents the distance the drone has flown from the current moment, undergoing uniform deceleration with maximum acceleration in the direction perpendicular to the wall. When the drone's velocity component perpendicular to the wall is its maximum velocity... Among them, v max a max These represent the drone's maximum speed and maximum acceleration, respectively. Since the drone needs to collect measurement data from the ultrasonic sensor over a time interval Δt from the start of the alarm trigger, the distance traveled during that time interval must be taken into account. Therefore, Mrange's calculation formula is as follows:

[0016]

[0017] Here, α represents the adjustment factor, which is set according to the specific flight environment.

[0018] Preferably, step S2 specifically includes:

[0019] From the moment the alarm is triggered, the ultrasonic sensor monitors obstacle data over the next Δt time period. Combined with the current state information of the UAV, the coordinates of the corresponding obstacle surface points are calculated and stored in a buffer. After storage, the average value of the data in the buffer is calculated. If the average value is greater than the alarm threshold Mrange, the alarm is deactivated and the flag value avaiFlag remains unchanged. If the average value is less than the alarm threshold Mrange, the least squares method is used for plane fitting to obtain the corresponding obstacle surface model with spatial point coordinates.

[0020] Preferably, the process of deduplicating the obstacle surface model in S4 is as follows:

[0021] Several ultrasonic sensors will generate several corresponding obstacle surface models in parallel. These obstacle surface models are stored in a WallQueue. Each newly added obstacle surface model in the WallQueue is deduplicated. The deduplication process is as follows:

[0022] Calculate the angle between the newly added obstacle surface model and the existing obstacle surface models in the queue. If the calculated angle is less than 30°, it is determined that the newly added obstacle surface model and the original obstacle surface model with an angle less than 30° belong to the same obstacle surface model, and the new obstacle surface model is removed from the WallQueue queue.

[0023] During the drone's flight, the distance between the drone and the obstacle surface model in the queue is calculated based on the drone's current position. If the calculated distance is greater than the initial distance between the drone and the obstacle surface model, it means that the drone has successfully avoided and moved away from the obstacle, and the obstacle surface model is then removed from the WallQueue queue.

[0024] Preferably, the process of determining whether the danger has been eliminated in S4 is as follows:

[0025] Determine if the drone's current direction of movement is far from the obstacle surface; by sampling the next position point reached by the drone along the current direction of movement, calculate the distance from the current position and the next position to the obstacle surface respectively. If the distance decreases, it means that the drone is approaching the obstacle surface, and an obstacle avoidance trajectory needs to be planned and an alarm should be triggered; otherwise, it is considered that the drone is far from the obstacle surface, the obstacle poses no threat to the drone's movement, and the danger is eliminated.

[0026] Preferably, the individual obstacle planning process for the obstacle avoidance trajectory is as follows:

[0027] When avoiding a single obstacle, the obstacle is treated as a hard constraint, and the current motion state of the UAV is used as the initial condition to calculate the second-order motion trajectory. The UAV undergoes uniformly decelerated motion in the y′ direction and uniform motion in the x′ direction. The current position of the UAV is taken as the origin of the body coordinate system, and the x′ axis and y′ axis are respectively defined as the directions parallel to the obstacle surface and the directions perpendicular to the obstacle surface. Let θ represent the angle between the current velocity direction of the UAV and the positive x′ direction, and let θ represent the angle between the obstacle surface direction and the positive x-axis direction of the ground coordinate system. The direction vector of the UAV's body coordinate system can then be obtained as:

[0028]

[0029] The components of the drone's velocity v relative to the ground coordinate system in the body coordinate system are:

[0030]

[0031] The required acceleration in the y′ direction is:

[0032] a y′ =a max ·(-1)·sign(v y′(4)

[0033] Here, sign() is the sign function. If the independent variable x is positive, then sign(x) = 1; if x is negative, then sign(x) = -1; if x = 0, then sign(x) = 0.

[0034] Therefore, the trajectory equation of a single obstacle avoidance path in the body coordinate system is:

[0035]

[0036] The transformation relationship between the body coordinate system and the ground coordinate system is as follows:

[0037]

[0038] Wherein, the pos vector represents the current position of the UAV in the ground coordinate system;

[0039] Therefore, the equation of a single obstacle avoidance trajectory in the ground coordinate system is:

[0040]

[0041] Preferably, the obstacle planning process for the obstacle avoidance trajectory is as follows:

[0042] In the obstacle avoidance trajectory, the drone undergoes uniform deceleration in the x′ direction. After the drone's velocity component in that direction becomes equal and reverses, it resumes uniform motion. Meanwhile, it continues uniform deceleration in the y′ direction. Let the acceleration in the x′ direction be a1, and the acceleration in the y′ direction be a2. Based on the motion along the x′ axis, the obstacle avoidance trajectory can be divided into two stages. Let the flight time of the first stage be t1, and the flight time of the second stage be t2. We can obtain the following relationship between a1, a2, t1, and t2:

[0043]

[0044] At the same time:

[0045]

[0046] According to equations (9) and (10), we get:

[0047]

[0048] make:

[0049]

[0050] According to equation (7), we get After calculating the acceleration, the trajectory equations of the multiple obstacle avoidance trajectories in the body coordinate system are as follows:

[0051]

[0052] The trajectory equation in the ground coordinate system is:

[0053]

[0054] (14).

[0055] Preferably, if the WallQueue contains only the current obstacle surface model, the UAV performs obstacle avoidance based on a single obstacle plan; if the WallQueue contains not only the obstacle surface model to be avoided but also an obstacle surface model currently being avoided, then based on the single obstacle plan and multiple obstacle plans, the endpoint positions d and d′ corresponding to the obstacle surface to be avoided are first calculated:

[0056]

[0057] Then, based on the distance between d and d′ relative to the surface of the obstacle being avoided, the optimal motion trajectory is selected to guide the drone to avoid the surface of the obstacle that needs to be avoided, while ensuring that the drone does not collide with the surface of the obstacle being avoided.

[0058] Preferably, in step S6, when the UAV executes the specified obstacle avoidance trajectory, a minbuffer value is set. When the minbuffer value is greater than the alarm threshold Mille and avaiFlag = 0, the avaiFlag value is set to 1, indicating that the corresponding ultrasonic sensor has reached a safe distance from the obstacle, and the corresponding ultrasonic sensor is reactivated.

[0059] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0060] 1. The invention fully explores and utilizes the single-point ranging capability of ultrasonic sensors and the movement of the UAV itself to achieve obstacle scanning and modeling, making obstacle perception simpler and faster, and requiring less computation.

[0061] 2. Two second-order smooth obstacle avoidance trajectories were specially designed to better fit the kinematic characteristics of the drone, enabling the drone to move more smoothly and steadily, while saving energy and reducing mechanical damage caused by sudden changes in trajectory;

[0062] 3. The designed obstacle avoidance trajectory will cause the drone to make a rebound motion, enabling the drone to avoid and move away from obstacles, while providing higher safety and more room for maneuver for the drone's subsequent flight;

[0063] 4. The designed multi-sensor simultaneous obstacle avoidance and multi-obstacle avoidance strategy enables the UAV to adapt to more complex environments, ensures the flight safety of the UAV, and prevents it from getting stuck in blind spots formed by obstacles. Attached Figure Description

[0064] The present invention will be described by way of example and with reference to the accompanying drawings, wherein:

[0065] Figure 1 This is a schematic diagram illustrating the calculation principle of the Mrange parameter in this invention;

[0066] Figure 2 This is a schematic diagram illustrating the process of fitting the obstacle surface based on ultrasonic sensor scanning data in this invention;

[0067] Figure 3 This is a schematic diagram illustrating the process of determining whether a drone is close to the surface of an obstacle in this invention;

[0068] Figure 4 This is a schematic diagram of the obstacle avoidance trajectory planned for a single obstacle in this invention;

[0069] Figure 5 This is a schematic diagram of the obstacle avoidance trajectory planned for multiple obstacles in this invention;

[0070] Figure 6 This is a schematic diagram showing the endpoint positions of obstacle avoidance trajectories for single obstacle planning and multiple obstacle planning in this invention;

[0071] Figure 7 This is a schematic diagram illustrating the principle of selecting the optimal obstacle avoidance trajectory in this invention;

[0072] Figure 8 This is a diagram showing the obstacle avoidance trajectory of the UAV of the present invention in a single obstacle scenario and the change in the closest distance to the obstacle;

[0073] Figure 9 This is a diagram showing the obstacle avoidance trajectory of the UAV of the present invention in a single obstacle scenario and the change in the closest distance to the obstacle;

[0074] Figure 10 This is a diagram showing the obstacle avoidance trajectory of the UAV of the present invention in a blind spot scenario composed of multiple obstacles;

[0075] Figure 11 This is a diagram showing the obstacle avoidance trajectory of the UAV of the present invention in a complex obstacle environment;

[0076] Figure 12 This is a schematic diagram of the overall system architecture of the present invention. Detailed Implementation

[0077] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0078] In the description of the embodiments of this application, it should be noted that the terms "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product of the invention is in use. They are only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this application. In addition, the terms "first," "second," and "third," etc., are only used to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0079] The following is combined Figures 1-12 The present invention will be described in detail below.

[0080] A reactive obstacle avoidance method for unmanned aerial vehicles (UAVs) based on ultrasonic sensors includes the following steps:

[0081] S1: Set alarm threshold Mrange and flag value avaiFlag for the ultrasonic sensor of the drone. Monitor and record obstacles in real time through the ultrasonic sensor, and store the real-time obstacle data information in the cache.

[0082] S2: Use the dataset in the buffer of S1 to model the obstacle and obtain the obstacle surface model with spatial point coordinates;

[0083] S3: Based on the alarm threshold Mrange set in S1 and the corresponding data information of the obstacle surface model in S2, determine whether to perform obstacle plane fitting.

[0084] S4: Add the obstacle surface model to the WallQueue queue, and perform obstacle surface model deduplication and hazard removal judgment;

[0085] S5: Obtain the obstacle surface to be avoided from the WallQueue queue, and determine the positional relationship between the drone's current movement direction and the obstacle surface;

[0086] S6: Based on obstacle data detected by ultrasonic sensors and the current flight status of the drone, the corresponding obstacle avoidance trajectory is calculated, and the drone avoids obstacles according to single obstacle planning or multiple obstacle planning.

[0087] The drone is equipped with at least three ultrasonic sensors. Each ultrasonic sensor has a corresponding alarm threshold Mrange and a flag value avaiFlag. The flag value avaiFlag reflects the usage status of the ultrasonic sensor. The initial default value of avaiFlag is avaiFlag=1, indicating that the corresponding ultrasonic sensor is in use. When the ultrasonic sensor measurement data is less than the alarm threshold Mrange and avaiFlag=1, an alarm is triggered and an obstacle avoidance trajectory is calculated. At the same time, the value of avaiFlag is set to 0.

[0088] The calculation principle of the alarm threshold Mrange is as follows:

[0089] like Figure 1 As shown, assuming the drone's camera field of view is θ, when the boundary of the field of view is perpendicular to the obstacle surface, if the measurement data from the ultrasonic sensor is less than the alarm threshold Mrange, it indicates that the drone is too close to the obstacle and needs to avoid it. According to geometric relationships, Mrange = h / sinθ, where h represents the distance the drone has flown from the current moment, undergoing uniform deceleration with maximum acceleration in the direction perpendicular to the wall. When the drone's velocity component perpendicular to the wall is at its maximum speed... Among them, v max a max These represent the drone's maximum speed and maximum acceleration, respectively. Since the drone needs to collect measurement data from the ultrasonic sensor over a time interval Δt from the start of the alarm trigger, the distance traveled during that time interval must be taken into account. Therefore, Mrange's calculation formula is as follows:

[0090]

[0091] Here, α represents the adjustment factor, which is usually set according to the specific flight environment.

[0092] The process of S2 calling the dataset in the buffer of S1 to model obstacles and obtain the obstacle surface model with spatial point coordinates is as follows:

[0093] From the moment the alarm is triggered, the ultrasonic sensor monitors obstacle data over the next Δt time period. Combining this with the drone's current state information, it calculates the coordinates of the corresponding obstacle surface points and stores these coordinates in a buffer. After storage, it calculates the average value of the data in the buffer. If the average value is greater than the alarm threshold Mrange, the alarm is deactivated, and the flag value avaiFlag remains unchanged. If the average value is less than the alarm threshold Mrange, the alarm is deactivated. Figure 2 As shown, the least squares method is used for plane fitting to obtain the corresponding obstacle surface model with spatial point coordinates.

[0094] The process of removing duplicates from the obstacle surface model in S4 is as follows:

[0095] Several ultrasonic sensors will generate several corresponding obstacle surface models in parallel. These obstacle surface models are stored in a WallQueue. Each newly added obstacle surface model in the WallQueue is deduplicated. The deduplication process is as follows:

[0096] Calculate the angle between the newly added obstacle surface model and the existing obstacle surface models in the queue. If the calculated angle is less than 30°, it is determined that the newly added obstacle surface model and the original obstacle surface model with an angle less than 30° belong to the same obstacle surface model, and the new obstacle surface model is removed from the WallQueue queue.

[0097] like Figure 3 As shown, during the drone's flight, the distance between the drone and the obstacle surface model in the queue is calculated based on the drone's current position. If the calculated distance is greater than the initial distance between the drone and the obstacle surface model, it means that the drone has successfully avoided and moved away from the obstacle, and the obstacle surface model is then removed from the WallQueue queue.

[0098] The process for determining whether danger has been eliminated in S4 is as follows:

[0099] Determine if the drone's current direction of movement is far from the obstacle surface; by sampling the next position point reached by the drone along the current direction of movement, calculate the distance from the current position and the next position to the obstacle surface respectively. If the distance decreases, it means that the drone is approaching the obstacle surface, and an obstacle avoidance trajectory needs to be planned and an alarm should be triggered; otherwise, it is considered that the drone is far from the obstacle surface, the obstacle poses no threat to the drone's movement, and the danger is eliminated.

[0100] like Figure 4 As shown, the process of planning a single obstacle in the obstacle avoidance trajectory is as follows:

[0101] When avoiding a single obstacle, the obstacle is treated as a hard constraint, and the current motion state of the UAV is used as the initial condition to calculate the second-order motion trajectory. The UAV undergoes uniformly decelerated motion in the y′ direction and uniform motion in the x′ direction. The current position of the UAV is taken as the origin of the body coordinate system, and the x′ axis and y′ axis are respectively defined as the directions parallel to the obstacle surface and the directions perpendicular to the obstacle surface. Let θ represent the angle between the current velocity direction of the UAV and the positive x′ direction, and let θ represent the angle between the obstacle surface direction and the positive x-axis direction of the ground coordinate system. The direction vector of the UAV's body coordinate system can then be obtained as:

[0102]

[0103] The components of the drone's velocity v relative to the ground coordinate system in the body coordinate system are:

[0104]

[0105] The required acceleration in the y′ direction is:

[0106]

[0107] Here, sign() is the sign function. If the independent variable x is positive, then sign(x) = 1; if x is negative, then sign(x) = -1; if x = 0, then sign(x) = 0.

[0108] Therefore, the trajectory equation of a single obstacle avoidance path in the body coordinate system is:

[0109]

[0110] The transformation relationship between the body coordinate system and the ground coordinate system is as follows:

[0111]

[0112] Wherein, the pos vector represents the current position of the UAV in the ground coordinate system;

[0113] Therefore, the equation of a single obstacle avoidance trajectory in the ground coordinate system is:

[0114]

[0115] When there is a single obstacle, a single obstacle avoidance trajectory is used.

[0116] like Figure 5 The obstacle planning process for the obstacle avoidance trajectory shown is as follows:

[0117] In the obstacle avoidance trajectory, the drone undergoes uniform deceleration in the x′ direction. After the drone's velocity component in that direction becomes equal and reverses, it resumes uniform motion. Meanwhile, it continues uniform deceleration in the y′ direction. Let the acceleration in the x′ direction be a1, and the acceleration in the y′ direction be a2. Based on the motion along the x′ axis, the obstacle avoidance trajectory can be divided into two stages. Let the flight time of the first stage be t1, and the flight time of the second stage be t2. We can obtain the following relationship between a1, a2, t1, and t2:

[0118]

[0119] At the same time:

[0120]

[0121] According to equations (9) and (10), we get:

[0122]

[0123] make:

[0124]

[0125] According to equation (7), we get After calculating the acceleration, the trajectory equations of the multiple obstacle avoidance trajectories in the body coordinate system are as follows:

[0126]

[0127] The trajectory equation in the ground coordinate system is:

[0128]

[0129] (14).

[0130] When there are multiple obstacles, multiple obstacle avoidance trajectories are used.

[0131] If the WallQueue only contains the current obstacle surface model, the drone will perform obstacle avoidance based on a single obstacle plan; if... Figure 6 As shown, if the WallQueue contains not only the obstacle surface model that needs to be avoided but also an obstacle surface model that is currently being avoided, then based on single obstacle planning and multiple obstacle planning, the endpoint positions d and d′ corresponding to the obstacle surface that needs to be avoided are first calculated:

[0132]

[0133] Then, based on the distances of d and d′ relative to the surface of the obstacle being avoided, the optimal trajectory is selected to guide the drone to avoid the obstacle surface while ensuring that the drone does not collide with it; specifically as follows: Figure 7 As shown, the solid lines represent the old wall and the dashed lines represent the new wall. The drone is currently executing an obstacle avoidance trajectory on the solid-line wall. At this moment, the sensor detects a new wall and determines that it needs to be avoided. Therefore, the endpoint positions of two candidate trajectories are first calculated, i.e., the two pentagrams in the diagram. Then, it is determined whether the endpoint position is within the old wall. The method is to calculate whether the two endpoint positions and the drone's current position are within the same area of ​​the new wall's equation. If one is within the old wall, the obstacle avoidance trajectory corresponding to the other endpoint position is selected; if neither is within the old wall, the obstacle avoidance trajectory corresponding to the endpoint position farther from the old wall is selected, i.e., the trajectory corresponding to the bold dashed line in the diagram, and given to the drone to execute.

[0134] In S6, when the drone executes a specified obstacle avoidance trajectory, the minbuffer value is set. When the minbuffer value is greater than the alarm threshold Mrange and avaiFlag = 0, the avaiFlag value is set to 1, indicating that the corresponding ultrasonic sensor has reached a safe distance from the obstacle, and the corresponding ultrasonic sensor is reactivated.

[0135] like Figure 12 As shown, the overall system architecture used in this method includes: an ultrasonic sensor alarm module, used to acquire ultrasonic sensor observation data and determine whether an obstacle avoidance alarm is triggered; an obstacle wall calculation module, used to calculate and save the positions of obstacle surface points based on ultrasonic sensor data and UAV status, and then generate obstacle wall equations; and a wall processing module, which includes a WallQueue data structure, a wall deduplication submodule, and a wall update submodule. The WallQueue data structure stores the obstacle wall equations detected by the three ultrasonic sensors. The wall deduplication submodule then performs wall deduplication on the WallQueue. The wall update module calculates the distance between the drone and the walls in the WallQueue in real time and promptly removes walls that have been avoided. The obstacle avoidance judgment module calculates the drone's approach trend to the walls and determines whether walls removed from the WallQueue need to be avoided. The trajectory calculation module calculates two candidate trajectories. The trajectory selection module takes the drone's current motion state and the wall to be avoided as input and selects the obstacle avoidance trajectory to be executed by the drone according to predefined rules. The ultrasonic sensor restart module determines whether previously disabled ultrasonic sensors can be reactivated.

[0136] The embodiments described above merely illustrate specific implementation methods of this application, and while the descriptions are detailed and specific, they should not be construed as limiting the scope of protection of this application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the technical solution of this application, and these modifications and improvements all fall within the scope of protection of this application.

Claims

1. A reactive obstacle avoidance method for unmanned aerial vehicles (UAVs) based on ultrasonic sensors, characterized in that, Includes the following steps: S1: Set alarm thresholds for the ultrasonic sensors of the drone. The obstacle is monitored and recorded in real time by an ultrasonic sensor, and the real-time obstacle data is stored in a cache. S2: Use the dataset in the buffer of S1 to model the obstacle and obtain the obstacle surface model with spatial point coordinates; S3: Based on the alarm threshold set in S1 Based on the corresponding data information of the obstacle surface model in S2, determine whether to perform obstacle plane fitting; S4: Add the obstacle surface model to the WallQueue queue, and perform obstacle surface model deduplication and hazard removal judgment; S5: Obtain the obstacle surface to be avoided from the WallQueue queue, and determine the positional relationship between the drone's current movement direction and the obstacle surface; S6: Based on the obstacle data information monitored by the ultrasonic sensor and the current flight status of the drone, the corresponding obstacle avoidance trajectory is calculated, and the drone avoids obstacles according to the single obstacle plan or multiple obstacle plans; The process of removing duplicates from the obstacle surface model in S4 is as follows: Several ultrasonic sensors will generate several corresponding obstacle surface models in parallel. These obstacle surface models are stored in a WallQueue. Each newly added obstacle surface model in the WallQueue is deduplicated. The deduplication process is as follows: Calculate the angle between the newly added obstacle surface model and the existing obstacle surface models in the queue. If the calculated angle is less than 30°, it is determined that the newly added obstacle surface model and the original obstacle surface model with an angle less than 30° belong to the same obstacle surface model, and the newly added obstacle surface model is removed from the WallQueue queue. During the drone's flight, the distance between the drone and the obstacle surface model in the queue is calculated based on the drone's current position. If the calculated distance is greater than the initial distance between the drone and the obstacle surface model, it means that the drone has successfully avoided and moved away from the obstacle, and the obstacle surface model is then removed from the WallQueue queue. The individual obstacle planning process for the obstacle avoidance trajectory is as follows: When avoiding a single obstacle, the obstacle is treated as a hard constraint, and the current motion state of the drone is used as the initial condition to calculate a second-order motion trajectory; the drone in It undergoes uniformly decelerated motion in the axial direction. The drone moves at a constant speed along its axis, with its current position as the origin of its coordinate system. The directions parallel to and perpendicular to the obstacle surface are used as coordinates. shaft and axis, Indicates the current speed direction of the drone and The angle along the positive direction of the axis. Indicates the orientation of the obstacle surface relative to the ground coordinate system The angle between the positive axes gives the direction vector of the body coordinate system as: (2) The speed of the drone relative to the ground coordinate system The components in the body coordinate system are: (3)； exist The required acceleration in the axial direction is: (4)； in, This indicates the maximum acceleration of the drone. For a sign function, if the independent variable If it is a positive number, then =1; if If it is negative, then =-1; if =0, then =0; Therefore, the trajectory equation of a single obstacle avoidance path in the body coordinate system is: (5)； The transformation relationship between the body coordinate system and the ground coordinate system is as follows: （6）； in, The vector represents the current position of the UAV in the ground coordinate system; Therefore, the equation of a single obstacle avoidance trajectory in the ground coordinate system is: (7)。 2. The reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1, characterized in that, The drone is equipped with at least three ultrasonic sensors, each with a corresponding alarm threshold. The avaiFlag value reflects the usage status of the ultrasonic sensor. The avaiFlag value is initially set to avaiFlag=1, indicating that the corresponding ultrasonic sensor is in use. When the ultrasonic sensor's measured data is less than the alarm threshold... When avaiFlag=1, an alarm is triggered and the obstacle avoidance trajectory is calculated, while the value of avaiFlag is set to 0.

3. A reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1 or 2, characterized in that, The alarm threshold The calculation principle is as follows: Assuming the drone camera's field of view is When the field of view boundary is perpendicular to the obstacle surface, the measurement data of the ultrasonic sensor is less than the alarm threshold. When the drone is too close to an obstacle, it needs to avoid it; according to geometric relationships, , This represents the distance the drone has flown from the current moment, undergoing uniform deceleration with maximum acceleration in the direction perpendicular to the wall; when the drone's velocity component perpendicular to the wall is at its maximum speed, ,in, , These represent the drone's maximum speed and maximum acceleration, respectively; since the drone needs to collect data from the ultrasonic sensor starting from the alarm trigger point... Measurement data within a given time period, therefore, must be... Taking into account the distance traveled over time, we get The calculation formula is as follows: (1)； in, This indicates the adjustment factor, which is set according to the specific flight environment.

4. The reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1, characterized in that, The specific steps of S2 are as follows: From the moment the alarm is triggered, the ultrasonic sensor monitors the following... Obstacle data within a given time period is combined with the drone's current state information to calculate the coordinates of corresponding obstacle surface points, which are then stored in a buffer. After storage, the average value of the data in the buffer is calculated; if the average value exceeds an alarm threshold... If the alarm is cleared, the flag value avaiFlag remains unchanged. If the average value is less than the alarm threshold... Then, the least squares method is used to perform plane fitting to obtain the corresponding obstacle surface model with spatial point coordinates.

5. The reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1, characterized in that, The process of determining whether the danger has been eliminated in S4 is as follows: Determine if the drone's current direction of movement is far from the obstacle surface; by sampling the next position point reached by the drone along the current direction of movement, calculate the distance from the current position and the next position to the obstacle surface respectively. If the distance decreases, it means that the drone is approaching the obstacle surface, and an obstacle avoidance trajectory needs to be planned and an alarm should be triggered; otherwise, it is considered that the drone is far from the obstacle surface, the obstacle poses no threat to the drone's movement, and the danger is eliminated.

6. The reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1, characterized in that, The obstacle avoidance trajectory planning process for multiple obstacles is as follows: While dodging multiple obstacle trajectories, the drone... The drone undergoes uniformly decelerated motion along the axial direction. Once the component of its velocity in that direction becomes equal in magnitude and reverses, it resumes uniform motion. The motion continues to decelerate uniformly in the axial direction. Let it be... The acceleration in the axial direction is ,exist The acceleration in the axial direction is ;according to The motion of the axis and the obstacle avoidance trajectory can be divided into two stages. Let the flight time of the first stage be... The flight time for the second stage is We can get information about , , , The relationship is as follows: (8)； At the same time: (9)； (10)； According to equations (9) and (10), we get: (11)； make: (12)； According to equation (7), we get After calculating the acceleration, the trajectory equations of the multiple obstacle avoidance trajectories in the body coordinate system are as follows: (13)； The trajectory equation in the ground coordinate system is: (14)。 7. The reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1, characterized in that, If the WallQueue contains only the current obstacle surface model, the drone performs obstacle avoidance based on a single obstacle plan. If the WallQueue contains not only the obstacle surface model to be avoided but also an obstacle surface model currently being avoided, then based on both the single and multiple obstacle plans, the endpoint position relative to the obstacle surface to be avoided is first calculated. and : (15)； Then according to and Based on the distance to the obstacle surface being avoided, the optimal motion trajectory is selected to guide the drone to avoid the obstacle surface that is about to be avoided, while ensuring that the drone does not collide with the obstacle surface being avoided.

8. The reactive obstacle avoidance method for unmanned aerial vehicles based on ultrasonic sensors according to claim 1, characterized in that, In step S6, when the drone executes the specified obstacle avoidance trajectory, a minbuffer value is set. When the minbuffer value exceeds an alarm threshold... When avaiFlag=0, the value of avaiFlag is set to 1, indicating that the corresponding ultrasonic sensor has reached a safe distance from the obstacle, and the corresponding ultrasonic sensor is reactivated.

Images