A method for mapping unknown environments with a single laser beam for aircraft

Abstract

The present invention discloses a method for mapping an unknown environment of an aircraft using a single-beam laser, which belongs to the technical field of three-dimensional map reconstruction. First, the data collected by a single-beam laser radar is time-stamp aligned with a stepper motor, and the obtained point cloud data is filtered and feature matched to obtain three-dimensional map information. The position and posture of the aircraft are estimated by a wheel odometer and an IMU, and loop detection is performed at the same time to reduce the cumulative error and update the three-dimensional map information in real time. A frontier search algorithm is used to determine the target point of the aircraft's flight, an A* algorithm is used to plan the path trajectory, and a B-spline curve optimization algorithm is used to make the aircraft's motion trajectory smoother. The control command is sent to the aircraft, and a global three-dimensional map of the unknown environment is obtained.

Description

Technical Field

[0001] The invention belongs to the technical field of autonomous navigation robots and laser radar three-dimensional space reconstruction, and specifically relates to a method for mapping an unknown environment of an aircraft with a single laser beam. Background Art

[0002] The traditional surveying and mapping method mainly relies on manual measurement by surveying and mapping engineers, but the measurement steps are relatively cumbersome, time-consuming, and waste a lot of manpower and material resources. However, the measurement effect is often not particularly ideal, and the measurement error is not easy to control, which leads to relatively low measurement accuracy. However, with the development of three-dimensional space reconstruction technology, the surveying staff only need to use equipment with surveying and mapping tools to walk around the environment where the map needs to be built, and they can more conveniently obtain environmental structure information, thereby greatly reducing the errors caused by manual measurement. Three-dimensional reconstruction technology is an important technical means for aircraft to perceive their surrounding environment. By using different types of sensors installed on the aircraft itself, various feature information of the surrounding environment can be actively obtained. However, the multi-beam laser radar with better mapping effect on the market is expensive and not cost-effective in some environmental measurement scenarios.

[0003] In order to solve the drawbacks of traditional single-beam lidar three-dimensional reconstruction methods and improve portability, measurement accuracy and economy, the patent of this invention designs a single-beam laser unknown environment mapping system. The three-dimensional mapping mainly uses a single-beam radar to complete the three-dimensional reconstruction of the environment. The system has the advantages of multi-beam radar, such as long measurement range, no influence of light intensity and weather factors, and can obtain three-dimensional information in the environment with high accuracy. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the method for mapping an unknown environment of an aircraft with a single-beam laser is provided by the present invention, which solves the problems of insufficient measurement accuracy, poor portability and economy in the existing related methods. It completes the three-dimensional reconstruction of the environment by using a single-beam laser, and has the advantages of multi-beam radar, such as a long measurement range, no influence of light intensity and weather factors, and can obtain three-dimensional information in the environment with high accuracy.

[0005] In order to achieve the above-mentioned invention object, the technical solution adopted by the present invention is: a method for mapping an unknown environment using a single-beam laser of an aircraft, wherein the aircraft is equipped with a single-beam laser radar, a stepper motor and a binocular camera, and the point cloud data of the single-beam laser radar and the stepper motor data are aligned in terms of timestamp;

[0006] The method comprises the following steps:

[0007] S1. De-noise the point cloud data collected at the current position, and use the de-noised point cloud data to adjust the position information of the aircraft, thereby unifying the coordinate system of the aircraft and the single-beam laser radar;

[0008] S2. Based on the unified coordinate system, the point cloud data at adjacent positions are preliminarily aligned, and feature extraction, point cloud alignment and point cloud splicing are performed in sequence to generate a unified point cloud map;

[0009] S3. For the point cloud data in the unified point cloud map, the local neighborhood of each point cloud is established according to the K-nearest neighbor search algorithm, and the local tangent space of each point cloud in its local neighborhood is determined by the principal component analysis method. The information of all local tangent spaces is integrated by the multidimensional scaling analysis method to obtain the global parameterized representation of the point cloud data on the Riemann manifold.

[0010] S4, based on the point cloud data represented by global parameterization, a continuous three-dimensional manifold surface is generated according to the Delaunay triangulation method, and the surface is smoothed and the curvature is adjusted to generate a local map of the current position;

[0011] S5. During the aircraft data collection process, the aircraft posture is corrected using a loop detection algorithm, and then the local map is optimized to obtain an optimized local map;

[0012] S6. Take the current optimized local map as the starting point, perform boundary exploration and path planning, and collect data according to the planned path. Repeat steps S1 to S5 to gradually generate an optimized local map, and then complete the unknown environment mapping.

[0013] Furthermore, the step S1 includes the following sub-steps:

[0014] S11, calculating the distance between each point cloud data collected at the current position and its adjacent point cloud;

