A lightweight SLAM method

Abstract

The present invention relates to a lightweight SLAM method, belonging to the fields of SLAM and robotic motion control technology. The method comprises: acquiring a laser radar point cloud; extracting features from the laser radar point cloud based on curvature; calculating five feature parameters, namely, map coordinates, feature direction, hash value, neighboring hash value, and neighboring distance; matching nearest neighbor features using a locality-sensitive hash table based on the feature parameters; optimizing the position and orientation using the Newton iteration method based on the matching results; and updating the map based on the position and orientation and feature parameters. The method enables the deployment of SLAM algorithms on low-cost, computing-constrained embedded devices such as microprocessors.

Description

Technical Field

[0001] The present invention belongs to the technical field of SLAM and robot motion control, and relates to a lightweight SLAM method, in particular to a lightweight SLAM method based on laser radar. Background Art

[0002] Simultaneous localization and mapping (SLAM) is a key technology for autonomous mobile robots and autonomous driving. Depending on the primary sensors used in SLAM systems, SLAM technology can be categorized into laser SLAM, visual SLAM, and multi-source fusion SLAM. Laser SLAM uses lidar (LiDAR) to perceive the environment, directly acquiring three-dimensional information. This technology offers precise positioning and high system reliability, making it widely used in fields such as indoor navigation and autonomous driving. Based on the principle of front-end odometry, laser SLAM can be categorized into algorithms based on iterative closest point (ICP), Gaussian mixture models (GMM), and graph optimization. While ICP-based SLAM algorithms offer high accuracy, they also carry a high computational load, making them unsuitable for deployment on low-cost, computationally constrained embedded devices such as microprocessors. This limits the application of SLAM in low-cost, miniaturized robotic systems. Summary of the Invention

[0003] The purpose of the present invention is to propose a lightweight SLAM method to solve the problems that the current SLAM algorithm has large computational complexity, large memory usage and cannot be deployed on a microprocessor.

[0004] In order to achieve the above-mentioned purpose, the present invention adopts the following technical solutions.

[0005] A lightweight SLAM method based on a two-dimensional point cloud is described, comprising the following steps:

[0006] Step 1: Extract features based on curvature and calculate the characteristic parameters of each feature, specifically:

[0007] Step 11, calculate the approximate curvature of each data point of the two-dimensional point cloud;

[0008] The two-dimensional point cloud described in Step 11 is a point set in the polar coordinate system of the sensor; Step 11 is specifically as follows: take the N points before and after the data point to be determined C data points, sum the differences between the polar diameters of these data points and the polar diameters of the data points to be determined, divide by the polar diameter of the data points to be determined, and then divide by the polar angle range of these data points to obtain the approximate curvature of the data points to be determined; the NC Greater than or equal to 1 and less than or equal to 20.

[0009] Step 12: Take the absolute value of the approximate curvature of all data points and perform non-maximum suppression to select the maximum and minimum values ​​as candidate data points;

[0010] Step 13: Extract features from candidate data points based on the set line feature recognition threshold, corner feature recognition threshold, and outlier recognition threshold;

[0011] Step 14. Calculate the characteristic parameters of each feature;

[0012] The characteristic parameters include hash value, adjacent hash value and adjacent distance;

[0013] Step 2: Match the features with the map features to obtain matched feature pairs;

[0014] The features extracted in Step 1 are called scanning features; the map features are the features in the feature map;

[0015] For each scan feature, traverse all map features in the hash bucket corresponding to its hash value and calculate and obtain the minimum feature distance of the hash bucket, which is recorded as the first distance; determine whether the traversal operation meets the following conditions: 1) the adjacent hash value is equal to the hash value; 2) the first distance is less than the adjacent distance; 3) the adjacent distance is greater than the search radius; if one of 1), 2), or 3) is met, then find the map feature corresponding to the first distance and together with the scan feature, they are a matched feature pair; if 1), 2), or 3) are not met, then traverse all map features in the hash bucket corresponding to the adjacent hash value and obtain the minimum feature distance of the hash bucket, which is recorded as the second distance; find the map feature corresponding to the smaller value of the first distance and the second distance and together with the scan feature, they are a matched feature pair;

