A robot positioning and mapping method based on an improved octree structure

Abstract

The application relates to a robot positioning and mapping method based on an improved octree structure, which comprises the following steps: obtaining the residual errors of the robot posture, speed and pose through IMU pre-integration; performing loop candidate frame retrieval through the improved octree structure; calculating the transformation matrix between the loop frames through a registration method combining coarse registration based on a random sample consensus algorithm and fine registration based on an iterative closest point algorithm to obtain a loop detection factor; and introducing the odometry factor, the IMU pre-integration factor and the loop detection factor into a factor graph optimization model to solve the pose. The efficient data processing capacity of the application can greatly improve the decision speed and operation accuracy of the system under complex working conditions, provides key technical support for the digital transformation of the power industry, and can significantly improve the processing efficiency and modeling accuracy of point cloud data in the process of simultaneous localization and mapping algorithm of the robot in a complex live-wire environment.

Description

TECHNICAL FIELD

[0001] The application relates to the field of robot perception in bypass live working scenes, and in particular to a robot positioning and mapping method based on an improved octree structure. BACKGROUND

[0002] With rapid economic development and continuous growth of power demand, power supply reliability faces higher standards. As an important means to ensure uninterrupted power supply, the intelligent transformation demand of bypass live working technology is increasingly urgent. However, the existing robot working system faces the bottleneck of perception data storage and processing efficiency in complex power scenes - the traditional point cloud storage method is difficult to support real-time access of massive environment data, and the inefficient retrieval mechanism further restricts the real-time positioning and modeling, directly affecting the operation safety and precision.

[0003] To solve this core problem, breaking through the point cloud storage structure and retrieval speed technology has become the key to realizing efficient full-scene perception. SUMMARY

[0004] In order to overcome the above technical problems, the application provides a robot positioning and mapping method based on an improved octree structure, which can realize accurate and fast positioning and mapping of robots in complex bypass live working scenes, greatly improve the autonomous operation ability of robots in complex live working environments, and efficiently complete the maintenance and repair work of power distribution network without power interruption, thereby meeting the increasing demand for uninterrupted operation in modern society.

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

[0006] Step 1: obtaining the residual error of robot pose, speed and pose through the IMU (Intertial Measurement Unit, inertial measurement unit) pre-integration link;

[0007] Step 2: for the preprocessed robot point cloud data, the improved octree structure is used for loop candidate frame retrieval;

[0008] Step 3: a registration method combining coarse registration based on the random sample consensus algorithm and fine registration based on the iterative closest point algorithm is used to calculate the transformation matrix between loop frames and obtain the loop detection factor;

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

[0010] In step 1, the robot pose, velocity and position residuals are obtained by the IMU pre-integration block, which is:

[0011] The core role of IMU pre-integration is to integrate the acceleration and angular velocity data of the IMU within a local time period to generate compact motion constraints, including displacement, velocity change and rotation. Specifically, by pre-integration, the relative displacement, velocity increment and rotation increment between two time points are calculated, and the influence of IMU bias on measurement data is considered, and linearized bias is used to compensate for it.

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

[0013]

[0014] (1)

[0015] wherein, , represents the rotation matrix of the IMU coordinate system to the world coordinate system in the i-th frame, is the incremental measurement value of the pose, which is obtained from the gyroscope measurement value and the estimation of the gyroscope zero bias, represents the zero bias of the gyroscope, represents the zero bias of the gyroscope, represents the zero bias measurement noise of the i-th frame.

[0016] (2)

[0017] wherein, is the velocity increment calculated from the measurement value of the IMU accelerometer, is the velocity increment measurement value, represents the velocity vector of the i-th frame, is the time difference between the i-th frame, represents the gravity acceleration, represents the zero bias of the accelerometer, represents the zero bias measurement noise of the i-th frame. (3)

[0018] wherein, is the displacement increment calculated from the measurement value of the IMU accelerometer, is the displacement increment measurement value,

[0019] represents the velocity vector of the i-th frame, is the time difference between the i-th frame, represents the gravity acceleration, represents the zero bias of the accelerometer, represents the zero bias measurement noise of the i-th frame. ​​a time difference between frames, denotes the gravitational acceleration, denotes the zero offset of the accelerometer, denotes the zero offset measurement noise of the frame. denotes the zero offset measurement noise of the frame.

[0020] In step 2, for the preprocessed robot point cloud data, loop candidate frame retrieval is carried out by improving the octree structure, specifically: an incremental octree is adopted, and through strategies including incremental updating and box deletion, the storage efficiency and query performance are optimized.

