Quad-rotor unmanned aerial vehicle outdoor sensor fusion autonomous navigation system and method

Abstract

The invention relates to the technical field of unmanned aerial vehicle autonomous navigation, and discloses a quad-rotor unmanned aerial vehicle outdoor sensor fusion autonomous navigation system and method. The method specifically comprises the steps that the current position and the current attitude of the four-rotor unmanned aerial vehicle are obtained through a global positioning system and an inertial sensor; obstacle information is obtained through combined scanning of the 3D laser radar and the depth camera; and in the take-off stage, the server generates a minimum cost path according to the current position and the current attitude of the quadrotor unmanned aerial vehicle and the position of the target location, and indicates the flight control unit to control the quadrotor unmanned aerial vehicle to take off. In the flight process, the server instructs the flight control unit to control the quadrotor unmanned aerial vehicle to avoid the obstacle when scanning the obstacle, so that the safety of the vehicle body is ensured; meanwhile, the minimum-price path is optimized according to the current position, the current posture and the obstacle information of the quadrotor unmanned aerial vehicle, it is guaranteed that the quadrotor unmanned aerial vehicle can autonomously and efficiently move towards the target site and reach the target site with the lowest consumption, and cost is saved.

Description

Technical Field

[0001] The present application relates to the field of autonomous navigation technology for unmanned aerial vehicles (UAVs), and in particular to an outdoor sensor fusion autonomous navigation system and method for a quad-rotor UAV. Background Art

[0002] The realization of autonomous navigation of drones is inseparable from the support of a series of technologies, such as GNSS, inertial navigation systems, radar, cameras, etc. These sensors enable drones to accurately achieve positioning and navigation. In addition, they can be used to perceive the environment and obstacles, provide support for drones' obstacle avoidance, and provide a basis for drones' navigation decisions. The continuous development and integration of these background technologies provide drones with higher reliability, accuracy and safety for autonomous navigation.

[0003] However, in complex outdoor environments, such as urban or mountainous areas, drones struggle to plan appropriate flight paths to avoid collisions and comply with airspace restrictions. Furthermore, considering the drone's power system and energy constraints, improving flight efficiency while maintaining control performance is also a challenge. Furthermore, robust algorithms and systems must be designed to handle abnormal conditions such as sensor failures, communication interruptions, and inclement weather, ensuring that the drone can autonomously execute safety measures when encountering problems. Summary of the Invention

[0004] This application provides a quad-rotor UAV outdoor sensor fusion autonomous navigation system to solve the problems in the existing technology that UAVs have difficulty planning a suitable flight path and are unable to complete complex tasks for a long time due to the limitations of their own software functions.

[0005] Correspondingly, the present application also provides a quadrotor drone outdoor sensor fusion autonomous navigation method to ensure the implementation and application of the above system.

[0006] In order to solve the above technical problems, the present application discloses an outdoor sensor fusion autonomous navigation system for a quad-rotor drone, the system comprising a global positioning system, an inertial sensor, a 3D laser radar, a depth camera, a flight control unit, and a server;

[0007] The global positioning system, inertial sensor, 3D lidar, depth camera and flight control unit are all connected to the server;

[0008] A global positioning system and an inertial sensor are used to obtain the current position and current attitude of the quadrotor drone respectively;

[0009] 3D LiDAR and depth camera are used for joint scanning to obtain obstacle information; obstacle information includes point cloud images and locations of obstacles;

[0010] The server is used to generate a minimum cost path based on the current position, current posture and target location of the quadcopter before takeoff, and instruct the flight control unit to control the quadcopter to take off;

[0011] The server is also used to optimize the minimum cost path in real time according to the current position, current posture and obstacle information of the quadrotor drone during flight, and instruct the flight control unit to control the quadrotor drone to avoid the obstacle when an obstacle is scanned.

[0012] Preferably, the system further comprises an electric regulator and a motor;

[0013] Connect the motor to the ESC;

[0014] The ESC is connected to the flight control unit for receiving a start signal from the flight control unit and controlling the speed and / or direction of the motor according to the start signal.

[0015] This application also discloses a method for outdoor sensor fusion autonomous navigation of a quadrotor drone, the method comprising:

[0016] The current position and current attitude of the quadrotor drone are obtained through the global positioning system and inertial sensors respectively;

[0017] Obstacle information is obtained through joint scanning of 3D laser radar and depth camera; obstacle information includes point cloud images and locations of obstacles;

[0018] The server generates a minimum cost path based on the current position, current posture, and target location of the quadcopter before takeoff, and instructs the flight control unit to control the quadcopter to take off;

[0019] The server optimizes the minimum cost path in real time based on the quadrotor drone's current position, current posture, and obstacle information during flight, and instructs the flight control unit to control the quadrotor drone to avoid obstacles when an obstacle is scanned.

[0020] Preferably, the server generates a minimum cost path based on the current position, current posture, and target location of the quadrotor drone before takeoff, and instructs the flight control unit to control the quadrotor drone to take off, including:

[0021] Establish the world coordinate system of the quadrotor drone;

[0022] Calculate the minimum cost path based on the current position of the quadcopter in the world coordinate system before takeoff and the position of the target location;

[0023] Send a takeoff command to the flight control unit to instruct the flight control unit to control the quadcopter to take off.

[0024] Preferably, obstacle information is obtained by jointly scanning with a 3D laser radar and a depth camera, including:

[0025] Scan the surrounding environment through 3D lidar and build a point cloud map of the surrounding environment;

[0026] Obstacles are scanned by 3D lidar and depth camera, point cloud images of the obstacles are generated, and the positions of the obstacles in the point cloud map are obtained.

