A laser SLAM positioning method integrating optimized visual odometry
By integrating visual odometry and laser SLAM, and utilizing data from multiple sensors for joint calibration and optimization, a high-precision map is constructed. This solves the problem of inaccurate positioning of substation inspection robots in outdoor environments, and enables high-precision autonomous navigation and all-round inspection.
Patent Information
- Application Number
- CN202311650366.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-12-05
- Publication Date
- 2025-09-16
- Estimated Expiration
- 2043-12-05
AI Technical Summary
Substation inspection robots cannot accurately locate themselves in outdoor environments, making it difficult to conduct all-round autonomous inspections, especially when GPS signal errors are large and positioning accuracy is low on uneven roads.
By adopting the fusion optimized visual odometry and laser SLAM positioning method, through the joint calibration and optimization of 2D lidar, RGB image data, depth image data and wheel odometry data, high-precision local submaps and global maps are constructed for path planning and autonomous navigation.
It improves the robot's positioning accuracy and mapping accuracy in outdoor environments, solves the problem of inaccurate positioning of single SLAM in complex environments, and has better robustness and autonomous navigation capabilities.
Smart Images
Figure CN117671025B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of SLAM technology for laser and vision fusion positioning and mapping, and in particular to a laser SLAM positioning method integrating an optimized visual odometer. Background Art
[0002] With the development of the economy and society, the scale of power grids has continued to grow, and smart grids have been fully developed. Substations, as a key component of smart grids, play a vital role in transforming voltage, exchanging electrical energy, and collecting and distributing electrical energy. Therefore, the reliable operation of substations is crucial to ensuring the smooth and reliable operation of the entire power system.
[0003] Traditional manual inspections of substations have huge drawbacks. They cannot work in severe weather conditions such as strong winds and heavy rains. In addition, manual judgment is prone to errors, leaving huge hidden dangers to the safe operation of substation equipment.
[0004] In recent years, with the development of intelligent substations, intelligent inspections have become popular, but there are some problems with existing inspection methods: substations are generally located in the wild, GPS signals have large errors and high delays, and track inspection robots need to lay tracks before inspections. In actual working environments, the robots' positioning accuracy is low, making it difficult to complete all-round inspections.
[0005] In view of the various problems existing in the autonomous positioning of substation inspection robots, how to study an algorithm that can realize the trackless autonomous positioning of inspection robots in the substation environment and establish high-precision maps is of great significance to promoting the intelligence of substations and power systems. Summary of the Invention
[0006] Aiming at the problem that single SLAM positioning is inaccurate in outdoor environments such as substations, this paper proposes a laser SLAM positioning method that integrates and optimizes visual odometry, so that the robot can still have relatively accurate positioning in outdoor environments, making mapping more precise.
[0007] To achieve the above objectives, the present invention provides a laser SLAM positioning method integrating an optimized visual odometry, comprising:
[0008] Obtain 2D lidar point cloud data, RGB image data, depth image data, and wheel odometer data respectively;
[0009] Acquire a first pose based on the RGB image data and the depth image data, acquire a second pose based on the wheel odometry data, perform joint calibration of the visual odometry and the wheel odometry using the first pose and the second pose to obtain a joint calibration result,
[0010] Optimizing the pose of a current frame using the joint calibration result, wherein the current frame is an RGBD image frame;
[0011] The 2D lidar point cloud data is fused with the current frame pose optimization result for positioning, the fused pose is used as the optimal pose, and a local subgraph is constructed;
[0012] Perform loop optimization on the local subgraph to construct a global map for subsequent inspection path planning and autonomous navigation.
[0013] Preferably, obtaining the 2D lidar point cloud data, RGB image data, depth image data and wheel odometer data includes:
[0014] The robot scans the surrounding environment with the 2D laser radar it carries to obtain the 2D laser radar point cloud data of the surrounding environment;
[0015] Acquire the RGB image data and the depth image data of the surrounding environment through the RGBD camera carried;
[0016] The acceleration and angular velocity of the robot's motion are obtained through the wheel encoder, that is, the wheel odometer data.
[0017] Preferably, acquiring a first pose based on the RGB image data and the depth image data comprises:
[0018] Performing grayscale processing on the RGB image data to obtain a grayscale image, and combining the grayscale image with the depth image to form an RGBD image frame;
[0019] Extracting ORB feature points from the RGBD image frame; if the number of ORB feature points is greater than a preset threshold, adding the RGBD image frame to a key frame group, obtaining depth values of the ORB feature points using the depth image, and constructing 3D feature points of the current frame; wherein the key frame group is a combination of RGBD image frames having a number of ORB feature points greater than a preset threshold;
[0020] The two image frames in the key frame group are matched to obtain the matched 3D feature points, and the pose transformation relationship T between the key frames is solved by the ICP algorithm. cac , that is, the posture one.
[0021] Preferably, the second posture is obtained through a track deduction algorithm based on the wheel odometer.
[0022] Preferably, optimizing the current frame pose using the joint calibration result includes:
[0023] The data of the current wheel odometer is read, and the current pose of the odometer is calculated using the dead reckoning algorithm. The current pose is multiplied by the joint calibration result to obtain the initial estimated pose of the current frame. The 3D feature points of the previous frame are projected to the current frame using the initial estimated pose, and projection matching is performed with the feature points of the current frame. A visual reprojection error function is constructed, and a least squares problem is constructed and iteratively solved through nonlinear optimization to complete the optimization of the current frame pose.
[0024] Preferably, the visual reprojection error function is:
[0025]
[0026] Among them, u i is the observation position of the i-th feature point, s i is the depth of the i-th feature point in the camera coordinate system, K is the intrinsic parameter of the camera, P i is the i-th 3D feature point, T vi is the visual reprojection error function, T ca is the estimated initial pose.
[0027] Preferably, constructing the local subgraph includes:
[0028] Add the real-time pose of the visual odometry to the pose queue, convert it to the pose under the lidar, and remove the distortion of the laser point cloud;
[0029] The optimized current frame pose is used as the estimated pose of the robot at the current moment;
[0030] The dedistorted laser point cloud data and the estimated pose are simultaneously input into the pose matcher, a search range is set with the estimated pose as the center, and each pose in the search range is enumerated to find the best matching pose;
[0031] The best matching pose is taken as the optimal pose T e , the optimal posture T e As the best insertion position of the laser point cloud of this frame in the current sub-image, and then insert the laser point cloud of this frame into the sub-image.
[0032] Preferably, loop closure optimization is performed on the local subgraph, comprising:
[0033] The current frame laser data is used as a node, and the node poses in the local subgraph are optimized and searched through the branch and bound method. If a target matching result is obtained, it means that a loop is detected;
[0034] According to the pose of the current node and a pose in the closest matched local subgraph, the poses in all completed local subgraphs are optimized, and the poses of all local subgraphs are dynamically adjusted to minimize the residual and complete the optimization.
[0035] Preferably, constructing the global map includes:
[0036] The first node pose of the first local subgraph is used as the origin of the global map coordinate system, and the first node pose of the second local subgraph is used as its pose in the global map coordinate system, and so on, until all local subgraphs are connected to complete the construction of the global map.
[0037] Compared with the prior art, the present invention has the following advantages and technical effects:
[0038] By integrating a camera with a wheel odometer, this invention addresses the shortcomings of pure visual SLAM, such as the tendency to lose tracking during rapid motion and the tendency of wheel odometers to slip on uneven surfaces. This approach leverages the complementary nature of the two technologies. The pose inferred by the visual odometer is used to dedistort the laser point cloud. This inferred pose is then used as an estimated pose, along with the laser point cloud data, to be fed into a pose matcher for matching, ultimately outputting fused positioning information. This approach addresses the issue of inaccurate positioning in complex outdoor environments using a single SLAM. Combining the advantages of both visual and laser SLAM, this approach offers enhanced robustness. BRIEF DESCRIPTION OF THE DRAWINGS
[0039] The accompanying drawings, which constitute part of this application, are intended to provide a further understanding of this application. The exemplary embodiments and descriptions of this application are intended to explain this application and do not constitute an improper limitation on this application. In the accompanying drawings:
[0040] Figure 1 This is a flow chart of a laser SLAM positioning method integrating an optimized visual odometry according to an embodiment of the present invention;
[0041] Figure 2 Schematic diagram of loop optimization in an embodiment of the present invention. DETAILED DESCRIPTION
[0042] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0043] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.
[0044] This paper proposes a laser SLAM positioning method that integrates an optimized visual odometer. Figure 1 , the specific steps include:
[0045] Step 1: Data collection.
[0046] The inspection robot uses the 2D laser radar on board to scan the surrounding environment to obtain single-plane laser point cloud data of the surrounding environment; the onboard depth camera obtains RGB image data and depth data of the surrounding environment; the wheel encoder obtains the acceleration and angular velocity of the robot's movement, and uploads the collected sensor data in real time through the industrial computer.
[0047] Step 2: Jointly calibrate the visual odometry and wheel odometry.
[0048] The present invention uses ORB feature points as a visual SLAM method, extracts ORB feature points from the RGB image taken by the camera, and if the number of extracted ORB feature points is greater than a certain threshold, the frame is added to the key frame group, and the depth value of the ORB feature point is obtained using the depth image. The depth value of the feature point is combined to construct the 3D feature point. Feature matching is performed on the two image frames of the key frame group to obtain the matched 3D feature points. The camera's posture transformation is solved by iterative closest point (ICP) to complete the initialization of the visual odometer. The posture transformation matrix T solved by the wheel odometer is used. odc and the pose transformation matrix T obtained by the visual odometry cac Align and solve the transformation matrix.
[0049] Assume that the two are directly linearly related: Tcac = X*Todc;
[0050] in: Expand the formula to get: further:
[0051]
[0052] This simplifies it into the form of AX=b, and finally the transformation matrix X between the two can be solved, which is the joint calibration result.
[0053] Step 3: Fusion and optimization of visual odometry data and wheel odometry data.
[0054] This step is responsible for tracking and calculating the pose information of the visual odometry, that is, using the pose of the wheel odometry as the initial value and using nonlinear optimization to obtain the pose of the visual odometry.
[0055] The basic workflow is as follows: calculate the initial estimated pose of the current frame based on the wheel odometry, project the 3D feature points obtained in the previous frame onto the current frame, and perform feature matching; calculate the reprojection error between the two frames, construct a least squares problem through nonlinear optimization, and iteratively solve it to optimize the state variables of the current frame, that is, the pose information.
[0056] By reading the wheel encoder, the dead reckoning algorithm can be used to calculate the current position T according to the position at the previous moment. od , multiplying the obtained odometer pose by the transformation matrix X obtained in step 2 can calculate the initial estimated pose T of the current frame ca , the 3D feature points of the previous frame are estimated through the initial pose T ca Project to the current frame, perform projection matching with the feature points of the current frame, and construct the visual reprojection error function T vi :
[0057]
[0058] Among them, u i is the observation position of the i-th feature point, s i is the depth of the i-th feature point in the camera coordinate system, K is the intrinsic parameter of the camera, P i is the i-th 3D feature point;
[0059] The meaning of this formula is to make a difference between the 3D point position and the observed position, so it is called the reprojection error. Finally, the current frame pose T is optimized by nonlinear optimization. vi .
[0060] Step 4: Use laser vision fusion for local positioning and construct a local submap.
[0061] First, dedistort the laser data in step 1 and obtain the laser start time t of the current frame. s , end time t e and the time Δt between the laser beams; according to the pose queue obtained in step 3, the pose corresponding to each laser point of the current frame laser data can be solved by linear interpolation, that is, (t s ,t s +Δt,t s +2Δt,...,t e ) moment; According to the solved posture, the posture of each laser point is converted to the laser starting time t sIn the coordinate system at the moment, the visual odometry completes the distortion compensation of the point cloud. The pose optimized in step 3 is used as the estimated pose of the robot, and the dedistorted laser point cloud data and the estimated pose are simultaneously input into the pose matcher. The pose matcher establishes a search range near the estimated pose, enumerates each pose (x, y, θ) in the search range, converts the laser dedistorted point cloud into a grid coordinate point cloud, and then rotates the point cloud with (x, y, θ). After the rotation, each point of the point cloud falls into a grid with an occupancy probability value. All the probability values of the point cloud are added together, then divided by the number of point clouds, and multiplied by a factor to obtain the final scoring result. The pose with the highest score is taken as the optimal pose T. e . The optimal pose T e As the best insertion position of the laser point cloud of this frame in the current sub-image, the laser point cloud of this frame is then inserted into the sub-image, and the cycle is repeated to complete the construction of the local sub-image.
[0062] A local subgraph is constructed by inserting a certain number of laser frames. When constructing a subgraph, new laser frames are inserted into the subgraph sequentially. Once the number of laser frames in the current subgraph exceeds the set value, the construction is completed and the next subgraph is created, repeating the cycle. When inserting a frame of laser point cloud into its corresponding subgraph, the optimal pose described above is used as the optimal insertion position for that frame of laser point cloud in the local subgraph.
[0063] Step 5: Perform loop optimization and build a global map. Figure 2 .
[0064] The current frame laser data is taken as a node, and the distance from the node pose in the constructed subgraph is less than a certain threshold, and loop detection is performed. That is, for the confidence sum of each point beam in the current laser frame when it is inserted into the subgraph, the matching frame with the largest confidence sum in the search space with the current laser frame as the origin is searched. If the confidence sum of the matching frame is greater than the set threshold, it means that a loop is detected. Then, the corresponding subgraph and node pose are optimized according to the current node pose and the matched pose to minimize the residual E.
[0065] The accuracy of the local subgraph is relatively reliable in the short term, but there will be cumulative errors over a long period of time. The purpose of loop optimization is to eliminate the cumulative errors.
[0066] The steps of loop optimization are as follows:
[0067] 1. Detect loops. Use the current frame laser data as a node and optimize the search with the node poses in the constructed subgraph using the branch-and-bound method. If a good match is obtained, it means that a loop has been detected.
[0068] 2. Optimize. Then optimize the poses of all completed subgraphs based on the pose of the current node and the pose of the closest matched subgraph, and dynamically adjust the poses of all subgraphs to minimize the residual E.
[0069] The first node pose of the first local subgraph is used as the origin of the global map coordinate system, and the first node pose of the second local subgraph is used as the pose of the subgraph in the global map coordinate system. And so on, continuously splicing together to finally construct the global map.
[0070] This invention first addresses the shortcomings of pure visual SLAM (SLAM)—the tendency to lose tracking during rapid motion—and wheel odometers' tendency to slip on uneven surfaces—by fusing a camera with a wheel odometer. This approach leverages the complementary nature of the two technologies. The pose inferred by the visual odometer is used to dedistort the laser point cloud. This inferred pose is then used as an estimated pose, along with the laser point cloud data, to be fed into a pose matcher for matching, ultimately outputting fused positioning information. This approach addresses the issue of inaccurate positioning in complex outdoor environments with a single SLAM. By combining the advantages of both visual and laser SLAM, the algorithm achieves enhanced robustness.
[0071] The above are merely preferred embodiments of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.
Claims
1. A laser SLAM positioning method integrating optimized visual odometry, characterized in that: include: Obtain 2D lidar point cloud data, RGB image data, depth image data, and wheel odometer data respectively; Obtaining a first pose based on the RGB image data and the depth image data, and obtaining a second pose based on the wheel odometry data, performing a joint calibration of the visual odometry and the wheel odometry using the first pose and the second pose to obtain a joint calibration result, and optimizing the pose of a current frame using the joint calibration result, wherein the current frame is an RGBD image frame; The 2D lidar point cloud data is fused with the current frame pose optimization result for positioning, the fused pose is used as the optimal pose, and a local subgraph is constructed; Perform loop optimization on the local subgraph to construct a global map for subsequent inspection path planning and autonomous navigation; Wherein, obtaining a first pose based on the RGB image data and the depth image data includes: Performing grayscale processing on the RGB image data to obtain a grayscale image, and combining the grayscale image with the depth image to form an RGBD image frame; Extracting ORB feature points from the RGBD image frame; if the number of ORB feature points is greater than a preset threshold, adding the RGBD image frame to a key frame group, obtaining depth values of the ORB feature points using the depth image, and constructing 3D feature points of the current frame; wherein the key frame group is a combination of RGBD image frames having a number of ORB feature points greater than a preset threshold; The two image frames in the key frame group are matched to obtain the matched 3D feature points, and the pose transformation relationship T between the key frames is solved by the ICP algorithm. cac , i.e. the posture 1; The second posture is obtained based on the wheel odometer through a dead reckoning algorithm; Optimizing the current frame pose using the joint calibration result includes: Read the data of the current wheel odometer, calculate the current pose of the odometer using the dead reckoning algorithm, multiply the current pose by the joint calibration result to obtain the initial estimated pose of the current frame, project the 3D feature points of the previous frame to the current frame using the initial estimated pose, perform projection matching with the feature points of the current frame, construct a visual reprojection error function, and construct a least squares problem through nonlinear optimization and iteratively solve it to complete the optimization of the current frame pose; Constructing the local subgraph includes: Add the real-time pose of the visual odometry to the pose queue, convert it to the pose under the lidar, and remove the distortion of the laser point cloud; The optimized current frame pose is used as the estimated pose of the robot at the current moment; The dedistorted laser point cloud data and the estimated pose are simultaneously input into the pose matcher, a search range is set with the estimated pose as the center, and each pose in the search range is enumerated to find the best matching pose; The best matching pose is taken as the optimal pose T e , the optimal posture T e As the best insertion position of the laser point cloud of this frame in the current sub-image, and then insert the laser point cloud of this frame into the sub-image.
2. The laser SLAM positioning method of the fusion optimized visual odometer according to claim 1 is characterized in that: Acquiring the 2D lidar point cloud data, RGB image data, depth image data, and wheel odometry data, including: The robot scans the surrounding environment with the 2D laser radar it carries to obtain the 2D laser radar point cloud data of the surrounding environment; Acquire the RGB image data and the depth image data of the surrounding environment through the RGBD camera carried; The acceleration and angular velocity of the robot's motion are obtained through the wheel encoder, that is, the wheel odometer data.
3. The laser SLAM positioning method of the fusion optimized visual odometer according to claim 1 is characterized in that, The visual reprojection error function is: Among them, u i is the observation position of the i-th feature point, s i is the depth of the i-th feature point in the camera coordinate system, K is the intrinsic parameter of the camera, P i is the i-th 3D feature point, T vi is the visual reprojection error function, T ca is the estimated initial pose.
4. The laser SLAM positioning method of the fusion optimized visual odometer according to claim 1 is characterized in that: Performing loop closure optimization on the local subgraph includes: The current frame laser data is used as a node, and the node poses in the local subgraph are optimized and searched through the branch and bound method. If a target matching result is obtained, it means that a loop is detected; According to the pose of the current node and a pose in the closest matched local subgraph, the poses in all completed local subgraphs are optimized, and the poses of all local subgraphs are dynamically adjusted to minimize the residual and complete the optimization.
5. The laser SLAM positioning method of the fusion optimized visual odometer according to claim 4 is characterized in that, Constructing the global map includes: The first node pose of the first local subgraph is used as the origin of the global map coordinate system, and the first node pose of the second local subgraph is used as its pose in the global map coordinate system, and so on, until all local subgraphs are connected to complete the construction of the global map.