Multi-mode RGBD robot autonomous positioning method based on optimal luminosity difference

Abstract

The invention discloses a multi-mode RGBD robot autonomous positioning method based on an optimal luminosity difference, and the method comprises the steps: carrying out the pixel-level alignment in a coincident view field based on a camera-laser radar joint calibration parameter, and generating an RGBD point; combining distance information and color information in an iterative nearest point to construct an objective function, matching the current point with a historical map point, and calculating to obtain a pose; adding the currently scanned laser point cloud into a historical map, carrying out color estimation on the point cloud which does not obtain RGB information, expanding the color detail expression of the map, and obtaining an optimized pose; and updating the scene RGBD map based on the optimized pose, and outputting the pose estimation result of the current frame to obtain the position of the current robot. According to the method, the depth information acquired by the laser radar and the color texture information acquired by the camera are fused, the environment features are extracted from two dimensions, and the limitation of the low-harness laser radar under the conditions of low point cloud density and insufficient environment detail expression is effectively made up.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous positioning, and in particular to a multimodal RGBD robot autonomous positioning method based on optimal photometric difference. Background Art

[0002] The autonomous positioning capability of mobile robots is a key technology for achieving autonomous navigation and operation, directly impacting the safety of system operations and the accuracy of task execution. In various application scenarios, particularly in tunnels, underground spaces, and narrow passageways, where spatial structures are regular and environmental features are sparse and highly repetitive, traditional methods relying on the Global Navigation Satellite System (GNSS) are often ineffective due to signal obstruction or attenuation. Therefore, reliable autonomous positioning based on native sensors is urgently needed.

[0003] Currently, positioning methods based on laser radar (LiDAR) are widely used in both outdoor and indoor environments. LiDAR collects distance information from objects in the environment, generating dense or sparse three-dimensional point cloud data. The robot's position and posture are estimated through point cloud matching and feature extraction. However, in degraded scenarios such as tunnels, underground garages, and corridors, the lack of significant three-dimensional geometric features causes point cloud data to be highly repetitive or degraded. This can easily lead to failure of autonomous LiDAR-based positioning methods, manifesting as increased matching errors, severe position drift, or even a complete loss of positioning capability.

[0004] Some existing technologies attempt to incorporate LiDAR reflection intensity information as an auxiliary feature to compensate for the shortcomings of using only geometric information. However, because reflection intensity is significantly affected by material, angle, and ambient lighting, its stability and discrimination are limited. In complex or frequently changing environments, it still cannot meet the requirements of high-precision positioning. Summary of the Invention

[0005] The purpose of the present invention is to overcome the shortcomings of the existing technology. To achieve the above purpose, a multimodal RGBD robot autonomous positioning method based on optimal photometric difference is adopted to solve the problems raised in the above background technology.

[0006] A multimodal RGBD robot autonomous positioning method based on optimal photometric difference comprises the following steps:

[0007] Step S1: Based on the camera-lidar joint calibration parameters, the lidar point cloud and the synchronized RGB image are aligned at the pixel level in the overlapping field of view to obtain the RGB information corresponding to each point cloud and generate RGBD points;

[0008] Step S2: Based on the generated RGBD points, the nearest neighbor iterative estimation based on the optimal luminosity difference is performed. The objective function is constructed by combining the distance information and the color information in the iterative nearest point. The current point and the historical map point are matched to calculate the pose.

[0009] Step S3: According to the calculated pose, the laser point cloud of the current scan is added to the historical map, and the RGBD points in the map are used to perform color estimation on the point cloud of the laser point of the current scan that does not obtain RGB information, thereby expanding the color detail expression of the map and obtaining the optimized pose;

[0010] Step S4: Update the scene RGBD map based on the optimized pose, and output the pose estimation result of the current frame to obtain the current position of the robot.

[0011] As a further solution of the present invention: the specific steps in step S1 include:

