Robot positioning and mapping method based on improved octree structure

Abstract

The invention relates to a robot positioning and mapping method based on an improved octree structure. The robot positioning and mapping method comprises the steps that residual errors of the posture, the speed and the pose of a robot are obtained through IMU pre-integration; performing loopback candidate frame retrieval by improving an octree structure; a transformation matrix between loopback frames is calculated through a registration method combining coarse registration based on a random sampling consensus algorithm and fine registration based on an iterative nearest point algorithm, and loopback detection factors are obtained; and introducing the odometer factor, the IMU pre-integration factor and the loopback detection factor into a factor graph optimization model to solve the pose. According to the invention, the decision-making speed and the operation precision of the system under complex working conditions can be greatly improved through the efficient data processing capability, and key technical support is provided for digital transformation of the power industry; and the processing efficiency and the modeling precision of the point cloud data in the synchronous positioning and mapping algorithm process of the robot in the complex electrified environment can be remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of robot perception technology for live bypass operation scenarios, and in particular to a robot positioning and mapping method based on an improved octree structure. Background Art

[0002] With rapid economic development and continued growth in electricity demand, power supply reliability faces higher standards. As a key means of ensuring uninterrupted power supply, the need for intelligent transformation of bypass live working technology is increasingly urgent. However, existing robotic operating systems face bottlenecks in the storage and processing efficiency of sensory data in complex power scenarios. Traditional point cloud storage methods struggle to support real-time access to massive amounts of environmental data, while inefficient retrieval mechanisms hinder the real-time performance of positioning and modeling, directly impacting operational safety and precision.

[0003] To address this core issue, breakthroughs in point cloud storage structure and retrieval speed technology have become the key to achieving efficient full-scene perception. Summary of the Invention

[0004] In order to overcome the above technical problems, the present invention provides a robot positioning and mapping method based on an improved octree structure, which can realize the robot's accurate and rapid positioning and mapping in complex bypass live operation scenarios, can greatly improve the robot's autonomous operation capability in complex live environments, and can efficiently complete the distribution network's non-stop operation and maintenance and inspection work, thereby meeting the modern society's growing demand for non-stop operations.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a robot positioning and mapping method based on an improved octree structure, comprising the following steps:

[0006] Step 1: Obtain the residuals of the robot's posture, velocity, and position through the IMU (Intertial Measurement Unit) pre-integration link;

[0007] Step 2: For the pre-processed robot point cloud data, loop candidate frame retrieval is performed by improving the octree structure;

[0008] Step 3: Calculate the transformation matrix between the loop frames by combining the coarse registration based on the random sampling consensus algorithm and the fine registration based on the iterative closest point algorithm to obtain the loop detection factor;

[0009] Step 4: Use factor graph optimization to introduce the odometry factor, IMU pre-integration factor, and loop detection factor into the factor graph optimization model, solve the pose, and complete the map construction.

[0010] In step 1, the residuals of the robot's posture, velocity, and position are obtained through the IMU pre-integration link, specifically:

[0011] The core function of IMU pre-integration is to integrate the IMU's acceleration and angular velocity data over a local time period to generate compact motion constraints, including displacement, velocity change, and rotation. Specifically, pre-integration calculates the relative displacement, velocity increment, and rotation increment between two moments. It also considers the impact of IMU bias on the measurement data and compensates for it using linearized bias.

[0012] In step 1, the residuals of the robot's rotation transformation, velocity and position are obtained through IMU pre-integration

[0013]

[0014] (1)

[0015] in, , Indicates the The frame transforms the IMU coordinate system into the world coordinate system’s rotation matrix. is the incremental measurement of attitude, obtained from the gyroscope measurements and the estimate of the gyroscope bias, represents the zero bias of the gyroscope, Indicates the The zero-bias measurement noise of the frame.

[0016] (2)

[0017] in, It is the velocity increment calculated based on the measurement value of the IMU accelerometer. is the velocity increment measurement, Indicates the The velocity vector of the frame, It is The time difference between frames, represents the acceleration due to gravity, is the zero bias of the accelerometer, Indicates the The zero-bias measurement noise of the frame.

[0018] (3)

[0019] in, It is the displacement increment calculated based on the measurement value of the IMU accelerometer. is the displacement increment measurement, Indicates the The velocity vector of the frame, It is The time difference between frames, represents the acceleration due to gravity, is the zero bias of the accelerometer, Indicates the The zero-bias measurement noise of the frame.

[0020] In step 2, for the preprocessed robot point cloud data, loop candidate frames are retrieved by improving the octree structure. Specifically, an incremental octree is adopted to optimize storage efficiency and query performance through strategies including incremental update and box deletion.

[0021] Due to the high-frequency sampling characteristics of LiDAR, the typical frequency is 10Hz-20Hz. If loop closure detection is performed on each frame of point cloud, the consumption of computing resources will increase exponentially, making it difficult to meet the real-time requirements of bypass operations. To this end, the present invention introduces the key frame screening criteria:

[0022] Keyframe selection conditions: Set the time interval threshold and motion distance threshold. Only when the robot posture changes beyond the preset range, the current frame will be marked as a keyframe.

[0023] Loop closure candidate frame retrieval: Filter temporally and spatially adjacent candidate frames from the historical keyframe library.

[0024] Step 2 specifically includes the following steps:

[0025] Step 2-1, data structure and construction: build incremental octree;