[0016] Step 3: Calculate the error function of each feature pair and sum them to obtain the constructed optimization function. Then, update the pose through the Newton iteration method based on the optimization function.

[0017] Step 4, Convergence judgment: Determine whether the L2 norm of the Jacobian matrix of the optimization function is less than the set convergence threshold. If so, jump to Step 5, otherwise jump to Step 14;

[0018] Step 5: Update the mismatch scan features, update the feature pairs, and update the mismatch map features.

[0019] The features described in Step 13 include line features and corner features; the curvature minimum point whose absolute value of curvature is less than the line feature recognition threshold is a line feature; the curvature maximum point whose absolute value of curvature is greater than the corner feature recognition threshold but the negative number of curvature is not greater than the outlier recognition threshold is a corner feature.

[0020] The hash value in step 14 is calculated using a local sensitive hash function, including the hash value of the line feature and the hash value of the corner feature;

[0021] Step 14 The hash value of the line feature is obtained by dividing the sum of the map coordinates by The value is obtained by adding the offset to the hash bucket width, rounding down, and limiting.

[0022] Step 14: The corner feature hash value is the line feature hash value plus the integer H max ; The H max is the upper limit of the line feature hash value.

[0023] In step 14, the neighboring hash value is the closest hash value except the hash value of the feature itself; the neighboring distance is the distance between the feature and the neighboring hash value.

[0024] Step 14: The feature parameters also include map coordinates and feature directions. The map coordinates are calculated by transforming the feature points in the polar coordinate system of the sensor to obtain rectangular coordinates in the map coordinate system. The feature directions include the feature directions of line features and the feature directions of angular features. The feature directions of line features are the N directions before and after the feature. C The direction of the approximate straight line formed by the data points; the characteristic direction of the angle feature is the direction of the N before and after the feature. C The direction of the sum of the vectors connecting the data points and the feature.

[0025] The feature map in Step 2 is a hash table, which is divided into 2H max -1 hash bucket, each hash bucket corresponds to a hash value and is represented as a linked list; each linked list includes a header and a table node; the header is the hash value of the hash bucket, and the table node stores the map coordinates, feature direction and weight of the map feature.

[0026] Step 3 The error function of the feature pair includes the error function of the line feature pair and the error function of the corner feature pair; the error function of the line feature pair is calculated as the weighted sum of the square of the distance from the scan feature to the straight line where the map feature is located and the square of the difference in the feature directions of the two features; the error function of the corner feature pair is calculated as the square of the feature distance from the scan feature to the map feature.

[0027] Step 3 uses the Newton iteration method to update the posture, specifically: the updated posture is equal to the posture before the update plus the posture increment, and the posture increment is the matrix product of the Jacobian matrix of the optimization function and the inverse Hessian matrix.

[0028] Step 5 updates the mismatch scan feature, specifically: storing the mismatch scan feature into the corresponding hash bucket in the feature map hash table according to its hash value and assigning a weight to k; the value range of k is 1 to 20.

[0029] Step 5 updates the feature pair, specifically by taking the weighted average of the map feature coordinates and the scan feature coordinates of the feature point pair as the new map feature coordinates, and adding Δk to the weight; the value range of Δk is (0-1]; after the map coordinates are updated, their hash values ​​are recalculated, and the updated map features are re-stored into the corresponding hash bucket in the feature map hash table according to the hash values.

[0030] Step 5 updates the mismatch map feature, specifically by mapping the mismatch map feature to the sensor polar coordinate system; finding the sensor data point whose polar angle is closest to the mismatch map feature, and determining the size of the polar diameter between the sensor data point and the mismatch map feature; if the polar diameter of the mismatch map feature is smaller than the sensor data point and the difference in polar diameter is larger than the sensor ranging error, it indicates that the mismatch map feature is unstable; therefore, the weight of the map feature is reduced by Δk; if the weight of the map feature is less than 0, it is removed from the feature map hash table.

[0031] Beneficial effects

[0032] 1. The method only requires a laser radar as a sensor and does not require other sensors to assist, thus reducing the cost of the sensor;