[0012] S11. Obtaining extrinsic parameters between the camera and the lidar through pre-completion joint calibration, which is used to implement spatial transformation between the lidar coordinate system and the camera coordinate system;

[0013] Let any point in the lidar point cloud be P L =(x L ,y L ,z L ) T , then its coordinate P in the camera coordinate system C =(x C ,y C ,z C ) T Obtained by the following transformation:

[0014] P C =R B ·P L +T B

[0015] Among them, R B is the rotation matrix, T B is the translation vector;

[0016] S12, using the camera's internal parameters to transform the three-dimensional point P C Project it to the two-dimensional image plane of the camera to obtain the corresponding pixel coordinates (u, v); the specific projection relationship formula is:

[0017]

[0018] Among them, f x and f y are the focal lengths of the camera in the x-axis and y-axis directions, cx and c y are the pixel coordinates of the center point of the image respectively;

[0019] S13, based on the obtained pixel coordinates (u, v), extract the color information (R L ,G L ,B L ); If the projection result falls outside the image range, or the pixel has no valid information, the lidar point can be selectively removed or filled with a default color value as required; Finally, the three-dimensional space coordinates (x L ,y L ,z L ) and its corresponding color information (R L ,G L ,B L ) are combined to form an RGBD point containing spatial position and color information, expressed as:

[0020] (x L ,y L ,z L ,R L ,G L ,B L ).

[0021] As a further solution of the present invention: the specific steps in step S2 include:

[0022] S21, set the source point cloud P obtained by the current laser radar scanning = {p i} and the target point cloud Q in the historical map = {q j}, and simultaneously obtain the spatial coordinates and color information of each point in the overlapping field of view; the source point cloud and the target point cloud are then expressed as:

[0023] p i =(x i ,y i ,z i ,r i ,g i ,b i )

[0024] q j =(x j ,y j ,z j ,r j ,g j ,b j )

[0025] S22. The objective function of the nearest neighbor iterative estimation algorithm based on the optimal luminosity difference, taking into account both spatial position and color information, is defined as:

[0026]

[0027] Among them, |Rp i +Tq closest(i) |2 is the geometric distance error; |c(p i )-c(q closest(i) )|2 is the color distance error, which is calculated by taking the RGB channels as three-dimensional information and using the Euclidean distance of the RGB three-dimensional information; c(p i ) and c(q closest(i) ) represent p i and q closest(i) The color information of ;λ is the weight parameter; R and T represent the rotation matrix and translation vector respectively;

[0028] S23. According to the defined objective function, an iterative optimization strategy is used to continuously optimize the objective function E(R, T) until convergence, and the pose estimation result of the current point and the historical map point is obtained.

[0029] As a further solution of the present invention: the specific steps in step S3 include:

[0030] S31. Design of neighbor point color weight based on distance distribution, assuming m is a point without color;

[0031] Use Octree to find the five nearest colored points around point m, marked as (Q1, Q2, ..., Q5), C i It's Q i The known RGB color value, d(m,Q i ) represents points m and Q i The Euclidean distance between

[0032] S32, use Gaussian distribution to weightedly calculate RGB values, according to each neighbor point Q i The Gaussian distribution of the distance to the current colorless point m determines the weight w of the color of each neighboring point i :

[0033]

[0034] Among them, σ is the parameter that controls the Gaussian decay rate;

[0035] S33, based on the Gaussian distribution of the point cloud RGB value estimation, use the Gaussian weight to perform weighted averaging on the color value, and estimate the RGB value C of the colorless point m m , the formula is:

[0036]

[0037] Among them, the denominator is a normalization factor that ensures that the sum of the weights is 1;

[0038] S34, weight w i Substitute this into the calculation formula to obtain the RGB points estimated based on Gaussian distribution:

[0039]

[0040] Through the above formula, the colorless RGB points are estimated to enrich the details of the scene map.

[0041] As a further solution of the present invention: the specific steps in step S4 include:

[0042] The RGBD points estimated in step S3, the RGBD points obtained using the calibration relationship in step S1, and the current frame pose change results R and T obtained by iterative optimization in step S2;

[0043] Transform all the obtained RGBD to its position in the global map and add it to the historical RGBD map to complete the update of the scene RGBD map; output the current frame pose estimation results R, T to obtain the current position of the robot.

[0044] Compared with the prior art, the present invention has the following technical effects:

[0045] Using the above technical solution, the robot's autonomous positioning method achieves high-precision real-time positioning through a multimodal data fusion and joint optimization framework: first, based on the camera-lidar joint calibration parameters, the laser point cloud and RGB image are aligned in the overlapping field of view to construct RGBD points with both geometry and texture; then, a joint optimization objective of distance error and photometric error is introduced into the iterative closest point (ICP) algorithm to hierarchically estimate the precise pose of the current frame and the historical map; then, using a dynamic 3D Gaussian distribution model, the map is expanded according to the optimized pose increment, and the missing color information of the new point cloud is supplemented through spatial weighted interpolation to enhance the expression of scene details; finally, through sliding window bundle adjustment and global pose graph optimization, the accumulated error is closed-loop corrected and the real-time positioning result is output, while the global RGBD map is updated to support continuous mapping.

[0046] By fusing the depth information obtained by the lidar with the color and texture information collected by the camera, environmental features are fully extracted from the two dimensions of spatial geometry and visual photometry, effectively making up for the limitations of low-beam lidar in conditions of low point cloud density and insufficient expression of environmental details. Through multimodal data fusion, the accuracy and stability of feature matching are improved, and the autonomous positioning accuracy and robustness of mobile robots in feature-degraded environments such as tunnels and underground spaces are significantly enhanced. It not only alleviates the failure problem of traditional single lidar positioning methods in scenes with sparse features and single structures, but also provides data support for the construction of rich and detailed RGBD maps, further improving the robot's perception and understanding of complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The specific embodiments of the present invention are described in detail below with reference to the accompanying drawings:

[0048] Figure 1 This is a schematic diagram of the steps of the robot autonomous positioning method according to the embodiment disclosed in this application. DETAILED DESCRIPTION

[0049] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0050] Please refer to Figure 1 In an embodiment of the present invention, a multimodal RGBD robot autonomous positioning method based on optimal photometric difference includes the following steps:

[0051] Step S1: Based on the camera-lidar joint calibration parameters, the lidar point cloud and the synchronized RGB image are aligned at the pixel level in the overlapping field of view, the RGB information corresponding to each point cloud is obtained, and RGBD points are generated. The specific steps include:

[0052] In this embodiment, the RGBD points are first constructed jointly. The camera-lidar joint calibration parameters are used to find the corresponding RGB information for each lidar point cloud within the overlapping observation field of view of the camera and lidar, and the RGBD points are constructed.

[0053] S11. Obtaining extrinsic parameters between the camera and the lidar through pre-completion joint calibration, including a rotation matrix and a translation vector, for implementing spatial transformation between the lidar coordinate system and the camera coordinate system;

[0054] Let any point in the lidar point cloud be P L =(xL ,y L ,z L ) T , then its coordinate P in the camera coordinate system C =(x C ,y C ,z C ) T Obtained by the following transformation:

[0055] P C =R B ·P L +T B

[0056] Among them, R B is the rotation matrix, T B is the translation vector;

[0057] S12, using the camera's internal parameters, including focal length and principal point coordinates, to transform the three-dimensional point P C Project it to the two-dimensional image plane of the camera to obtain the corresponding pixel coordinates (u, v); the specific projection relationship formula is:

[0058]

[0059] Among them, f x and f y are the focal lengths of the camera in the x-axis and y-axis directions, c x and c y are the pixel coordinates of the center point of the image respectively;