[0021] Due to the high-frequency sampling characteristics of the laser radar, the typical frequency is 10Hz-20Hz, if loop detection is performed on each frame of point cloud, the calculation resource consumption will increase exponentially, which is difficult to meet the real-time requirement of bypass operation. Therefore, the application introduces a key frame screening criterion:

[0022] Key frame selection condition: set a time interval threshold and a motion distance threshold, only when the robot pose changes more than the preset range, mark the current frame as a key frame.

[0023] Loop candidate frame retrieval: select the candidate frame which is adjacent in space and time from the historical key frame library.

[0024] In step 2, the following steps are specifically included:

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

[0026] Step 2-2, incremental updating: when inserting a new point, whether the point is out of the axis-aligned bounding box range of the original tree is considered, once the point is out of the range of the octree, a new root octant is created to expand the bounding box, the child octants of which contain the current root octant, then the new point is added to the expanded octree; the downsampling operation is performed at the same time as the point insertion;

[0027] Step 2-3, box deletion: check whether the octant is in the given bounding box, all octants in the given bounding box will be directly deleted; for the leaf octant overlapping with the given bounding box, the points in the deletion range are deleted, and new memory segments are allocated 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: use the axis-aligned box of each octant to accelerate the nearest neighbor search, use the boundary overlap sphere test and the priority search order pre-calculated according to the fixed index of the child octant;

[0029] Step 2-5, radius search: radius neighbor search method is used for search.

[0030] Step 2-1, the data structure and the specific method of construction are as follows:

[0031] An octree node has at most eight child nodes, each of which corresponds to one-eighth of the area covered by the axis-aligned bounding box of the octree node; the area after each cube is divided into eight parts is called an octant; each child node will start with a center and an axis-aligned bounding box with equal edge lengths and recursively subdivide into smaller octants with edge length 0.5e until the number of points it contains is less than a given number b size or its edge length is less than the minimum edge length e min R represents the real set;

[0032] For memory efficiency, octants with no points are not created, and only the indices and coordinates of the points in each leaf octant are kept, 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 spatial continuous storage strategy is used to access points in each leaf octant, which reallocates space for the continuous memory segment storing point information in the leaf octant (octant composed of leaf nodes) after subdivision; In addition, reallocation facilitates box deletion and incremental update, as it allows a segment of memory to be operated without affecting other parts.

[0033] Based on the above considerations, for an octant C0 in an octree, the center , the edge length e0, the points I0 with stored coordinates and indices, and the pointer to the address of the first child octant, the subscript "0" is used to distinguish different octants, and let C r be the root octant (octant composed of root nodes), I r , e r and be the points, edge length and center of the root octant, respectively;

[0034] When constructing an incremental octree, first remove invalid points, calculate the axis-aligned bounding box of all valid points, and only keep the indices and coordinates of the valid points; Then, starting from the root node, recursively split the axis-aligned bounding box at the center into eight cubes indexed by the Morton code (Morton code), and subdivide all points in the current octant into each cube according to the calculated cube index; When the stop condition is met, a leaf octant will be created, and a continuous memory segment will be allocated to store the information of the points in the leaf node;

[0035] Step 2-2, the specific method of incremental update is as follows:

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

[0037] Considering the need for efficient point queries in robotic applications, the octree supports the execution of downsampling operations while inserting points. Downsampling focuses on new points and removes points that meet certain conditions: these removable points are subdivided into a leaf octant whose range is less than 2e min and 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, i.e. old points and newly added points, will be recursively subdivided into child octants; if downsampling is enabled, and , the new point will be removed later instead of being added to the target octant C0; otherwise, allocate a segment of continuous memory for the updated points; if the target octant C0 has child octants, allocate the newly added points to each child octant, and only the new points need to be subdivided;

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

[0039] When removing unnecessary points within the 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 directly deleted without searching for points within them, which significantly reduces the deletion time. Due to the local spatial continuous storage strategy, the deletion of an octant will not affect other octants. For leaf octants overlapping with the given bounding box, remove points within the deletion 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;

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

[0041] Using the octree, retrieve the k nearest neighbors for any query point q index ; the nearest search on the octree is an exact nearest search. By maintaining a priority queue h with a maximum length of l h , store the k nearest neighbors encountered so far and their distances from the query point q indexThe distance; whether pushed in or popped out, the last element of h always has the largest distance; the axis alignment boxes of each octet are efficiently utilized to accelerate nearest neighbor search, using boundary overlap ball testing and a priority search order pre-computed based on the fixed index of the sub-octets:

[0042] First, recursively search the octree from its root node until the closest node to q is reached. index leaf nodes; then, q index The distances to all points in the leaf nodes and their corresponding indices are pushed into the priority queue h; all leaf nodes encountered before h is full will be searched; if h is full, and q is used to search the leaf nodes, the search will continue. index The maximum distance d between h and h max Defined search ball S(q) index ,d max If a region is located within the axis alignment box of the current eight-partition, the search ends; if an eight-partition C... k Excluding the search ball S(q) index ,d max If the condition is met, then one of the three conditions in formulas (4)-(6) must be satisfied:

[0043] (4)

[0044] (5)

[0045] (6)

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

[0047] If none of the above conditions are met, then the search ball is located within the eight partitions; by querying and searching S(q) index ,d max Update h using the eight overlapping sections of the spheres, and define q. index and C k The distance d between them is

[0048] (7)

[0049] Where, when x > 0, In other cases ;

[0050] When d < d max When, it indicates C k With S(q) index ,d max Overlap, and to accelerate this process, according to C ncandidate sub-octants to C k are sorted by their distance to q

[0051] Step 2-5, radius search:

[0052] For any query point and radius r, the radius neighbor search method finds every point p that satisfies Before verifying whether S(q index , r) is completely contained in an octant C k , it is necessary to determine whether r 2 is greater than .

[0053] In step 3, the transformation matrix between loopback frames is calculated by a registration method combining coarse registration based on the random sample consensus algorithm and fine registration based on the iterative closest point algorithm, and a loopback detection factor is obtained, which is specifically:

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

[0055] (8)

[0056] wherein, and represent points in the source point cloud and corresponding points in the target point cloud, respectively, and represent the optimal rotation matrix and translation vector to be solved, respectively. 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, and the purpose is to find the optimal and to minimize the gap between the two point clouds.

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

[0059] Step 3-1, coarse registration based on the random sample consensus algorithm:

[0060] Random sampling: randomly select a set of point pairs from the source point cloud and the target point cloud, assuming they are correct matching point pairs, for estimating 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 evaluation: evaluate the matching degree between the transformed point cloud and the target point cloud; judge whether it belongs to the inlier by the distance threshold; the more the number of inliers, the higher the robustness of the model.

[0063] Repeated iteration: repeat the above steps for several times, generate a new model each time by random sampling, and finally select the model with the most inliers as the best transformation;

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

[0065] Find the closest point:

[0066] Use the initial rotation matrix R0 and translation vector t0 or R m-1 and t m-1 , m represents the number of iterations, Transform the initial point cloud to get a temporary transformed point cloud, and then compare it with the target point cloud to find the nearest neighbor of each point in the source point cloud in the target point cloud;

[0067] Solve the optimal transformation:

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

[0069] (9)

[0070] (10)

[0071] Where, , , and represent the center of mass of the target point cloud and the source point cloud, respectively.

[0072] By minimizing the loss function loss, the optimal translation vector and the optimal rotation matrix are obtained:

[0073] (11)

[0074] Optimal rotation matrix :

[0075] (12)

[0076] Where, U and V are The unitary matrix generated by singular value decomposition, that is , is a diagonal matrix of semipositive, and the diagonal elements are singular values of .

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

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

[0079] The specific method of step 4 is:

[0080] The iSAM2 based on Bayes tree optimization is used to solve the factor graph:

[0081] First, the nonlinear factor set F, the linearized point set and the Bayes tree T are initialized to be empty, that is, let ; then the new factor is added to the nonlinear factor set F, the new variable is initialized and added to the linearized point set ; then the marked state quantity M is obtained by smoothing and linearization, the relevant factors are eliminated using F and M, the Bayes tree is reconstructed, and the update amount Δ caused by the addition of the new factor is solved; finally, the is updated to the new ;

[0082] In the iSAM2 algorithm process, the smoothing and linearization module linearizes the nonlinear problem, generates a linear equation by twice approximating the objective function through the Gauss-Newton method or Lie algebra expansion, the marked state quantity is used to track which variables need to be updated, and the variables not affected are marked as inactive to avoid repeated calculation, the elimination and reconstruction of the Bayes tree module re-performs the Gauss elimination of the sparse matrix for the affected part, and the Bayes tree is reconstructed.

[0083] The factor graph optimization problem is solved by the above-mentioned iSAM2 method, that is, the actual pose of the robot in the scene can be obtained, and the point cloud in the radar coordinate system is converted to the global coordinate system to obtain the global map.

