Unmanned aerial vehicle fusion mapping and positioning method and device, computer equipment and storage medium

Abstract

The invention relates to an unmanned aerial vehicle fusion mapping and positioning method and device, computer equipment and a storage medium. A solid-state laser radar and a rotatable support are arranged on the unmanned aerial vehicle, the rotatable support drives the solid-state laser radar to rotate, and the method comprises the following steps: obtaining an original laser point cloud data frame according to the solid-state laser radar; obtaining IMU data according to the body IMU of the unmanned aerial vehicle; preprocessing the original laser point cloud data frame and the IMU data to obtain preprocessed data; according to the preprocessed data, the pose of the unmanned aerial vehicle is obtained; according to the unmanned aerial vehicle pose and the preprocessed data, obtaining a double-layer map; wherein the double-layer map comprises a sparse feature map and a visual dense voxel map. The precision of the unmanned aerial vehicle can be improved, and the mass of the unmanned aerial vehicle is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned aerial vehicles, and in particular to an unmanned aerial vehicle fusion mapping and positioning method and device, computer equipment and a storage medium. BACKGROUND

[0002] The flight, positioning and map construction of unmanned aerial vehicles rely on the cooperation of laser radars and other devices.

[0003] Laser radars are generally divided into rotating laser radars and solid-state laser radars. The rotating laser radar realizes 360-degree scanning of laser through the rotation of a laser inside the laser radar. The rotating laser radar is usually 32 lines or 64 lines. Due to the volume of the laser emitter itself in the laser radar, the volume of the optical equipment matched in the laser radar, and the volume of the circuit, it is difficult for the rotating laser radar to realize more lines, which restricts the resolution of the rotating laser radar.

[0004] The solid-state laser radar usually realizes laser scanning through a high-speed vibrating galvanometer, so it can have very high resolution. However, the detection field of view of the solid-state laser radar is limited, usually 120 degrees to the left and right, and it is difficult to achieve comprehensive detection.

[0005] To solve the limitation of the field of view of the solid-state laser radar, multiple solid-state laser radars can be set up to achieve a 360-degree field of view. However, the solid-state laser radar has a large volume and heavy weight, and the cost is much higher than that of the rotating laser radar, so it is difficult to configure multiple solid-state laser radars on the unmanned aerial vehicle.

[0006] It can be seen that the unmanned aerial vehicle in the prior art is difficult to realize high-precision fusion mapping and positioning through a solid-state laser radar. SUMMARY

[0007] In order to solve the above technical problems or at least partially solve the above technical problems, the present application provides an unmanned aerial vehicle fusion mapping and positioning method, device, computer equipment and storage medium.

[0008] In a first aspect, the present application provides an unmanned aerial vehicle fusion mapping and positioning method, wherein a solid-state laser radar and a rotatable support are arranged on the unmanned aerial vehicle, the rotatable support drives the solid-state laser radar to rotate, and the method comprises: acquiring an original laser point cloud data frame according to the solid-state laser radar; acquiring IMU data according to the body IMU of the unmanned aerial vehicle; preprocessing the original laser point cloud data frame and the IMU data to obtain preprocessed data; acquiring the unmanned aerial vehicle pose according to the preprocessed data; acquiring a double-layer map according to the unmanned aerial vehicle pose and the preprocessed data; The double-layer map comprises a sparse feature map and a visual dense voxel map.

[0009] Optionally, the raw laser point cloud data frame is acquired according to the solid-state laser radar, and the method comprises: According to the phase of the rotatable support, the field of view corresponding to one rotation of the solid-state laser radar is divided into a plurality of angular windows, and the boundary between adjacent two angular windows is taken as an angular window boundary. The solid-state laser radar rotates under the driving of the rotatable support and acquires raw point cloud data. The raw point cloud data is divided into a plurality of quasi-spiral sub-frames according to the angular window boundary. The plurality of quasi-spiral sub-frames constitute the raw laser point cloud data frame.

[0010] Optionally, the width constraint condition of the angular window is: The linear displacement of each quasi-spiral sub-frame does not exceed a maximum linear displacement, and the angular displacement of each quasi-spiral sub-frame does not exceed a maximum angular displacement.

[0011] Optionally, the field of view corresponding to one rotation of the solid-state laser radar is divided into a plurality of angular windows according to the phase of the rotatable support, and the method comprises: According to the shaking degree during flight of the unmanned aerial vehicle, the width of the angular window is adaptively adjusted. The shaking degree during flight is greater, and the width of the angular window is smaller.