[0027] Preferably, the surrounding environment is scanned by a 3D laser radar, and a point cloud map of the surrounding environment is constructed, including:

[0028] Scan the surrounding environment with 3D laser radar to obtain multi-frame point clouds, and generate an initial point cloud map based on the multi-frame point clouds;

[0029] If the current frame point cloud produces motion distortion, the server matches the current frame point cloud with the next frame point cloud according to the curvature of the current frame point cloud to obtain a new next frame point cloud, and fuses the current frame point cloud and the new next frame point cloud with the initial point cloud map to obtain a new point cloud map;

[0030] If the current frame point cloud does not produce motion distortion, the server will fuse the current frame point cloud and the next frame point cloud with the initial point cloud map to obtain a new point cloud map.

[0031] Preferably, the obstacle is scanned by a 3D laser radar and a depth camera, a point cloud image of the obstacle is generated, and the position of the obstacle in the point cloud map is obtained, including:

[0032] Obstacles are scanned by 3D laser radar and depth camera, generating laser point cloud images and camera point cloud images respectively;

[0033] Use the server to denoise the camera point cloud image;

[0034] Each frame of the point cloud in the denoised camera point cloud image is fused with the point cloud of the corresponding frame in the laser point cloud image to obtain the point cloud image and position of the obstacle.

[0035] Preferably, the server optimizes the minimum cost path in real time according to the current position, current posture and obstacle information of the quadrotor drone during flight, and instructs the flight control unit to control the quadrotor drone to avoid the obstacle according to the optimized minimum cost path when encountering an obstacle, including:

[0036] When the 3D lidar and depth camera scan an obstacle, the server calculates the cost of the quadcopter flying from its current position to all positions adjacent to the obstacle, and uses the minimum cost and the corresponding position as the target adjacent position;

[0037] Instruct the flight control unit to control the quadrotor drone to fly to the target adjacent position and update the minimum cost path based on the target adjacent position.

[0038] Preferably, when the 3D laser radar and depth camera scan an obstacle, the server calculates the cost of the quadcopter flying from the current position to all positions adjacent to the obstacle, and uses the minimum cost and the corresponding position as the target adjacent position, including:

[0039] The server calculates the cost of the quadcopter flying from its current position to all positions adjacent to obstacles using the following formula:

[0040]

[0041] Where τ is the hysteresis factor, (s next_l_px , s next_l_py , s next_l_pz ) is the lth position adjacent to the obstacle, (x plane ,y plane , z plane ) is the current position of the quadrotor drone, cost next_l is the cost of the lth position adjacent to the obstacle;

[0042] Generate a set of adjacent position costs based on the cost of each position adjacent to the obstacle;

[0043] The minimum cost is selected from the set of adjacent position costs, and the position corresponding to the minimum cost is used as the target adjacent position.

[0044] Preferably, the hysteresis factor τ is expressed as:

[0045]

[0046] Where, (r plane ,p plane ,γ plane ) is the current attitude of the quadrotor drone obtained by the inertial sensor, r plane ,p plane ,γ plane are the roll angle, pitch angle, and yaw angle of the quadrotor drone, G is the universal gravitational constant, c is the air velocity in the area where the quadrotor drone is located, and E is the electric field strength in the area where the quadrotor drone is located.

[0047] This application has at least the following beneficial effects:

[0048] 1. The current position and current attitude of the quadcopter are obtained through the global positioning system and inertial sensors respectively; obstacle information is obtained through a joint scan of the 3D lidar and depth camera; obstacle information includes the point cloud image and position of the obstacle. During the takeoff phase, the server generates a minimum cost path based on the current position, current attitude and location of the quadcopter before takeoff, and instructs the flight control unit to control the quadcopter to take off. During the flight, when the server scans an obstacle, it instructs the flight control unit to control the quadcopter to avoid the obstacle to ensure the safety of the fuselage; at the same time, the minimum cost path is optimized based on the current position, current attitude and obstacle information of the quadcopter to ensure that the quadcopter can move to the target location autonomously and efficiently, and reach the target location with the lowest consumption, saving costs.

[0049] 2. By integrating multiple sensors such as the flight control unit, global positioning system, inertial sensor, 3D lidar and depth camera, the quadcopter can perceive the external environment, obtain its own position status in real time, and autonomously control its own flight status. The server can also plan the minimum cost path to the target location. This allows the quadcopter to autonomously perceive the environment outdoors, conduct autonomous and safe navigation, reduce human intervention, and improve the efficiency of autonomous navigation of outdoor drones.

[0050] 3. During the flight of the quadcopter, the 3D lidar scans and constructs a point cloud map of the surrounding environment, enabling the server to obtain the status of the quadcopter's surrounding environment in real time, facilitating the timely sending of instructions to the flight control unit, which in turn sends signals to the electronic speed controller to control the speed and direction of the motor, thereby ensuring that the quadcopter operates safely and smoothly.

[0051] 4. During the flight of the quadcopter, the 3D lidar scans obstacles simultaneously with the depth camera and generates a point cloud image of the obstacles. By effectively fusing the point cloud data scanned by the two sensors, the server can accurately obtain the outline of the obstacle and its position in the point cloud map, and send a signal to the flight control unit, allowing the flight control unit to control the quadcopter to smoothly avoid the obstacle and ensure the safety of the fuselage.

[0052] Additional aspects and advantages of the present application will be given in the following description, which will become apparent from the following description, or will be understood through practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0054] Figure 1 A schematic diagram of the structure of the outdoor sensor fusion autonomous navigation system for a quadrotor drone provided in an embodiment of the present application;

[0055] Figure 2 A flowchart for constructing a point cloud map using a 3D laser radar provided in an embodiment of the present application;