[0015] S12, determining the outlier point cloud determination threshold according to the average value and standard deviation of each distance;

[0016] S13, removing outlier point cloud data according to an outlier point cloud determination threshold, and completing point cloud denoising;

[0017] S14, extracting feature points from the denoised point cloud data;

[0018] S15, according to the type of feature points and the nearest point search method, a geometric relationship is established between the feature points after noise reduction at different times, and then the pose estimation of the single-line laser radar within the time is calculated;

[0019] S16, based on the inferred posture estimation, three-dimensional reconstruction is performed using point cloud data at different viewing angles to determine the initial posture of the aircraft;

[0020] S17, according to the initial posture of the aircraft, using the Kalman-based fusion matching algorithm to predict the next state estimation value of the aircraft, and according to the difference between the predicted value and the actual observation value, perform state estimation correction to obtain the next posture of the aircraft;

[0021] S18, performing weighted average fusion of the initial posture and the next posture of the aircraft, adjusting the attitude information of the aircraft, and then unifying the single-beam laser radar coordinate system of the aircraft.

[0022] Furthermore, the step S2 includes the following sub-steps:

[0023] S21, according to the random sampling consistency algorithm, using the registration points, two points to be configured in the point cloud data at adjacent positions are aligned to the coordinate system, completing preliminary alignment and obtaining roughly registered point cloud data;

[0024] S22, clustering the continuous point cloud data collected by the single-beam laser radar to extract feature points;

[0025] The types of feature points include scattered points, corner points and breakpoints;

[0026] S23, using an improved iterative adaptive point algorithm to screen corner points, using a continuous edge tracking algorithm to screen breakpoints, and then screening out all real corner points and breakpoints, and filtering out scattered points;

[0027] S24, using the least square method to perform characteristic straight line fitting on the screened corner points and breakpoints to determine characteristic line segments;

[0028] S25, using the ICP algorithm to perform fine registration on the roughly registered point cloud data and the feature line segments;

[0029] S26, fusing the precisely registered point cloud data into a unified voxel grid, and performing point cloud stitching to generate a unified point cloud map.

[0030] Furthermore, in step S22, the method for clustering the continuous point cloud data is specifically as follows:

[0031] With the position of a single laser radar as the origin O, the angles Scan the straight line L representing the detected object to obtain continuous point cloud data , the length distance between the origin and each point cloud data is expressed as , and then calculate the slope value of the i-th point cloud data. When the difference in the slope values ​​of adjacent point cloud data is less than the set threshold, it is determined that they are on the same straight line, otherwise it is determined to be a mutation point and used as a feature point;

[0032] Among them, the slope value It is expressed as:

[0033] .

[0034] Furthermore, the step S3 includes the following sub-steps:

[0035] S31, using a K-nearest neighbor search algorithm to search each point cloud in the point cloud data of the unified point cloud map to find its K nearest neighbor point clouds;

[0036] S32, using a principal component analysis network to perform principal component analysis on the neighborhood of the searched point cloud, calculate the covariance matrix and extract eigenvalues ​​and eigenvectors, and then obtain the local tangent space of each point cloud in its neighborhood;

[0037] S33, calculating the Riemann metric matrix of each point cloud according to the local tangent space;

[0038] S34. The Riemannian metric matrices of all point clouds are integrated using a multidimensional scaling analysis method to obtain a global parameter representation of the point cloud data on the Riemannian manifold.

[0039] Furthermore, the step S32 is specifically as follows:

[0040] The principal component analysis network is used to perform principal component analysis on the neighborhood of the searched point cloud, the analyzed point cloud data is decentralized, the covariance matrix of the decentralized neighborhood point cloud data is calculated, the covariance matrix is ​​eigendecomposed to obtain eigenvalues ​​and eigenvectors, and the local tangent space of each point cloud in its field is calculated based on the eigenvalues ​​and eigenvectors;

[0041] In the three-dimensional space where the local tangent space is located, the eigenvector Corresponding to the main directions of the local geometry, the eigenvectors Represents the main direction of the point cloud data distribution, eigenvector Represents the secondary direction, eigenvector Represents the local normal vector, that is, the normal vector of the point cloud surface, and the eigenvector and Composed of local tangent space, eigenvector Perpendicular to the local tangent space.

[0042] Furthermore, the step S4 includes the following sub-steps:

[0043] S41, using the Delaunay triangulation method to generate a triangular mesh in the manifold space where the point cloud data represented by the global parameterization is located, and generating a continuous three-dimensional manifold surface from the triangular mesh;

[0044] S42, extracting the topological structure of the three-dimensional surface;

[0045] S43. Use the discrete Morse theory algorithm to extract the topological structure features of the three-dimensional manifold surface and optimize the topological structure;