[0026] Step 2-2, incremental update: When inserting a new point, consider whether the point is outside the axis-aligned bounding box range of the original tree. If the point is outside the range of the octree, expand the bounding box by creating a new root octant whose child octant contains the current root octant. Then, add the new point to the expanded octree; perform downsampling operations while inserting the point;

[0027] Step 2-3, box deletion: Check whether the octant is within the given bounding box. All octants within the given bounding box will be deleted directly. For leaf octants that overlap with the given bounding box, delete the points within the range and allocate new memory segments for the remaining points. If a leaf octant does not contain any points after deletion, it will be deleted.

[0028] Step 2-4, K-nearest neighbor search: Utilize the axis-aligned box of each octant to accelerate the nearest neighbor search, using the bounding overlapping sphere test and a priority search order precomputed based on the fixed index of the sub-octant;

[0029] Step 2-5, radius search: Use radius neighbor search method to search.

[0030] Step 2-1, data structure and construction specific methods are:

[0031] An octree node has up to eight child nodes, each of which corresponds to an eighth of the axis-aligned cube covering it; the area after each cube is partitioned into eighths is called an octant; each child node will have a center as and the sides are equal in length Starting with the axis-aligned bounding box of , recursively subdivide into smaller octants of side length 0.5e until it contains fewer than the given number of points b. size , or its side length is less than the minimum side length e min , R represents the set of real numbers;

[0032] For memory efficiency, octants without points are not created, and only the indices and coordinates of points are stored in the octant of each leaf node. Octants of non-leaf nodes have no points. To achieve extremely fast access to every point in each leaf octant, a local space contiguous storage strategy is used to access points in each leaf octant. This local space contiguous storage strategy reallocates space for contiguous memory segments storing point information in leaf octants (octants consisting of leaf nodes) after subdivision. Furthermore, this reallocation facilitates per-box deletion and incremental updates because it allows operations on a section of memory without affecting other sections.

[0033] Based on the above considerations, for an octet C0 in the octree, the center , side length e0, point I0 storing coordinates and index, and pointer to the first sub-eighth partition address. The subscript "0" is used to distinguish different eight partitions. Let C r is the root octant (octant consisting of the root node), I r 、e r and are the point, side length and center of the root octant respectively;

[0034] When constructing the incremental octree, invalid points are first removed, the axis-aligned bounding boxes of all valid points are calculated, and only the indices and coordinates of the valid points are retained; then, starting from the root node, the axis-aligned bounding box is recursively split into eight cubes indexed by the Morton code at the center, and all points in the current octant are subdivided into each cube according to the calculated cube index; when the stopping condition is met, a leaf octant is created and a continuous memory is allocated to store the information of the points in the leaf node;

[0035] Step 2-2: The specific method for incremental update is:

[0036] When inserting a new point, consider whether the point is outside the axis-aligned bounding box of the original tree. If the point is outside the range of the octree, expand the bounding box by creating a new root octant whose child octants contain the current root octant. This process may need to be performed multiple times to ensure that all new points are within the range of the tree. Then, add the new point to the expanded octree;

[0037] Considering the need for efficient point query in robotics applications, the octree supports downsampling while inserting points. Downsampling focuses on new points and deletes points that meet certain conditions: these deletable points are subdivided into a leaf octant whose range is less than 2e min and its size is greater than b size / 8; The process of adding a new point to the target octant C0 is as follows: if the target octant C0 is a leaf node and meets the subdivision criteria, all points in the target octant C0, that is, old points and newly added points, will be recursively subdivided into sub-octants; if downsampling is enabled, and , the new point will be deleted later instead of being added to the target octant C0; otherwise, a continuous memory segment will be allocated for the updated point; if the target octant C0 has sub-octants, the newly added point will be assigned to each sub-octant, and only the new point needs to be subdivided;

[0038] Step 2-3, the specific method of cassette deletion is:

[0039] When removing unnecessary points within an axis-aligned cube, the octree first checks whether the octant is within the given bounding box, rather than directly searching for points within the given space and deleting them. All octants within the given bounding box will be deleted directly without searching for points within them, which significantly reduces the deletion time. Due to the local space continuous storage strategy, the deletion of an octant will not affect other octants. For leaf octants that overlap with the given bounding box, the points within the range are deleted and a new memory segment is allocated for the remaining points; if a leaf octant does not contain any points after deletion, it will be deleted;

[0040] The specific method of step 2-4, K nearest neighbor search is:

[0041] Using octree, for any query point q index Retrieve the k nearest neighbors; the nearest search on the octree is an exact nearest search. By maintaining a priority queue h, whose maximum length is l h , used to store the k nearest neighbors encountered so far and their relationships with the query point q indexThe last element of h always has the largest distance, whether pushed or popped; the axis-aligned boxes of each octant are effectively exploited to speed up the nearest neighbor search, using a bounding overlapping sphere test and a priority search order precomputed according to the fixed index of the sub-octant:

[0042] First, recursively search the octree from its root node until it reaches the node closest to q index The leaf node of index The distances to all points in the leaf nodes and their corresponding indices are pushed into the priority queue h; before h is filled, all leaf nodes encountered will be searched; if h is full and is filled by q index and the maximum distance d in h max The search ball S(q index ,d max ) is within the axis-aligned box of the current octant, the search ends; if an octant C k Does not include the search ball S(q index ,d max ), then one of the three conditions in formulas (4)-(6) must be met:

[0043] (4)

[0044] (5)

[0045] (6)

[0046] in, , , e k Indicates eight partitions C k The side length of

[0047] If the above conditions are not met, the search ball is located in the eight partitions; by querying and searching S(q index ,d max ) Update h by the overlapping octaves of the balls and define q index and C k The distance d between

[0048] (7)

[0049] Among them, when x>0, , in other cases ;

[0050] When d<d max When C k With S(q index ,d max ) overlap, in order to speed up this process, according to C nThe candidate sub-octant is partitioned into C k The distances are sorted and 8 different sequences are obtained; in this way, closer octets will be searched earlier and the search will end earlier.

[0051] Steps 2-5, Radius Search:

[0052] For any query point and radius r, the radius neighbor search method finds Each point p; in verifying S(q index ,r) whether it completely contains an octant C k Before, we need to judge r 2 Is it greater than .

[0053] In step 3, the transformation matrix between the loop frames is calculated by combining the coarse registration based on the random sampling consensus algorithm and the fine registration based on the iterative closest point algorithm to obtain the loop detection factor, which is specifically:

[0054] Point cloud registration, converted into mathematical form:

[0055] (8)

[0056] in, and Represent the points in the source point cloud and the corresponding points in the target point cloud, and Respectively represent the optimal rotation matrix and translation vector to be sought. represents the number of points in the source point cloud, represents the rotation matrix, Represents the translation vector.

[0057] Through the above formula, point cloud registration can be converted into a least squares optimization problem, the purpose of which is to find the optimal and Minimize the gap between the two point clouds.

[0058] The specific registration process of step 3 is as follows:

[0059] Step 3-1: Rough registration based on random sampling consensus algorithm:

[0060] Random sampling: randomly select a set of point pairs from the source point cloud and the target point cloud, assuming that they are correct matching point pairs, to estimate the rigid body transformation matrix between the point clouds, including rotation and translation;

[0061] Model fitting: Use the selected point pairs to calculate the initial transformation matrix and transform the source point cloud into the coordinate system of the target point cloud;

[0062] Consistency assessment: Evaluate the degree of match between the transformed point cloud and the target point cloud; use the distance threshold to determine whether it is an inlier point; the more inliers there are, the more robust the model is.

[0063] Repeated iteration: Repeat the above steps several times, randomly sampling to generate a new model each time, and finally select the model with the largest number of inliers as the best transformation;

[0064] Step 3-2: Precise registration based on iterative closest point algorithm:

[0065] Find the closest point:

[0066] Using the initial rotation matrix R0 and translation vector t0 or the R obtained in the last iteration m-1 and t m-1 , m represents the number of iterations, , transform the initial point cloud to obtain a temporary transformed point cloud, and then use this point cloud to compare with the target point cloud to find the nearest neighbor point of each point in the source point cloud in the target point cloud;

[0067] Solve for the optimal transformation:

[0068] First, find the optimal translation, let N p =|P s |, represents the number of points in the source point cloud. The difference between the two point clouds, the loss function loss is expressed as a function of the translation vector t and the rotation matrix R:

[0069] (9)

[0070] (10)

[0071] in, , , and represent the centroids of the target point cloud and the source point cloud respectively.

[0072] The optimal translation vector is obtained by minimizing the loss function loss and the optimal rotation matrix :

[0073] (11)

[0074] Optimal rotation matrix :

[0075] (12)

[0076] Among them, U and V are The unitary matrix produced by the singular value decomposition is , is a semi-positive definite diagonal matrix whose diagonal elements are The singular values ​​of .

[0077] The matching accuracy is evaluated by the mean square error. If the matching error is lower than the preset threshold, the translation error is less than 0.1m, and the rotation error is less than 1°, then a valid loop is detected and the point cloud registration is successful.

[0078] In step 4, the odometry factor, IMU pre-integration factor, and loop closure factor are introduced into the factor graph optimization model through factor graph optimization. iSAM2 (Incremental Smoothing and Mapping Using the BayesTree) is used to solve the pose and complete the map construction. iSAM2 is an optimization algorithm for incremental smoothing and mapping.

[0079] The specific method of step 4 is:

[0080] Use iSAM2 based on Bayesian tree optimization to solve the factor graph:

[0081] First, the nonlinear factor set F and the linearization point set And the Bayesian tree T is initialized to empty, that is, ; Then the new factor Add nonlinear factor set F and initialize new variables And add it to the linearization point set Then, the labeled state quantity M is obtained through smoothing and linearization, and the relevant factors are eliminated using F and M, the Bayesian tree is reconstructed, and the update amount Δ caused by the addition of the new factor is solved; finally, Update to new ;

[0082] In the iSAM2 algorithm flow, the smoothing and linearization module linearizes the nonlinear problem, performs quadratic approximation on the objective function through the Gauss-Newton method or Lie algebra expansion, and generates a linear equation. The state marker is used to track which variables need to be updated and mark unaffected variables as inactive to avoid repeated calculations. The elimination and Bayesian tree reconstruction module re-performs Gaussian elimination of the sparse matrix on the affected parts and reconstructs the Bayesian tree.

[0083] By solving the factor graph optimization problem using the iSAM2 method described above, we can obtain the actual position of the robot in the scene. Then, we can convert the point cloud in the radar coordinate system into the global coordinate system to obtain a global map.