[0056] Figure 3 A flowchart of 3D laser radar and depth camera data scanning for obstacles provided in an embodiment of the present application;

[0057] Figure 4 A flowchart of the path planning for a quadcopter drone provided in an embodiment of the present application. DETAILED DESCRIPTION

[0058] The following describes embodiments of the present application in detail. Examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application and are not to be construed as limiting the present application.

[0059] It will be understood by those skilled in the art that, unless expressly stated otherwise, the singular forms "a", "an", "said" and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of this application refers to the presence of features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or combinations thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, "connected" or "coupled" as used herein may include wireless connections or wireless couplings. The term "and / or" used herein includes all or any units and all combinations of one or more associated listed items.

[0060] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by those skilled in the art in the art to which the present invention pertains. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art and, unless specifically defined as herein, will not be interpreted in an idealized or overly formal sense.

[0061] In response to the technical problems existing in the prior art, the quad-rotor drone outdoor sensor fusion autonomous navigation system and method provided in this application are intended to solve at least one of the technical problems of the prior art.

[0062] The following specific embodiments describe in detail the technical solution of the present application and how the technical solution of the present application solves the above-mentioned technical problems. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below in conjunction with the accompanying drawings.

[0063] The present application embodiment provides a quad-rotor UAV outdoor sensor fusion autonomous navigation system, such as Figure 1 As shown in , the system includes a global positioning system, inertial sensors, 3D lidar, depth camera, flight control unit and server;

[0064] The global positioning system, inertial sensor, 3D lidar, depth camera, and flight control unit are all connected to the server. The server can receive data collected by the global positioning system, 3D lidar, depth camera, and inertial sensor, and can communicate with the flight control unit, that is, it can send signals to the flight control unit and also receive signals from the flight control unit.

[0065] The global positioning system and inertial sensor are used to obtain the current position and current attitude of the quadcopter respectively; the full authority positioning system can also obtain the location of the target location;

[0066] 3D lidar and depth camera are used for joint scanning to obtain obstacle information; obstacle information includes point cloud images and locations of obstacles; 3D lidar is also used to scan the environment around the quadcopter and build a point cloud map.

[0067] The server is used to generate a minimum cost path based on the current position, current posture and target location of the quadcopter before takeoff, and instruct the flight control unit to control the quadcopter to take off;

[0068] The server is also used to optimize the minimum cost path in real time according to the current position, current posture and obstacle information of the quadrotor drone during flight, and instruct the flight control unit to control the quadrotor drone to avoid the obstacle when an obstacle is scanned.

[0069] For example, during takeoff, the global positioning system collects the current position of the quadcopter and the position of the target location, and the inertial sensor collects the current attitude of the quadcopter. The server receives the current position and the position of the target location sent by the full-authority positioning system, as well as the current attitude sent by the inertial sensor, and calculates the minimum cost path for the quadcopter to fly from the current position to the target location. The specific process can be:

[0070] Step 1.1. Based on the initial attitude sent by the inertial sensor, establish an O-XYZ rectangular coordinate system, named the world coordinate system W, with the projection of the quadrotor's center of gravity onto the ground as the origin O, the orientation of the quadrotor's nose as the X-axis, the orientation perpendicular to the quadrotor's nose as the Y-axis, and the orientation perpendicular to the ground as the Z-axis.

[0071] Step 1.2: Take the current position of the quadcopter (x plane ,y plane ,z plane ) as the starting point and the target location (x dest ,y dest ,z dest ) is set as the end point, the minimum cost path is initialized; and a take-off command is sent to the flight control unit, instructing the flight control unit to control the quadcopter to take off and fly towards the end point according to the initial minimum cost path.

[0072] Step 1.3, initialize the future_path and history_path collections in the server, and set the current position of the quadcopter (x plane ,y plane ,z plane ) is stored in the history_path collection.

[0073] During flight, the GPS and inertial sensors collect the quadcopter's current position and attitude in real time. The 3D LiDAR scans the surrounding environment in real time. When an obstacle is detected, the 3D LiDAR and depth camera simultaneously scan for obstacle information. After receiving data from the GPS, inertial sensors, 3D LiDAR, and depth camera, the server updates the minimum cost path. The specific process can be:

[0074] Step 1.4: During the flight, the 3D lidar constructs a point cloud map PL of the surrounding environment and saves the point cloud map PL to the server.

[0075] Step 1.5: If the quadcopter encounters an obstacle during flight, the 3D laser radar and depth camera will cur Start scanning obstacles synchronously to generate a point cloud image PL of the obstacle obst , and obtain the coordinates PL of the obstacle in the point cloud map PL obst (x,y,z).

[0076] Step 1.6, the server calculates the quadcopter's position from the current position (x plane ,y plane ,z plane )Fly to all positions adjacent to obstacles in the point cloud map {s next_lThe cost of |l∈[1,8]} next_l |l∈[1,8]}, and respectively from {s next_l |l∈[1,8]}, {cost next_l |l∈[1,8]} filter out the minimum cost cost min and its corresponding position s next_min .

[0077] Step 1.7, the server controls the quadcopter to fly to s next_min , will s next_min Added the history_path collection.

[0078] Step 1.8: Repeat steps 1.4 to 1.7 until the quadcopter reaches the target location. At this time, the server puts the target location into the set history_path, and the set history_path is the final minimum cost path s. init_min .

[0079] In the embodiments of the present application, by proposing to fuse multiple sets of sensor data to optimize the performance of the drone's outdoor autonomous navigation system, the quadcopter drone can autonomously perceive the environment outdoors, conduct autonomous and safe navigation, reduce human intervention, and improve the efficiency of outdoor drone autonomous navigation.