[0033] 2. The method obtains the curvature of the data point in the polar coordinate system by using an approximate curvature calculation formula, avoiding the huge amount of calculation required for transforming the polar coordinate system to the rectangular coordinate system. At the same time, the non-maximum suppression method is used to significantly reduce the number of features, thereby saving computational overhead.

[0034] 3. The method constructs a feature map of the surrounding environment, avoiding the large amount of information redundancy in dense grid maps and saving storage overhead. At the same time, it uses local sensitive hashing to store and search maps, thereby improving the efficiency of map search.

[0035] 4. The method combines the position information and direction information of the feature to jointly calculate the feature distance, thereby improving the accuracy of feature matching and thus improving the positioning accuracy;

[0036] 5. The method constructs different error functions for different features. For line features, the square of the point-to-line distance is used as the error function, and for angular features, the square of the point-to-point distance is used as the error function. At the same time, the directional information of the feature is combined in the error function to improve the expressive power of the error function, thereby improving the positioning accuracy and accelerating the convergence speed of the Newton iteration method.

[0037] 6. The method develops different map update strategies for different feature pairing situations, integrating the scan features in the feature pair into the original map features according to the weights, and promptly cleaning up the mismatched map features to avoid the expansion of the number of features, maintain the simplicity of the feature map, and improve the efficiency of map search. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 1 is a system structure diagram of a lightweight SLAM method during deployment according to the present invention;

[0039] Figure 2 It is a flowchart of deploying a lightweight SLAM method on a computer according to the present invention;

[0040] Figure 3 is a system structure diagram of the micro indoor drone and its supporting hardware;

[0041] Figure 4 It is the movement trajectory and feature map of the drone displayed on the computer during the operation of the drone;

[0042] Figure 5 The present invention is a software structure diagram of a miniature indoor drone that is equipped with the method of the present invention. DETAILED DESCRIPTION

[0043] The lightweight SLAM method of the present invention is further illustrated and described in detail below with reference to the accompanying drawings and embodiments.

[0044] Example 1

[0045] This embodiment describes the process of deployment and practical application of the method of the present invention.

[0046] Figure 1 This is a system structure diagram when the method of the present invention is deployed on a computer. Figure 1 As can be seen in the figure, the system includes a laser radar, a laser radar driver module, a computer, and a display. The laser radar is connected to the computer via the laser radar driver module and transmits the laser radar point cloud to the computer. The computer receives the data and uses the method described in the present invention to construct a feature map and perform real-time positioning, while displaying the map information and positioning information on the screen.

[0047] Figure 2 It is a flow chart of the method of the present invention deployed on a computer. Figure 2 As can be seen from the figure, the method of the present invention is divided into 8 steps when deployed on a computer, specifically:

[0048] S1. Obtain the lidar point cloud through the USB interface;

[0049] The laser radar point cloud is a point set in the radar polar coordinate system, and the i-th data point in the point set is represented by (ρ i ,θ i ), ρ i is the distance from the i-th data point to the radar coordinate origin, θ i is the polar angle of the i-th data point in the radar coordinate system. i ranges from 0 to N, where N is the number of lidar points minus 1. The number of points in this model of lidar varies slightly due to changes in the lidar's rotational speed, ranging from approximately 870 to 930.

[0050] The method only requires a laser radar as a sensor and does not require the assistance of other sensors, thereby reducing the cost of the sensor;

[0051] S2. Extract features based on curvature, including the following sub-steps:

[0052] S21, calculating the approximate curvature of each data point;

[0053] The i-th data point (ρ i ,θ i )’s approximate curvature c i The calculation formula is:

[0054]

[0055] Among them, N Indicates modulo N, the value range of j is <i-N C > N arrive <i+N C > N ; N C Is the curvature calculation range, here we take N C =7;

[0056] S22, taking the absolute value of the approximate curvature of all data points and then performing non-maximum suppression to screen out the maximum and minimum values;