[0084] This invention presents a robotic positioning and mapping method based on an improved octree structure, primarily applied to the field of scene perception technology for live distribution network bypass operations. Through innovative point cloud data compression storage architecture and spatial indexing mechanisms, it significantly improves the read and write efficiency of high-density point clouds. This not only provides a highly timely foundation for environmental perception during live-line robotic operations, but also promotes technological upgrades to intelligent power operations and maintenance. Combined with an optimized nearest neighbor search algorithm, it enables rapid matching of key feature points, providing real-time data support for back-end optimization of simultaneous localization and mapping (SLAM). This efficient data processing capability significantly improves the system's decision-making speed and operational accuracy under complex operating conditions, significantly contributing to ensuring operational safety and improving power supply reliability, and providing key technical support for the digital transformation of the power industry. This technological breakthrough significantly enhances the processing efficiency and modeling accuracy of point cloud data during the robot's simultaneous localization and mapping algorithm in complex live environments, addressing challenges such as delays in 3D cable modeling and lags in response to dynamic obstacles in operational scenarios.

[0085] The beneficial effects of the present invention are specifically as follows:

[0086] (1) By using the IMU pre-integration factor, the initial value is separated, frequent integration processes are avoided, and the computational cost is significantly reduced;

[0087] (2) A loop detection method that combines random sampling consistent coarse registration with iterative nearest point fine registration is used to effectively avoid the local optimal problem caused by using only iterative nearest point fine registration and significantly improve the loop detection accuracy;

[0088] (3) By designing a key frame storage structure based on an improved octree, the loop detection process is further accelerated and more efficient loop detection is achieved;

[0089] (4) The laser odometry factor, IMU pre-integration factor, and loop detection factor are integrated into the factor graph optimization framework. After optimization, accurate robot pose information is obtained, achieving the coordinated completion of mapping and positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] The present invention will be further described below with reference to the accompanying drawings and examples.

[0091] Figure 1 It is a schematic diagram of the process of the present invention.

[0092] Figure 2 This is a schematic diagram of the local space continuous storage strategy involved in the present invention. (1) shows that the midpoints of the eight partitions are stored in a dispersed manner, and (2) shows that the midpoints of the eight partitions are stored in a continuous manner.

[0093] Figure 3 Schematic diagram of adding new points to the improved octree involved in the present invention.

[0094] Figure 4 This is a schematic diagram of the improved octree new node storage method involved in the present invention.

[0095] Figure 5 It is a schematic diagram of the cassette deletion involved in the present invention.

[0096] Figure 6 This is a schematic diagram of the point cloud registration process involved in the present invention.

[0097] Figure 7 This is a comparison diagram before and after the introduction of loop closure detection involved in the present invention, (1) is the mapping result before the introduction of loop closure, and (2) is the mapping result after the introduction of loop closure.

[0098] Figure 8 This is a schematic diagram of the iSAM2 algorithm flow chart involved in the present invention.

[0099] Figure 9 This is a schematic diagram of the Bayesian tree reconstruction steps involved in the present invention.

[0100] Figure 10 This is a schematic diagram of the 10kV overhead line experimental site involved in the present invention.

[0101] Figure 11 This is a schematic diagram of the SLAM mapping effect involved in the present invention.

[0102] Figure 12 Schematic diagrams showing the comparison of mapping effects for the bypass operation scenarios involved in the present invention, (1) showing the comparison of mapping effects for scenario one, (2) showing the comparison of mapping effects for scenario two, and (3) showing the comparison of mapping effects for scenario three. DETAILED DESCRIPTION

[0103] In order to illustrate the technical solution and technical purpose of the present invention, the present invention is further introduced below in conjunction with the accompanying drawings and the best embodiment.

[0104] like Figure 1 As shown, a robot positioning and mapping method based on an improved octree structure includes the following steps:

[0105] Step 1: Obtain the residuals of the robot’s posture, velocity, and position through the IMU pre-integration link, specifically:

[0106] The core function of IMU pre-integration is to integrate the IMU's acceleration and angular velocity data within a local time period to generate compact motion constraints, including displacement, velocity change, and rotation. Specifically, pre-integration calculates the relative displacement, velocity increment, and rotation increment between two moments. It also considers the impact of IMU bias on the measurement data and compensates for it using linearized bias.

[0107] The residuals of the robot's rotation transformation, velocity and position can be obtained through IMU pre-integration :

[0108] (1)

[0109] in, , Indicates the The frame transforms the IMU coordinate system into the world coordinate system’s rotation matrix. is the incremental measurement of attitude, obtained from the gyroscope measurements and the estimate of the gyroscope bias, represents the zero bias of the gyroscope, Indicates the The zero-bias measurement noise of the frame.

[0110] (2)

[0111] in, It is the velocity increment calculated based on the measurement value of the IMU accelerometer. is the velocity increment measurement, Indicates the The velocity vector of the frame, It is The time difference between frames, represents the acceleration due to gravity, is the zero bias of the accelerometer, Indicates the The zero-bias measurement noise of the frame.

[0112] (3)

[0113] in, It is the displacement increment calculated based on the measurement value of the IMU accelerometer. is the displacement increment measurement, Indicates the The velocity vector of the frame, It is The time difference between frames, represents the acceleration due to gravity, is the zero bias of the accelerometer, Indicates the The zero-bias measurement noise of the frame.

[0114] Step 2: For the pre-processed point cloud data, loop candidate frame retrieval is performed by improving the octree structure, specifically:

[0115] Due to the high-frequency sampling characteristics of LiDAR, the typical frequency is 10Hz-20Hz. If loop closure detection is performed on each frame of point cloud, the consumption of computing resources will increase exponentially, making it difficult to meet the real-time requirements of bypass operations. To this end, the present invention introduces the key frame screening criteria:

[0116] Keyframe selection conditions: Set the time interval threshold and motion distance threshold. Only when the robot posture changes beyond the preset range, the current frame will be marked as a keyframe.

[0117] Loop closure candidate frame retrieval: Filter temporally and spatially adjacent candidate frames from the historical keyframe library.

[0118] Step 2-1, data structure and construction:

[0119] An octree node has up to eight child nodes, each of which corresponds to an eighth of the axis-aligned cube covering it; the area after each cube is partitioned into eighths is called an octant; each child node will have a center as and the sides are equal in length Starting with the axis-aligned bounding box of , recursively subdivide into smaller octants of side length 0.5e until it contains fewer than the given number of points b. size , or its side length is less than the minimum side length e min , R represents the set of real numbers; for memory efficiency, octants without points will not be created. In addition, the index and coordinates of the points are only kept in the octant of each leaf node, and there are no points in the octants of non-leaf nodes. In order to achieve extremely fast access to each point in each leaf octant, a local space continuous storage strategy is used, which reallocates space for the continuous memory segments storing point information in the leaf octant (the octant formed by the leaf nodes) after subdivision. In addition, reallocation facilitates box-by-box deletion and incremental update because it allows operating on a section of memory without affecting other parts. Figure 2 As shown, Figure 2 Figure (1) shows that the points in the eight partitions are stored in a scattered manner at the beginning, and Figure (2) shows how the points in the eight partitions are stored in the memory after the continuous storage strategy is adopted. Figure 2 Here, 100 represents the location of the point in memory, and 200 represents the location of the point in the octant.

[0120] Based on the above considerations, for an octet C0 in the octree, the center , side length e0, point I0 storing coordinates and index, and pointer to the address of the first sub-octant. The subscript "0" is used to distinguish different octants. In particular, let C r is the root octant (octant consisting of the root node), I r 、e r and are the point, side length, and center respectively.

[0121] When constructing the incremental octree, invalid points are first removed, the axis-aligned bounding box of all valid points is calculated, and only the indices and coordinates of the valid points are retained. Then, starting from the root node, the axis-aligned bounding box is recursively split into eight cubes indexed by the Morton code at the center, and all points in the current octant are subdivided into each cube according to the calculated cube index. When the stopping condition is met, a leaf octant is created and a continuous memory is allocated to store the information of the points in the leaf node.

[0122] Step 2-2, incremental update:

[0123] Combine Figure 3 and Figure 4 When inserting new points, we must consider the possibility that some points may be outside the axis-aligned bounding box of the original tree. Once a point is outside the range of the octree, the bounding box must be expanded by creating a new root octant whose children contain the current root octant. This process may need to be performed multiple times to ensure that all new points are within the range of the tree. The new point is then added to the expanded octree. Figure 3 and Figure 4 shows the process of inserting a new point into the octree, Figure 3 Medium cube 300 and Figure 4 The midpoint 600 is a new point. Figure 3 In , cube 300 is both the root octant and the leaf octant, and contains the points at the beginning. After the insertion, the root octant becomes cube 500. Figure 4 In FIG, node 600 is the root octant, which is updated after the new octant is inserted. The area selected by the dotted box is the new octant.

[0124] Considering the need for efficient point query in robotics applications, the octree supports downsampling while inserting points. Downsampling focuses on new points and deletes points that meet certain conditions: these deletable points are subdivided into a leaf octant whose range is less than 2e min and its size is greater than b size / 8; The process of adding a new point to the target octant C0 is as follows: if the target octant C0 is a leaf node and meets the subdivision criteria, all points in the target octant C0, that is, old points and newly added points, will be recursively subdivided into sub-octants; if downsampling is enabled, and , the new point will be deleted later instead of being added to the target octant C0; otherwise, a continuous memory segment will be allocated for the updated point; if the target octant C0 has sub-octants, the newly added point will be assigned to each sub-octant, and only the new point needs to be subdivided;

[0125] Step 2-3, cassette deletion:

[0126] Combine Figure 5 , when removing unnecessary points in an axis-aligned cube, the octree first checks whether the octant is within the given bounding box, rather than directly searching for points in the given space and deleting them. All octants within the given bounding box will be deleted directly without searching for points in them, which significantly reduces the deletion time. Due to the local space continuous storage strategy, the deletion of an octant will not affect other octants. For leaf octants that overlap with the given bounding box, the points within the range are deleted and a new memory segment is allocated for the remaining points. If a leaf octant does not contain any points after deletion, it will be deleted. Figure 5 As shown, box 00 is the designated deletion area, and 0 to 7 are the indices of the eight quadrants obtained through Morton encoding. The quadrants with indices 0, 1, 4, and 5 are directly deleted. Since the 7th quadrant has no points, it is also deleted.

[0127] Step 2-4, K nearest neighbor search:

[0128] Using octree, we can find any query point q index Retrieve the k nearest neighbors. The nearest search on the octree is an exact nearest search. By maintaining a priority queue h, whose maximum length is l h , used to store the k nearest neighbors encountered so far and their relationships with the query point q index The distance. Whether pushed or popped, the last element of h always has the largest distance. The axis-aligned boxes of each octant are effectively exploited to speed up the nearest neighbor search, using a bounding overlap sphere test and a priority search order precomputed based on the fixed index of the sub-octant.