[0012] Optionally, the width constraint condition of the angular window is: The phase span of the angular window is as follows: wherein, is the width of the angular window, is the maximum linear displacement, is the average linear speed of the rotating motor, is the maximum angular displacement, is the average angular speed of the rotating motor, is the laser radar electric frequency, is the minimum number of points of a single quasi-spiral sub-frame, is the phase span, is the starting phase of the m-th quasi-spiral sub-frame, is the starting phase of the m-th quasi-spiral sub-frame.

[0013] Optionally, the raw laser point cloud data frame and the IMU data have a time stamp, ​​The pre-processing of the original laser point cloud data frame and the IMU data comprises: Each of the corner window boundaries is taken as a completion phase of a current quasi-spiral frame and a starting phase of a next quasi-spiral frame, and the starting phase of the current quasi-spiral frame is taken as a reference time; According to the timestamps, the original point cloud in each of the quasi-spiral frames is rolled back to a corresponding reference time according to the IMU data to obtain a rolled-back data frame; According to the IMU rotation rate, a time deviation of the rolled-back data frame is compensated to remove distortion in the point cloud data to obtain a de-distorted data frame; According to voxel filtering, statistical cluster point removal filtering and intensity threshold filtering, the de-distorted data frame is filtered to obtain pre-processed data; The rolled-back data frame is phase-aligned with the IMU data.

[0014] Optionally, the compensation of the time deviation of the rolled-back data frame according to the IMU rotation rate to remove distortion in the point cloud data to obtain a de-distorted data frame comprises: An encoder angular velocity is obtained according to the IMU rotation rate; According to the pre-integration of the IMU data and the encoder angular velocity, a relative pose of the point cloud of the rolled-back data frame at the reference time of the quasi-spiral frame is obtained; Through coordinate transformation, the point cloud of all the rolled-back data frames is transformed to a radar coordinate system or a body coordinate system to obtain a corrected point cloud consistent in space and time as the de-distorted data frame.

[0015] Optionally, the coordinate transformation of the point cloud of all the rolled-back data frames to the radar coordinate system or the body coordinate system comprises: An extrinsic parameter matrix between the solid-state laser radar and the body IMU is obtained; A current angle of the rotatable support is obtained; According to the current angle of the rotatable support and the current extrinsic parameter matrix, the point cloud of the rolled-back data frame is mapped from the radar coordinate system to the body coordinate system or from the body coordinate system to the radar coordinate system.

[0016] Optionally, the obtaining of the UAV pose according to the pre-processed data comprises: According to the pre-integration of the IMU data, an inter-frame motion initial value is obtained, and a state prediction is obtained according to the inter-frame motion initial value; According to the state prediction, the current quasi-spiral frame and the adjacent quasi-spiral frame are quickly registered to obtain a coarse pose; According to the coarse pose, a plurality of continuous quasi-spiral subframes are aggregated to form a local submap; The local submap is precisely registered with a historical submap to obtain an accurate pose.

[0017] Optionally, the double-layer map is obtained according to the pose of the unmanned aerial vehicle and the preprocessed data, and the method further comprises: According to the state prediction, the current quasi-spiral subframe is quickly registered with the adjacent quasi-spiral subframe to obtain a sparse feature map, and the sparse feature map comprises line features, surface features, feature covariance, phase coverage histogram of features and observation times; According to the coarse pose, a plurality of continuous quasi-spiral subframes are aggregated to form a local submap; The local submap is precisely registered with a historical submap to obtain an accurate pose.

[0018] Optionally, the double-layer map is obtained according to the pose of the unmanned aerial vehicle and the preprocessed data, and the method further comprises: The coarse pose is tightly coupled and fused with IMU prediction in an iterative error state Kalman filter to obtain an optimal state; According to the optimal state, the current key frame in the iterative error state Kalman filter is projected to a global coordinate system, and is fused into a previous double-layer map to obtain a current double-layer map.

[0019] In a second aspect, a device for unmanned aerial vehicle fusion mapping and positioning is provided, the unmanned aerial vehicle is provided with a solid-state laser radar and a rotatable support, the rotatable support drives the solid-state laser radar to rotate, and the device comprises: A point cloud acquisition unit is configured to acquire an original laser point cloud data frame according to the solid-state laser radar; An IMU data acquisition unit is configured to acquire IMU data according to a body IMU of the unmanned aerial vehicle; A preprocessing unit is configured to preprocess the original laser point cloud data frame and the IMU data to obtain preprocessed data; A pose acquisition unit is configured to acquire a pose of the unmanned aerial vehicle according to the preprocessed data; A map acquisition unit is configured to obtain a double-layer map according to the pose of the unmanned aerial vehicle and the preprocessed data; The double-layer map comprises a sparse feature map and a visual dense voxel map.

[0020] In a third aspect, a computer device is provided, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the method according to any one of the preceding aspects when executing the computer program.

[0021] In a fourth aspect, a computer readable storage medium is provided, having stored thereon a computer program which, when executed by a processor, implements the method according to any one of the preceding aspects.