[0046] S44, using a graph cut optimization method to smooth and adjust the curvature of the three-dimensional manifold surface according to the optimized topological structure;

[0047] S45, performing three-dimensional visualization on the smoothed and curvature-adjusted three-dimensional manifold surface to generate a local map of the current position.

[0048] Furthermore, the step S5 includes the following sub-steps:

[0049] S51, during the process of generating the local map, continuously collect the odometer data of the aircraft, obtain the real-time estimated posture information, and determine its current position;

[0050] S52, when the Euclidean distance between the current position of the aircraft and the historical position is less than a set threshold, triggering loop detection;

[0051] S53, extracting feature points from the data collected by the binocular camera configured on the aircraft;

[0052] S54, performing feature point matching and data splicing on the extracted feature points in sequence, and then fusing them with the point cloud data of the laser radar at the current position;

[0053] S55, calculating the posture error of the aircraft between the current frame and the historical frame according to the fused data, and optimizing the adjusted aircraft posture information according to the posture error, and then fusing the point cloud data corresponding to the optimized aircraft posture information into the local map;

[0054] S56. Smoothing and curvature adjusting the three-dimensional manifold surface corresponding to the local map after the fusion of the point cloud data, to obtain an optimized local map.

[0055] Furthermore, the step S6 includes the following sub-steps:

[0056] S61, detecting the boundary of the unexplored area in the current optimized local map, and selecting a target point closest to the aircraft;

[0057] S62, using the A* path planning algorithm to calculate the optimal path from the current boundary point to the target point;

[0058] S63, uniformly sampling the determined optimal path, calculating the B-spline basis function using the selected control points, curve order and node vectors, and then subdividing the B-spline curve into a number of discrete points as the waypoints of the aircraft;

[0059] S64. At each of the waypoints, execute steps S1 to S5 to gradually generate an optimized local map, thereby completing the unknown environment mapping.

[0060] Further, in step S63, after selecting a set of control points { , , , }, the B-spline curve is expressed as:

[0061]

[0062] In the formula, represents the point of the B-spline curve at parameter t, represents the nth control point, represents the k-th order B-spline basis function corresponding to the n-th control point, which is expressed as:

[0063]

[0064] In the formula, Represents a knot value in the knot vector of the spline.

[0065] The beneficial effects of the present invention are:

[0066] (1) The method of the present invention saves the cost of mapping and compensates for the lack of point cloud data collected in a single frame time by single-line laser radar compared to multi-line laser radar through time cost.

[0067] (2) The method of the present invention has made changes to the mapping algorithm itself, so that the mapping method of the present invention has higher accuracy when the hardware conditions are consistent.

[0068] (3) The method of the present invention makes a change in the mapping strategy algorithm and plans the mapping path of the aircraft, which not only saves the time of spatial search but also ensures the accuracy of mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 This is a flow chart of the method for mapping an unknown environment using a single laser beam for an aircraft in the present invention. DETAILED DESCRIPTION

[0070] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0071] An embodiment of the present invention provides a method for mapping an unknown environment of an aircraft using a single-beam laser, wherein the aircraft is equipped with a single-beam laser radar, a stepper motor and a binocular camera, and the point cloud data of the single-beam laser radar and the stepper motor data are aligned at the timestamp; specifically, in this embodiment, a KNN algorithm is used to align the point cloud data of the single-beam laser radar and the stepper motor data at the timestamp.

[0072] In the present invention, when the aircraft is moving, point cloud data is acquired by a single-beam laser radar equipped with a stepper motor, the aircraft posture is determined by the odometer and IMU, and data fusion under multiple postures is completed by feature extraction and feature matching of point cloud data. A three-dimensional map is constructed through Riemann space, and loop detection is used to correct the aircraft posture to optimize the accuracy of map construction. At the same time, a path planning trajectory is formed by combining the frontier search algorithm with the A* algorithm to realize 3D map construction in an unknown environment with a single laser beam.

[0073] Based on this, Figure 1 As shown, the single laser beam unknown environment mapping method in the embodiment of the present invention includes the following steps:

[0074] S1. De-noise the point cloud data collected at the current position, and use the de-noised point cloud data to adjust the position information of the aircraft, thereby unifying the coordinate system of the aircraft and the single-beam laser radar;

[0075] S2. Based on the unified coordinate system, the point cloud data at adjacent positions are preliminarily aligned, and feature extraction, point cloud alignment and point cloud splicing are performed in sequence to generate a unified point cloud map;

[0076] S3. For the spliced ​​point cloud data, the local neighborhood of each point cloud is established according to the K-nearest neighbor search algorithm, and the local tangent space of each point cloud in its local neighborhood is determined by the principal component analysis method. The information of all local tangent spaces is integrated by the multidimensional scaling analysis method to obtain the global parameterized representation of the point cloud data on the Riemann manifold.