[0129] First, recursively search the octree from its root node until it reaches the node closest to q index Then, q index The distances to all points in the leaf nodes and their corresponding indices are pushed into the priority queue h. Before h is filled, all leaf nodes encountered will be searched. If h is full and is filled by q index and the maximum distance d in h max The search ball S(q index ,d max ) is within the axis-aligned box of the current octant, the search ends. k Does not include the search ball S(q index ,d max ), then one of the three conditions in formulas (4)-(6) must be met:

[0130] (4)

[0131] (5)

[0132] (6)

[0133] in, , , e k Indicates eight partitions C k The side length of

[0134] If the above conditions are not met, the search ball is located in the eight partitions; by querying and searching S(q index ,d max ) Update h by the overlapping octaves of the balls and define q index and C k The distance d between

[0135] (7)

[0136] Among them, when x>0, , in other cases ;

[0137] when When C k With S(q index ,d max ) overlap, in order to speed up this process, according to C n The candidate sub-octant is partitioned into C k The distances are sorted and 8 different sequences are obtained. In this way, the closer octants will be searched earlier and the search will end earlier.

[0138] Steps 2-5, Radius Search:

[0139] For any query point and radius r, the radius neighbor search method finds Each point p. In verifying S(q index , r) whether it completely contains an octant C k Before, you need to judge Is it greater than Also, try to avoid extracting square roots in your algorithms.

[0140] Step 3: By combining the coarse registration based on the random sampling consensus algorithm and the fine registration based on the iterative closest point algorithm, the transformation matrix between the loop frames is calculated to obtain the loop detection factor, which is specifically:

[0141] Point cloud registration can be converted into mathematical form:

[0142] (8)

[0143] in, and They represent the points in the source point cloud and the corresponding points in the target point cloud respectively. and Respectively represent the optimal rotation matrix and translation vector to be sought. represents the number of points in the source point cloud, represents the rotation matrix, Represents the translation vector.

[0144] Through the above formula, point cloud registration can be converted into a least squares optimization problem, the purpose of which is to find the optimal and Minimize the gap between the two point clouds.

[0145] The specific registration process of step 3 is as follows:

[0146] Step 3-1: Rough registration based on random sampling consensus algorithm:

[0147] Random sampling: A set of point pairs are randomly selected from the source point cloud and the target point cloud, assuming that they are correct matching point pairs, and used to estimate the rigid body transformation matrix between the point clouds, including rotation and translation.

[0148] Model fitting: The selected point pairs are used to calculate the initial transformation matrix to transform the source point cloud into the coordinate system of the target point cloud.

[0149] Consistency assessment: Evaluate the degree of match between the transformed point cloud and the target point cloud, and use a distance threshold to determine whether it is an inlier point. The more inliers there are, the more robust the model is.

[0150] Repeated iteration: Repeat the above steps several times, randomly sampling to generate a new model each time, and finally select the model with the largest number of inliers as the best transformation.

[0151] Combine Figure 6 , step 3-2, the precise registration based on the iterative closest point algorithm is as follows:

[0152] Find the closest point:

[0153] Using the initial rotation matrix R0 and translation vector t0 or the R obtained in the last iteration m-1 and t m-1 , m represents the number of iterations, , transform the initial point cloud to obtain a temporary transformed point cloud, and then use this point cloud to compare with the target point cloud to find the nearest neighbor point of each point in the source point cloud in the target point cloud.

[0154] Solve for the optimal transformation:

[0155] First, find the optimal translation, let N p =|P s |, represents the number of points in the source point cloud. The difference between the two point clouds, the loss function loss is expressed as a function of the translation vector t and the rotation matrix R:

[0156] (9)

[0157] (10)

[0158] in, , , and represent the centroids of the target point cloud and the source point cloud respectively.

[0159] The optimal translation vector is obtained by minimizing the loss function loss and the optimal rotation matrix :

[0160] (11)

[0161] Optimal rotation matrix :

[0162] (12)

[0163] Among them, U and V are The unitary matrix produced by the singular value decomposition is , is a semi-positive definite diagonal matrix whose diagonal elements are The singular values ​​of .

[0164] The matching accuracy is evaluated by the mean square error. If the matching error is lower than the preset threshold, the translation error is less than 0.1m, and the rotation error is less than 1°, then a valid loop is detected and the point cloud registration is successful.

[0165] like Figure 7 As shown, Figure 7 (1) shows the mapping effect without loop detection. It can be seen that when the bypass robot passes through the same path, it is not recognized and drifts, resulting in ghosting in the image. Figure 7 (2) After the introduction of loop closure detection, the image remains consistent when it passes through the same position twice.

[0166] In step 4, the odometry factor, IMU pre-integration factor, and loop detection factor are introduced into the factor graph optimization model through factor graph optimization, and the pose is solved by iSAM2 to complete the map construction. Specifically:

[0167] The present invention uses iSAM2 based on Bayesian tree optimization to solve the factor graph.