[0084] The robot positioning and mapping method based on the improved octree structure mainly applies to the scene perception technology field of distribution network bypass live working, through the innovative point cloud data compression storage architecture and spatial index mechanism, the read-write efficiency of high-density point cloud is significantly improved, not only provides high timeliness environment perception basis for robot live working, but also promotes the technical upgrading of power operation intelligence. Combined with the optimized neighbor search algorithm, the fast matching of key feature points can be realized, and real-time data support is provided for the back-end optimization of simultaneous localization and mapping (SLAM, real-time positioning and map construction). The efficient data processing capability can greatly improve the decision speed and operation accuracy of the system under complex working conditions, has important value for ensuring operation safety and improving power supply reliability, and provides key technical support for digital transformation of the power industry. The technical breakthrough of the present application can significantly improve the processing efficiency and modeling accuracy of point cloud data in the process of simultaneous localization and mapping algorithm of robot in complex live environment, and can solve the problems of cable three-dimensional modeling delay and dynamic obstacle response lag in the working scene.

[0085] The beneficial effects of the present application are specifically:

[0086] (1) The initial value is separated by the IMU pre-integration factor, the frequent integration process is avoided, and the calculation cost is significantly reduced;

[0087] (2) A loop detection method combining random sampling consensus coarse registration and iterative closest point fine registration is used to effectively avoid the problem of local optimum caused by simply using iterative closest point fine registration, and significantly improve the loop detection accuracy;

[0088] (3) An improved octree-based key frame storage structure is designed to further speed up the loop detection process and realize more efficient loop detection;

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

[0090] The present application will be further described below in combination with the drawings and examples.

[0091] Figure 1 The flowchart of the present application.

[0092] Figure 2 The local space continuous storage strategy involved in the present application is shown in the figure, (1) the figure shows that the points in the octant are stored in a dispersed form, and (2) the figure shows that the points in the octant are stored in a continuous form.

[0093] Figure 3 The improved octree adding new points involved in the present application is shown in the figure.

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

[0095] Figure 5 This is a schematic diagram of the box-type deletion method 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 The above is a schematic diagram comparing the loop closure detection before and after the present invention. (1) is the mapping result before the loop closure is introduced, and (2) is the mapping result after the loop closure is introduced.

[0098] Figure 8 This is a schematic diagram of the iSAM2 algorithm process involved in this 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 test 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 The following are comparative diagrams of the mapping effects of bypass operation scenarios involved in this invention: (1) is a comparative diagram of the mapping effects of scenario one, (2) is a comparative diagram of the mapping effects of scenario two, and (3) is a comparative diagram of the mapping effects of scenario three. Detailed Implementation

[0103] To illustrate the technical solution and objectives of the present invention, the present invention will be further described below in conjunction with the accompanying drawings and preferred embodiments.

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

[0105] Step 1: Obtain the residuals of robot attitude, velocity, and pose through the IMU pre-integration process, specifically as follows:

[0106] The core function of IMU pre-integration is to integrate the IMU's acceleration and angular velocity data over a local time interval to generate compact motion constraints, including displacement, velocity changes, and rotation. Specifically, this is achieved by calculating the relative displacement, velocity increment, and rotation increment between two moments through pre-integration, while also compensating for the influence of the IMU's bias on the measurement data using linearized bias.

[0107] Residuals of robot rotation transformation, velocity and position can be obtained by IMU pre-integration

[0108] (1)

[0109] where, , is the rotation matrix that transforms the IMU coordinate frame to the world coordinate frame at frame t, is the incremental measurement of the pose, which is obtained from the gyroscope measurement and the estimation of the gyroscope bias, is the bias of the gyroscope, is the bias of the gyroscope at frame t, is the bias measurement noise at frame t.

[0110] (2)

[0111] where, is the incremental velocity calculated from the IMU accelerometer measurement, is the incremental velocity measurement, is the velocity vector at frame t, is the time difference between frame t and frame t-1, is the gravity acceleration, is the bias of the accelerometer, is the bias of the accelerometer at frame t. (3)

[0112] where,

[0113] is the incremental displacement calculated from the IMU accelerometer measurement, is the incremental displacement measurement, is the velocity vector at frame t, is the time difference between frame t and frame t-1, is the gravity acceleration, is the bias of the accelerometer, is the bias of the accelerometer at frame t.

[0114] Step 2, for the pre-processed point cloud data, the improved octree structure is used to retrieve the loop candidate frames, specifically:

[0115] ​​​​​​​​Due to the high-frequency sampling characteristics of lidar, typically between 10Hz and 20Hz, performing loop closure detection on every frame of point cloud would lead to an exponential increase in computational resource consumption, making it difficult to meet the real-time requirements of bypass operations. Therefore, this invention introduces a keyframe selection criterion:

[0116] Keyframe selection criteria: Set time interval threshold and motion distance threshold, and mark the current frame as a keyframe only when the robot pose change exceeds the preset range.

[0117] Loopback candidate frame retrieval: Filtering candidate frames that are spatiotemporally close from the historical keyframe library.

[0118] Step 2-1, Data Structure and Construction:

[0119] An octree node has a maximum of eight child nodes, each child node corresponding to one-eighth of an axis-aligned cube covering it; the region resulting from partitioning each cube into one-eighths is called an octet; each child node is centered at a certain point. And the side lengths are equal Starting with an axis-aligned bounding box, recursively subdivide it into smaller octets with side length 0.5e, until it contains fewer than a given number of points b. size Or its side length is less than the minimum side length e min Let R represent the set of real numbers; for memory efficiency, eight-partitions without points are not created. Furthermore, only the point index and coordinates are stored in the eight-partitions of each leaf node; the eight-partitions of non-leaf nodes contain no points. To achieve extremely fast access to every point in each leaf eight-partition, a local contiguous storage strategy is used. This strategy reallocates space after subdivision into contiguous memory segments storing point information within the leaf eight-partitions (eight partitions composed of leaf nodes). Moreover, this reallocation facilitates box-by-box deletion and incremental updates, as it allows operations on one segment of memory to occur without affecting other parts. Figure 2 As shown, Figure 2 Figure (1) shows that the points in the eight partitions were initially stored in a scattered manner. Figure (2) shows how the points in the eight partitions are stored in memory after adopting a contiguous storage strategy. Figure 2 In the diagram, 100 represents the location of the point in memory, and 200 represents the location of the point in the eight partitions.

[0120] Based on the above considerations, for an octree, an octet C0 contains a center. Let C be a point with side length e0, a point I0 storing coordinates and index, and a pointer to the address of the first sub-eight partition. The subscript "0" is used to distinguish different eight partitions. Specifically, let C... r For the root 8-partition (an 8-partition consisting of the root node), I r e r and Point, edge length and center, respectively.

[0121] When building the incremental octree, first remove invalid points, calculate the axis-aligned bounding box of all valid points, and only keep the index and coordinates of the valid points. Then, starting from the root node, recursively split the axis-aligned bounding box at the center into eight cubes indexed by the Morton code, and subdivide all points in the current octant into each cube according to the calculated cube index. When the stop condition is met, a leaf octant will be created, and a continuous memory will be allocated to store the information of the points in the leaf node.

[0122] Step 2-2, incremental update:

[0123] In combination with Figure 3 and Figure 4 , when inserting new points, some points may be out of the range of the axis-aligned bounding box of the original tree. Once a point is out of the range of the octree, the bounding box must be expanded 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 points to the expanded octree. Figure 3 and Figure 4 demonstrates the process of inserting new points into the octree, Figure 3 the node 600 in Figure 4 and Figure 3 , the cube 300 is both a root octant and a leaf octant, containing the points at the beginning. After insertion, the root octant becomes the cube 500. In Figure 4 , the node 600 is a root octant, which is updated after inserting a new octant, and the area selected by the dashed line frame is the new octant.

[0124] Considering the need for efficient point query in robot applications, the octree supports the downsampling operation while inserting points, which 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 size greater than b size / 8; the process of adding new points to the target octant C0 is: if the target octant C0 is a leaf node and meets the subdivision criteria, all points in the target octant C0, i.e. old points and newly added points, will be recursively subdivided into child octants; if downsampling is enabled, and , the new point will be deleted later instead of being added to the target octant C0; otherwise, allocate a continuous memory for the updated point; if the target octant C0 has child octants, allocate the newly added points to each child octant, and only the new points need to be subdivided;

[0125] Steps 2-3: Box deletion:

[0126] Combination Figure 5 When removing unnecessary points within an axis-aligned cube, the octree first checks if the octet is within a given bounding box, rather than directly searching for and deleting points within the given space. All octets within the given bounding box are directly deleted without searching for points within them, significantly reducing deletion time. Due to the local space contiguous storage strategy, deleting an octet does not affect other octets. For leaf octets overlapping with a given bounding box, points within the range are deleted, and new memory segments are allocated for the remaining points. If a leaf octet contains no points after deletion, it will be deleted. Figure 5 As shown, box 00 represents the given deletion region, and 0 to 7 are the indices of the eight quadrants obtained through Merton coding. Quadrants with indices 0, 1, 4, and 5 are deleted directly. Since there are no points in quadrant 7, it is also deleted.