[0022] The unmanned aerial vehicle fusion mapping and positioning method, device, computer equipment and storage medium provided by the application, the unmanned aerial vehicle is provided with a solid-state laser radar and a rotatable support, the rotatable support drives the solid-state laser radar to rotate, the method comprises the following steps: obtaining an original laser point cloud data frame according to the solid-state laser radar; obtaining IMU data according to the body IMU of the unmanned aerial vehicle; preprocessing the original laser point cloud data frame and the IMU data to obtain preprocessed data; obtaining the unmanned aerial vehicle pose according to the preprocessed data; obtaining a double-layer map according to the unmanned aerial vehicle pose and the preprocessed data; wherein the double-layer map comprises a sparse feature map and a visual dense voxel map. In the embodiment of the application, the unmanned aerial vehicle is provided with a solid-state laser radar and a rotatable support, the rotatable support drives the solid-state laser radar to rotate, without the need to equip multiple solid-state laser radars to obtain a 360-degree field of view, thereby solving the problem that the left and right fields of view of the solid-state laser radar are less than 360 degrees, and also reducing the overall mass of the unmanned aerial vehicle and improving the endurance of the unmanned aerial vehicle. In addition, the embodiment of the application also provides a method for preprocessing the original laser point cloud data frame of the solid-state laser radar and the IMU data of the body IMU to obtain preprocessed data, and obtaining the unmanned aerial vehicle pose and the double-layer map according to the preprocessed data, wherein the double-layer map comprises a sparse feature map and a visual dense voxel map. The sparse feature map has a relatively small amount of calculation and can realize real-time positioning and mapping under lower computing resources; the visual dense voxel map has visualization and combines the advantages of dense mapping and voxel modeling, has the advantages of high precision and flexible expansion, can enhance observability and data reliability, and improve the precision of the unmanned aerial vehicle. BRIEF DESCRIPTION OF DRAWINGS

[0023] The accompanying drawings, which are incorporated herein and form a part of the specification, illustrate embodiments consistent with the present application and, together with the description, further serve to explain the principles of the application.

[0024] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, for those skilled in the art, other drawings can also be obtained from these drawings without creative labor.

[0025] Figure 1 The application environment diagram of the unmanned aerial vehicle fusion mapping and positioning method of the embodiment of the application is shown; Figure 2A flowchart of a UAV fusion mapping and positioning method according to an embodiment of the present application is shown. Figure 3 A structural block diagram of a UAV fusion mapping and positioning device according to an embodiment of the present application is shown. Figure 4 An internal structure diagram of a computer device according to an embodiment of the present application is shown. DETAILED DESCRIPTION

[0026] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described below in conjunction with the accompanying drawings of the embodiments of the present application. Obviously, the described embodiments are only some but not all of the embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of the present application.

[0027] Figure 1 An application environment diagram of a UAV fusion mapping and positioning method in an embodiment is shown. Referring to Figure 1 , the UAV fusion mapping and positioning method is applied to a UAV fusion mapping and positioning system. The UAV fusion mapping and positioning method comprises a terminal 110 and / or a server 120. The terminal 110 and the server 120 are connected through a network. The terminal 110 can be specifically a desktop terminal or a mobile terminal, and the mobile terminal can be specifically at least one of a mobile phone, a tablet computer, a notebook computer, etc. The terminal 110 is arranged on a UAV.

[0028] The server 120 can be implemented by an independent server or a server cluster composed of multiple servers. The server 120 can be arranged on a UAV or can not be arranged on a UAV.

[0029] The UAV fusion mapping and positioning method of the present application is applied to the terminal 110 and / or the server 120.

[0030] As shown in Figure 2 , in an embodiment, a UAV fusion mapping and positioning method is provided.

[0031] In the embodiment of the present application, a solid-state laser radar and a rotatable support are arranged on the UAV, and the rotatable support drives the solid-state laser radar to rotate.

[0032] This embodiment mainly takes the method applied to the terminal 110 and / or the server 120 in the above Figure 1 as an example for illustration.

[0033] Referring to Figure 2 , the UAV fusion mapping and positioning method comprises: Step 210, acquiring a raw laser point cloud data frame according to a solid-state laser radar; Step 220, acquiring IMU data according to a body IMU (Inertial Measurement Unit, IMU) of the unmanned aerial vehicle; Step 230, pre-processing the raw laser point cloud data frame and the IMU data to acquire pre-processed data; Step 240, acquiring a UAV pose according to the pre-processed data; Step 250, acquiring a double-layer map according to the UAV pose and the pre-processed data; The double-layer map comprises a sparse feature map and a visual dense voxel map.