[0168] Combine Figure 8 First, the nonlinear factor set F and the linearization point set And the Bayesian tree T is initialized to empty, that is, ; Then the new factor Add nonlinear factor set F and initialize new variables And add it to the linearization point set Then, the labeled state quantity M is obtained through smoothing and linearization, and the relevant factors are eliminated using F and M, the Bayesian tree is reconstructed, and the update amount Δ caused by the addition of the new factor is solved; finally, Update to new .

[0169] In the iSAM2 algorithm process, the smoothing and linearization module linearizes the nonlinear problem, performs quadratic approximation on the objective function through the Gauss-Newton method or Lie algebra expansion, and generates a linear equation; the marking state is used to track which variables need to be updated, and the unaffected variables are marked as inactive to avoid repeated calculations; the elimination and reconstruction of the Bayesian tree module re-performs the Gaussian elimination of the sparse matrix for the affected parts and reconstructs the Bayesian tree. Figure 9 As shown, Figure 9 (1) is the original Bayesian tree, Figure 9 (2) From Figure 9 (1) The factor graph obtained by transformation, the dotted line between x1 and x2 indicates that a loop is added, which has no effect on l2. Figure 9 (3) represents the transformed Bayesian network, Figure 9 (4) represents the updated Bayesian tree. It can be seen that after adding the new factor, only the affected part of the Bayesian tree has changed. Figure 9 In the figure, x1, x2, x3 represent the state of the robot, and l1, l2 represent the state of the landmark points.

[0170] By solving the factor graph optimization problem using the iSAM2 method described above, we can obtain the actual position of the robot in the scene. Then, by converting the point cloud in the radar coordinate system to the global coordinate system, we can obtain the global map.

[0171] like Figure 10 As shown, in the outdoor simulated bypass operation scenario, there are multiple utility poles connected by cables, some trees obstructing the view, and shrubs in the lower right corner. The obstruction of the right utility pole by the trees places high demands on the actual mapping process.

[0172] Figure 11The display is the ROS Visualization Tool (rviz) interface under the ROS (Robot Operating System), which is the 3D visualization platform in the ROS system. Figure 10 The live working scene is the central area, and the result of mapping the environment around the live working scene is shown in the figure. In order to show the details of the scene, some areas are enlarged, such as Figure 12 As shown in the figure, the electric poles and cables are well restored, and the details of the trunks and branches of the surrounding trees can also be captured. Figure 12 In (3), the electric pole on the right side is clearly visible, which is blocked by trees. This proves that the method proposed in this invention can well complete the mapping of bypass operation scenes.

[0173] The robot positioning and mapping method based on the improved octree structure of the present invention has the following innovations:

[0174] 1. Through the IMU pre-integration factor, the initial value is separated to avoid frequent integration processes and significantly reduce the calculation cost;

[0175] 2. A loop detection method that combines random sampling consistent coarse registration with iterative closest point fine registration effectively avoids the local optimum problem caused by using only iterative closest point fine registration and significantly improves registration accuracy.

[0176] 3. By designing a key frame storage structure based on an improved octree, the loop detection process is further accelerated, achieving more efficient loop detection;

[0177] 4. Integrate the laser odometry factor, IMU pre-integration factor, and loop detection factor into the factor graph optimization framework. After optimization, accurate robot pose information is obtained, achieving the coordinated completion of mapping and positioning.

[0178] With the above-described preferred embodiments of the present invention as a guide, and with reference to the above description, relevant personnel are fully capable of making various changes and modifications without departing from the technical scope of this invention. The technical scope of this invention is not limited to the contents of the specification and must be determined according to the scope of the claims.

Claims

1. A robot positioning and mapping method based on an improved octree structure, characterized in that: The following steps are involved: Step 1: Obtain the residuals of the robot’s posture, velocity, and position through the IMU pre-integration link; Step 2: For the pre-processed robot point cloud data, loop closure candidate frame retrieval is performed by improving the octree structure. An incremental octree is used to optimize storage efficiency and query performance through strategies including incremental update and box deletion. Step 3: Calculate the transformation matrix between the loop frames by combining the coarse registration based on the random sampling consensus algorithm and the fine registration based on the iterative closest point algorithm to obtain the loop detection factor; Step 4: Use factor graph optimization to introduce the odometry factor, IMU pre-integration factor, and loop detection factor into the factor graph optimization model, solve the pose, and complete the map construction.

2. The robot positioning and mapping method based on the improved octree structure according to claim 1, characterized in that: In step 1, the residuals of the robot's posture, velocity, and position are obtained through the IMU pre-integration link, specifically: The core function of IMU pre-integration is to integrate the acceleration and angular velocity data of the IMU in a local time period to generate compact motion constraints, and calculate the relative displacement, velocity increment and rotation increment between two moments through pre-integration.

3. The robot positioning and mapping method based on the improved octree structure according to claim 2, characterized in that: In step 1, the residuals of the robot's rotation transformation, velocity and position are obtained through IMU pre-integration : （1） in, , Indicates the The frame transforms the IMU coordinate system into the world coordinate system’s rotation matrix. is the incremental measurement of attitude, obtained from the gyroscope measurements and the estimate of the gyroscope bias, represents the zero bias of the gyroscope, Indicates the Zero-bias measurement noise of the frame; （2） in, It is the velocity increment calculated based on the measurement value of the IMU accelerometer. is the velocity increment measurement, Indicates the The velocity vector of the frame, It is The time difference between frames, represents the acceleration due to gravity, is the zero bias of the accelerometer, Indicates the Zero-bias measurement noise of the frame; （3） in, It is the displacement increment calculated based on the measurement value of the IMU accelerometer. is the displacement increment measurement, Indicates the The velocity vector of the frame, It is The time difference between frames, represents the acceleration due to gravity, is the zero bias of the accelerometer, Indicates the The zero-bias measurement noise of the frame.