[0127] Steps 2-4: K-nearest neighbor search:

[0128] Using an octree, any query point q can be... index Find the k nearest neighbors. The nearest search on an octree is an exact nearest search. This is achieved by maintaining a priority queue h with a maximum length l. h It is used to store the k nearest neighbors encountered so far and their relationship to the query point q. index The distance. Whether pushed in or popped out, the last element of h always has the largest distance. The axis-aligned boxes of each octet are efficiently utilized to accelerate nearest neighbor search, using boundary overlap ball testing and a priority search order pre-computed based on the fixed index of the sub-octets.

[0129] First, recursively search the octree from its root node until the closest node to q is reached. index The leaf nodes. Then, q index The distances to all points in the leaf nodes and their corresponding indices are pushed into the priority queue h. All leaf nodes encountered will be searched before h is full. If h is full, and q... index The maximum distance d between h and h max Defined search ball S(q) index ,d max If a region is located within the axis alignment box of the current eight-partition, the search ends. k Excluding the search ball S(q) index ,d max If the condition is met, then one of the three conditions in formulas (4)-(6) must be satisfied:

[0130] (4)

[0131] (5)

[0132] (6)

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

[0134] If none of the above conditions are met, then the search ball is located within the eight partitions; by querying and searching S(q) index ,d max Update h using the eight overlapping sections of the spheres, and define q. index and C k The distance d between them is

[0135] (7)

[0136] Where, when x > 0, In other cases ;

[0137] when When it indicates C k With S(q) index ,d max Overlap, and to accelerate this process, according to C n Candidate sub-eight partitions to C k Sort the data by distance and obtain 8 distinct sequences. This way, closer octets will be searched earlier, and the search will end earlier.

[0138] Steps 2-5: Radius Search

[0139] For any query point Given a radius r, the radius neighbor search method finds the solution that satisfies the given condition. For each point p, in verifying S(q) index Does r) completely contain an eight-part C? k Previously, it was necessary to determine Is it greater than Furthermore, try to avoid extracting square roots in the algorithm.

[0140] Step 3: Using a registration method combining coarse registration based on the random sampling consensus algorithm and fine registration based on the iterative nearest point algorithm, calculate the transformation matrix between loop closure frames to obtain the loop closure detection factor, specifically:

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

[0142] (8)

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

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

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

[0146] Step 3-1, coarse registration based on the random sample consensus algorithm:

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

[0148] 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.

[0149] Consistency evaluation: evaluate the matching degree between the transformed point cloud and the target point cloud, and determine whether it belongs to an inlier through a distance threshold. The more inliers, the higher the robustness of the model.

[0150] Repeat iteration: repeat the above steps several times, each time randomly sampling a new model, and finally select the model with the most inliers as the best transformation.

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

[0152] Find the closest point:

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

[0154] Solve the optimal transformation:

[0155] First, solve the optimal translation, let N p = |P s |, which 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] where , , and represent the centroids of the target point cloud and the source point cloud, respectively.

[0159] By minimizing the loss function loss, the optimal translation vector and the optimal rotation matrix are obtained:

[0160] (11)

[0161] The optimal rotation matrix is:

[0162] (12)

[0163] where U and V are both unitary matrices resulting from singular value decomposition, i.e. , is a semi-positive definite diagonal matrix, and the diagonal elements are the singular values of .

[0164] The matching accuracy is evaluated by the mean square error. If the matching error is below the preset threshold, the translation error is <0.1m, and the rotation error is <1°, it is determined that an effective loop is detected, and the point cloud registration is successful.

[0165] As shown in Figure 7 , where Figure 7 (1) represents the mapping effect without introducing loop detection. It can be seen that when the bypass operating robot passes through the same path, it is not recognized and drifts, causing ghosting in the image, and Figure 7 (2) After introducing loop detection, the image remains unified when passing 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 iSAM2 based on Bayesian tree optimization is used to solve the factor graph.

[0168] Combining Figure 8 , first, the nonlinear factor set F, the linearization point set and the Bayesian tree T are initialized to be empty, that is, let ; then the new factor is added to the nonlinear factor set F, the new variable is initialized and added to the linearization point set ; then the marked state quantity M is obtained through smoothing and linearization processing, the relevant factors are eliminated using F and M, the Bayesian tree is reconstructed, and the update quantity Delta caused by the addition of the new factor is solved; finally, the is updated to the new .

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