[0034] In the embodiment of the application, the solid-state laser radar and the rotatable support are arranged on the unmanned aerial vehicle, the rotatable support drives the solid-state laser radar to rotate, the 360-degree field of view can be obtained without arranging multiple solid-state laser radars, the problem of insufficient 360-degree field of view of the solid-state laser radar is solved, the overall mass of the unmanned aerial vehicle is reduced, and the endurance of the unmanned aerial vehicle is improved. In addition, the embodiment of the application also provides a method for pre-processing raw laser point cloud data frames of the solid-state laser radar and IMU data of the body IMU to acquire pre-processed data, and acquiring a UAV pose and a double-layer map according to the pre-processed data, the double-layer map comprises a sparse feature map and a visual dense voxel map. The sparse feature map has a relatively small amount of calculation, and real-time positioning and mapping can be realized under low computing resources; the visual dense voxel map has the advantages of visualization, combination of dense mapping and voxel modeling, high precision, flexible expansion, enhanced observability and data reliability, and improved precision of the unmanned aerial vehicle.

[0035] In the step 210, the raw laser point cloud data frame is acquired according to the solid-state laser radar, which comprises the following steps: According to the phase of the rotatable support, the field of view corresponding to one rotation of the solid-state laser radar is divided into multiple angular windows, and the boundary between adjacent two angular windows is taken as an angular window boundary; The solid-state laser radar rotates under the driving of the rotatable support and acquires raw point clouds; The raw point clouds are divided into multiple quasi-spiral sub-frames according to the angular window boundary; The multiple quasi-spiral sub-frames constitute the raw laser point cloud data frame.

[0036] In the embodiment of the application, the width constraint condition of the angular window is as follows: The linear displacement of each quasi-spiral sub-frame does not exceed the maximum linear displacement, and the angular displacement of each quasi-spiral sub-frame does not exceed the maximum angular displacement.

[0037] In the embodiment of the application, the field of view corresponding to one rotation of the solid-state laser radar is divided into a plurality of angular windows according to the phase of the rotatable support, and the method comprises: According to the degree of jitter during flight of the unmanned aerial vehicle, the width of the angular window is adaptively adjusted. The greater the degree of jitter during flight, the smaller the width of the angular window.

[0038] In the embodiment of the application, the width of the angular window is adaptively adjusted, which is different from the non-rotating scheme of frame cutting only according to the time period. The method limits the upper limit of the space-time distortion of a single frame, which is beneficial to subsequent distortion removal and registration.

[0039] In the embodiment of the application, the width of the angular window is constrained according to the following condition: The phase span of the angular window is according to the following manner: Wherein, is the width of the angular window, is the maximum linear displacement, is the average linear speed of the rotating motor, is the maximum angular displacement, is the average angular speed of the rotating motor, is the laser radar electric frequency, is the minimum number of single quasi-spiral frames, is the phase span, is the starting phase of the m-th quasi-spiral frame, is the starting phase of the m-th quasi-spiral frame.

[0040] In the embodiment of the application, the original laser point cloud data frame and the IMU data have a time stamp, In step 230, the original laser point cloud data frame and the IMU data are preprocessed to obtain preprocessed data, comprising: Each angular window boundary is used as the completion phase of the current quasi-spiral frame and the starting phase of the next quasi-spiral frame, and the starting phase of the current quasi-spiral frame is used as the reference time; According to the time stamp, the original point cloud in each quasi-spiral frame is rolled back to the corresponding reference time according to the IMU data to obtain a rolled-back data frame; According to the IMU rotation rate, the time deviation of the rolled-back data frame is compensated to remove the distortion in the point cloud data to obtain a de-distorted data frame; ​​Filtering the post-distortion data frames according to voxel filtering, statistical outlier removal filtering and intensity threshold filtering to obtain pre-processed data. Wherein, the post-retrieval data frames are phase-aligned with the IMU data.

[0041] Retrieving the original point cloud in each of the quasi-helical frames to the corresponding reference time according to the IMU data is for relative alignment, which is conducive to improving the accuracy of subsequent processing.

[0042] In the embodiment of the application, the post-distortion data frames are filtered according to voxel filtering, statistical outlier removal filtering and intensity threshold filtering to obtain pre-processed data, comprising: According to the phase weight of the post-distortion data frames, the curvature and normal vector are obtained in the quasi-helical frame of the post-distortion data frames. According to the curvature and normal vector, the radial direction is relaxed and the tangential direction is tightened at the reduced sampling rate. Filtering the post-distortion data after downsampling.

[0043] In the embodiment of the application, filtering the post-distortion data frames can alleviate the anisotropic sampling bias caused by the rotation of the solid-state laser radar and improve the geometric consistency.