[0057] For j= <i-N S > N , <i-N S +1> N ,..., <i+N S > N For every data point of i |≥|c j |, then the i-th data point is called the maximum curvature point. If all have |c i |≤|c j |, then the i-th data point is called the curvature minimum point; N S is the non-maximum suppression range, where N is taken S=8; Data points that are neither the maximum nor the minimum curvature points are not considered as features;

[0058] S23, extracting features based on the set line feature recognition threshold, corner feature recognition threshold, and outlier recognition threshold;

[0059] The features include line features and angle features;

[0060] The curvature minimum point whose absolute value of curvature is less than the line feature recognition threshold is a line feature; the curvature maximum point whose absolute value of curvature is greater than the corner feature recognition threshold but the negative value of curvature is not greater than the outlier recognition threshold is a corner feature;

[0061] The curvatures calculated by the existing technology are all positive numbers, without any distinction between positive and negative. When identifying outliers, only the positive curvature can be compared with the outlier identification threshold. This has the disadvantage of misidentifying some outliers as corner features.

[0062] Among them, the line feature recognition threshold, corner feature recognition threshold and outlier recognition threshold are all positive numbers and the line feature recognition threshold < corner feature recognition threshold < outlier recognition threshold; here, the line feature recognition threshold is 3, the corner feature recognition threshold is 20, and the outlier recognition threshold is 60;

[0063] S3, calculating the characteristic parameters of each feature;

[0064] The feature parameters include map coordinates, feature direction, hash value, adjacent hash value and adjacent distance;

[0065] The calculation characteristic parameters include:

[0066] S31, calculating map coordinates;

[0067] The map coordinates of the i-th feature (x i ,y i ) is calculated as:

[0068]

[0069] in, is the position of the lidar in the map coordinate system, and its initial value is 0,0,0;

[0070] S32, calculating feature direction;

[0071] If the i-th feature is a line feature, its feature direction α i The calculation formula is:

[0072]

[0073] If the i-th feature is an angular feature, its feature direction α i The calculation formula is:

[0074]

[0075] S33, calculating a hash value using a locality-sensitive hash function;

[0076] If the i-th feature is a line feature, its hash value h i The calculation formula is:

[0077]

[0078] Among them, H w H is the width of the hash bucket, which is 500mm here; b is the bias, which is 0 here; H max It is the upper limit of the hash value of the line feature, which is 16 here; Indicates rounding down;

[0079] If the i-th feature is an angle feature, its hash value h i The calculation formula is:

[0080]

[0081] According to the calculation formula of hash value, the range of line feature hash value is 0 to H max -1, the range of the corner feature hash value is H max to 2 hours max -1, so line features and corner features will not be assigned the same hash value;

[0082] S34, calculating the neighboring hash value and the neighboring distance;

[0083] If the i-th feature is a line feature, its neighboring hash value h′ i The calculation formula is:

[0084]

[0085] Where R is the search radius, and the requirement is Here, R=200mm;

[0086] If the i-th feature is a corner feature, its neighboring hash value h′ i The calculation formula is:

[0087]

[0088] The neighboring distance d′ of the i-th feature i The calculation formula is:

[0089]

[0090] S4, matching the features obtained in S2 with the features in the feature map to obtain matched feature pairs;

[0091] The features obtained by S2 are called scan features; the features in the feature map are called map features;

[0092] The feature map is a hash table divided into several hash buckets, each hash bucket corresponds to a hash value and is represented as a linked list; each linked list includes a header and a table node; the header is the hash value of the hash bucket, and the table node stores the map coordinates, feature direction and weight of the map feature;

[0093] In the initial state, there is no feature in the feature map;

[0094] The map coordinates include x-coordinates and y-coordinates;

[0095] For each scan feature, traverse all map features in the hash bucket corresponding to its hash value and calculate the feature distance;

[0096] The feature distance D between the i-th scan feature and the j-th map feature ij The calculation formula is:

[0097]

[0098] Among them, w is the feature direction weight, which is 0.2 here. After traversal, the minimum value in the feature distance is called the minimum feature distance of the hash bucket, which is recorded as the first distance.