[0060] S13, based on the obtained pixel coordinates (u, v), extract the color information (R L ,G L ,B L ); If the projection result falls outside the image range, or the pixel has no valid information, the lidar point can be selectively removed or filled with a default color value as required; Finally, the three-dimensional space coordinates (x L ,y L ,z L ) and its corresponding color information (R L ,G L ,B L ) are combined to form an RGBD point containing spatial position and color information, expressed as:

[0061] (x L ,y L ,z L ,R L ,G L ,BL ).

[0062] In this embodiment, through the above steps, the fusion of the sparse point cloud of the lidar and the rich texture information of the camera can be effectively realized to generate high-quality RGBD points, providing rich data support for subsequent applications such as three-dimensional reconstruction, environmental perception or object recognition.

[0063] Step S2: Based on the generated RGBD points, the nearest neighbor iterative estimation based on the optimal luminosity difference is performed. The objective function is constructed by combining the distance information and color information in the iterative nearest point. The current point and the historical map point are matched to calculate the pose. The specific steps include:

[0064] S21, set the source point cloud P obtained by the current laser radar scanning = {p i} and the target point cloud Q in the historical map = {q j Each point in} is obtained by the method in S1, and the spatial coordinates and color information of each point in the overlapping field of view are obtained at the same time; the source point cloud and the target point cloud are then expressed as:

[0065] p i =(x i ,y i ,z i ,r i ,g i ,b i )

[0066] q j =(x j ,y j ,z j ,r j ,g j ,b j )

[0067] In this embodiment, spatial alignment initial values ​​and color normalization processing are used to ensure the initial consistency of the source point cloud and the target point cloud in geometric and color dimensions, reduce the matching deviation introduced by sensor noise or ambient lighting differences, and provide high-precision, low-noise multimodal data input for subsequent joint optimization. It is particularly suitable for point cloud registration scenarios in dynamic lighting environments.

[0068] S22. The objective function of the nearest neighbor iterative estimation algorithm based on the optimal luminosity difference, taking into account both spatial position and color information, is defined as:

[0069]

[0070] Among them, |Rp i +tq closest(i) |2 is the geometric distance error, which is the same as the traditional ICP; |c(pi )-c(q closest(i) )|2 is the color distance error, which is calculated by taking the RGB channels as three-dimensional information and using the Euclidean distance of the RGB three-dimensional information; c(p i ) and c(q closest(i) ) represent p i and q closest(i) The color information of ;λ is the weight parameter; R and T represent the rotation matrix and translation vector respectively;

[0071] By fusing the geometric distance error and the color and photometric error to construct a joint objective function, we break through the limitation of traditional ICP that relies only on spatial information. We use color information to enhance feature recognition in weak-texture areas, while maintaining geometric accuracy in texture-rich areas. The introduction of weight parameters can dynamically balance the contribution of multimodal information, significantly improving the matching robustness and adaptability in complex scenes.

[0072] S23. According to the defined objective function, an iterative optimization strategy is used to continuously optimize the objective function E(R, T) until convergence, and the pose estimation result of the current point and the historical map point is obtained.

[0073] A nonlinear optimization strategy combined with a robust kernel function is used to effectively suppress the interference of mismatched points on pose estimation. At the same time, through a dynamic weight adjustment mechanism, color information is used to accelerate convergence in the early stage of iteration, and geometric information is relied upon to improve accuracy in the later stage, ultimately achieving high-precision and high-stability pose estimation.

[0074] Step S3: Based on the RGBD map expansion of the 3D Gaussian distribution, the currently scanned laser point cloud is added to the historical map according to the calculated pose, and the RGBD points in the map are used to perform color estimation on the point cloud of the currently scanned laser points that do not have RGB information, thereby expanding the color detail expression of the map and obtaining the optimized pose. The specific steps include:

[0075] S31. Design of neighbor point color weight based on distance distribution, assuming m is a point without color;

