A point cloud-based anti-collision method for remote control of UAV in closed space

Abstract

The present invention discloses a closed space UAV remote control anti-collision method based on point cloud, which relates to the field of closed space UAV safety control. The method is composed of a point cloud extraction module, a point cloud processing module, a dangerous direction judgment module, a size expansion module, and a remote control dangerous direction filtering module. This technical method solves the problem that in a closed space, due to the narrow, complex, harsh space environment, or even the need for non-line-of-sight flight, the UAV controlled by the remote control is prone to collide with space objects, resulting in body damage and mission failure.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) safety control, and in particular to a point cloud-based closed space UAV remote control anti-collision method. Background Art

[0002] Multi-rotor drones, commonly referred to as "multi-rotor aircraft" or "multi-rotor aircraft", are drones that use multiple rotors to provide lift and propulsion. These rotors are usually installed perpendicular to the fuselage of the aircraft and are independently driven by electric motors. With the continuous advancement of drone technology, multi-rotor drones are increasingly widely used in military, commercial, entertainment and other fields due to their simple structure, flexible operation and easy control. However, when drones fly in closed spaces (such as indoors, underground mines, tunnels, etc.), due to limited space, many obstacles and complex environment, collision accidents are prone to occur, causing equipment damage or casualties.

[0003] Traditional anti-collision methods based on vision or ultrasound have limitations in closed spaces, such as being affected by light and having limited detection distance. Point cloud is three-dimensional spatial data generated by laser radar scanning the environment, which can accurately describe the shape, position and distance of objects in the environment. Using point cloud data for anti-collision can achieve accurate perception of the environment and improve the obstacle avoidance ability and safety of drones. When a drone flies in a closed space, the operator controls the flight through a remote controller, which can flexibly and quickly achieve the operator's intention. However, some scenarios require the operator to use the returned point cloud or image information for non-line-of-sight flight. Due to problems such as delay in returned data and human error, the drone may collide with the robot, causing equipment damage or casualties. The existing drone remote control technology does not combine point cloud to perform anti-collision processing on control instructions. Therefore, it is necessary to design a closed space drone remote control anti-collision method based on point cloud to solve the above technical problems. Summary of the invention

[0004] In view of the defects existing in the prior art, the present invention discloses a closed space UAV remote control anti-collision method based on point cloud, so as to solve the problem that the UAV is prone to collision in closed spaces such as tunnels, underground mines, pipe galleries, and indoors due to limited space, many obstacles, and complex environment, and even in some cases, the operator needs to fly without line of sight through the remote control. In order to achieve the above purpose, the technical solution adopted by the present invention is as follows:

[0005] The UAV obtains the point cloud in the northeast sky coordinate system at a regular frequency of 10 Hz, establishes a point cloud queue with a length of L, and updates the queue once a frame of point cloud is received according to the first-in-first-out principle. At the same time, the updated point cloud queue is merged to obtain a set of point clouds to be processed. The point cloud set to be processed is extracted within a certain range. The specific range is a cylinder with the center of mass of the UAV as the center, the horizontal direction r as the radius, and the height h above and below the center of mass;

[0006] Map the extracted point cloud to the xy plane, and perform voxel filtering on the mapped point cloud;

[0007] In the horizontal direction, with the center of mass of the drone as the center of the circle and δ as the angle resolution, 360 / δ rays are fitted. The ray angle follows the direction of the northeast sky coordinate system. Each point in the processed point cloud is distributed to the nearest neighbor ray according to the angle Ω relative to the center of mass of the drone. The distributed rays are marked as dangerous.

[0008] The ray occupied by the point cloud is expanded to both sides by a number n, and the expanded ray is also marked as dangerous;

[0009] Read the expected speed (vx, vy) of the current drone remote controller joystick in the pitch and roll directions, and convert it into the expected horizontal speed direction in the northeast sky coordinate system in combination with the drone yaw angle. Distribute the direction to the ray with the nearest neighbor angle to determine whether the ray is marked as dangerous. If it is dangerous, set the expected horizontal speed (vx, vy) to zero. If it is not dangerous, do not modify the (vx, vy) value, and the drone moves normally in the horizontal direction.