[0044] In the embodiment of the application, the time deviation of the post-retrieval data frames is compensated according to the IMU rotation rate to remove distortion in the point cloud data and obtain post-distortion data frames, comprising: Obtaining the encoder angular velocity according to the IMU rotation rate; According to the pre-integration of the IMU data and the encoder angular velocity, the relative pose of the point cloud of the post-retrieval data frames at the reference time of the quasi-helical frame is obtained. By coordinate transformation, the point clouds of all the post-retrieval data frames are transformed into the radar coordinate system or the body coordinate system to obtain a spatiotemporally consistent corrected point cloud as the post-distortion data frames.

[0045] In the embodiment of the application, the point clouds of all the post-retrieval data frames are transformed into the radar coordinate system or the body coordinate system by coordinate transformation, comprising: Obtaining the extrinsic matrix between the solid-state laser radar and the body IMU; Obtaining the current angle of the rotatable support; According to the current angle of the rotatable support and the current extrinsic matrix, the point clouds of the post-retrieval data frames are mapped from the radar coordinate system to the body coordinate system, or from the body coordinate system to the radar coordinate system.

[0046] In the embodiment of the present application, step 240, the UAV pose is obtained according to the preprocessed data, comprising: According to the pre-integration of the IMU data, the inter-frame motion initial value is obtained, and the state prediction is obtained according to the inter-frame motion initial value; According to the state prediction, the current quasi-spiral frame is quickly registered with the adjacent quasi-spiral frame to obtain a coarse pose; According to the coarse pose, a plurality of continuous quasi-spiral frames are aggregated to form a local submap; The local submap is precisely registered with a historical submap to obtain an accurate pose.

[0047] In the embodiment of the present application, step 250, the double-layer map is obtained according to the UAV pose and the preprocessed data, comprising: According to the state prediction, the current quasi-spiral frame is quickly registered with the adjacent quasi-spiral frame to obtain the sparse feature map, the sparse feature map comprising: line features, surface features, feature covariance, phase coverage histogram of features and observation times; According to the coarse pose, a plurality of continuous quasi-spiral frames are aggregated to form a local submap; The local submap is precisely registered with a historical submap to obtain the visual dense voxel map.

[0048] In the embodiment of the present application, step 250, the double-layer map is obtained according to the UAV pose and the preprocessed data, further comprising: The coarse pose is tightly coupled and fused in the iterative error state Kalman filtering with the IMU prediction to obtain an optimal state; According to the optimal state, the current key frame in the iterative error state Kalman filtering is projected to a global coordinate system and fused into a previous double-layer map to obtain a current double-layer map.

[0049] As Figure 3 shown, the present application further provides a UAV fusion mapping and positioning device, the UAV is provided with a solid-state laser radar and a rotatable support, the rotatable support drives the solid-state laser radar to rotate, the device comprising: A point cloud acquisition unit 310 is configured to acquire an original laser point cloud data frame according to the solid-state laser radar; An IMU data acquisition unit 320 is configured to acquire IMU data according to a body IMU of the UAV; A preprocessing unit 330 is configured to preprocess the original laser point cloud data frame and the IMU data to obtain preprocessed data; A pose acquisition unit 340 is configured to acquire a UAV pose according to the preprocessed data; The map acquisition unit 350 is configured to acquire a double-layer map according to the pose of the UAV and the preprocessed data. The double-layer map includes a sparse feature map and a visual dense voxel map.

[0050] In the embodiment of the present application, the point cloud acquisition unit 310 is further configured to: According to the phase of the rotatable support, the field of view corresponding to one rotation of the solid-state laser radar is divided into a plurality of corner windows, and the boundary between adjacent two corner windows is taken as a corner window boundary. The solid-state laser radar rotates under the driving of the rotatable support and acquires original point cloud. According to the corner window boundary, the original point cloud is divided into a plurality of quasi-spiral sub-frames. The plurality of quasi-spiral sub-frames constitute the original laser point cloud data frame.

[0051] In the embodiment of the present application, the width constraint condition of the corner window is: The linear displacement of each quasi-spiral sub-frame does not exceed the maximum linear displacement, and the angular displacement of each quasi-spiral sub-frame does not exceed the maximum angular displacement.

[0052] In the embodiment of the present application, the point cloud acquisition unit 310 is further configured to: According to the shaking degree during flight of the UAV, the width of the corner window is adaptively adjusted. The greater the shaking degree during flight, the smaller the width of the corner window.

[0053] In the embodiment of the present application, the width constraint condition of the corner window is: The phase span of the corner window is as follows: Wherein, is the width of the corner window, is the maximum linear displacement, is the average linear speed of the rotating motor, is the maximum angular displacement, is the average angular speed of the rotating motor, is the frequency of the laser radar, is the minimum number of points of a single quasi-spiral sub-frame, is the phase span, is the starting phase of the m-th quasi-spiral sub-frame, is the starting phase of the m-th quasi-spiral sub-frame.