[0076] Use Octree to find the five nearest colored points around point m, marked as (Q1, Q2, ..., Q5), C i It's Q i The known RGB color value, d(m,Q i ) represents points m and Q i The Euclidean distance between

[0077] S32, use Gaussian distribution to weightedly calculate RGB values, according to each neighbor point Q i The Gaussian distribution of the distance to the current colorless point m determines the weight w of the color of each neighboring point i :

[0078]

[0079] Among them, σ is the parameter that controls the Gaussian decay rate;

[0080] S33, based on the Gaussian distribution of the point cloud RGB value estimation, use the Gaussian weight to perform weighted averaging on the color value, and estimate the RGB value C of the colorless point m m , the formula is:

[0081]

[0082] Among them, the denominator is a normalization factor that ensures that the sum of the weights is 1;

[0083] S34, weight w i Substitute this into the calculation formula to obtain the RGB points estimated based on Gaussian distribution:

[0084]

[0085] Through the above formula, the colorless RGB points are estimated to enrich the details of the scene map.

[0086] Step S4: Update the scene RGBD map based on the optimized pose, and output the pose estimation result of the current frame to obtain the current position of the robot. The specific steps include:

[0087] The RGBD points estimated in step S3, the RGBD points obtained using the calibration relationship in step S1, and the current frame pose change results R and T obtained by iterative optimization in step S2;

[0088] Transform all the obtained RGBD to its position in the global map and add it to the historical RGBD map to complete the update of the scene RGBD map; output the current frame pose estimation results R, T to obtain the current position of the robot.

[0089] In this embodiment, by integrating step S13 (RGBD points generated by calibration), step S23 (current frame pose after iterative optimization), and step S34 (optimized estimated RGBD points), the multi-source point cloud data is uniformly transformed into the global coordinate system, realizing the dynamic update of the scene RGBD map and the real-time positioning of the robot. The specific process is as follows:

[0090] First, based on the current frame pose output in step S23, the RGBD point cloud in steps S13 and S34 is rigidly transformed and mapped to the global map coordinate system;

[0091] Subsequently, a point cloud fusion strategy is used to fuse the transformed point cloud with the historical map, removing duplicate points and adding new areas to improve the geometric consistency and texture integrity of the map;

[0092] Finally, the high-precision pose estimation result of the current frame is synchronously output as the core input for robot positioning and navigation. This process ensures real-time map performance and low latency through timestamp synchronization and incremental update mechanisms, while supporting long-term robust operation in large-scale scenarios.

[0093] Although the embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and variations may be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents, and all should be included within the scope of protection of the present invention.

Claims

1. A multimodal RGBD robot autonomous positioning method based on optimal photometric difference, characterized in that: The following steps are involved: Step S1: Based on the camera-lidar joint calibration parameters, the lidar point cloud and the synchronized RGB image are aligned at the pixel level in the overlapping field of view to obtain the RGB information corresponding to each point cloud and generate RGBD points; Step S2: Based on the generated RGBD points, the nearest neighbor iterative estimation based on the optimal luminosity difference is performed. The objective function is constructed by combining the distance information and the color information in the iterative nearest point. The current point is matched with the historical map point to calculate the pose. Step S3: According to the calculated pose, the laser point cloud of the current scan is added to the historical map, and the RGBD points in the map are used to perform color estimation on the point cloud of the laser point of the current scan that does not obtain RGB information, thereby expanding the color detail expression of the map and obtaining the optimized pose; Step S4: Update the scene RGBD map based on the optimized pose, and output the pose estimation result of the current frame to obtain the current position of the robot.