[0170] By solving the factor graph optimization problem in the above-mentioned iSAM2 manner, the actual pose of the robot in the scene can be obtained, and the point cloud in the radar coordinate system can be converted to the global coordinate system, so that the global map can be obtained.

[0171] As shown in Figure 10 , there are multiple power poles in the outdoor simulation bypass operation scene, the power poles are connected by cables, and there are some tree obstructions around, and there are also shrubs in the lower right. The right power pole is obstructed by trees, and the actual mapping process has high requirements for mapping accuracy.

[0172] Figure 11 ​The rviz (ROS Visualization Tool) interface under the ROS (Robot Operating System) system, that is, a three-dimensional visualization platform in the ROS system, is displayed. Figure 10 The live-wire working scene is the central area, and the environment around the live-wire working scene is mapped, and a top view of the scene is shown. In order to show the details of the scene, some areas are enlarged and displayed, as shown in Figure 12 It can be seen from the figure that the poles and cables located therein are well restored, and the details of the tree trunks and branches of the surrounding trees can also be captured, and Figure 12 The pole on the right side blocked by the trees can be clearly seen in (3). It is proved that the method proposed in the application can well complete the live-wire working scene mapping work.

[0173] The robot positioning and mapping method based on the improved octree structure has the following innovative points:

[0174] 1. The initial value is separated by IMU pre-integration factors, the frequent integration process is avoided, and the calculation cost is significantly reduced;

[0175] 2. A loop detection method combining random sampling consensus coarse registration and iterative closest point fine registration is used to effectively avoid the problem of local optimum caused by simply using iterative closest point fine registration, and significantly improve the registration accuracy;

[0176] 3. An improved octree-based key frame storage structure is designed to further speed up the loop detection process and achieve more efficient loop detection;

[0177] 4. The laser odometry factor, IMU pre-integration factor and loop detection factor are integrated into the factor graph optimization framework, and accurate robot pose information is obtained after optimization, so that the mapping and positioning are cooperatively completed.

[0178] Based on the above ideal embodiments according to the application, through the above description, relevant personnel can make various changes and modifications without deviating from the technical idea of the application. The technical scope of the application is not limited to the contents of the specification, and the technical scope must be determined according to the scope of the claims.

Claims

1. A robot localization and mapping method based on an improved octree structure, characterized in that, The method comprises the following steps: Step 1, obtaining the residual of robot pose, speed and position through the IMU pre-integration link; Step 2, for the pre-processed robot point cloud data, the loop candidate frame is searched through the improved octree structure, the incremental octree is adopted, the storage efficiency and query performance are optimized through the strategy including incremental update and box deletion; Step 3, the transformation matrix between the loop frames is calculated through the coarse registration based on the random sample consensus algorithm and the fine registration based on the iterative closest point algorithm, and the loop detection factor is obtained; Step 4, the odometry factor, the IMU pre-integration factor and the loop detection factor are introduced into the factor graph optimization model through factor graph optimization, the pose is solved, and the map construction is completed; In step 2, the following steps are specifically included: Step 2-1, data structure and construction: incremental octree is constructed; Step 2-2, incremental update: when inserting a new point, whether the point is out of the axis-aligned boundary box range of the original tree is considered, once the point is out of the range of the octree, a new root octant is created to expand the boundary box, the child octants of which contain the current root octant, then the new point is added to the expanded octree; the downsampling operation is performed at the same time as the point insertion; Step 2-3, box deletion: check whether the octant is in the given bounding box, all octants in the given bounding box will be directly deleted; for the leaf octant overlapping with the given bounding box, the points in the deletion range are deleted, and the remaining points are assigned to a new memory segment; if a leaf octant does not contain any points after deletion, it will be deleted; Step 2-4, K nearest neighbor search: use the axis-aligned box of each octant to accelerate the nearest neighbor search, use the boundary overlap sphere test and the priority search order pre-calculated according to the fixed index of the child octant; Step 2-5, radius search: radius neighbor search method is used for searching; In step 3, the transformation matrix between the loop frames is calculated through the coarse registration based on the random sample consensus algorithm and the fine registration based on the iterative closest point algorithm, and the loop detection factor is obtained, specifically: Point cloud registration, converted into mathematical form is: （8） wherein, and Pi and Pj represent a point in the source point cloud and a corresponding point in the target point cloud, respectively, and R and t represent the optimal rotation matrix and translation vector to be solved, respectively, N represents the number of points in the source point cloud, R represents the rotation matrix, t represents the translation vector; In step 4, the odometry factor, the IMU pre-integration factor and the loop detection factor are introduced into the factor graph optimization model through factor graph optimization, the pose is solved through iSAM2, and the map construction is completed; The specific method of step 4 is: iSAM2 based on Bayesian tree optimization is used to solve the factor graph: First, the nonlinear factor set F, the linearization point set and the Bayesian tree T are initialized as null, i.e. ; then a new factor is added to the nonlinear factor set F, a new variable is initialized and added to the linearization point set ; then the labeled state quantity M is obtained through smoothing and linearization, the relevant factors are eliminated using F, M the Bayesian tree is reconstructed, and the update quantity Δ caused by the addition of the new factor is solved; finally, the is updated to the new ; In the iSAM2 algorithm process, the smoothing and linearization module linearizes the nonlinear problem, and through the Gauss-Newton method or Lie algebra expansion, the target function is twice approximated to generate a linear equation; the marked state quantity is used to track which variables need to be updated, and the variables not affected are marked as inactive to avoid repeated calculation; the elimination and reconstruction of the Bayesian tree module re-performs the Gaussian elimination of the sparse matrix for the affected part, and reconstructs the Bayesian tree; Finally, the actual pose of the robot in the scene is obtained, and the point cloud in the radar coordinate system is converted to the global coordinate system to obtain the global map.