[0080] In an embodiment of the present application, the current position and current posture of the quadcopter are obtained respectively by the global positioning system and the inertial sensor; the obstacle information is obtained by the joint scanning of the 3D laser radar and the depth camera; the obstacle information includes the point cloud image and position of the obstacle. During the take-off phase, the server generates a minimum cost path based on the current position, current posture and location of the target location of the quadcopter before take-off, and instructs the flight control unit to control the quadcopter to take off. During the flight, when an obstacle is scanned, the server instructs the flight control unit to control the quadcopter to avoid the obstacle to ensure the safety of the fuselage. The server also optimizes the minimum cost path based on the current position, current posture and obstacle information of the quadcopter to ensure that the quadcopter can move to the target location autonomously and efficiently, and reach the target location with the lowest consumption, saving costs.

[0081] In an optional embodiment, the system further includes an electric regulator and a motor;

[0082] Connect the motor to the ESC;

[0083] The ESC is communicatively connected to the flight control unit to receive a start signal from the flight control unit and control the speed and / or direction of the motor according to the start signal.

[0084] The motor is connected to the rotor of the quadcopter, such as Figure 1As shown in , the quadcopter includes four motors (motor 1, motor 2, motor 3, and motor 4) and four ESCs (ESC 1, ESC 2, ESC 3, and ESC 4) in a one-to-one correspondence. During takeoff, the server sends instructions to the flight control unit, which transmits a start signal to the ESCs. The ESCs receive the start signal and send control instructions to the motors. The motors rotate to generate lift for the rotors, causing the quadcopter to take off. During flight, the server sends instructions to the flight control unit based on the quadcopter's current position and obstacle information obtained by the inertial sensor. The flight control unit transmits a start signal to the ESCs. The ESCs receive the start signal and change the motor speed. The motors adjust their own steering to drive the quadcopter to avoid obstacles.

[0085] In an embodiment of the present application, the server can also receive signals sent by the flight control unit, the electronic speed controller, the four independently driven rotors, and the motor.

[0086] The various processes implemented in the quadrotor drone outdoor sensor fusion autonomous navigation system provided in the embodiment of the present application can be referred to the following quadrotor drone outdoor sensor fusion autonomous navigation method. To avoid repetition, they will not be described here.

[0087] Based on the same principle as the system provided in the embodiment of the present application, the embodiment of the present application also provides a quadrotor drone outdoor sensor fusion autonomous navigation method, which is implemented based on the above system and includes:

[0088] The current position and current attitude of the quadrotor drone are obtained through the global positioning system and inertial sensors respectively;

[0089] Obstacle information is obtained through joint scanning of 3D laser radar and depth camera; obstacle information includes point cloud images and locations of obstacles;

[0090] The server generates a minimum cost path based on the current position, current posture, and target location of the quadrotor drone before takeoff, and instructs the flight control unit to control the quadrotor drone to take off.

[0091] The server optimizes the minimum cost path in real time based on the quadrotor drone's current position, current posture, and obstacle information during flight, and instructs the flight control unit to control the quadrotor drone to avoid obstacles when an obstacle is scanned.

[0092] In the embodiment of the present application, during the take-off phase, the server receives the remotely set target location (x dest ,y dest ,z dest ), and according to the target location (x dest ,y dest ,z dest) and the current position of the quadrotor drone obtained through the global positioning system (x plane ,y plane ,z plane ), initialize the minimum cost path from the current position to the target location; then send instructions to the flight control unit to control the quadcopter to take off.

[0093] During the flight, the full-authority positioning system and inertial sensor obtain the current position of the quadcopter (x plane ,y plane ,z plane ) and current posture (r plane ,p plane ,γ plane ), 3D laser radar and depth camera scan in real time to obtain obstacle information, and the server updates the minimum cost path s based on the real-time data received from the above sensors init_min Among them, when the quadcopter encounters an obstacle, the server instructs the flight control unit to control the quadcopter to avoid the obstacle and plan the local minimum cost path s to the target location. re_min , to update the minimum cost path s init_min The local minimum cost path s re_min It will not deviate from the target location, ensuring the safe operation of the quadrotor drone.

[0094] In an embodiment of the present application, the rotors of a quadcopter drone are connected to motors, which can be used to drive the quadcopter drone to take off or turn. The motors are connected to electronic speed controllers (ESCs), which can be used to control the motor's speed and / or direction. The ESCs are in communication with a flight control unit and are controlled by a start signal transmitted by the flight control unit. Based on this, the server instructs the flight control unit to control the quadcopter drone to take off or avoid obstacles. In this way, the server can specifically implement the following: during the takeoff phase, the server sends a command to the flight control unit, which transmits a start signal to the ESCs. The ESCs receive the start signal and send control commands to the motors. The motors rotate to generate lift on the rotors, causing the quadcopter drone to take off. During flight, the server sends a command to the flight control unit based on the quadcopter's current position and obstacle information obtained by the inertial sensor. The flight control unit transmits a start signal to the ESCs. The ESCs receive the start signal and change the motor's speed. The motors adjust their own direction, driving the quadcopter drone to avoid obstacles.

[0095] The embodiments of the present application optimize the performance of the drone's outdoor autonomous navigation system by fusing multiple sets of sensor data, thereby enabling the quadcopter to autonomously perceive the environment outdoors, conduct autonomous and safe navigation, reduce human intervention, and improve the efficiency of outdoor drone autonomous navigation.