[0054] ​​In the embodiment of the present application, the original laser point cloud data frame and the IMU data have timestamps, and the preprocessing unit 330 is further configured to: taking each of the corner window boundaries as a finishing phase of a current quasi-spiral frame and a starting phase of a next quasi-spiral frame, and taking a starting phase of the current quasi-spiral frame as a reference time; according to the timestamps, rolling back original point clouds in each of the quasi-spiral frames to corresponding reference times according to the IMU data to obtain a rolled-back data frame; according to an IMU rotation rate, compensating for time deviation of the rolled-back data frame to remove distortion in the point cloud data, and obtaining a de-distorted data frame; filtering the de-distorted data frame according to voxel filtering, statistical cluster point removal filtering and intensity threshold filtering to obtain preprocessed data; wherein the rolled-back data frame is phase-aligned with the IMU data.

[0055] In the embodiment of the present application, the preprocessing unit 330 is further configured to: obtaining an encoder angular velocity according to the IMU rotation rate; obtaining a relative pose of point clouds of the rolled-back data frame at the reference time of the quasi-spiral frame according to pre-integration of the IMU data and the encoder angular velocity; transforming the point clouds of all the rolled-back data frames to a radar coordinate system or a body coordinate system through coordinate transformation to obtain a spatiotemporally consistent corrected point cloud as the de-distorted data frame.

[0056] In the embodiment of the present application, the preprocessing unit 330 is further configured to: obtaining an extrinsic parameter matrix between the solid-state laser radar and the body IMU; obtaining a current angle of the rotatable support; mapping the point clouds of the rolled-back data frame from the radar coordinate system to the body coordinate system or from the body coordinate system to the radar coordinate system according to the current angle of the rotatable support and the current extrinsic parameter matrix.

[0057] In the embodiment of the present application, the pose obtaining unit 340 is further configured to: obtaining an inter-frame motion initial value according to pre-integration of the IMU data, and obtaining a state prediction according to the inter-frame motion initial value; quickly registering a current quasi-spiral frame and an adjacent quasi-spiral frame according to the state prediction to obtain a coarse pose; aggregating a plurality of continuous quasi-spiral frames to form a local submap according to the coarse pose; The local sub-map is finely registered with a historical sub-map to obtain an accurate pose.

[0058] In the embodiment of the application, the map obtaining unit 350 is further configured to: According to the state prediction, the current quasi-spiral sub-frame is quickly registered with the adjacent quasi-spiral sub-frame to obtain the sparse feature map, which includes line features, surface features, feature covariance, phase coverage histogram of features and observation times. According to the coarse pose, a plurality of continuous quasi-spiral sub-frames are aggregated to form a local sub-map according to the coarse pose; The local sub-map is finely registered with a historical sub-map to obtain the visual dense voxel map.

[0059] In the embodiment of the application, the map obtaining unit 350 is further configured to: The coarse pose and IMU prediction are tightly coupled and fused in the iterative error state Kalman filter to obtain an optimal state. According to the optimal state, the current key frame in the iterative error state Kalman filter is projected to a global coordinate system and fused into a previous double-layer map to obtain a current double-layer map.

[0060] In the embodiment of the application, the unmanned aerial vehicle is provided with a solid-state laser radar and a rotatable support, the rotatable support drives the solid-state laser radar to rotate, and 360-degree field of view can be obtained without equipping multiple solid-state laser radars, thereby solving the problem that the left and right fields of view of the solid-state laser radar are less than 360 degrees, and the overall mass of the unmanned aerial vehicle is reduced and the endurance of the unmanned aerial vehicle is improved. In addition, the embodiment of the application also provides a method for preprocessing raw laser point cloud data frames of the solid-state laser radar and IMU data of the body IMU, obtaining preprocessed data, and obtaining the unmanned aerial vehicle pose and the double-layer map according to the preprocessed data, the double-layer map including a sparse feature map and a visual dense voxel map. The sparse feature map has a relatively small amount of calculation and can realize real-time positioning and mapping under lower computing resources; the visual dense voxel map has visualization and combines the advantages of dense mapping and voxel modeling, has the advantages of high precision and flexible expansion, and can enhance observability and data reliability.

[0061] This invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the following method: acquiring raw laser point cloud data frames based on a solid-state lidar; acquiring IMU data based on the UAV's airframe IMU; preprocessing the raw laser point cloud data frames and the IMU data to obtain preprocessed data; acquiring the UAV pose based on the preprocessed data; and acquiring a two-layer map based on the UAV pose and the preprocessed data. The two-layer map includes a sparse feature map and a visualized dense voxel map.