[0077] S4, based on the point cloud data represented by global parameterization, a continuous three-dimensional manifold surface is generated according to the Delaunay triangulation method, and the surface is smoothed and the curvature is adjusted to generate a local map of the current position;

[0078] S5. During the aircraft data collection process, the aircraft posture is corrected using a loop detection algorithm, and then the local map is optimized to obtain an optimized local map;

[0079] S6. Take the current optimized local map as the starting point, perform boundary exploration and path planning, and collect data according to the planned path. Repeat steps S1 to S5 to gradually generate an optimized local map, and then complete the unknown environment mapping.

[0080] Step S1 of the embodiment of the present invention includes the following sub-steps:

[0081] S11, calculating the distance between each point cloud data collected at the current position and its adjacent point cloud;

[0082] Optionally, a Statistical Outlier Removal algorithm is used to calculate the distance between each point cloud data and its neighboring point clouds;

[0083] S12, determining the outlier point cloud determination threshold according to the average value and standard deviation of each distance;

[0084] S13, removing outlier point cloud data according to an outlier point cloud determination threshold, and completing point cloud denoising;

[0085] S14, extracting feature points from the denoised point cloud data;

[0086] S15, according to the type of feature points and the nearest point search method, a geometric relationship is established between the feature points after noise reduction at different times, and then the pose estimation of the single-line laser radar within the time is calculated;

[0087] S16, based on the inferred posture estimation, three-dimensional reconstruction is performed using point cloud data at different viewing angles to determine the initial posture of the aircraft;

[0088] S17, according to the initial posture of the aircraft, using the Kalman-based fusion matching algorithm to predict the next state estimation value of the aircraft, and according to the difference between the predicted value and the actual observation value, perform state estimation correction to obtain the next posture of the aircraft;

[0089] S18, performing weighted average fusion of the initial posture and the next posture of the aircraft, adjusting the attitude information of the aircraft, and then unifying the single-beam laser radar coordinate system of the aircraft.

[0090] Specifically, in this embodiment, during the process of estimating and locating the attitude of the aircraft, in the initialization phase, the aircraft starts from a stationary state to obtain an initial attitude. Then, in the data acquisition and preprocessing phase, the acceleration and angular velocity sampled by the IMU are denoised and calibrated to reduce the impact of sensor noise; the same is true for the odometer data, and the accuracy of the measured data is ensured by filtering and calibrating the measured data.

[0091] In the data fusion stage, the EKF filter fuses the high-frequency, short-term precise information provided by the IMU with the low-frequency, long-term stable information provided by the odometer through a prediction-to-update cycle. In the prediction step, the acceleration and angular velocity data of the IMU are used to predict the change in the aircraft's posture through the inertial navigation equation. Then, in the update step, the displacement measurement provided by the odometer is integrated to correct the prediction result, thereby obtaining a more accurate posture estimate.

[0092] In the specific implementation process, the motion model and sensor model of the aircraft are established, the state vector and observation vector are defined, and the prediction and update formulas of the Kalman filter are used for iterative calculations. In addition, the timestamp alignment step is used to ensure the time synchronization between sensors to ensure the real-time and accuracy of data fusion. Finally, through experimental verification and parameter adjustment, the filter parameters are continuously optimized to improve the robustness and accuracy of pose estimation.

[0093] Step S2 of the embodiment of the present invention includes the following sub-steps:

[0094] S21, according to the random sampling consistency algorithm, using the registration points, the two points to be configured in the point cloud data at adjacent positions are aligned to the coordinate system, completing the preliminary alignment and obtaining the roughly registered point cloud data;

[0095] S22, clustering the continuous point cloud data collected by the single-beam laser radar to extract feature points;

[0096] The types of feature points include scattered points, corner points and breakpoints;

[0097] S23, using an improved iterative adaptive point algorithm to screen corner points, using a continuous edge tracking algorithm to screen breakpoints, and then screening out all real corner points and breakpoints, and filtering out scattered points;

[0098] S24, using the least square method to perform characteristic straight line fitting on the screened corner points and breakpoints to determine characteristic line segments;

[0099] S25, using the ICP algorithm to perform fine registration on the roughly registered point cloud data and the feature line segments;

[0100] S26, fusing the precisely registered point cloud data into a unified voxel grid, and performing point cloud stitching to generate a unified point cloud map.

[0101] In step S22 of this embodiment, the method for clustering continuous point cloud data is specifically as follows:

[0102] With the position of a single laser radar as the origin O, the angles Scan the straight line L representing the detected object to obtain continuous point cloud data , the length distance between the origin and each point cloud data is expressed as , and then calculate the slope value of the i-th point cloud data. When the difference in the slope values ​​of adjacent point cloud data is less than the set threshold, it is determined that they are on the same straight line, otherwise it is determined to be a mutation point and used as a feature point;