[0096] In the embodiment of the present application, the current position and current attitude of the quadcopter are obtained respectively by the global positioning system and the inertial sensor; the obstacle information is obtained by the joint scanning of the 3D laser radar and the depth camera; the obstacle information includes the point cloud image and position of the obstacle. During the take-off phase, the server generates a minimum cost path based on the current position, current attitude and the position of the target location of the quadcopter before take-off, and instructs the flight control unit to control the quadcopter to take off. During the flight, when the server scans an obstacle, it instructs the flight control unit to control the quadcopter to avoid the obstacle to ensure the safety of the fuselage; at the same time, the minimum cost path is optimized based on the current position, current attitude and obstacle information of the quadcopter to ensure that the quadcopter can move to the target location autonomously and efficiently, and reach the target location with the lowest consumption, saving costs.

[0097] In an optional embodiment, the server generates a minimum cost path based on the current position, current posture, and target location of the quadrotor drone before takeoff, and instructs the flight control unit to control the quadrotor drone to take off, including:

[0098] Establish the world coordinate system of the quadrotor drone;

[0099] Calculate the minimum cost path based on the current position of the quadcopter in the world coordinate system before takeoff and the position of the target location;

[0100] Send a takeoff command to the flight control unit to instruct the flight control unit to control the quadcopter to take off.

[0101] The specific implementation steps are as follows:

[0102] Step 2.1. Establish an O-XYZ rectangular coordinate system with the projection of the quadcopter's center of gravity onto the ground as the origin O, the orientation of the quadcopter's nose as the X-axis, the orientation perpendicular to the quadcopter's nose as the Y-axis, and the orientation perpendicular to the ground as the Z-axis. Name this the world coordinate system W.

[0103] Step 2.2, take the current position of the quadcopter (x plane ,y plane ,z plane ) as the starting point and the target location (x dest ,y dest ,z dest ) is set as the end point, the minimum cost path is initialized, and a takeoff command is sent to the flight control unit, instructing the flight control unit to control the quadrotor drone to take off and fly towards the end point along the initial minimum cost path.

[0104] Step 2.3, initialize the future_path and history_path collections in the server, and set the current position of the quadcopter (xplane ,y plane ,z plane ) is stored in the history_path collection.

[0105] In an optional embodiment, obstacle information is obtained by jointly scanning with a 3D laser radar and a depth camera, including:

[0106] Scan the surrounding environment through 3D lidar and build a point cloud map of the surrounding environment;

[0107] During the quadrotor's flight, the 3D LiDAR constructs a point cloud map PL of the surrounding environment and saves it to the server. In this embodiment, the point cloud map of the surrounding environment scanned and constructed by the 3D LiDAR is uploaded to the server's message cache queue in real time. This message cache queue only receives the latest point cloud map. Upon receiving a newly uploaded point cloud map, the previous point cloud map is cleared, thereby reducing memory load and ensuring the normal operation of the quadrotor.

[0108] Obstacles are scanned by 3D lidar and depth camera, point cloud images of the obstacles are generated, and the positions of the obstacles in the point cloud map are obtained.

[0109] In the embodiment of the present application, if the quadcopter encounters an obstacle during flight, the 3D laser radar and the depth camera are at the current time t cur Start scanning obstacles synchronously to generate a point cloud image PL of the obstacle obst , and obtain the coordinates PL of the obstacle in the point cloud map PL obst (x,y,z).

[0110] In an optional embodiment, the surrounding environment is scanned by a 3D laser radar, and a point cloud map of the surrounding environment is constructed, including:

[0111] Scan the surrounding environment with 3D laser radar to obtain multi-frame point clouds, and generate an initial point cloud map based on the multi-frame point clouds;

[0112] If the current frame point cloud produces motion distortion, the server matches the current frame point cloud with the next frame point cloud according to the curvature of the current frame point cloud to obtain a new next frame point cloud, and fuses the current frame point cloud and the new next frame point cloud with the initial point cloud map to obtain a new point cloud map;

[0113] If the current frame point cloud does not produce motion distortion, the server will fuse the current frame point cloud and the next frame point cloud with the initial point cloud map to obtain a new point cloud map.

[0114] In the embodiment of the present application, the server starts the A-LOAM algorithm, takes the n-frame point cloud data scanned by the 3D laser radar as the algorithm input, and introduces the positioning data of the global positioning system and inertial sensor to integrate with the n-frame point cloud data, thereby constructing a high-precision point cloud map. n .like Figure 2 As shown, the specific implementation steps are as follows:

[0115] Step 3.1, Data Collection: The 3D LiDAR of the quadcopter scans n frames of point cloud and generates a point cloud map n Wherein, the point cloud map n ={(x point_r ,y point_r ,z point_r ) T |r=1,2,…n}, and map n Any r-th frame point cloud contained in is represented as (x point_r ,y point_r ,z point_r ) T .

[0116] Step 3.2: Determine whether the i-th frame point cloud has motion distortion. Assume that any i-th frame point cloud scanned by the 3D lidar has motion distortion. The server pre-processes the i-th frame point cloud as shown in formula (1-1):

[0117]

[0118] In this embodiment, the 3D lidar itself is affected by sparsity and motion disturbances, resulting in reduced perception accuracy, and in some cases of 3D lidar degradation, the number of perception points is severely reduced, which greatly affects the observation accuracy of the point cloud scanned by the 3D lidar.

[0119] Step 3.3, feature extraction: Calculate the curvature k of any i-th frame point cloud after the preprocessing in step 3.3. Assume that for any i-th frame point cloud i, its neighborhood point set is N(p) = {p1, p2, ... p m}, where m is the number of neighborhood points. Then, the local fitting plane equation at the point cloud p of the i-th frame can be obtained by formula (1-2):