[0062] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the following method: acquiring raw laser point cloud data frames from a solid-state lidar; acquiring IMU data from the UAV's airframe IMU; preprocessing the raw laser point cloud data frames and the IMU data to obtain preprocessed data; acquiring the UAV pose based on the preprocessed data; and acquiring a two-layer map based on the UAV pose and the preprocessed data; wherein the two-layer map includes a sparse feature map and a visualized dense voxel map.

[0063] The aforementioned UAV-integrated mapping and positioning method achieves the beneficial effect of solving the technical problems raised in the background section.

[0064] Figure 2 This is a flowchart illustrating a UAV fusion mapping and localization method in one embodiment. It should be understood that, although... Figure 2 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 2 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0065] Figure 4 An internal structural diagram of a computer device in one embodiment is shown. Specifically, this computer device may be... Figure 1 Server 120 in the middle. For example... Figure 4As shown, the computer device includes a processor, a memory, a network interface, an input device and a display screen connected through a system bus. The memory includes a non-volatile storage medium and an internal memory. The non-volatile storage medium of the computer device stores an operating system, and can also store a computer program which, when executed by the processor, can enable the processor to implement the unmanned aerial vehicle fusion mapping and positioning method. The internal memory can also store a computer program which, when executed by the processor, can enable the processor to execute the unmanned aerial vehicle fusion mapping and positioning method. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer overlaid on the display screen, or can be a key, trackball or touchpad provided on the shell of the computer device, or can be an external keyboard, touchpad or mouse, etc.

[0066] Those skilled in the art can understand that, Figure 4 The structure shown in the figure is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. A specific computer device can include more or fewer components than those shown in the figure, or can combine certain components, or have a different arrangement of components.

[0067] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by a computer program instructing related hardware. The program can be stored in a non-volatile computer readable storage medium, and when executed, can include the processes of the above-mentioned embodiments. Any reference to memory, storage, database or other medium used in the embodiments provided by the present application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM) and memory bus dynamic RAM (RDRAM), etc.

[0068] It has to be noted that, in the present document, relational terms are intended only to convey a possible relationship between elements or

[0069] The foregoing is considered as illustrative only of the principles of the application. Numerous modifications and changes will readily occur to those skilled in the art, which modifications and changes are to be understood as intended to be encompassed by the general scope of the application. Accordingly, the application is not to be limited to the above described or illustrated embodiments that are merely given by way of example. It is also be understood that various combinations of the above described embodiments and variations thereof are encompassed by the application.

Claims

1. A method for fusion mapping and positioning of unmanned aerial vehicles (UAVs), characterized in that, The unmanned aerial vehicle is provided with a solid-state laser radar and a rotatable support, the rotatable support drives the solid-state laser radar to rotate, and the method comprises: According to the solid-state laser radar, an original laser point cloud data frame is acquired; According to the body IMU of the unmanned aerial vehicle, IMU data is acquired; The original laser point cloud data frame and the IMU data are preprocessed to acquire preprocessed data; According to the preprocessed data, an unmanned aerial vehicle pose is acquired; According to the unmanned aerial vehicle pose and the preprocessed data, a double-layer map is acquired; The double-layer map comprises a sparse feature map and a visual dense voxel map.

2. The method of claim 1, wherein, According to the solid-state laser radar, an original laser point cloud data frame is acquired, which comprises: According to the phase of the rotatable support, the field of view corresponding to one rotation of the solid-state laser radar is divided into a plurality of angular windows, and the boundary between adjacent two angular windows is taken as an angular window boundary; The solid-state laser radar rotates under the driving of the rotatable support and acquires original point cloud; According to the angular window boundary, the original point cloud is divided into a plurality of quasi-spiral subframes; The plurality of quasi-spiral subframes constitute the original laser point cloud data frame.

3. The method of claim 2, wherein, The width constraint condition of the angular window is that: The linear displacement of each quasi-spiral subframe does not exceed the maximum linear displacement, and the angular displacement of each quasi-spiral subframe does not exceed the maximum angular displacement.

4. The method of claim 2, wherein, According to the phase of the rotatable support, the field of view corresponding to one rotation of the solid-state laser radar is divided into a plurality of angular windows, which comprises: According to the shaking degree when the unmanned aerial vehicle is flying, the width of the angular window is adaptively adjusted; The greater the shaking degree when the unmanned aerial vehicle is flying, the smaller the width of the angular window.

5. The method of claim 2, wherein, The width constraint condition of the angular window is that: The phase span of the angular window is as follows: wherein, is the width of the corner window, is the maximum linear displacement, is the average linear velocity of the rotating motor, is the maximum angular displacement, is the average angular velocity of the rotating motor, is the electrical frequency of the lidar, is the minimum number of points in a single quasi-spiral frame, is the phase span, is the start phase of the th quasi-spiral frame, is the start phase of the th quasi-spiral frame.