[0103] Among them, the slope value It is expressed as:

[0104]

[0105] In step S24 of this embodiment, the least square method is used to perform characteristic straight line fitting. For a set of two-dimensional data points ( , ), by finding a straight line y=mx+b to fit these points, where m is the slope and b is the intercept; in this embodiment, m and b are determined by the following objective function;

[0106]

[0107] Where S is the sum of squared errors. By taking the partial derivative of S and setting it to zero, we can get the analytical solution for m and b. The solution equation is as follows:

[0108]

[0109] The objective function expression of the solution is as follows:

[0110]

[0111] In step S25 of this embodiment, the ICP algorithm is used to accurately align the point cloud data. The core idea of ​​the ICP algorithm is to find the optimal rigid body transformation between the source point cloud and the target point cloud through iteration so that the sum of the squares of the point pair distances between the two point clouds is minimized. , the target point cloud is , the expression of the rigid body transformation matrix T is as follows:

[0112]

[0113] Where R is the rotation matrix and t is the translation vector.

[0114] The error function is defined as:

[0115]

[0116] In step S26 of the present embodiment, each frame of point cloud data after precise registration is divided into voxel grids, and the average value of each voxel is calculated; the point cloud data of different frames are aligned, and the optimal transformation matrix is ​​determined using the Grey Wolf Optimization Algorithm; the aligned point cloud data are voxel fused to generate a unified point cloud map.

[0117] Step S3 of the embodiment of the present invention includes the following sub-steps:

[0118] S31, using a K-nearest neighbor search algorithm to search each point cloud in the point cloud data of the unified point cloud map to find its K nearest neighbor point clouds;

[0119] S32, using a principal component analysis network to perform principal component analysis on the neighborhood of the searched point cloud, calculate the covariance matrix and extract eigenvalues ​​and eigenvectors, and then obtain the local tangent space of each point cloud in its neighborhood;

[0120] S33, calculating the Riemann metric matrix of each point cloud according to the local tangent space;

[0121] S34. The Riemannian metric matrices of all point clouds are integrated using a multidimensional scaling analysis method to obtain a global parameter representation of the point cloud data on the Riemannian manifold.

[0122] In step S31 of this embodiment, for any high-dimensional point cloud data in the unified point cloud map, a K-nearest neighbor search algorithm is used to find the K nearest neighboring points of each point in the high-dimensional space.

[0123] In an optional embodiment, the search for neighboring point clouds may be accelerated by using data structures such as KD trees and ball trees.

[0124] Step S32 of this embodiment is specifically as follows:

[0125] The principal component analysis network is used to perform principal component analysis on the neighborhood of the searched point cloud, the analyzed point cloud data is decentralized, the covariance matrix of the decentralized neighborhood point cloud data is calculated, the covariance matrix is ​​eigendecomposed to obtain eigenvalues ​​and eigenvectors, and the local tangent space of each point cloud in its field is calculated based on the eigenvalues ​​and eigenvectors;

[0126] In the three-dimensional space where the local tangent space is located, the eigenvector Corresponding to the main directions of the local geometry, the eigenvectors Represents the main direction of the point cloud data distribution, eigenvector Represents the secondary direction, eigenvector Represents the local normal vector, that is, the normal vector of the point cloud surface, and the eigenvector and Composed of local tangent space, eigenvector Perpendicular to the local tangent space.

[0127] Among them, the formula for decentralizing the neighborhood point cloud is:

[0128] In the formula, represents the mean of the domain point cloud,

[0129] The covariance matrix is ​​expressed as:

[0130] The formula for eigendecomposition is: ,in is the characteristic value, is the eigenvector, which is usually permuted.

[0131] Through the above steps, the global parameterized representation of high-dimensional point cloud data on the Riemann manifold can be achieved, effectively revealing the intrinsic structure and relationship of the data.

[0132] Step S4 of the embodiment of the present invention includes the following sub-steps:

[0133] S41, using the Delaunay triangulation method to generate a triangular mesh in the manifold space where the point cloud data represented by the global parameterization is located, and generating a continuous three-dimensional manifold surface from the triangular mesh;

[0134] S42, extracting the topological structure of the three-dimensional surface;

[0135] S43. Use the discrete Morse theory algorithm to extract the topological structure features of the three-dimensional manifold surface and optimize the topological structure;

[0136] S44, using a graph cut optimization method to smooth and adjust the curvature of the three-dimensional manifold surface according to the optimized topological structure;

[0137] S45, performing three-dimensional visualization on the smoothed and curvature-adjusted three-dimensional manifold surface to generate a local map of the current position.