[0099] Determine whether the scan feature meets one of the following three conditions: 1) the neighboring hash value is equal to the hash value; 2) the first distance is less than the neighboring distance; 3) the neighboring distance is greater than the search radius. If one of the three conditions is met, find the map feature corresponding to the first distance and combine it with the scan feature to form a matched feature pair. If none of the three conditions are met, traverse all map features in the hash bucket corresponding to the neighboring hash value and obtain the minimum feature distance in the hash bucket, which is recorded as the second distance. Find the map feature corresponding to the smaller value of the first distance and the second distance and combine it with the scan feature to form a matched feature pair.

[0100] According to the calculation formula of hash value and adjacent hash value, the two features in a feature pair can only be line features or angle features. The feature pair with two features that are both line features is called a line feature pair, and the feature pair with two features that are both angle features is called an angle feature pair.

[0101] S5. Calculate the error function of each feature pair based on the feature direction and weight and sum them to obtain the constructed optimization function. Then, update the pose through the Newton iteration method based on the optimization function, including:

[0102] S51. Calculate the error function of each feature pair. If the i-th feature pair is a line feature pair, then its error function E i The calculation formula is:

[0103] E i =(sinα M，i (x S,i -x M,i )+cosα M，i (y M，i -y S,i )) 2 +w(α M，i -α S，i ) 2 ;

[0104] Among them, (x S，i ,y S,i ) is the map coordinate of the scanned feature in the i-th feature pair, α S，i is the feature direction of the scanned feature in the ith feature pair, (x M,i ,y M,i ) is the map coordinate of the map feature in the i-th feature pair, α M，i is the feature direction of the map feature in the i-th feature pair;

[0105] If the i-th feature pair is an angular feature pair, then its error function E i The calculation formula is:

[0106] E i =(x S,i -x M,i ) 2 +(y S,i -y M，i ) 2 +w(α M，i -α S，i ) 2 ;

[0107] S52. Construct optimization function

[0108]

[0109] The N M is the number of feature pairs obtained in S4.

[0110] S53. Update pose;

[0111] The calculation formula for the updated pose is:

[0112]

[0113] described is the updated posture, the above is the Jacobian matrix of the optimization function G, and the above H is the Hessian matrix of the optimization function; after the calculation is completed, the updated posture Assign to

[0114] S6, convergence judgment: judge whether the L2 norm of J is less than the set convergence threshold. If so, jump to S7, otherwise jump to S3;

[0115] S7. Update map;

[0116] During the repetition of step S6, the feature parameters of the scan feature calculated in S3 change due to the change in pose, which in turn changes the pairing relationship between the scan feature and the map feature in S4. The feature pairing relationship is not confirmed until step S6 converges. At this point, the unpaired scan feature is called a mismatched scan feature, and the unpaired map feature is called a mismatched map feature.

[0117] The updating of the map includes the following sub-steps:

[0118] S71, updating mismatch scanning features;

[0119] Store the mismatch scan feature into the corresponding hash bucket in the feature map hash table according to its hash value and assign a weight of 1;

[0120] S72, updating feature pairs;

[0121] The update formula for the feature pair is:

[0122]

[0123] Among them, (x′ M , y′ M ) is the map coordinate of the updated map feature, w′ M is the weight of the updated map feature, (x M ,y M ) is the map coordinate of the map feature before updating, w M is the weight of the map feature before updating, (x S ,y S ) are the map coordinates of the scanned features;

[0124] After the map coordinates are updated, their hash values ​​are recalculated and stored back into the corresponding hash bucket in the feature map hash table based on their hash values;

[0125] S73, updating mismatch map features;

[0126] First, calculate the polar coordinates of each mismatch map feature in the radar coordinate system (ρ M ,θ M ):

[0127]

[0128] Where (x M ,y M ) are the map coordinates of the mismatched map features.

[0129] Then, find the polar angle and θ M The closest lidar data point (ρ P ,θ P ), if ρ P >ρ M +R E , then the weight of the map feature is reduced by 1. E is the laser radar ranging error, which is taken as 30mm here;

[0130] If the weight of the map feature decreases to 0, it will be removed from the feature map hash table;