6. The method of claim 2, wherein, The original laser point cloud data frame and the IMU data have timestamps, The original laser point cloud data frame and the IMU data are preprocessed to acquire preprocessed data, which comprises: Each angular window boundary is taken as the completion phase of the current quasi-spiral subframe and the starting phase of the next quasi-spiral subframe, and the starting phase of the current quasi-spiral subframe is taken as a reference time; According to the timestamps, the original point cloud in each quasi-spiral subframe is rolled back to the corresponding reference time according to the IMU data to obtain a rolled-back data frame; According to the IMU rotation rate, the time deviation of the rolled-back data frame is compensated to remove the distortion in the point cloud data to obtain a de-distorted data frame; According to voxel filtering, statistical cluster point removal filtering and intensity threshold filtering, the de-distorted data frame is filtered to obtain preprocessed data; The phase of the rolled-back data frame is aligned with the IMU data.

7. The method of claim 6, wherein, According to the IMU rotation rate, the time deviation of the rolled-back data frame is compensated to remove the distortion in the point cloud data to obtain a de-distorted data frame, which comprises: According to the IMU rotation rate, an encoder angular velocity is acquired; According to the pre-integration of the IMU data and the encoder angular velocity, the relative pose of the point cloud of the rolled-back data frame at the reference time of the quasi-spiral subframe is acquired; Transforming point clouds of all the data frames after the back call to a radar coordinate system or a body coordinate system through coordinate transformation to obtain a corrected point cloud consistent in time and space as the data frame after the distortion removal.

8. The method of claim 7, wherein, The transforming point clouds of all the data frames after the back call to a radar coordinate system or a body coordinate system through coordinate transformation comprises: Obtaining an external parameter matrix between the solid-state laser radar and the body IMU; Obtaining a current angle of the rotatable support; According to the current angle of the rotatable support and the current external parameter matrix, mapping the point cloud of the data frame after the back call from the radar coordinate system to the body coordinate system or from the body coordinate system to the radar coordinate system.

9. The method of claim 1, wherein, The obtaining the pose of the unmanned aerial vehicle according to the preprocessed data comprises: According to the pre-integration of the IMU data, obtaining an initial value of inter-frame motion, and obtaining a state prediction according to the initial value of inter-frame motion; According to the state prediction, quickly registering a current quasi-spiral frame and an adjacent quasi-spiral frame to obtain a coarse pose; According to the coarse pose, aggregating a plurality of continuous quasi-spiral frames to form a local submap; Precise registration of the local submap and a historical submap is performed to obtain an accurate pose.

10. The method of claim 9, wherein, The obtaining the double-layer map according to the pose of the unmanned aerial vehicle and the preprocessed data comprises: According to the state prediction, quickly registering the current quasi-spiral frame and the adjacent quasi-spiral frame to obtain the sparse feature map, the sparse feature map comprising: line features, surface features, feature covariance, phase coverage histogram of features and observation times; According to the coarse pose, aggregating a plurality of continuous quasi-spiral frames to form a local submap; Precise registration of the local submap and a historical submap is performed to obtain the visual dense voxel map.

11. The method of claim 10, wherein, The obtaining the double-layer map according to the pose of the unmanned aerial vehicle and the preprocessed data further comprises: The coarse pose and the IMU prediction are tightly coupled and fused in an iterative error state Kalman filter to obtain an optimal state; According to the optimal state, projecting the current key frame in the iterative error state Kalman filter to a global coordinate system and fusing it into a previous double-layer map to obtain a current double-layer map.

12. An unmanned aerial vehicle fusion mapping and positioning device, comprising: The unmanned aerial vehicle is provided with a solid-state laser radar and a rotatable support, the rotatable support driving the solid-state laser radar to rotate, the device applying the method of any one of claims 1 to 11, the device comprising: A point cloud acquisition unit configured to acquire an original laser point cloud data frame according to the solid-state laser radar; An IMU data acquisition unit configured to acquire IMU data according to a body IMU of the unmanned aerial vehicle; A preprocessing unit configured to preprocess the original laser point cloud data frame and the IMU data to obtain preprocessed data; A pose acquisition unit configured to obtain a pose of the unmanned aerial vehicle according to the preprocessed data; A map acquisition unit configured to obtain a double-layer map according to the pose of the unmanned aerial vehicle and the preprocessed data; The double-layer map comprises a sparse feature map and a visual dense voxel map.

13. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the method of any one of claims 1 to 11. The processor executes the computer program to implement the method of any one of claims 1 to 11.

14. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program, which is executed by a processor, implements the method of any one of claims 1 to 11.

Images