[0138] In step S41 of this embodiment, in three-dimensional space, Delaunay triangulation is performed on the point cloud data represented by global parameterization to generate a triangular mesh or a tetrahedral mesh, and the properties of each triangle or tetrahedron, such as area, volume, and adjacency, are calculated. A discrete Morse function f is defined to map each point to a real value, and the key points (extreme points, saddle points) and key lines are identified based on the Delaunay triangulation using the discrete Morse theory, and the Morse simplification algorithm is used to remove redundant topological structures and retain key features, thereby obtaining a three-dimensional manifold surface.

[0139] In step S44 of this embodiment, the result of discrete Morse theory is represented as a graph model, and the optimal cut is solved by using the minimum cut / maximum flow algorithm. According to the optimal cut result, an optimized triangle or tetrahedron mesh model is generated; texture mapping is performed on the three-dimensional model to enhance the visual effect; and post-processing operations such as smoothing and denoising are performed to improve the quality of the three-dimensional map.

[0140] Step S5 of the embodiment of the present invention includes the following sub-steps:

[0141] S51, during the process of generating the local map, continuously collect the odometer data of the aircraft, obtain the real-time estimated posture information, and determine its current position;

[0142] S52, when the Euclidean distance between the current position of the aircraft and the historical position is less than a set threshold, triggering loop detection;

[0143] S53, extracting feature points from the data collected by the binocular camera configured on the aircraft;

[0144] S54, performing feature point matching and data splicing on the extracted feature points in sequence, and then fusing them with the point cloud data of the laser radar at the current position;

[0145] S55, calculating the posture error of the aircraft between the current frame and the historical frame according to the fused data, and optimizing the adjusted aircraft posture information according to the posture error, and then fusing the point cloud data corresponding to the optimized aircraft posture information into the local map;

[0146] S56. Smoothing and curvature adjusting the three-dimensional manifold surface corresponding to the local map after the fusion of the point cloud data, to obtain an optimized local map.

[0147] In the above steps, when loop detection is performed, when the aircraft is identified to pass through the previous area again, the reuse constraints of the global pose are established. These constraints are globally optimized using the graph optimization algorithm to eliminate the accumulated errors, improve the global consistency of the map, and obtain the optimized local map.

[0148] Specifically, a binocular camera is used to obtain left and right images, and depth information is calculated by stereo matching. A single-beam laser radar is used to scan the environment to obtain point cloud data. The external parameter matrix of the binocular camera and the single-beam laser radar is obtained through calibration to achieve the conversion of the coordinate systems of the two. The binocular camera depth information is fused with the single-beam laser radar point cloud data, and the Kalman filter algorithm is used for precise alignment. The depth information and point cloud are integrated through weighted averaging or other fusion methods to generate a high-precision 3D model.

[0149] Maps from multiple perspectives are updated and fused based on new poses and sensor data to generate a more accurate panoramic map. Finally, the optimized panoramic map is displayed and stored in real time for subsequent analysis and use. The entire process ensures that the aircraft can build a high-precision, globally consistent panoramic map in unknown or dynamic environments.

[0150] Step S6 of the embodiment of the present invention includes the following sub-steps:

[0151] S61, detecting the boundary of the unexplored area in the current optimized local map, and selecting a target point closest to the aircraft;

[0152] S62, using the A* path planning algorithm to calculate the optimal path from the current boundary point to the target point;

[0153] S63, uniformly sampling the determined optimal path, calculating the B-spline basis function using the selected control points, curve order and node vectors, and then subdividing the B-spline curve into a number of discrete points as the waypoints of the aircraft;

[0154] S64. At each of the waypoints, execute steps S1 to S5 to gradually generate an optimized local map, thereby completing the unknown environment mapping.

[0155] Specifically, in the above process, environmental data is collected by sensors, and maps are constructed and updated in real time to identify known areas, unknown areas and obstacles. Next, the starting point and target point of the aircraft are determined. Then, the frontier search algorithm is used to detect the boundaries between the known area and the unknown area, and a frontier point set is generated. These frontier point sets represent new areas to be explored and are used to guide the search expansion of the aircraft. Subsequently, starting from the current starting point, the A algorithm is used to perform path planning in the known map to find the optimal path from the starting point to the target point. The A* algorithm ensures the efficiency and reliability of the path by combining the path cost with the heuristic function. After the path is generated, it is smoothed and optimized, taking into account the motion constraints and environmental characteristics of the aircraft to ensure that the path is actually feasible. Finally, the control system drives the aircraft to move according to the optimized path, and performs dynamic obstacle avoidance and path adjustment during the path execution process to cope with real-time environmental changes and ensure that the aircraft reaches the target point safely.

[0156] In step S63 of this embodiment, after selecting a set of control points { , , , }, the B-spline curve is expressed as:

[0157]

[0158] In the formula, represents the point of the B-spline curve at parameter t, represents the nth control point, represents the k-th order B-spline basis function corresponding to the n-th control point, which is expressed as:

[0159]

[0160] In the formula, Represents a knot value in the knot vector of the spline.

[0161] In the present invention, based on the above step description, the data collected by the single-beam laser radar is first time-stamped with the stepper motor, and the obtained point cloud data is filtered and feature-matched to obtain three-dimensional map information. The position and posture of the aircraft are estimated by the wheel odometer and IMU, and loop detection is performed at the same time to reduce the cumulative error and update the three-dimensional map information in real time. The frontier search algorithm is used to determine the target point of the aircraft's flight, the A* algorithm is used to plan the path trajectory, and the B-spline curve optimization algorithm is used to make the aircraft's motion trajectory smoother, and the control command is sent to the aircraft.

[0162] The present invention uses specific embodiments to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

[0163] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific variations and combinations that do not deviate from the essence of the present invention based on the technical revelations disclosed by the present invention, and these variations and combinations are still within the protection scope of the present invention.

Claims

1. A method for mapping an unknown environment using a single laser beam from an aircraft, characterized in that: The aircraft is equipped with a single-beam laser radar, a stepper motor and a binocular camera, and the point cloud data of the single-beam laser radar and the stepper motor data are aligned in terms of timestamps; The method comprises the following steps: S1. De-noise the point cloud data collected at the current position, and use the de-noised point cloud data to adjust the position information of the aircraft, thereby unifying the coordinate system of the aircraft and the single-beam laser radar; S2. Based on the unified coordinate system, the point cloud data at adjacent positions are preliminarily aligned, and feature extraction, point cloud alignment and point cloud splicing are performed in sequence to generate a unified point cloud map; S3. For the point cloud data in the unified point cloud map, the local neighborhood of each point cloud is established according to the K-nearest neighbor search algorithm, and the local tangent space of each point cloud in its local neighborhood is determined by the principal component analysis method. The information of all local tangent spaces is integrated by the multidimensional scaling analysis method to obtain the global parameterized representation of the point cloud data on the Riemann manifold. S4, based on the point cloud data represented by global parameterization, a continuous three-dimensional manifold surface is generated according to the Delaunay triangulation method, and the surface is smoothed and the curvature is adjusted to generate a local map of the current position; S5. During the aircraft data collection process, the aircraft posture is corrected using a loop detection algorithm, and then the local map is optimized to obtain an optimized local map; S6. Take the current optimized local map as the starting point, perform boundary exploration and path planning, and collect data according to the planned path. Repeat steps S1 to S5 to gradually generate an optimized local map, and then complete the unknown environment mapping.

2. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 1, characterized in that: The step S1 comprises the following sub-steps: S11, calculating the distance between each point cloud data collected at the current position and its adjacent point cloud; S12, determining the outlier point cloud determination threshold according to the average value and standard deviation of each distance; S13, removing outlier point cloud data according to an outlier point cloud determination threshold, and completing point cloud denoising; S14, extracting feature points from the denoised point cloud data; S15, according to the type of feature points and the nearest point search method, a geometric relationship is established between the feature points after noise reduction at different times, and then the pose estimation of the single-beam laser radar within the time is calculated; S16, based on the inferred posture estimation, three-dimensional reconstruction is performed using point cloud data at different viewing angles to determine the initial posture of the aircraft; S17, according to the initial posture of the aircraft, using the Kalman-based fusion matching algorithm to predict the next state estimation value of the aircraft, and according to the difference between the predicted value and the actual observation value, perform state estimation correction to obtain the next posture of the aircraft; S18, performing weighted average fusion of the initial posture and the next posture of the aircraft, adjusting the attitude information of the aircraft, and then unifying the single-beam laser radar coordinate system of the aircraft.

3. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 1, characterized in that: The step S2 comprises the following sub-steps: S21, according to the random sampling consistency algorithm, using the registration points, two points to be configured in the point cloud data at adjacent positions are aligned to the coordinate system, completing preliminary alignment and obtaining roughly registered point cloud data; S22, clustering the continuous point cloud data collected by the single-beam laser radar to extract feature points; The types of feature points include scattered points, corner points and breakpoints; S23, using an improved iterative adaptive point algorithm to screen corner points, using a continuous edge tracking algorithm to screen breakpoints, and then screening out all real corner points and breakpoints, and filtering out scattered points; S24, using the least square method to perform characteristic straight line fitting on the screened corner points and breakpoints to determine characteristic line segments; S25, using the ICP algorithm to perform fine registration on the roughly registered point cloud data and the feature line segments; S26, fusing the precisely registered point cloud data into a unified voxel grid, and performing point cloud stitching to generate a unified point cloud map.

4. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 3, characterized in that: In step S22, the method for clustering continuous point cloud data is specifically as follows: With the position of a single laser radar as the origin O, the angles Scan the straight line L representing the detected object to obtain continuous point cloud data , the length distance between the origin and each point cloud data is expressed as , and then calculate the slope value of the i-th point cloud data. When the difference in the slope values ​​of adjacent point cloud data is less than the set threshold, it is determined that they are on the same straight line, otherwise it is determined to be a mutation point and used as a feature point; Among them, the slope value It is expressed as: 。 5. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 1, characterized in that: The step S3 comprises the following sub-steps: S31, using a K-nearest neighbor search algorithm to search each point cloud in the point cloud data of the unified point cloud map to find its K nearest neighbor point clouds; S32, using a principal component analysis network to perform principal component analysis on the neighborhood of the searched point cloud, calculate the covariance matrix and extract eigenvalues ​​and eigenvectors, and then obtain the local tangent space of each point cloud in its neighborhood; S33, calculating the Riemann metric matrix of each point cloud according to the local tangent space; S34. The Riemannian metric matrices of all point clouds are integrated using a multidimensional scaling analysis method to obtain a global parameter representation of the point cloud data on the Riemannian manifold.

6. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 5, characterized in that: The step S32 is specifically as follows: The principal component analysis network is used to perform principal component analysis on the neighborhood of the searched point cloud, the analyzed point cloud data is decentralized, the covariance matrix of the decentralized neighborhood point cloud data is calculated, the covariance matrix is ​​eigendecomposed to obtain eigenvalues ​​and eigenvectors, and the local tangent space of each point cloud in its field is calculated based on the eigenvalues ​​and eigenvectors; In the three-dimensional space where the local tangent space is located, the eigenvector Corresponding to the main directions of the local geometry, the eigenvectors Represents the main direction of the point cloud data distribution, eigenvector Represents the secondary direction, eigenvector Represents the local normal vector, that is, the normal vector of the point cloud surface, and the eigenvector and Composed of local tangent space, eigenvector Perpendicular to the local tangent space.

7. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 1, characterized in that: The step S4 comprises the following sub-steps: S41, using the Delaunay triangulation method to generate a triangular mesh in the manifold space where the point cloud data represented by the global parameterization is located, and generating a continuous three-dimensional manifold surface from the triangular mesh; S42, extracting the topological structure of the three-dimensional surface; S43. Use the discrete Morse theory algorithm to extract the topological structure features of the three-dimensional manifold surface and optimize the topological structure; S44, using a graph cut optimization method to smooth and adjust the curvature of the three-dimensional manifold surface according to the optimized topological structure; S45, performing three-dimensional visualization on the smoothed and curvature-adjusted three-dimensional manifold surface to generate a local map of the current position.

8. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 1, characterized in that: The step S5 comprises the following sub-steps: S51, during the process of generating the local map, continuously collect the odometer data of the aircraft, obtain the real-time estimated posture information, and determine its current position; S52, when the Euclidean distance between the current position of the aircraft and the historical position is less than a set threshold, triggering loop detection; S53, extracting feature points from the data collected by the binocular camera configured on the aircraft; S54, performing feature point matching and data splicing on the extracted feature points in sequence, and then fusing them with the point cloud data of the laser radar at the current position; S55, calculating the posture error of the aircraft between the current frame and the historical frame according to the fused data, and optimizing the adjusted aircraft posture information according to the posture error, and then fusing the point cloud data corresponding to the optimized aircraft posture information into the local map; S56. Smoothing and curvature adjusting the three-dimensional manifold surface corresponding to the local map after the fusion of the point cloud data, to obtain an optimized local map.

9. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 1, characterized in that: The step S6 comprises the following sub-steps: S61, detecting the boundary of the unexplored area in the current optimized local map, and selecting a target point closest to the aircraft; S62, using the A* path planning algorithm to calculate the optimal path from the current boundary point to the target point; S63, uniformly sampling the determined optimal path, calculating the B-spline basis function using the selected control points, curve order and node vectors, and then subdividing the B-spline curve into a number of discrete points as the waypoints of the aircraft; S64. At each of the waypoints, execute steps S1 to S5 to gradually generate an optimized local map, thereby completing the unknown environment mapping.

10. The method for mapping an unknown environment using a single laser beam for an aircraft according to claim 9, characterized in that: In step S63, a set of control points { , , , }, the B-spline curve is expressed as: ； In the formula, represents the point of the B-spline curve at parameter t, represents the nth control point, represents the k-th order B-spline basis function corresponding to the n-th control point, which is expressed as: ； In the formula, Represents a knot value in the knot vector of the spline.

Images