[0120] z point_pro_i =ax point_pro_i +by point_pro_i +c (1-2)

[0121] In formula (1-2), a, b, and c are the parameters of the fitting plane, and a, b, and c are not all 0. The curvature k is shown in formula (1-3):

[0122]

[0123] In this embodiment, point cloud data is typically very large, containing a large number of points. Feature extraction can reduce the high-dimensional point cloud data to a low-dimensional feature representation, thereby reducing storage and computational costs. Furthermore, by extracting local features of the point cloud, such as descriptors and feature points, point cloud matching and alignment can be achieved.

[0124] Step 3.4, point cloud matching: Match the i-th frame point cloud after feature extraction in step 3.3 with the i+1-th frame point cloud to obtain a new i+1-th frame point cloud, as shown in formula (1-4):

[0125]

[0126] In formula (1-4), R, t are the rotation matrix and translation matrix required to transform the i-th frame point cloud to the new i+1-th frame point cloud, and the new i+1-th frame point cloud is (x point_re_(i+1) ,y point_re_(i+1) ,z poin t _re_(i+1) ) T In this embodiment of the present application, the server uses the Eigen library to accelerate the calculation of rotation matrices and translation matrices, thereby improving the efficiency of point cloud matching. The registered point cloud data can improve the processing efficiency of scene map reconstruction and the accuracy of scene restoration.

[0127] Step 3.5, map update: merge the i-th frame point cloud generated by step 3.4, the i+1-th frame point cloud matched by step 3.4, and the point cloud map generated by step 3.1 to obtain a new point cloud map re_n , as shown in formula (1-5):

[0128]

[0129] In this embodiment, gradually adding new point cloud data to the point cloud map and performing map fusion can keep the map information up to date, and incremental map construction only needs to process the newly added point cloud data without having to reprocess the point cloud data, which can save a lot of computing resources and improve the response speed of the system.

[0130] Step 3.6: If the point cloud is not distorted, only steps 3.1 and 3.5 are executed until the point cloud map is obtained. re_n If the construction is complete, repeat steps 3.1 to 3.5 until the point cloud map is complete. re_n Build complete.

[0131] In an optional embodiment, obstacles are scanned by a 3D laser radar and a depth camera to generate a point cloud image of the obstacle and obtain the position of the obstacle in the point cloud map, including:

[0132] Obstacles are scanned by 3D laser radar and depth camera, generating laser point cloud images and camera point cloud images respectively;

[0133] Use the server to denoise the camera point cloud image;

[0134] Each frame of the point cloud in the denoised camera point cloud image is fused with the point cloud of the corresponding frame in the laser point cloud image to obtain the point cloud image and position of the obstacle.

[0135] like Figure 3 As shown, the specific implementation steps are as follows:

[0136] Step 4.1: The 3D laser radar and the depth camera jointly scan the obstacle and generate point cloud images L_O and D_O respectively. The point cloud α of the u-th frame in the point cloud image L_O can be expressed as (L_O x ,L_O y ,L_O z ) T , the u-th frame point cloud β in the point cloud image D_O can be expressed as (D_O x ,D_O y ,D_O z ) T , and u∈[1,n];

[0137] Step 4.2: Denoise the point cloud image D_O generated by the depth camera scanning the obstacle, and obtain the u-th frame point cloud I in the denoised point cloud image D_O. smooth_u (x, y, z), as shown in formula (1-6):

[0138]

[0139] In formula (1-6), w j represents the Gaussian weight, M represents the total number of point clouds contained in the obstacle point cloud image, and I smooth_u (x, y, z) can be further expressed as formula (1-7):

[0140] I smooth_u (x,y,z)=(D_O smooth_x ,D_O smooth_y ,D_O smooth_z ) T (1-7)

[0141] In this embodiment, by denoising the point cloud scanned by the depth camera, the quality of the point cloud data can be improved, the errors and uncertainties caused by noise can be reduced, and subsequent processing and analysis can be more accurate and reliable. In addition, when constructing the obstacle point cloud image together with the point cloud scanned by the lidar, denoising can make the obstacle point cloud image more detailed and accurate, avoiding the discontinuity or irregularity of the geometric shape caused by noise interference.

[0142] Step 4.3: The u-th frame point cloud I in the point cloud image D_O obtained in step 4.2 smooth_u (x, y, z) and the u-th frame point cloud (L_O) in the point cloud image L_O generated by 3D laser radar scanning obstacles x ,L_O y ,L_O z ) T Perform weighted fusion, and the fused point cloud of the u-th frame is shown in formula (1-8):

[0143] I fusion_u (x,y,z)=θ1*(L_O x ,L_O y ,L_O z ) T +θ2*I smooth_u (x,y,z) (1-8)

[0144] In formula (1-8), θ1 and θ2 are feature weighting factors, and both θ1 and θ2 are not zero.

[0145] Step 4.4: Repeat steps 4.1 to 4.3 until all the frame point clouds of the sensor are weighted and fused, and the point cloud image of the obstacle and the corresponding coordinates PL are obtained. obst ={I fusion_u (x,y,z)|u=1,2,…n}.

[0146] In an embodiment, the point cloud images scanned by the lidar and the depth camera can complement each other's shortcomings. By fusing the data of the two, a more accurate and complete three-dimensional reconstruction result can be obtained, thereby improving the reconstruction quality and the authenticity of the geometric shape, thereby assisting the quadcopter to perform more reliable obstacle identification and improve the safety of autonomous navigation.

[0147] In an optional embodiment, the server optimizes the minimum cost path in real time based on the current position, current posture, and obstacle information of the quadrotor drone during flight, and instructs the flight control unit to control the quadrotor drone to avoid the obstacle along the optimized minimum cost path when encountering an obstacle, including:

[0148] When the 3D lidar and depth camera scan an obstacle, the server calculates the cost of the quadcopter flying from its current position to all positions adjacent to the obstacle, and uses the minimum cost and the corresponding position as the target adjacent position;

[0149] Instruct the flight control unit to control the quadrotor drone to fly to the target adjacent position and update the minimum cost path based on the target adjacent position.

[0150] like Figure 4 As shown, based on the initialization of the path sets future_path and history_path in step 2.3, the specific implementation steps are as follows:

[0151] Step 5.1, taking the case where there are 8 positions adjacent to obstacles in the point cloud map PL as an example: the server stores the current position of the quadcopter into history_path and stores the positions adjacent to obstacles in the point cloud map PL into history_path. next_l |l∈[1,8]} is stored in the set future_path, and the quadrotor drone is calculated from the current position (x plane ,y plane ,z plane )Fly to all positions adjacent to obstacles in the point cloud map {s next_l The cost of |l∈[1,8]} next_l |l∈[1,8]}, and respectively from {s next_l |l∈[1,8]}, {cost next_l |l∈[1,8]} filter out the minimum cost cost min and its corresponding position s next_min ;

[0152] Step 5.2: The server controls the quadcopter to fly to the position s corresponding to the minimum cost. next_min , will s next_min Add the history_path collection and clear the future_path collection;

[0153] Step 5.3: Obstacle information is obtained through real-time scanning with 3D laser radar and depth camera, and steps 5.1 to 5.2 are repeated on the server until the quadcopter reaches the target location. At this time, the server puts the target location into the set history_path and outputs it. The final output set history_path is the minimum cost path s init_min .

[0154] In an optional embodiment, when the 3D laser radar and depth camera scan an obstacle, the server calculates the cost of the quadcopter flying from the current position to all positions adjacent to the obstacle, and uses the minimum cost and the corresponding position as the target adjacent position, including:

[0155] The server calculates the cost of the quadcopter flying from its current position to all positions adjacent to obstacles using the following formula:

[0156]

[0157] Where τ is the hysteresis factor, (s next_l_px , s next_l_py , s next_l_pz ) is the lth position adjacent to the obstacle, (x plane ,y plane , z plane ) is the current position of the quadrotor drone, cost next_l is the cost of the lth position adjacent to the obstacle;

[0158] Generate a set of adjacent position costs based on the cost of each position adjacent to the obstacle;

[0159] The minimum cost is selected from the set of adjacent position costs, and the position corresponding to the minimum cost is used as the target adjacent position.

[0160] In the embodiment of this application, the specific implementation method is as follows:

[0161] Step 6.1. Get the position of the quadrotor drone in the world coordinate system W (x plane ,y plane ,z plane );

[0162] Step 6.2: Convert all coordinates adjacent to the obstacle coordinates from the coordinate system of the point cloud map to the world coordinate system W. Assume {s next_l The coordinates of any location in |l∈[1,8]} based on the point cloud map coordinate system can be expressed as (s next_l_px ,s next_l_py ,s next_l_pz ), then any location s next_l The coordinates after the coordinates are transformed into the world coordinate system W are shown in formula (1-10):

[0163]

[0164] Step 6.3, continue to execute step 6.2, and change {s next_l The coordinates of all locations in |l∈[1,8]} are converted to the world coordinate system W;

[0165] Step 6.4: Calculate the cost using the above formula (1-9) next_l ;

[0166] Step 6.5, repeat steps 6.1 to 6.4 to calculate {cost next_l |l∈[1,8]}, and get the cost min = min ({cost next_l |l∈[1,8]}) and the corresponding location s next_min .

[0167] In an optional embodiment, in formula (1-9), the hysteresis factor τ is expressed as:

[0168]

[0169] Where, (r plane ,p plane ,γ plane ) is the current attitude of the quadrotor drone obtained by the inertial sensor, r plane ,p plane ,γ plane are the roll angle, pitch angle, and yaw angle of the quadrotor drone, G is the universal gravitational constant, c is the air velocity in the area where the quadrotor drone is located, and E is the electric field strength in the area where the quadrotor drone is located.

[0170] In the embodiments of the present application, consideration of environmental factors can help the quadcopter avoid potential risks and dangers, ensure the safety of the quadcopter during flight, and choose an effective path that can avoid obstacles to prevent the quadcopter from being disturbed and causing unstable flight status.

[0171] In this embodiment, designing a minimum cost path for autonomous navigation of the drone is beneficial to saving energy and costs, enhancing flight stability, optimizing mission execution, adapting to complex environments, improving safety, and reducing the impact on the environment. These advantages make the autonomous navigation of the quadcopter drone in various scenarios more efficient, reliable, and sustainable.

[0172] The above description is merely a preferred embodiment of the present application and an illustration of the technical principles employed. Those skilled in the art should understand that the scope of disclosure in this application is not limited to the technical solutions formed by a specific combination of the above-mentioned technical features, but also encompasses other technical solutions formed by any combination of the above-mentioned technical features or their equivalents without departing from the above-mentioned disclosed concepts. For example, a technical solution formed by replacing the above-mentioned features with (but not limited to) technical features with similar functions disclosed in this application.

Claims