4. The robot positioning and mapping method based on the improved octree structure according to claim 1, characterized in that: Step 2 specifically includes the following steps: Step 2-1, data structure and construction: build incremental octree; Step 2-2, incremental update: When inserting a new point, consider whether the point is outside the axis-aligned bounding box range of the original tree. If the point is outside the range of the octree, expand the bounding box by creating a new root octant whose child octant contains the current root octant. Then, add the new point to the expanded octree; perform downsampling operations while inserting the point; Step 2-3, box deletion: Check whether the octant is within the given bounding box. All octants within the given bounding box will be deleted directly. For leaf octants that overlap with the given bounding box, delete the points within the range and allocate new memory segments for the remaining points. If a leaf octant does not contain any points after deletion, it will be deleted. Step 2-4, K-nearest neighbor search: Utilize the axis-aligned box of each octant to accelerate the nearest neighbor search, using the bounding overlapping sphere test and a priority search order precomputed based on the fixed index of the sub-octant; Step 2-5, radius search: Use radius neighbor search method to search.

5. The robot positioning and mapping method based on the improved octree structure according to claim 4, characterized in that: Step 2-1, data structure and construction specific methods are: Recursively subdivide the three-dimensional space into axis-aligned cubic octants, with each node containing at most eight children. If the number of points in the eight partitions exceeds the threshold b size And the side length is greater than the minimum side length e min , then continue to subdivide; Only the index and coordinates of the points are stored in the leaf nodes, and the storage space is dynamically allocated using the local continuous memory strategy; The Morton code index of the calculated point is used to assign it to the corresponding sub-octant.

6. The robot positioning and mapping method based on the improved octree structure according to claim 1, characterized in that: In step 3, the transformation matrix between the loop frames is calculated by combining the coarse registration based on the random sampling consensus algorithm and the fine registration based on the iterative closest point algorithm to obtain the loop detection factor, which is specifically: Point cloud registration, converted into mathematical form: （8） in, and Represent the points in the source point cloud and the corresponding points in the target point cloud, and Respectively represent the optimal rotation matrix and translation vector to be sought, represents the number of points in the source point cloud, represents the rotation matrix, Represents the translation vector.

7. The robot positioning and mapping method based on the improved octree structure according to claim 6, characterized in that: The specific registration process of step 3 is as follows: Step 3-1: Rough registration based on random sampling consensus algorithm: Randomly sample point pairs from the source point cloud and the target point cloud to estimate the rigid body transformation matrix; Evaluate the number of internal points in the transformed point cloud by using a distance threshold; Iteratively perform sampling and evaluation, and select the transformation with the largest number of inliers as the optimal coarse registration result; Step 3-2: Precise registration based on iterative closest point algorithm: Corresponding point search: Use the coarse registration result R0 and the translation vector t0 or the R obtained in the last iteration m-1 and t m-1 Transform the source point cloud and find the nearest neighbor point for each source point in the target point cloud. m represents the number of iterations. ; Find the nearest point: Use the initial rotation matrix R0 and translation vector t0 or R obtained from the last iteration m-1 and t m-1 , m represents the number of iterations, , transform the initial point cloud to obtain a temporary transformed point cloud, and then use this point cloud to compare with the target point cloud to find the nearest neighbor point of each point in the source point cloud in the target point cloud; Solve the optimal transformation: First solve the optimal translation, let N p =|P s |, represents the number of points in the source point cloud; the difference between the two point clouds, the loss function loss is expressed as a function of the translation vector t and the rotation matrix R; the optimal translation vector is obtained by minimizing the loss function loss and the optimal rotation matrix ; Matching accuracy is evaluated by mean square error.

8. The robot positioning and mapping method based on the improved octree structure according to claim 1, characterized in that: In step 4, the odometry factor, IMU pre-integration factor, and loop detection factor are introduced into the factor graph optimization model through factor graph optimization, and the pose is solved through iSAM2 to complete the map construction.

9. The robot positioning and mapping method based on the improved octree structure according to claim 8, characterized in that: The specific method of step 4 is: Use iSAM2 based on Bayesian tree optimization to solve the factor graph: First, the nonlinear factor set F and the linearization point set And the Bayesian tree T is initialized to empty, that is, ; Then the new factor Add nonlinear factor set F and initialize new variables And add it to the linearization point set Then, the labeled state quantity M is obtained through smoothing and linearization, and the relevant factors are eliminated using F and M, the Bayesian tree is reconstructed, and the update amount Δ caused by the addition of the new factor is solved; finally, Update to new ; In the iSAM2 algorithm flow, the smoothing and linearization module linearizes the nonlinear problem, performs quadratic approximation on the objective function through the Gauss-Newton method or Lie algebra expansion, and generates a linear equation. The state marker is used to track which variables need to be updated and mark unaffected variables as inactive to avoid repeated calculations. The elimination and Bayesian tree reconstruction module re-performs Gaussian elimination of the sparse matrix on the affected parts and reconstructs the Bayesian tree. Finally, the actual position of the robot in the scene is obtained, and then the point cloud in the radar coordinate system is converted to the global coordinate system to obtain the global map.

Images