[0010] A distance measurement module is installed directly above and below the drone, and the two values ​​(d u , d d ), when d u When the value is less than the threshold value D, if the joystick of the drone remote controller is in the vertical direction, the expected speed v z If v is greater than 0, z Set to zero; when d d When the value is less than the threshold value D, if the joystick of the drone remote controller is in the vertical direction, the expected speed v z If v is less than 0, z As a further optimization solution of the present invention, the radius r of the horizontal extraction point cloud range is equal to the size R of the circular envelope of the drone in the horizontal direction. h , UAV weight M, UAV current horizontal flight speed V h , safety threshold S h Regarding, specifically:

[0011] r=R h +α*V h *M+Sh

[0012] Where α is the scaling factor, which can be adjusted according to the actual test situation; where h is the maximum size R of the drone in the vertical direction v , safety threshold S v Regarding, specifically:

[0013] h=R v +S v

[0014] As a further optimization scheme of the present invention, the extracted point cloud is mapped to the xy plane, that is, each point in the point cloud is traversed and the z value of each point is set to 0; the mapped point cloud is subjected to voxel filtering, and the voxel filtering parameter (f x ,f y ,f z ) is related to the horizontal extraction point cloud range radius r and the angular resolution δ. Since it has been mapped to the xy plane, f z Just take a certain value, the specific f x =f y =r*δ

[0015] f z =0.1

[0016] As a further optimization scheme of the present invention, the angular resolution δ is related to the size of the dangerous object to be identified. The larger the size of the dangerous object, the larger δ is, and the smaller the size of the dangerous object, the smaller δ is. The specific calculation method of the angle Ω of a point i in the point cloud relative to the center of mass of the drone is:

[0017]

[0018] Where (x pi ,y pi ) is the coordinate of point i in the point cloud, (x b ,y b ) is the coordinate of the center of mass of the drone;

[0019] As a further optimization scheme of the present invention, the expansion number n is related to the size R of the circular envelope of the drone in the horizontal direction. h , horizontal extraction point cloud range radius r, angle resolution δ, safety threshold S d Regarding, specifically:

[0020]

[0021] On the basis of the above technical scheme, compared with the prior art, the advantages of the present invention are: providing a safer, more accurate and effective method for controlling collision avoidance of remote controllers of unmanned aerial vehicles based on point cloud, for unmanned aerial vehicles in closed spaces such as tunnels, underground mines, pipe galleries, and indoors, due to limited space, many obstacles, and complex environment, in some cases, operators need to use the returned point cloud or image information to control the flight without line of sight through the remote controller, and there may be delays in the returned data, erroneous operation of personnel, etc., which may cause unmanned aerial vehicles to be prone to collision accidents, resulting in equipment damage or casualties. Based on point cloud information, the present invention further judges the instructions issued by the remote controller after performing the above data processing and calculation to avoid collisions, making it safer for unmanned aerial vehicles to fly in closed spaces through remote controller control. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0023] Figure 1 A schematic flow chart of a point cloud-based closed space UAV remote control anti-collision method provided in Embodiment 1 of the present invention. DETAILED DESCRIPTION

[0024] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0025] The embodiments of the present invention are further described in detail below with reference to the accompanying drawings.

[0026] Figure 1 The flowchart of a method for controlling a closed space UAV remote controller and avoiding collision based on point cloud provided by the first embodiment of the present invention is as follows. Figure 1 As shown, the following steps are included:

[0027] S101: The drone obtains the point cloud in the northeast sky coordinate system at a regular frequency of 10 Hz, establishes a point cloud queue with a length of L, and updates the queue once a frame of point cloud is received based on the first-in-first-out principle;

[0028] S102: The updated point cloud queues are merged to obtain a set of point clouds to be processed, and the set of point clouds to be processed is extracted within a certain range. The specific range is a cylinder with the center of mass of the drone as the circle center, the horizontal direction r as the radius, and the height h above and below the center of mass. The calculation method of r and h is:

[0029] r=R h +α*V h *M+S h

[0030] h=R v +S v

[0031] Where R h is the size of the circular envelope of the drone in the horizontal direction, R v is the maximum vertical dimension of the drone, M is the weight of the drone, V is h is the current horizontal flight speed of the UAV, S h is the horizontal safety threshold, S v is the vertical safety threshold, and α is the horizontal scaling factor, which can be adjusted according to the actual test situation.