[0131] S8. Repeat S1 to S7 to achieve real-time position update and map update, that is, to achieve synchronous positioning and mapping.

[0132] Table 1 is a comparison of the efficiency of the method of the present invention, the traditional ICP-SLAM algorithm and the LOAM-SLAM algorithm.

[0133] Table 1 Efficiency comparison of the method of the present invention, the traditional ICP-SLAM algorithm and the LOAM-SLAM algorithm

[0134]

[0135]

[0136] As can be seen from Table 1, compared to the traditional ICP algorithm, the method of the present invention adds a feature extraction step, thereby reducing the number of points to be matched during feature matching, thereby significantly reducing the amount of computation required for feature matching and posture optimization steps. Compared to the LOAM-SLAM algorithm, the method of the present invention obtains the curvature of the data point in the polar coordinate system by an approximate curvature calculation formula, avoids the huge amount of computation required for the polar coordinate system to rectangular coordinate system transformation, simplifies the step of feature extraction, and reduces the amount of computation required for feature extraction; At the same time, local sensitive hashing is used to store and search maps, reducing the amount of computation required for feature matching. The method of the present invention can be deployed on an STM32H7 microprocessor. Compared to a computer, the CPU main frequency of the microprocessor is lower, so the time consuming of each step is increased; In addition, the CPU of the microprocessor uses RISC instructions, and trigonometric function operations require a combination of multiple instructions to complete, so the increase in the time consuming of feature extraction and feature parameter calculation is greater. The laser radar rotation frequency used in Example 1 is 10 Hz, so the total time consumption of the SLAM algorithm shall not exceed 100 ms. The total time consumption of the method described in the present invention on STM32H7 is 14.22 ms, and the total time consumption on i7-10875H is 5.23 ms, both of which meet the real-time requirements.

[0137] Example 2

[0138] This embodiment describes the process of achieving indoor navigation and positioning of a micro-UAV based on the method described in the present invention.

[0139] Figure 3 This is the system structure diagram of the micro indoor drone and its supporting hardware. Figure 3 As shown in the figure, the drone is connected to the remote control via the LORA wireless communication protocol, and the remote control is connected to the computer via the USB interface; the remote control sends movement instructions to the drone, or forwards movement instructions generated by the computer; the drone sends back power, position and map information to the remote control, and the remote control forwards the map information to the computer, realizing the real-time display of the drone's position and the characteristic map of the drone's surrounding environment on the computer.

[0140] Figure 4 The black track in the figure is the movement track of the drone, and the gray dots are the feature maps.

[0141] Figure 5 This is a software structure diagram of a miniature indoor drone that uses the method described in the present invention. Figure 5As shown, the drone software is divided into a driver layer, a real-time operating system layer, a support library layer, and an application software layer; the driver layer includes a LORA driver module, an IMU driver module, an optical flow sensor driver module, and a lidar driver module for remote control and wireless data transmission; the real-time operating system layer adopts an RT-THREAD real-time operating system to ensure that the threads of the application layer do not interfere with each other and execute concurrently; the support library layer is composed of commonly used mathematical operation functions, mainly including matrix operations, posture solution, data statistics, check code calculation, etc.; the application software layer is composed of four threads, including a SLAM thread deployed with the method of the present invention, a flight control thread for controlling the position and attitude of the drone, a remote control thread for controlling real-time communication between the drone and the remote control, and a path following thread. The present invention is divided into three steps when used for indoor navigation and positioning of micro drones, specifically:

[0142] StepA, path following;

[0143] The path following step is controlled by a path following thread; the path following module stores a preset path for the drone's movement, the preset path consisting of a series of waypoints, and the first waypoint is sent to the flight control thread as the desired position of the drone;

[0144] StepB,SLAM;

[0145] The SLAM steps are controlled by the SLAM thread and are divided into 8 sub-steps:

[0146] Step B1, obtaining the laser radar point cloud through the serial port, corresponding to S1 in the invention content;

[0147] The laser radar point cloud is represented as a set of points in the radar polar coordinate system. The number of points of this model of laser radar varies slightly due to the change of the laser radar rotation speed, and is approximately 480 to 520.