1. A quadrotor drone outdoor sensor fusion autonomous navigation system, characterized by: The system includes a global positioning system, an inertial sensor, a 3D lidar, a depth camera, a flight control unit, and a server; The global positioning system, the inertial sensor, the 3D laser radar, the depth camera and the flight control unit are all communicatively connected to the server; The global positioning system and the inertial sensor are used to respectively obtain the current position and current posture of the quadrotor drone; The 3D laser radar and the depth camera are used for joint scanning to obtain obstacle information; The obstacle information includes a point cloud image and a position of the obstacle; The server is configured to generate a minimum cost path based on the current position, current posture, and target location of the quadrotor drone before takeoff, and instruct the flight control unit to control the quadrotor drone to take off; The server is further configured to optimize the minimum cost path in real time according to the current position, current posture and obstacle information of the quadrotor drone during flight, and to instruct the flight control unit to control the quadrotor drone to avoid the obstacle when an obstacle is scanned.

2. The outdoor sensor fusion autonomous navigation system for a quadrotor drone according to claim 1 is characterized in that: The system also includes an electric regulator and a motor; The motor is connected to the electric regulator; The ESC is communicatively connected to the flight control unit for receiving a start signal transmitted by the flight control unit and controlling the speed and / or direction of the motor according to the start signal.

3. A method for outdoor sensor fusion autonomous navigation of a quadrotor drone, characterized in that: The method comprises: The current position and current attitude of the quadrotor drone are obtained through the global positioning system and inertial sensors respectively; Obstacle information is obtained through joint scanning of 3D laser radar and depth camera; the obstacle information includes point cloud images and positions of obstacles; The server generates a minimum cost path based on the current position, current posture, and target location of the quadcopter before takeoff, and instructs the flight control unit to control the quadcopter to take off; The server optimizes the minimum cost path in real time according to the current position, current posture and obstacle information of the quadrotor drone during flight, and instructs the flight control unit to control the quadrotor drone to avoid the obstacle when an obstacle is scanned.

4. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 3 is characterized in that: The server generates a minimum cost path based on the current position, current posture, and target location of the quadrotor drone before takeoff, and instructs the flight control unit to control the quadrotor drone to take off, including: Establish the world coordinate system of the quadrotor drone; Calculating the minimum cost path based on the current position of the quadrotor drone in the world coordinate system and the position of the target location before takeoff; Send a takeoff instruction to the flight control unit to instruct the flight control unit to control the quadrotor drone to take off.

5. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 3 is characterized in that: Obstacle information is obtained by jointly scanning with a 3D laser radar and a depth camera, including: Scanning the surrounding environment by using the 3D laser radar and constructing a point cloud map of the surrounding environment; Obstacles are scanned by the 3D laser radar and the depth camera to generate a point cloud image of the obstacle, and the position of the obstacle in the point cloud map is obtained.

6. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 5 is characterized in that: The 3D laser radar is used to scan the surrounding environment and construct a point cloud map of the surrounding environment, including: Scanning the surrounding environment with the 3D laser radar to obtain multi-frame point clouds, and generating an initial point cloud map based on the multi-frame point clouds; If the current frame point cloud produces motion distortion, the server matches the current frame point cloud with the next frame point cloud according to the curvature of the current frame point cloud to obtain a new next frame point cloud, and fuses the current frame point cloud and the new next frame point cloud with the initial point cloud map to obtain a new point cloud map; If the current frame point cloud does not produce motion distortion, the server will fuse the current frame point cloud and the next frame point cloud with the initial point cloud map to obtain a new point cloud map.

7. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 5, characterized in that: Scanning obstacles using the 3D laser radar and the depth camera to generate a point cloud image of the obstacle and obtaining the position of the obstacle in the point cloud map includes: Scan obstacles by using the 3D laser radar and the depth camera to generate a laser point cloud image and a camera point cloud image respectively; De-noising the camera point cloud image using a server; Each frame of point cloud in the denoised camera point cloud image is fused with the point cloud of the corresponding frame in the laser point cloud image to obtain the point cloud image and position of the obstacle.

8. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 3 or 4, characterized in that: The server optimizes the minimum cost path in real time according to the current position, current posture and obstacle information of the quadrotor drone during flight, and instructs the flight control unit to control the quadrotor drone to avoid the obstacle according to the optimized minimum cost path when encountering an obstacle, including: When the 3D laser radar and the depth camera scan an obstacle, the server calculates the cost of the quadcopter flying from the current position to all positions adjacent to the obstacle, and uses the minimum cost and the corresponding position as the target adjacent position; Instruct the flight control unit to control the quadrotor drone to fly to the target adjacent position, and update the minimum cost path according to the target adjacent position.

9. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 8, characterized in that: When the 3D laser radar and the depth camera scan an obstacle, the server calculates the cost of the quadcopter flying from the current position to all positions adjacent to the obstacle, and uses the minimum cost and the corresponding position as the target adjacent position, including: The server calculates the cost of the quadcopter flying from its current position to all positions adjacent to obstacles using the following formula: Where τ is the hysteresis factor, (s next_l_px , s next_l_py , s next_l_pz ) is the lth position adjacent to the obstacle, (x plane ,y plane , z plane ) is the current position of the quadrotor drone, cost next_l is the cost of the lth position adjacent to the obstacle; Generate a set of adjacent position costs based on the cost of each position adjacent to the obstacle; The minimum cost is screened out from the adjacent position cost set, and the position corresponding to the minimum cost is used as the target adjacent position.

10. The outdoor sensor fusion autonomous navigation method for a quadrotor drone according to claim 9, characterized in that: The hysteresis factor τ is expressed as: In the formula, (r plane ,p plane ,γ plane ) is the current attitude of the quadrotor drone obtained by the inertial sensor, r plane ,p plane ,γ plane are the roll angle, pitch angle, and yaw angle of the quadrotor drone, G is the universal gravitational constant, c is the air velocity in the area where the quadrotor drone is located, and E is the electric field strength in the area where the quadrotor drone is located.

Images