[0032] S103: Map the extracted point cloud to the xy plane, that is, traverse each point in the point cloud and set the z value of each point to 0; perform voxel filtering on the mapped point cloud, and the voxel filtering parameter (f x , f y , f z ) is related to the horizontal extraction point cloud range radius r and the angular resolution δ. Since it has been mapped to the xy plane, f z Just take a certain value, the specific f x =f y =r*δ

[0033] f z =0.1

[0034] S104: In the horizontal direction, with the center of mass of the drone as the center of the circle and δ as the angle resolution, fit 360 / δ rays, the ray angle follows the direction of the northeast celestial coordinate system, the angle resolution δ is related to the size of the dangerous object to be identified, the larger the size of the dangerous object, the larger δ, and the smaller the size of the dangerous object, the smaller δ; each point in the processed point cloud is distributed to the nearest neighbor ray according to the angle Ω relative to the center of mass position of the drone, and the distributed rays are marked as dangerous, wherein the specific calculation method of the angle Ω of a certain point i in the point cloud relative to the center of mass position of the drone is:

[0035]

[0036] Where (x pi ,y pi) is the coordinate of point i in the point cloud, (x b ,y b ) is the coordinate of the center of mass of the drone;

[0037] S105: The ray occupied by the point cloud is expanded to both sides by a number n, where n is calculated as follows:

[0038]

[0039] Where R h The size of the circular envelope of the drone in the horizontal direction, r is the radius of the horizontal point cloud extraction range, δ is the angle resolution, S d is the safety threshold; the expanded rays are also marked as dangerous;

[0040] S106: Read the expected speed (v) of the current UAV remote controller joystick in the pitch and roll directions x ,v y ), and calculate the expected horizontal direction of the speed in the northeast sky coordinate system by combining the yaw angle φ of the UAV Distribute the direction to the rays with the nearest neighbor angles, where The calculation method is:

[0041]

[0042] S107: Determine whether the ray in the direction of the expected speed is marked as dangerous. If it is dangerous, the expected horizontal speed (v x ,v y ) are all set to zero, and if it is not dangerous, it is not modified (v x ,v y ) value, the drone moves normally in the horizontal direction;

[0043] S108: Install a distance measurement module directly above and below the drone, and read the two values ​​at a frequency of 10 Hz (d u ,d d ), when d u When the value is less than the threshold value D, if the joystick of the drone remote controller is in the vertical direction, the expected speed v z If v is greater than 0, z Set to zero; when d d When the value is less than the threshold value D, if the joystick of the drone remote controller is in the vertical direction, the expected speed v z If v is less than 0, z Set to zero.

[0044] Embodiment 2 of the present invention provides a method for controlling a closed space UAV remote control collision avoidance based on a point cloud, which only uses the current frame of the laser radar. In this embodiment, considering that it is impossible to equip some models with a powerful airborne processor, a point cloud queue of a certain length is stored by the method of embodiment 1. The large amount of data processed will cause the processor to run stuck or even crash. In this embodiment, only the current frame data of the laser radar is used for processing. Each time a frame of point cloud data is received, the point cloud within a certain range of the UAV in the frame point cloud is extracted. The specific range is a cylinder with the center of mass of the UAV as the center, r as the radius in the horizontal direction, and h as the height above and below the center of mass. The calculation method of r and h is:

[0045] r=R h +α*V h *M+S h

[0046] h=R v +S v

[0047] Where R h is the size of the circular envelope of the drone in the horizontal direction, R v is the maximum vertical dimension of the drone, M is the weight of the drone, V is h is the current horizontal flight speed of the UAV, S h is the horizontal safety threshold, S v is the vertical safety threshold, α is the horizontal scaling factor, which can be adjusted according to the actual test situation, and the subsequent processing method is consistent with the first embodiment.

[0048] Embodiment 3 of the present invention provides an embodiment of a method for controlling a remote control anti-collision of an unmanned aerial vehicle in a closed space based on a point cloud when the laser radar does not meet full coverage in the horizontal direction. In this embodiment, considering that the vertical field of view of the laser radar is limited, when the laser radar is needed to detect the situation below the space, the laser radar needs to be installed at a certain inclination angle, resulting in the inability to achieve full coverage in the horizontal direction. In the direction without a point cloud, a collision may occur through remote control control. For the situation in this embodiment, the processing method in the direction with a point cloud is consistent with that in embodiment 1. In the direction without a point cloud, the ray is marked as dangerous, and an expected horizontal speed threshold Vt is set. When it is detected that the expected horizontal speed in the direction without a point cloud is greater than the threshold, the expected speed may not be restricted. That is, when the operator confirms that there is no danger in the direction without a point cloud and hopes that the drone will move in that direction, the drone flight is controlled by setting the expected horizontal speed greater than the expected horizontal speed threshold by hitting the stick.