[0148] Step B2, extracting features based on curvature, corresponding to Step 1 in the summary of the invention;

[0149] Step B3, calculating characteristic parameters, corresponding to Step 1 in the summary of the invention;

[0150] Step B4, feature matching, corresponding to Step 2 in the summary of the invention;

[0151] Step B5, using Newton iteration method to optimize the posture, corresponding to Step 3 in the invention content;

[0152] Step B6, convergence judgment: If the L2 norm of the Jacobian matrix is ​​less than the convergence threshold (less than 0.1), the observation pose information is sent to the flight control thread and the process jumps to Step B7; otherwise, the process jumps to Step B3. This Step B6 corresponds to Step 4 in the Summary of the Invention.

[0153] Step B7, update the map, send the map information to the remote control thread, and the remote control thread forwards the map information to the computer; this step Step B7 corresponds to Step 5 in the invention summary;

[0154] Step B8, repeat Step B1 to Step B7;

[0155] Step C, flight control;

[0156] The flight control steps are specifically divided into 6 sub-steps:

[0157] Step C1, calculate the expected speed through the PID controller based on the observed pose information given by the SLAM thread and the expected pose information given by the path following thread;

[0158] Step C2: Read the optical flow sensor to obtain the observed speed, and calculate the expected attitude angle through the PID controller based on the observed speed and the expected speed calculated in Step C1;

[0159] Step C3: Read the IMU to obtain the observed angular velocity and observed acceleration, and calculate the observed attitude angle using the Mahony algorithm.

[0160] Step C4: Calculate the desired angular velocity using the PID controller based on the observed attitude angle calculated in Step C3 and the desired attitude angle calculated in Step C2.

[0161] Step C5: Calculate the PWM duty cycle of the four motors using the PID controller based on the observed angular velocity obtained in Step C3 and the desired angular velocity obtained in Step D3.

[0162] Step C6: Send the PWM duty cycle of the motor obtained in Step C5 to the electronic speed controller. The electronic speed controller changes the motor output power according to the duty cycle to achieve flight control.

[0163] The above description is only a preferred embodiment of the present invention, and the present invention should not be limited to the contents disclosed in the embodiment and the accompanying drawings. Any equivalent or modification completed without departing from the spirit disclosed in the present invention shall fall within the scope of protection of the present invention.

Claims

1. A lightweight SLAM method based on a two-dimensional point cloud; characterized in that: include: Step 1: Extract features based on curvature and calculate the characteristic parameters of each feature, specifically: Step 11, calculate the approximate curvature of each data point of the two-dimensional point cloud; Step 12: Take the absolute value of the approximate curvature of all data points and perform non-maximum suppression to select the maximum and minimum values ​​as candidate data points; Step 13: Extract features from candidate data points based on the set line feature recognition threshold, corner feature recognition threshold, and outlier recognition threshold; Step 14. Calculate the characteristic parameters of each feature; The characteristic parameters include hash value, adjacent hash value and adjacent distance; Step 2: Match the features with the map features to obtain matched feature pairs; The features extracted in Step 1 are called scanning features; the map features are the features in the feature map; For each scan feature, traverse all map features in the hash bucket corresponding to its hash value and calculate and obtain the minimum feature distance of the hash bucket, which is recorded as the first distance; determine whether the traversal operation meets the following conditions: 1) the adjacent hash value is equal to the hash value; 2) the first distance is less than the adjacent distance; 3) the adjacent distance is greater than the search radius; if one of 1), 2), or 3) is met, then find the map feature corresponding to the first distance and together with the scan feature, they are a matched feature pair; if 1), 2), or 3) are not met, then traverse all map features in the hash bucket corresponding to the adjacent hash value and obtain the minimum feature distance of the hash bucket, which is recorded as the second distance; find the map feature corresponding to the smaller value of the first distance and the second distance and together with the scan feature, they are a matched feature pair; Step 3: Calculate the error function of each feature pair and sum them to obtain the constructed optimization function. Then, update the pose through the Newton iteration method based on the optimization function. Step 4, Convergence judgment: Determine whether the L2 norm of the Jacobian matrix of the optimization function is less than the set convergence threshold. If so, jump to Step 5, otherwise jump to Step 14; Step 5: Update the mismatch scan features, update the feature pairs, and update the mismatch map features.