2. The robot localization and mapping method based on an improved octree structure of claim 1, wherein, In step 1, the residual of robot pose, velocity and position is obtained by IMU pre-integration, which is specifically: The core role of IMU pre-integration is to integrate the acceleration and angular velocity data of IMU within a local time period to generate compact motion constraints, and to calculate the relative displacement, velocity increment and rotation increment between two time points through pre-integration.

3. The robot localization and mapping method based on the improved octree structure of claim 2, wherein, In step 1, the robot rotation transform, velocity, and position residuals are obtained by IMU pre-integration : （1） wherein, , denotes the frame converts the IMU coordinate frame to the world coordinate frame, is an incremental measurement of the pose, obtained from the gyroscope measurements and the estimate of the gyroscope bias, denotes the gyroscope bias, denotes the bias measurement noise of the frame; （2） wherein, is a velocity increment calculated from the measurements of the IMU accelerometer, is a velocity increment measurement, denotes the velocity vector of the first frame, is the time difference between the first frame, denotes the gravitational acceleration, denotes the accelerometer's bias, denotes the bias measurement noise of the first frame; （3） wherein, is a displacement increment calculated from the measurements of the IMU accelerometer, is a displacement increment measurement, denotes the velocity vector of the first frame, is the time difference between the first frame, denotes the gravitational acceleration, denotes the accelerometer bias, denotes the bias measurement noise of the first frame. 4.The robot localization and mapping method based on improved octree structure of claim 1, wherein, Step 2-1, data structure and specific method for construction: Recursively subdivide the three-dimensional space into axis-aligned octree, each node contains at most eight child nodes; If the number of points in the octant exceeds a threshold b size and the edge length is greater than a minimum edge length e min then continue subdividing; Only store the index and coordinates of the points in the leaf nodes, and dynamically allocate storage space using local continuous memory strategy; Assign points to the corresponding sub-octree by calculating their Morton code index. 5.The robot localization and mapping method based on improved octree structure of claim 1, wherein, The specific registration process of step 3 is as follows: Step 3-1, coarse registration based on random sample consensus algorithm: Randomly sample point pairs from source point cloud and target point cloud, and estimate the rigid transformation matrix; Evaluate the number of inliers in the transformed point cloud by distance threshold; Iteratively perform sampling and evaluation, and select the transformation with the most inliers as the optimal coarse registration result; Step 3-2, fine registration based on iterative closest point algorithm: Correspondence search: using the coarse registration result R 0 and the translation vector t 0 or the last iteration R m-1 and t m-1 transform the source point cloud, find the nearest neighbor for each source point in the target point cloud, m denotes the iteration number, ; Finding the closest point: using the initial rotation matrix R 0 and the translation vector t 0 or the last iteration R m-1 and t m-1 , m denotes the iteration number, , the initial point cloud is transformed to get a temporary transformed point cloud, and then this point cloud is compared with the target point cloud to find the closest neighbor of each point in the source point cloud in the target point cloud; Solving 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 ; by minimizing the loss function loss , the optimal translation vector and the optimal rotation matrix are obtained; the matching accuracy is evaluated by the mean square error.

Images