[0049] The above description is only a specific implementation of the present application, so that those skilled in the art can understand or implement the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest range consistent with the principles and novel features applied for herein.

Claims

1. A closed space UAV remote control anti-collision method based on point cloud, comprising the following steps: The UAV obtains the point cloud in the northeast sky coordinate system at a regular frequency of 10 Hz, establishes a point cloud queue with a length of L, and updates the queue once a frame of point cloud is received according to the first-in-first-out principle. At the same time, the updated point cloud queue is merged to obtain a set of point clouds to be processed. The point cloud set to be processed is extracted within a certain range. The specific range is a cylinder with the center of mass of the UAV as the center, the horizontal direction r as the radius, and the height h above and below the center of mass; Map the extracted point cloud to the xy plane, and perform voxel filtering on the mapped point cloud; In the horizontal direction, with the center of mass of the drone as the center of the circle and δ as the angle resolution, 360 / δ rays are fitted. The ray angle follows the direction of the northeast sky coordinate system. Each point in the processed point cloud is distributed to the nearest neighbor ray according to the angle Ω relative to the center of mass of the drone. The distributed rays are marked as dangerous. The ray occupied by the point cloud is expanded to both sides by a number n, and the expanded ray is also marked as dangerous; Read the expected speed of the current drone remote controller stick in the pitch and roll directions (v x ,v y ), combined with the yaw angle of the drone, it is converted into the horizontal direction of the expected speed in the northeast sky coordinate system, and the direction is distributed to the ray with the nearest neighbor angle to determine whether the ray is marked as dangerous. If it is dangerous, the expected horizontal speed (v x ,v y ) are all set to zero, and if it is not dangerous, it is not modified (v x ,v y ) value, the drone moves normally in the horizontal direction; A distance measurement module is installed directly above and below the drone, and the two values ​​(d u ,d d ), when d u When the value is less than the threshold value D, if the joystick of the drone remote controller is in the vertical direction, the expected speed v z If v is greater than 0, z Set to zero; when d d When the value is less than the threshold value D, if the joystick of the drone remote controller is in the vertical direction, the expected speed v z If v is less than 0, z Set to zero.

2. A closed space UAV remote control anti-collision method based on point cloud as claimed in claim 1, characterized in that: The radius r of the horizontal point cloud extraction range and the size R of the circular envelope of the drone in the horizontal direction h , UAV weight M, UAV current horizontal flight speed V h , safety threshold S h Regarding, specifically: r=R h +α*V h *M+S h Where α is the scaling factor, which can be adjusted according to the actual test situation; where h is the maximum size R of the drone in the vertical direction v , safety threshold S v Regarding, specifically: h=R v +S v 3. A closed space UAV remote control anti-collision method based on point cloud as claimed in claim 1, characterized in that The extracted point cloud is mapped to the xy plane, that is, each point in the point cloud is traversed and the z value of each point is set to 0; the mapped point cloud is voxel filtered, and the voxel filtering parameter (f x ,f y ,f z ) is related to the horizontal extraction point cloud range radius r and the angular resolution δ. Since it has been mapped to the xy plane, f z Just take a certain value, specific f x =f y =r*δ f z ＝0.1 4. A closed space UAV remote control anti-collision method based on point cloud as claimed in claim 1, characterized in that The angular resolution δ is related to the size of the dangerous object to be identified. The larger the size of the dangerous object, the larger δ is, and the smaller the size of the dangerous object, the smaller δ is. The specific calculation method of the angle Ω of a point i in the point cloud relative to the center of mass of the drone is: Where (x pi ,y pi ) is the coordinate of point i in the point cloud, (x b ,y b ) is the coordinate of the center of mass of the UAV.

5. The point cloud-based closed space UAV remote control anti-collision method of claim 1 is characterized in that The expansion number n and the size R of the circular envelope of the drone in the horizontal direction h , horizontal extraction point cloud range radius r, angle resolution δ, safety threshold S d Regarding, specifically:

Images