2. A lightweight SLAM method according to claim 1, characterized in that, The hash value is calculated using a local sensitive hash function, including a hash value of a line feature and a hash value of an angle feature; The hash value of the line feature is obtained by dividing the sum of the map coordinates by times the hash bucket width plus the bias, rounded down and clipped; The corner feature hash value is the line feature hash value plus the integer H max ; The H max is the upper limit of the hash value of the line feature; The neighboring hash value is the hash value closest to the feature except the hash value of the feature itself; the neighboring distance is the distance between the feature and the neighboring hash value.

3. A lightweight SLAM method according to claim 1, characterized in that, Step 13: The features include line features and angle features; The curvature minimum point whose absolute value of curvature is less than the line feature recognition threshold is a line feature; the curvature maximum point whose absolute value of curvature is greater than the corner feature recognition threshold but the negative number of curvature is not greater than the outlier recognition threshold is a corner feature.

4. A lightweight SLAM method according to claim 1 or 2, characterized in that, Step 14: The feature parameters also include map coordinates and feature directions. The map coordinates are calculated by transforming the feature points in the polar coordinate system of the sensor to obtain rectangular coordinates in the map coordinate system. The feature directions include the feature directions of line features and the feature directions of angular features. The feature directions of line features are the N directions before and after the feature. C The direction of the approximate straight line formed by the data points; the characteristic direction of the angle feature is the direction of the N before and after the feature. C The direction of the sum of the vectors connecting the data points and the feature.

5. A lightweight SLAM method according to claim 1, characterized in that, The feature map in Step 2 is a hash table, which is divided into 2H max -1 hash bucket, each hash bucket corresponds to a hash value and is represented as a linked list; each linked list includes a header and a table node; the header is the hash value of the hash bucket, and the table node stores the map coordinates, feature direction and weight of the map feature.

6. A lightweight SLAM method according to claim 1, characterized in that, Step 3 The error function of the feature pair includes the error function of the line feature pair and the error function of the corner feature pair; the error function of the line feature pair is calculated as the weighted sum of the square of the distance from the scan feature to the straight line where the map feature is located and the square of the difference in the feature directions of the two features; the error function of the corner feature pair is calculated as the square of the feature distance from the scan feature to the map feature.

7. A lightweight SLAM method according to claim 1, characterized in that, The Newton iteration method is used to update the posture, specifically: the updated posture is equal to the posture before the update plus the posture increment, and the posture increment is the matrix product of the Jacobian matrix of the optimization function and the inverse Hessian matrix.

8. A lightweight SLAM method according to claim 1, characterized in that, Step 5 updates the mismatch scan feature, specifically: storing the mismatch scan feature into the corresponding hash bucket in the feature map hash table according to its hash value and assigning a weight to k; the value range of k is 1 to 20.

9. A lightweight SLAM method according to claim 1 or 8, characterized in that, Step 5 updates the feature pair, specifically by taking the weighted average of the map feature coordinates and the scan feature coordinates of the feature point pair as the new map feature coordinates, and adding Δk to the weight; the value range of Δk is (0-1]; after the map coordinates are updated, their hash values ​​are recalculated, and the updated map features are re-stored into the corresponding hash bucket in the feature map hash table according to the hash values.

10. A lightweight SLAM method according to claim 1 or 9, characterized in that: Step 5 updates the mismatch map feature, specifically by mapping the mismatch map feature to the sensor polar coordinate system; finding the sensor data point whose polar angle is closest to the mismatch map feature, and determining the size of the polar diameter between the sensor data point and the mismatch map feature; if the polar diameter of the mismatch map feature is smaller than the sensor data point and the difference in polar diameter is larger than the sensor ranging error, it indicates that the mismatch map feature is unstable; therefore, the weight of the map feature is reduced by Δk; if the weight of the map feature is less than 0, it is removed from the feature map hash table.

Images