2. The multimodal RGBD robot autonomous positioning method based on optimal photometric difference according to claim 1, characterized in that: The specific steps in step S1 include: S11. Obtaining extrinsic parameters between the camera and the lidar through pre-completion joint calibration, which is used to implement spatial transformation between the lidar coordinate system and the camera coordinate system; Let any point in the lidar point cloud be P L =(x L ,y L ,z L ) T , then its coordinate P in the camera coordinate system C =(x C ,y C ,z C ) T Obtained by the following transformation: P C =R B ·P L +T B Among them, R B is the rotation matrix, T B is the translation vector; S12, using the camera's internal parameters to transform the three-dimensional point P C Project it to the two-dimensional image plane of the camera to obtain the corresponding pixel coordinates (u, v); the specific projection relationship formula is: Among them, f x and f y are the focal lengths of the camera in the x-axis and y-axis directions, c x and c y are the pixel coordinates of the center point of the image respectively; S13, based on the obtained pixel coordinates (u, v), extract the color information (R L ,G L ,B L ); If the projection result falls outside the image range, or the pixel has no valid information, the lidar point can be selectively removed or filled with a default color value as required; Finally, the three-dimensional space coordinates (x L ,y L ,z L ) and its corresponding color information (R L ,G L ,B L ) are combined to form an RGBD point containing spatial position and color information, expressed as: (x L ,y L ,z L ,R L ,G L ,B L )。 3. The multimodal RGBD robot autonomous positioning method based on optimal photometric difference according to claim 1, characterized in that: The specific steps in step S2 include: S21, set the source point cloud P obtained by the current laser radar scanning = {p i } and the target point cloud Q in the historical map = {q j }, and simultaneously obtain the spatial coordinates and color information of each point in the overlapping field of view; the source point cloud and the target point cloud are then expressed as: p i =(x i ,y i ,z i ,r i ,g i ,b i ) q j =(x j ,y j ,z j ,r j ,g j ,b j ) S22. The objective function of the nearest neighbor iterative estimation algorithm based on the optimal luminosity difference, taking into account both spatial position and color information, is defined as: Among them, |Rp i +Tq closest(i) |2 is the geometric distance error; |c(p i )-c(q closest(i) )|2 is the color distance error, which is calculated by taking the RGB channels as three-dimensional information and using the Euclidean distance of the RGB three-dimensional information; c(p i ) and c)q closest(i) ) represent p i and q closest(i) The color information of ;λ is the weight parameter; R and T represent the rotation matrix and translation vector respectively; S23. According to the defined objective function, an iterative optimization strategy is used to continuously optimize the objective function E(R, T) until convergence, and the pose estimation result of the current point and the historical map point is obtained.

4. The multimodal RGBD robot autonomous positioning method based on optimal photometric difference according to claim 1, characterized in that: The specific steps in step S3 include: S31. Design of neighbor point color weight based on distance distribution, assuming m is a point without color; Use Octree to find the five nearest colored points around point m, marked as (Q1, Q2, ..., Q5), C i It's Q i The known RGB color value, d(m,Q i ) represents points m and Q i The Euclidean distance between S32, use Gaussian distribution to weightedly calculate RGB values, according to each neighbor point Q i The Gaussian distribution of the distance to the current colorless point m determines the weight w of the color of each neighboring point i : Among them, σ is the parameter that controls the Gaussian decay rate; S33, based on the Gaussian distribution of the point cloud RGB value estimation, use the Gaussian weight to perform weighted averaging on the color value, and estimate the RGB value C of the colorless point m m , the formula is: Among them, the denominator is a normalization factor that ensures that the sum of the weights is 1; S34, weight w i Substitute this into the calculation formula to obtain the RGB points estimated based on Gaussian distribution: Through the above formula, the colorless RGB points are estimated to enrich the details of the scene map.

5. The multimodal RGBD robot autonomous positioning method based on optimal photometric difference according to claim 1, characterized in that: The specific steps in step S4 include: The RGBD points estimated in step S3, the RGBD points obtained using the calibration relationship in step S1, and the current frame pose change results R and T obtained by iterative optimization in step S2; Transform all the obtained RGBD to its position in the global map and add it to the historical RGBD map to complete the update of the scene RGBD map; output the current frame pose estimation results R, T to obtain the current position of the robot.

Images