Map construction method and device, positioning method and device, electronic equipment and storage medium

Abstract

The invention discloses a map construction method and device, a positioning method and device, electronic equipment and a storage medium. The method comprises the following steps: determining multi-modal data collected by a sensor; constructing a first factor graph according to the point cloud data, the inertia measurement data and the wheel speed data; performing sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor graph to obtain a target pose of each point cloud frame in the point cloud data; generating a plurality of local point cloud maps according to the point cloud data and the target pose of each point cloud frame in the point cloud data; and constructing a global point cloud map based on the plurality of local point cloud maps. The point cloud data, the inertial measurement data and the wheel speed data are fused into the factor graph model to participate in point cloud pose optimization in the map construction process, the robustness and accuracy of point cloud pose estimation are improved, the point cloud data are grouped to generate the local point cloud maps, the local point cloud maps are globally optimized, and the accuracy of point cloud pose estimation is improved. The mapping precision is guaranteed, and the accuracy and stability of navigation positioning are improved.

Description

Technical Field

[0001] This disclosure relates to the field of navigation and positioning technology, and in particular to a map building method, positioning method, device, electronic device, and storage medium. Background Technology

[0002] In the field of high-precision navigation and positioning, accurate and stable positioning capabilities have a crucial impact on the safety of intelligent driving, and the accuracy and stability of positioning capabilities also depend on the accuracy of map construction. During map construction, pose estimation is performed on the collected mapping keyframes to obtain their pose information, and then different mapping keyframes are stitched together based on this pose information to form a global map.

[0003] In existing technologies, when using sensor data to estimate the pose of keyframes in mapping, drift occurs due to accumulated errors, affecting the accuracy and consistency of the stitched global map. Summary of the Invention

[0004] To address the aforementioned technical problems, this disclosure provides a map building method, a positioning method, an apparatus, an electronic device, and a storage medium to solve the problem of inaccurate pose estimation of key frames during the mapping stage, which affects mapping accuracy.

[0005] An embodiment of the first aspect of this disclosure provides a map construction method, the method comprising: determining multimodal data collected by sensors, the multimodal data including point cloud data, inertial measurement data, and wheel speed data; constructing a first factor map based on the point cloud data, inertial measurement data, and wheel speed data; performing sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor map to obtain the target pose of each point cloud frame in the point cloud data; generating multiple local point cloud maps based on the point cloud data and the target pose of each point cloud frame in the point cloud data; and constructing a global point cloud map based on the multiple local point cloud maps.

[0006] An embodiment of a second aspect of this disclosure provides a localization method, the method comprising: determining multimodal data collected by a sensor, the multimodal data including point cloud data, inertial measurement data, and wheel speed data; determining an initial global pose of the current point cloud frame based on the current point cloud frame and the point cloud map in the point cloud data; updating a point cloud factor map according to the point cloud data, inertial measurement data, wheel speed data, and the initial global pose of the current point cloud frame; performing graph optimization based on the point cloud factor map to obtain a relative pose between the current point cloud frame and the previous point cloud frame in the point cloud factor map; and determining the actual global pose of the current point cloud frame according to the relative pose and the actual global pose of the previous point cloud frame.

[0007] A third aspect of this disclosure provides a map building apparatus, comprising: a data determination module for determining multimodal data collected by sensors, the multimodal data including point cloud data, inertial measurement data, and wheel speed data; a factor graph module for constructing a first factor graph based on the point cloud data, inertial measurement data, and wheel speed data, and performing sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor graph to obtain the target pose of each point cloud frame in the point cloud data; a local mapping module for generating multiple local point cloud maps based on the point cloud data and the target pose of each point cloud frame in the point cloud data; and a global mapping module for constructing a global point cloud map based on the multiple local point cloud maps.

[0008] An embodiment of a fourth aspect of this disclosure provides a positioning device, comprising: a first determining module for determining multimodal data collected by sensors, the multimodal data including point cloud data, inertial measurement data, and wheel speed data; a second determining module for determining an initial global pose of a current point cloud frame based on a current point cloud frame and a point cloud map in the point cloud data; a graph updating module for updating a point cloud factor map based on the point cloud data, inertial measurement data, wheel speed data, and the initial global pose of the current point cloud frame; a graph optimization module for performing graph optimization based on the point cloud factor map to obtain a relative pose between the current point cloud frame and a previous point cloud frame in the point cloud factor map; and a global positioning module for determining the actual global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame.

[0009] A fifth aspect of this disclosure provides a computer-readable storage medium storing a computer program for performing the map construction method provided in the first aspect embodiment, or for performing the positioning method provided in the second aspect embodiment.

[0010] A sixth aspect of this disclosure provides an electronic device comprising: a processor; a memory for storing processor-executable instructions; and a processor configured to read the executable instructions from the memory and execute the instructions to implement the map construction method provided in the first aspect embodiment above, or to implement the positioning method provided in the second aspect embodiment above.

[0011] A seventh aspect of this disclosure provides a computer program product that, when instructions in the computer program product are executed by a processor, performs the map building method provided in the first aspect embodiment above, or implements the positioning method provided in the second aspect embodiment above.

[0012] Based on the map construction method provided in this disclosure, point cloud data, inertial measurement data, and wheel velocity data are incorporated into a factor graph model during the map construction process to participate in point cloud pose optimization. This improves the robustness and accuracy of point cloud pose estimation in complex environments and effectively suppresses pose estimation drift caused by cumulative errors. Furthermore, by grouping point cloud data to generate local point cloud maps and performing registration, fusion, and global optimization on multiple local point cloud maps, the accuracy and consistency of the constructed global point cloud map are further guaranteed, contributing to improved accuracy and stability of navigation and positioning. Attached Figure Description

[0013] Figure 1 This is a flowchart illustrating a map construction method provided in an exemplary embodiment of this disclosure.

[0014] Figure 2 This is a flowchart illustrating a map construction method provided in an exemplary embodiment of this disclosure.

[0015] Figure 3 This is a flowchart illustrating a map construction method provided in an exemplary embodiment of this disclosure.

[0016] Figure 4 This is a flowchart illustrating a map construction method provided in an exemplary embodiment of this disclosure.

[0017] Figure 5 This is a flowchart illustrating a map construction method provided in an exemplary embodiment of this disclosure.

[0018] Figure 6 This is a flowchart illustrating a positioning method provided in an exemplary embodiment of this disclosure.

[0019] Figure 7 This is a flowchart illustrating a positioning method provided in an exemplary embodiment of this disclosure.

[0020] Figure 8 This is a flowchart illustrating a positioning method provided in an exemplary embodiment of this disclosure.

[0021] Figure 9 This is a logical diagram illustrating map construction and real-time positioning provided by an exemplary embodiment of this disclosure.

[0022] Figure 10 This is a schematic diagram of a map building apparatus provided in an exemplary embodiment of the present disclosure.

[0023] Figure 11 This is a schematic diagram of a positioning device provided in an exemplary embodiment of the present disclosure.

[0024] Figure 12 This is a structural diagram of an electronic device provided in an exemplary embodiment of this disclosure. Detailed Implementation

[0025] To explain this disclosure, exemplary embodiments of the disclosure will now be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the disclosure, and not all of them. It should be understood that the disclosure is not limited to exemplary embodiments.

[0026] It should be noted that, unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of this disclosure.

[0027] Application Overview When constructing a high-precision point cloud map of a target area, data acquisition devices can be used to collect data multiple times within the target area to obtain mapping data related to the target area. For example, data acquisition devices can be deployed on autonomous mobile robots (AMRs), autonomous vehicles, or non-autonomous vehicles. The robot or vehicle can drive or move within the target area to collect mapping data through the data acquisition devices installed on the robot or vehicle.

[0028] Data acquisition devices can include multiple sensors, such as LiDAR (Light Detection and Ranging) and cameras, for collecting data. In practical applications, after acquiring mapping data, the data acquisition device can preprocess the data, such as by filtering or screening, to obtain preprocessed mapping data.

[0029] For example, for point cloud data, point cloud stitching can first be used to stitch together point cloud data corresponding to the same area. Furthermore, point cloud stitching can also be used to obtain the LiDAR pose corresponding to each target point cloud data. Specifically, the LiDAR pose corresponding to each frame of point cloud data can be determined with the center of the LiDAR as the origin. Based on the LiDAR pose, coordinate transformation can be performed on each laser point in the scanned point cloud data to obtain coordinate values ​​in the global coordinate system.

[0030] The initial pose value of a lidar can be obtained through an inertial measurement unit (IMU) or a global navigation satellite system (GNSS) device. For example, the pose acquired by the IMU and GNSS can be used as the initial pose value. The pose can include position and attitude angles. Position corresponds to three-dimensional coordinates (x, y, z), and attitude angles include rotation angles on the three coordinate axes: yaw, pitch, and roll.

[0031] In the process of constructing high-precision maps, after acquiring point cloud data, multiple keyframe images can be obtained through preprocessing, point cloud segmentation, surface mapping, and texture reconstruction. Each keyframe image corresponds to a pose information, which is the pose information of the data acquisition device when acquiring the point cloud data corresponding to that keyframe image.

[0032] Subsequently, graph optimization is performed on the poses of the aforementioned multiple keyframe images to obtain optimized pose information for each keyframe image. The optimized pose information is then used to stitch the keyframe images together. Map elements are then labeled on the stitched image to obtain a global point cloud map of the target area.

[0033] However, in the process of graph optimization of point cloud data, the pose optimization is usually performed by relying only on the point cloud data of LiDAR without fully integrating data from other sensors. This can lead to large deviations in pose estimation in dynamic environments, when features are missing, or when there is noise in the sensor data, which in turn affects the accuracy of subsequent point cloud stitching and the consistency of the global map.

[0034] To address the aforementioned issues, this disclosure provides a map construction method that integrates other sensor data, such as inertial measurement data and wheel speed data, into a factor graph model during point cloud data graph optimization. Information from these other sensor data is transformed into constraints that participate in point cloud pose optimization. This fully leverages the complementarity of multimodal sensor data, significantly improving the robustness and accuracy of point cloud pose estimation, effectively suppressing the accumulation of pose estimation errors, and ensuring the accuracy of the constructed point cloud map. This, in turn, enhances the accuracy and stability of navigation and positioning based on high-precision point cloud maps.

[0035] The map building and positioning methods disclosed herein can be applied in fields such as intelligent driving, robotics, and warehousing and logistics. For example, in the field of intelligent driving, vehicles can generate real-time maps of their surroundings and locate their positions using the map building and positioning methods provided herein. In the field of robot navigation, robots can build maps and plan paths in unknown or known environments using the map building and positioning methods provided herein, enabling them to avoid obstacles and complete tasks. In the field of warehousing and logistics, robots can accurately locate themselves in warehouses, perform precise navigation and item matching using the map building and positioning methods provided herein, effectively avoid obstacles, reduce labor costs, and improve work efficiency. In smart home scenarios, robots can generate real-time maps of their homes and automatically plan cleaning routes using the map building and positioning methods provided herein.

[0036] Exemplary methods Figure 1 This is a schematic flowchart of a map construction method provided in an exemplary embodiment of this disclosure. This embodiment can be applied to electronic devices, such as, but not limited to, terminal devices, computers, personal computers, tablets, servers, etc. Computers include, but are not limited to: personal computer systems, server computer systems, thin clients, thick clients, handheld or laptop devices, microprocessor-based systems, programmable consumer electronics, network personal computers, minicomputer systems, mainframe computer systems, and distributed cloud computing environments including any of the above systems; terminal devices may also include, in vehicle scenarios, in-vehicle intelligent terminals, in-vehicle infotainment terminals, in-vehicle sensing terminals, in-vehicle computing terminals, or in robot scenarios, robot body control terminals, robot sensing terminals, robot interaction terminals, robot execution terminals, etc. Figure 1 As shown, the process includes the following steps S101-S105: Step S101: Determine the multimodal data collected by the sensor. The multimodal data includes point cloud data, inertial measurement data, and wheel speed data.

[0037] For example, map building requires data collection before map construction. This disclosure allows for the collection of multimodal data, which refers to various data collected by different types of sensors. Point cloud data can be collected by LiDAR, providing three-dimensional coordinate information of objects in the environment and forming a dense set of points. Inertial measurement data, collected by an IMU, includes information such as acceleration and angular velocity, and can be used to sense the device's motion state and attitude changes. Wheel speed data is typically collected by a wheel speed encoder, reflecting the device's mileage and speed information.

[0038] By identifying the multimodal data collected by sensors, rich and complementary raw data information can be provided for subsequent map construction. Multimodal data can be acquired in real time via sensor interfaces or by retrieving historical data from storage devices for offline processing and analysis.

[0039] In some embodiments, after obtaining the multimodal data acquired by the sensors, the multimodal data can be preprocessed. For example, the point cloud data in the multimodal data can be downsampled, and a nearest neighbor search can be performed on the sampled point cloud to find neighboring points for each point in the point cloud data to build a neighborhood relationship, reducing the amount of data for subsequent processing and preserving key geometric features; zero-bias calibration and noise filtering can be performed on the inertial measurement data to eliminate the random noise caused by the system error of the sensor itself and environmental interference; outlier detection and removal can be performed on the wheel speed data to ensure the validity of the data. Thus, the preprocessing steps provide a high-quality data foundation for subsequent point cloud pose optimization, avoiding the influence of noise and outliers in the original data on the optimization results.

[0040] Step S102: Construct the first factor map based on point cloud data, inertial measurement data, and wheel speed data.

[0041] For example, a factor graph is a graphical model used for probabilistic modeling and reasoning. A factor graph includes variable nodes and factor nodes. Variable nodes represent unknown quantities to be estimated (such as the pose of each point cloud frame in point cloud data), and factor nodes represent the constraint relationships between variable nodes.

[0042] By constructing the first factor graph, the information contained in point cloud data, inertial measurement data, and wheel speed data can be transformed into constraints on the pose of each point cloud frame. By integrating these constraints into the same factor graph structure, the first factor graph can comprehensively reflect the joint constraints of multimodal data on the pose of each point cloud frame in the point cloud data.

[0043] Step S103: Based on the first factor graph, perform sliding window optimization on the pose of each point cloud frame in the point cloud data to obtain the target pose of each point cloud frame in the point cloud data.

[0044] For example, sliding window optimization is an optimization strategy for processing time-series data. Its core idea is to optimize the data within a fixed-size window. As new data is added, the window slides forward and discards older data outside the window. This effectively controls computation and storage while ensuring optimization accuracy, meeting the needs of real-time or near-real-time processing.

[0045] When performing sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor graph, a window containing pose variables of several consecutive point cloud frames can be selected in the first factor graph as the current optimization window. The size of this window can be set or dynamically adjusted according to the needs of the actual application scenario (such as computing resources, accuracy requirements, data frame rate, etc.). The pose variables of the point cloud frames within the window are the targets to be optimized, and the factor nodes within the window can provide optimization basis for the pose variables of the point cloud frames.

[0046] Within the sliding window, the sum of the error functions of all factor nodes is used as the overall error. The pose variable values ​​of each point cloud frame within the window that minimizes the overall error are then calculated. These pose variable values ​​are used as the pose optimization results for each point cloud frame within the current window. For example, for factors related to point cloud data, the error may stem from matching errors between adjacent point cloud frames; for factors related to inertial measurement data, the error may arise from the difference between the pose change obtained from the integration of inertial measurement data and the pose change of the point cloud frame; for factors related to wheel speed data, the error may arise from the inconsistency between the travel distance calculated from wheel speed data and the distance obtained from the pose change. Through this joint optimization under multi-source data constraints, data from different sensors can be fully integrated, thereby effectively correcting the initial estimation bias of the pose of each point cloud frame.

[0047] After optimizing the current window, the pose optimization results for each point cloud frame within the window are output. The window then slides forward to include the pose variables of new point cloud frames. Since the window size is fixed, one or more point cloud frame pose variables at the very front of the window will be moved out of the window. The pose optimization results of the point cloud frames that are moved out of the window can then be used as the target pose for that point cloud frame.

[0048] Furthermore, to reduce trajectory drift, a keyframe mechanism can be introduced. Keyframes are selected, representative point cloud frames. During sliding window optimization, when a new point cloud frame is processed, it can be determined whether it meets the keyframe selection criteria (such as translation distance or rotation angle exceeding a set threshold, or point cloud feature richness reaching a certain standard). If satisfied, it is designated as a new keyframe, and its pose variables are added to the keyframe set of the factor map as an important reference for subsequent pose optimization and map construction. For keyframes that move out of the window, their optimized poses can be fixed. The fixed keyframe poses can then be used to construct matching factors with subsequent keyframes to be optimized, constraining the optimization of subsequent keyframes.

[0049] By combining the keyframe mechanism with sliding window optimization, the long-term consistency of pose estimation is further improved, avoiding trajectory deviation problems caused by long-term accumulated errors. The target poses of each point cloud frame obtained after sliding window optimization have higher accuracy and reliability compared to the initial poses, laying a data foundation for subsequent local and global mapping.

[0050] Step S104: Generate multiple local point cloud maps based on the point cloud data and the target pose of each point cloud frame in the point cloud data.

[0051] For example, after obtaining the target pose of each point cloud frame in the point cloud data, the point cloud frames can be transformed into a unified global coordinate system based on this precise pose information. To avoid the computational load and potential cumulative errors caused by processing all point cloud data at once, continuous or adjacent multi-frame point cloud data can be divided into several groups according to preset time intervals, spatial region divisions, or keyframe distributions. For each group of point cloud data, using the target poses corresponding to each point cloud frame within that group, these point cloud frames are transformed and stitched together to obtain a local point cloud map that represents the area covered by that group of point cloud data. Each local point cloud map is relatively independent and contains environmental 3D point cloud information within its coverage area, which not only reduces the amount of data processed per operation but also provides a modular basic unit for subsequent global map construction.

[0052] Step S105: Construct a global point cloud map based on multiple local point cloud maps.

[0053] For example, after obtaining multiple local point cloud maps, these local point cloud maps need to be effectively integrated to form a complete global point cloud map. First, feature extraction is performed on each local point cloud map to obtain key information that characterizes its spatial location and environmental features, such as landmark objects, planar features, and edge features. Then, using multi-local point cloud map registration technology, overlapping areas or corresponding feature point pairs between different local point cloud maps are found, and the spatial transformation matrix between local point cloud maps is calculated based on these correspondences to achieve coordinate unification of the local point cloud maps. During the registration process, the pose information of each local point cloud map at the time of generation can be used as an initial transformation estimate to accelerate registration convergence and improve registration accuracy. For potential stitching gaps or redundant point clouds between local point cloud maps after registration, point cloud fusion algorithms can be used to process them, such as using voxel lattice filtering or distance threshold filtering to remove redundant points, and weighted averaging or normal vector consistency adjustment can be applied to the point clouds in overlapping areas to make the surface of the stitched global point cloud map smoother and more continuous.

[0054] Furthermore, to further improve the consistency and accuracy of the global point cloud map, the relative poses of all local point cloud maps can be globally optimized. For example, a factor graph containing the pose variables of all local point cloud maps and their relative constraints can be constructed. The global pose configuration that minimizes the overall error can be solved by graph optimization algorithms, thereby effectively eliminating accumulated errors and ensuring the geometric consistency and measurement accuracy of the global point cloud map over a large area.

[0055] Finally, after the above registration, fusion and optimization processes, multiple local point cloud maps are integrated into a global point cloud map of the target area.

[0056] In this embodiment, point cloud data, inertial measurement data, and wheel velocity data can be integrated into a factor graph model during map construction to participate in point cloud pose optimization. This improves the robustness and accuracy of point cloud pose estimation in complex environments and effectively suppresses the accumulation of pose estimation errors over time. Furthermore, by grouping point cloud data to generate local point cloud maps and performing registration, fusion, and global optimization on multiple local point cloud maps, the accuracy and consistency of the constructed global point cloud map are further guaranteed, contributing to improved accuracy and stability of navigation and positioning.

[0057] like Figure 2 As shown above, in the above Figure 1 Based on the illustrated embodiment, step S102 may include the following steps S1021-S1023: Step S1021: Determine the initial pose of each point cloud frame in the point cloud data based on the inertial measurement data and wheel speed data.

[0058] For example, in the initial stage of map construction, it is necessary to determine the initial pose for each point cloud frame in the point cloud data. The accuracy of the initial pose directly affects the efficiency and final accuracy of the pose optimization process. Inertial measurement data has the characteristic of high sampling rate, which can provide the acceleration and angular velocity information of the device in real time. By integrating this information, the motion trajectory and attitude changes of the device in a short period of time can be quickly obtained, thus providing high-frequency initial pose values ​​for the point cloud frames. Wheel speed data can provide the mileage information of the device. By counting and processing wheel speed pulses, the distance traveled by the device in each sampling period can be calculated. Combined with the motion model of the robot or vehicle, it can help estimate the position change of the device.

[0059] Specifically, IMU data can be used for real-time pose recursion, while wheel speed data can be used to periodically calibrate or constrain the IMU's integral results to correct IMU drift errors, thereby assigning an initial pose to each point cloud frame in the point cloud data. This initial pose provides a good starting point for subsequent sliding window optimization based on the first factor map, enabling the pose optimization algorithm to converge to the global optimum more quickly.

[0060] In some embodiments, the initial pose of the point cloud frame provided by IMU data and wheel speed data can be used to perform distortion correction processing on each point cloud frame. Specifically, for a point cloud frame, the point cloud of the point cloud frame can be transformed into the IMU coordinate system, and then, according to the motion pose of the IMU during the acquisition time period of the point cloud frame, the points in each point cloud are timestamped and motion compensated to eliminate the point cloud distortion caused by device motion, thereby further improving the quality of the point cloud data and the accuracy of subsequent registration.

[0061] Step S1022: Based on the point cloud data, inertial measurement data and wheel speed data in the multimodal data, determine the constraint information between point cloud frames in the point cloud data.

[0062] For example, the constraint information between point cloud frames in point cloud data is a key component in constructing the factor graph. This constraint information comes from the motion constraint information and geometric constraint information between point cloud frames reflected by different types of data in multimodal data.

[0063] In some embodiments, step S1022, based on point cloud data, inertial measurement data, and wheel speed data in multimodal data, determines the constraint information between point cloud frames in the point cloud data, including: determining the motion constraint information between adjacent point cloud frames in the point cloud data based on inertial measurement data and wheel speed data; performing point cloud matching on adjacent point cloud frames in the point cloud data to determine the first geometric constraint information between adjacent point cloud frames; and performing point cloud matching on key point cloud frames in the point cloud data to determine the second geometric constraint information between key point cloud frames.

[0064] The constraint information between point cloud frames includes motion constraint information, first geometric constraint information, and second geometric constraint information. Motion constraint information is mainly used to describe the pose change relationship between adjacent point cloud frames based on a kinematic model. For example, by using the integral result of IMU data between the acquisition times of adjacent point cloud frames, a predicted value about the relative pose change of the two frames can be obtained. The difference between this predicted value and the actual pose change of the point cloud frames constitutes the error term of the motion constraint. Wheel speed data can be used to calculate the travel distance within the time interval between adjacent point cloud frames, combined with a kinematic model (such as a differential speed model or an Ackermann model) to obtain constraints on the translational amount of adjacent point cloud frames. For example, if the wheel speed data indicates that the device traveled 5 meters within a certain time period, then the magnitude of the pose change in the translational direction of adjacent point cloud frames should be close to 5 meters. This constraint based on the laws of physical motion can effectively limit the divergence of pose estimation.

[0065] The first geometric constraint information is obtained through point cloud matching between adjacent point cloud frames, aiming to constrain the pose of two frames from the perspective of environmental geometry. In specific implementation, algorithms such as ICP (Iterative Closest Point) or its improved versions (e.g., point-to-surface ICP, color ICP, etc.) can be used to accurately register adjacent point cloud frames. By finding corresponding point pairs in the two point cloud frames and minimizing the sum of squared distances between corresponding point pairs, the relative pose transformation of adjacent point cloud frames is solved. The relative pose transformation of adjacent point cloud frames constitutes the first geometric constraint, and its error term reflects the accuracy of point cloud matching. Because the overlap area between adjacent point cloud frames is large, feature correspondences are relatively easy to establish, thus effectively correcting the cumulative errors that may be caused by motion constraints.

[0066] The second geometric constraint information targets the matching between keypoint cloud frames (i.e., keyframes), providing more global or sparser but more accurate geometric constraints. Keyframe selection is typically based on the relative motion magnitude with the previous keyframe or the richness of point cloud features to ensure the keyframe set sparsely and effectively covers the entire mapping area. When matching keypoint cloud frames, stable features in the point cloud (such as 2D image features combined with depth information like SIFT and SURF, or 3D point cloud features like FPFH and SHOT) are first extracted. Matching is then performed based on the similarity of feature descriptors. False matches are then eliminated using the RANSAC (Random Sample Consensus) algorithm. Finally, the relative pose between keyframes is calculated based on the correct feature correspondences. This second geometric constraint information establishes connections between non-adjacent but spatially related keyframes (such as in loop scenes), effectively suppressing trajectory drift during long-distance motion and ensuring global pose consistency.

[0067] In this embodiment, the information contained in the multimodal data collected by the sensor is transformed into constraint information between point cloud frames. The constraint information will be used as different types of factor nodes in the factor graph to participate in the optimization estimation of the pose variables of the point cloud frames.

[0068] In some embodiments, Figure 1 In step S101 of the illustrated embodiment, the multimodal data collected by the sensor may also include image data, so that the image data, point cloud data, IMU data and wheel speed data jointly participate in the pose estimation of the point cloud frame, and the information contained in the image data is also transformed into a constraint on the pose of the point cloud frame.

[0069] When image data is involved in the constraints, step S1022 determines the constraint information between point cloud frames in the point cloud data based on point cloud data, inertial measurement data, and wheel speed data in the multimodal data. This includes: determining the motion constraint information between adjacent point cloud frames in the point cloud data based on inertial measurement data and wheel speed data; performing point cloud matching on adjacent point cloud frames in the point cloud data to determine the first geometric constraint information between adjacent point cloud frames; performing point cloud matching on key point cloud frames in the point cloud data to determine the second geometric constraint information between key point cloud frames; determining multiple visual key frames based on image data in the multimodal data; and determining the third geometric constraint information between point cloud frames corresponding to adjacent visual key frames based on the multiple visual key frames and the initial pose of each point cloud frame.

[0070] When image data is involved in constraints, the constraint information between point cloud frames includes motion constraint information, first geometric constraint information, second geometric constraint information, and third geometric constraint information determined based on the image data. The selection of visual keyframes can refer to the point cloud keyframe selection strategy, such as determining them based on motion parameters (e.g., translation distance, rotation angle) or changes in image features (e.g., changes in the number of feature points, optical flow magnitude). After selecting visual keyframes, feature information from the image data (e.g., ORB features, SIFT features) is used to match between visual keyframes. The relative pose between visual keyframes is solved using methods such as the PNP (Perspective-n-Point) algorithm or BA (Bundle Adjustment) optimization. Since visual keyframes and point cloud frames are synchronized or correlated in time (e.g., aligned via sensor timestamps), combined with the initial pose of each point cloud frame, the relative pose constraints between visual keyframes can be transferred to the corresponding point cloud frames, thus forming the third geometric constraint information between the point cloud frames corresponding to adjacent visual keyframes.

[0071] Third geometric constraint information can utilize the rich texture and feature information of image data, especially in environments where point cloud features are relatively sparse or degraded (such as corridors, solid-color walls, etc.), to provide additional constraint information for point cloud frame pose optimization, further enhancing the robustness and accuracy of pose estimation.

[0072] In some embodiments, determining third geometric constraint information between point cloud frames corresponding to adjacent visual keyframes based on the initial poses of multiple visual keyframes and each point cloud frame includes: determining feature correspondences between adjacent visual keyframes in the multiple visual keyframes; determining the initial pose of each visual keyframe in the multiple visual keyframes based on the initial poses of each point cloud frame in the point cloud data; determining the visual reprojection error of each visual keyframe based on the initial poses and feature correspondences of each visual keyframe; and determining third geometric constraint information between point cloud frames corresponding to adjacent visual keyframes based on the visual reprojection errors of each visual keyframe.

[0073] Visual reprojection error is a crucial metric measuring the difference between the projected location of image feature points from 3D space onto the 2D image plane and their actual detection location. Specifically, for each visual keyframe, based on its initial pose, feature points in 3D space (depth information can be provided by point cloud data or estimated using visual SLAM methods) are projected onto the image plane using camera intrinsic and extrinsic parameters, yielding the coordinates of the projected points. These coordinates are then compared to the coordinates of the actual feature points extracted from the image, and the Euclidean distance between them is calculated; this distance represents the reprojection error of a single feature point. The overall visual reprojection error of the keyframe is obtained by summing or averaging the reprojection errors of all matching feature point pairs.

[0074] The process of determining the third geometric constraint information between point cloud frames corresponding to adjacent visual keyframes based on the visual reprojection error of each visual keyframe is as follows: The relative pose transformations between visual keyframes obtained through PNP algorithm or BA optimization are combined with the time synchronization relationship and initial pose mapping between visual keyframes and point cloud frames to convert them into relative pose constraints between corresponding point cloud frames. This constraint is represented as a new factor node in the factor graph model, and its error term is related to the visual reprojection error. That is, the smaller the reprojection error, the greater the constraint weight of this factor on the pose variables of the corresponding point cloud frame. Conversely, if the visual reprojection error is large, it may indicate that there are many mismatches or large initial pose deviations. In this case, the weight of this constraint can be reduced, or robust estimation methods such as RANSAC can be used to remove outliers with excessive errors to avoid their negative impact on the overall pose optimization.

[0075] In this embodiment, the information contained in the image data is converted into constraint information on the pose relationship between adjacent point cloud frames, which further enriches the constraint sources in the factor graph model and improves the overall accuracy and robustness of point cloud frame pose estimation.

[0076] Step S1023: Construct the first factor map based on the initial pose and constraint information of each point cloud frame in the point cloud data.

[0077] For example, the determined constraint information is used as factor nodes connecting variable nodes in the factor graph, and the optimal point cloud frame pose is solved by minimizing the sum of error terms of all factors. In the specific construction process, the initial pose of each point cloud frame in the point cloud data is first used as the initial value of the state variables in the first factor graph. These state variables are usually represented as translation vectors and rotation matrices in three-dimensional space.

[0078] Then, corresponding factor nodes are created based on different types of constraint information. For motion constraint information, a motion factor is created, whose error term is defined as the difference between the predicted relative pose of adjacent point cloud frames obtained based on inertial measurement data and wheel speed data and the actual pose state variables. For the first geometric constraint information, a first geometric factor is created, whose error term originates from the residual between the relative pose transformation obtained from point cloud matching of adjacent point cloud frames and the pose transformation represented by the state variables. For the second geometric constraint information, a second geometric factor is created, whose error term is similar to the first geometric factor, but applies between key point cloud frames. If a third geometric constraint information exists, a third geometric factor is created, whose error term is associated with the reprojection error of the visual keyframe, reflecting the deviation of the point cloud frame pose under visual constraints. These factor nodes are connected to the corresponding state variable nodes through edges, together forming a factor graph structure containing variable nodes and factor nodes composed of multi-source constraints.

[0079] In some embodiments, during the construction of the factor graph, different weights can be assigned to each factor based on the reliability and accuracy of different constraints. For example, in regions rich in point cloud features, the weights of the first and second geometric factors can be set higher, while in dynamic or feature-sparse regions, the weights of the motion factor and the third geometric factor (if any) can be appropriately increased to ensure that the factor graph model can adapt to different environmental conditions and provide an accurate and comprehensive constraint framework for subsequent sliding window optimization.

[0080] The method in this embodiment can fully integrate the advantages of multimodal data, providing a rich constraint basis for the construction of the first factor map. It not only realizes the effective collaboration of multi-source data, but also provides a complete optimization object for subsequent sliding window optimization, thereby providing an accurate point cloud pose basis for high-precision map construction.

[0081] like Figure 3 As shown above, in the above Figure 1 Based on the illustrated embodiment, step S103 performs sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor graph to obtain the target pose of each point cloud frame in the point cloud data, including the following steps S1031-S1034: Step S1031: Construct a total error function based on the constraint relationship in the first factor graph, and iteratively optimize the pose of each point cloud frame in the first factor graph based on the total error function.

[0082] For example, the total error function is obtained by weighted summation of the error terms of all factor nodes in the first factor graph, and its expression can be expressed as: F(x) = Σ(ω_i×e_i^T(x)×Σ_i^-1×e_i(x)), where x is the set of pose state variables of all point cloud frames to be optimized, e_i(x) is the error term of the i-th factor node, Σ_i is the covariance matrix corresponding to the error term, and ω_i is the weight coefficient of the factor.

[0083] The iterative optimization process can employ a nonlinear least squares optimization algorithm. First, given the initial pose x0 of each point cloud frame (i.e., the initial pose obtained in step S1021), in each iteration, based on the current estimated state variable x_k, the Jacobian matrix J of the total error function F(x_k) with respect to x is calculated. Next, a linear system of equations (J^T×Σ^-1×J)Δx=-J^T×Σ^-1×e is constructed based on the Jacobian matrix J and the error vector e. The increment Δx is solved, and the state variables are updated according to x_{k+1}=x_k+Δx. This process is repeated until the error function F(x) converges to its minimum or reaches the preset convergence conditions such as the number of iterations or the increment threshold. The resulting estimated state variable is the point cloud frame pose after preliminary optimization.

[0084] Step S1032: In response to the fact that the number of point cloud frames in the first factor graph exceeds the limit of the sliding window, the marginalized point cloud frames are determined based on the timestamps of each point cloud frame in the first factor graph.

[0085] For example, the limit number of frames for the sliding window can be preset according to factors such as system computing resources, real-time requirements, and environmental complexity, for example, set to 20 or 30 frames. When the cumulative number of point cloud frames in the first factor graph reaches or exceeds the limit number of frames, some early point cloud frames need to be removed through edge-mapping operations to control the size of the factor graph and ensure the real-time performance of the optimization process.

[0086] When determining the marginalized point cloud frames, the timestamps of each frame are the primary basis. Typically, the frame with the earliest timestamp is prioritized for marginalization because it is furthest from the current time, its environmental information may have been fully covered by subsequent frames, and its direct impact on the current pose estimation is relatively weak. Furthermore, in some special cases, the feature richness of the point cloud frame or the number of constraint relationships with other frames can be used to assist in the decision-making. For example, if an early point cloud frame has strong constraint relationships with multiple subsequent point cloud frames (such as a keyframe), its marginalization can be appropriately delayed to avoid the loss of important constraint information.

[0087] Step S1033: Remove the marginal point cloud frame from the first factor map, and use the pose of the marginal point cloud frame at the time of removal as the target pose of the marginal point cloud frame.

[0088] For example, once a point cloud frame that needs to be marginalized is determined, the state variable node corresponding to that point cloud frame is first removed from the first factor graph. Simultaneously, the pose estimation result obtained by the marginalized point cloud frame during this sliding window optimization process will be determined as the optimized target pose of that point cloud frame.

[0089] Step S1034: In response to the edge point cloud frame being a key point cloud frame, the target pose of the key point cloud frame is fixed and preserved in the first factor map.

[0090] Among them, the key point cloud frame is a representative point cloud frame selected based on the key frame mechanism. When the marginalized point cloud frame is a key point cloud frame, although its corresponding state variable node is removed from the active state variables of the current sliding window, in order to retain its continuous constraint effect on subsequent pose optimization, its target pose needs to be fixed and retained in the first factor graph.

[0091] Specifically, fixed preservation refers to treating the target pose of the key point cloud frame as a known constant, rather than a variable to be optimized. In subsequent sliding window optimization iterations, when a new point cloud frame is added to the window and forms a constraint relationship with the fixed-preserved key point cloud frame (such as when a loop closure detects a new association), the fixed pose of the key point cloud frame can serve as a stable reference benchmark, providing a reliable pose basis for new constraints. This effectively avoids the loss of global constraint information carried by the key point cloud frame due to its marginalization, ensuring that trajectory drift under long-distance motion can be continuously suppressed, and laying the foundation for building a globally consistent map.

[0092] The method employed in this embodiment utilizes sliding window optimization to ensure high-precision optimization of the current point cloud frame pose while dynamically adjusting the factor map size. Furthermore, by fixing and preserving the target pose of key point cloud frames, it effectively maintains global pose consistency. This approach fully leverages multi-source constraint information for local fine-tuning while balancing computational efficiency with global accuracy through edge-mapping and key-point fixing strategies. Consequently, the target pose estimation of point cloud frames maintains high accuracy and robustness under complex environments and long-term operating conditions.

[0093] like Figure 4 As shown above, in the above Figure 1 Based on the illustrated embodiment, step S104 generates multiple local point cloud maps according to the point cloud data and the target pose of each point cloud frame in the point cloud data, including the following steps S1041-S1043: Step S1041: Determine the mapping point cloud sequence based on the point cloud data, and if the mapping point cloud sequence meets the preset conditions, construct a second factor map based on the point cloud frames in the mapping point cloud sequence.

[0094] For example, the mapping point cloud sequence can be a set of point cloud frames selected from point cloud data for constructing a local point cloud map. For instance, point cloud frames whose target pose is obtained after sliding window optimization can be used as candidate frames for the mapping point cloud sequence. Preset conditions may include the number of point cloud frames in the mapping point cloud sequence reaching a preset threshold, the spatial range covered by the point cloud frames meeting the size requirements of the local point cloud map, or the cumulative relative motion parameters (such as translation distance and rotation angle) between adjacent point cloud frames reaching a set value. For example, when the cumulative translation distance of multiple consecutive point cloud frames exceeds 5 meters or the rotation angle exceeds 30 degrees, the conditions for constructing a new local point cloud map are deemed met.

[0095] When the mapping point cloud sequence meets preset conditions, a second factor map is constructed based on the point cloud frames in the sequence. Similar to the first factor map, the variable nodes in the second factor map represent the target poses of each point cloud frame in the mapping point cloud sequence, while the factor nodes include constraint information between these point cloud frames. The purpose of constructing the second factor map is to further refine the local poses of each point cloud frame within the mapping point cloud sequence, ensuring higher internal consistency of the point cloud frame poses used to generate the local point cloud map.

[0096] In some embodiments, step S1041, which involves determining a mapping point cloud sequence based on point cloud data and constructing a second factor map based on point cloud frames in the mapping point cloud sequence when the mapping point cloud sequence meets preset conditions, includes: for any point cloud frame in the point cloud data, determining the overlap between the point cloud frame and candidate point cloud frames in the mapping point cloud sequence, and adding the point cloud frame as the latest candidate point cloud frame to the mapping point cloud sequence when the overlap is less than a first preset threshold; and constructing a second factor map based on point cloud frames in the mapping point cloud sequence in response to the number of candidate point cloud frames in the mapping point cloud sequence being greater than a preset value or the overlap between the first and last candidate point cloud frames being less than a second preset threshold.

[0097] The first preset threshold is greater than the second preset threshold. The first preset threshold controls the conditions for adding new point cloud frames to the mapping point cloud sequence. When the overlap between the new point cloud frame and existing candidate point cloud frames in the sequence is less than this threshold, it indicates that the new point cloud frame contains a lot of new environmental information not covered by the mapping point cloud sequence, so it is added to the sequence to enrich the content of the local point cloud map. When the number of candidate point cloud frames in the mapping point cloud sequence is large enough, or the overlap between the first and last point cloud frames in the sequence is very low (less than the second preset threshold), it indicates that the current sequence has covered a relatively complete local spatial range. Continuing to add new frames may cause the local point cloud map to become too large or make it difficult to ensure internal consistency. At this time, the point cloud frame expansion of the mapping point cloud sequence can be terminated and the construction of the second factor map can be triggered.

[0098] For example, the first preset threshold can be set to 50%, that is, when the overlap between a new point cloud frame and any candidate point cloud frame in the mapping point cloud sequence is less than 50%, it is added to the sequence; the second preset threshold can be set to 20%, when the number of candidate point cloud frames in the mapping point cloud sequence exceeds the preset 20 frames, or when the overlap between the earliest added point cloud frame and the latest added point cloud frame in the sequence is less than 20%, then adding new frames stops, and the second factor map is started based on the current sequence.

[0099] In this embodiment, the construction range of the local point cloud map can be adaptively determined through the construction mechanism of the mapping point cloud sequence, ensuring that each local point cloud map contains sufficient environmental details and has good internal pose consistency.

[0100] In some embodiments, step S1041, which involves constructing a second factor map based on point cloud frames in the mapping point cloud sequence, includes: performing iterative nearest-point matching between each point cloud frame in the mapping point cloud sequence and other point cloud frames in the mapping point cloud sequence to obtain fourth geometric constraint information between each point cloud frame and other point cloud frames in the mapping point cloud sequence; performing pre-integration calculation based on inertial measurement data and wheel speed data to obtain motion constraint information between adjacent point cloud frames in the mapping point cloud sequence; determining the prior constraint information of each point cloud frame in the mapping point cloud sequence in the first factor map; and constructing a second factor map based on the target pose, fourth geometric constraint information, motion constraint information between adjacent point cloud frames in the mapping point cloud sequence, and prior constraint information of each point cloud frame in the mapping point cloud sequence.

[0101] For example, for each point cloud frame in the mapping point cloud sequence, iterative nearest-point matching is performed with all other point cloud frames in the sequence. Specifically, for point cloud frames Pi and Pj (i≠j), the optimal transformation matrix of corresponding point pairs in the two frames is found using the ICP algorithm. This transformation matrix represents the relative pose transformation from point cloud frame Pi to point cloud frame Pj, and is used as the fourth geometric constraint information. The fourth geometric constraint information can directly reflect the spatial geometric relationship between different point cloud frames within the mapping point cloud sequence, enhancing the tightness of local optimization. Simultaneously, based on the measurement data from the inertial measurement unit and wheel speed odometer data, pre-integration calculations are performed on temporally adjacent point cloud frames in the mapping point cloud sequence to obtain the relative pose prediction between adjacent point cloud frames, which serves as motion constraint information. This motion constraint information is similar to that used when constructing the first factor map, providing dynamic kinematic-based constraints for the pose relationship between adjacent frames.

[0102] Furthermore, the target poses obtained by each point cloud frame in the mapping point cloud sequence after sliding window optimization in the first factor map are regarded as prior information. When constructing the second factor map, these target poses are used as the initial values ​​of the state variables, and certain prior constraints are assigned to the state variables. For example, the error term of the prior constraint is defined as the difference between the current state variable and the prior target pose, and its covariance matrix is ​​set according to the uncertainty estimation of sliding window optimization.

[0103] Finally, the target pose of each point cloud frame in the point cloud construction sequence is used as the initial value of the state variable of the second factor graph. Combined with the constraint information obtained above, the second factor graph is constructed together.

[0104] Step S1042: Perform graph optimization on the second factor graph to obtain the optimized pose of each point cloud frame in the constructed point cloud sequence.

[0105] For example, the graph optimization process for the second factor graph is similar to the iterative optimization process for the first factor graph, also employing a nonlinear least squares optimization algorithm. First, the target pose of each point cloud frame in the mapping point cloud sequence is used as the initial value x0 of the state variable. Then, a total error function F'(x) = Σ(ω'_i×e'_i^T(x)×Σ'_i^-1×e'_i(x)) is constructed, which is the weighted sum of the error terms of all factor nodes in the second factor graph. Here, x is the set of pose state variables of all point cloud frames in the mapping point cloud sequence to be optimized, e'_i(x) is the error term of the i-th factor node in the second factor graph, Σ'_i is the covariance matrix corresponding to the error term, and ω'_i is the weight coefficient of the factor.

[0106] In each iteration, based on the current estimated value of the state variable x'_k, the Jacobian matrix J' of the total error function F'(x'_k) with respect to x is calculated. Then, based on the Jacobian matrix J' and the error vector e', a system of linear equations (J'^T×Σ'^-1×J') Δx' = -J'^T×Σ'^-1×e' is constructed, the increment Δx' is solved, and the state variable is updated according to x'_{k+1} = x'_k + Δx'.

[0107] Repeat this iterative process until the total error function F'(x) converges to the minimum value or meets the preset convergence conditions (such as reaching the maximum number of iterations, the norm of the increment Δx' being less than a set threshold, etc.). The estimated state variable obtained at this time is the optimized pose of each point cloud frame in the mapping point cloud sequence after optimization by the second factor map.

[0108] Graph optimization using the second factor graph can further eliminate the cumulative errors that may exist between the poses of each point cloud frame within the mapped point cloud sequence, significantly improving the consistency and accuracy of poses within a local range.

[0109] Step S1043: Merge each point cloud frame in the mapping point cloud sequence into a local point cloud map based on the optimized pose of each point cloud frame in the mapping point cloud sequence.

[0110] For example, after obtaining the optimized poses of each point cloud frame in the mapping point cloud sequence, a point cloud frame merging operation can be performed to generate a local point cloud map. Specifically, for each point cloud frame in the mapping point cloud sequence, the frame is first transformed from its own local coordinate system to a unified global coordinate system (or the local world coordinate system defined by the local point cloud map) based on its optimized pose. This coordinate transformation is achieved by multiplying the coordinates of each 3D point in the point cloud by the optimized pose transformation matrix of the point cloud frame. After completing the coordinate transformation of all point cloud frames, these point cloud frames in the same coordinate system are superimposed and fused. During the fusion process, point cloud simplification algorithms such as voxel grid filtering can be used to process the superimposed point cloud to remove redundant and noise points, reduce the data volume of the local point cloud map, and retain key environmental features. After the above steps, all point cloud frames in the mapping point cloud sequence are integrated into a local point cloud map, which accurately reflects the 3D structure of the environment within its coverage area.

[0111] The method described in this embodiment, by first constructing a second factor graph containing multi-source constraint information and then performing graph optimization, can significantly improve the local consistency and accuracy of the poses of each point cloud frame within the mapped point cloud sequence. This enables the generated local point cloud map to adaptively cover a suitable environmental range while possessing high internal geometric accuracy, effectively avoiding map distortion caused by single-frame pose errors or local cumulative errors, and improving the overall accuracy and reliability of map construction.

[0112] like Figure 5 As shown above, in the above Figure 1 Based on the illustrated embodiment, step S105 constructs a global point cloud map based on multiple local point cloud maps, including the following steps S1051-S1055: Step S1051: Determine the endpoint poses of each local point cloud map.

[0113] The endpoint pose includes the poses of the first and last point cloud frames in the local point cloud map relative to the local point cloud map. A local point cloud map is formed by fusing multiple point cloud frames from the mapping point cloud sequence. Each point cloud frame has a precise relative pose relationship after optimization by the second factor map. The first endpoint pose of the local point cloud map is the optimized pose of the first point cloud frame in the mapping point cloud sequence within the local point cloud map's own coordinate system, while the last endpoint pose is the optimized pose of the last point cloud frame in the mapping point cloud sequence within the local point cloud map's own coordinate system.

[0114] Since the optimized poses of all point cloud frames within a local point cloud map are obtained based on the same coordinate system, the optimized poses of the first and last point cloud frames directly reflect the spatial start and end positions and orientation of the local point cloud map in its own coordinate system. For example, if a local point cloud map consists of point cloud frames P1, P2, ..., Pn, where P1 is the first point cloud frame in the sequence with an optimized pose of T1, and Pn is the last point cloud frame with an optimized pose of Tn, then the first endpoint pose of the local point cloud map is T1, and the last endpoint pose is Tn. By extracting the endpoint poses, the entire local point cloud map can be abstracted into a "segment" with a start and end spatial state, providing a geometric reference for establishing connections with other local point cloud maps.

[0115] Step S1052: Determine multiple sets of associated map pairs in multiple local point cloud maps, each set of associated map pairs containing two local point cloud maps with an overlap greater than a third preset threshold.

[0116] For example, in order to accurately identify the relationships between local point cloud maps, it is necessary to calculate the overlap between any two local point cloud maps. This can be achieved in the following way: First, a certain number of 3D points are uniformly sampled from each local point cloud map as the feature point set of the map. For example, the Random Sample Consensus Algorithm (RANSAC) is used to extract geometric feature points such as key planes and edges for each local point cloud map, or 1,000-2,000 point cloud data points are directly and uniformly sampled.

[0117] Then, for two local point cloud maps A and B to be detected, the spatial transformation relationship between the feature point sets of map A and map B is calculated using a point cloud registration algorithm. The proportion of point pairs that can match each other after transformation is then counted out of the total number of feature points. This proportion is used as the overlap degree of the two local point cloud maps. A third preset threshold can be set according to the requirements for map association reliability in the actual application scenario. For example, when the overlap degree of two local point cloud maps is greater than 5%, they are determined to be associated map pairs, indicating that these two local point cloud maps have a certain overlapping area in physical space, possibly adjacent or partially overlapping areas in the same environment.

[0118] Step S1053: Construct a third factor map based on multiple local point cloud maps, the endpoint poses of each local point cloud map, and multiple sets of associated map pairs.

[0119] For example, the variable nodes in the third factor graph represent the global pose of each local point cloud map, i.e., the position and orientation of each local point cloud map in the global coordinate system. The factor nodes include different types of constraint information used to describe the relative spatial relationships between local point cloud maps and the prior information of the local point cloud maps themselves. Specifically, for each pair of associated maps (such as local point cloud map A and local point cloud map B), constraint factors describing the relative pose relationship between the two local point cloud maps can be constructed based on their overlap and endpoint poses. For instance, by performing precise point cloud registration on the overlapping area of ​​associated map pairs A and B, the relative pose transformation matrix from local point cloud map A to local point cloud map B is obtained, which serves as the constraint factor between the global pose variables of these two local point cloud maps.

[0120] Furthermore, the endpoint poses of each local point cloud map can also serve as an internal constraint, providing a certain prior reference for its global pose. For example, the first endpoint pose and the last endpoint pose of a local point cloud map are known in its own coordinate system. When the local point cloud map is placed in the global coordinate system, its global pose should satisfy the geometric relationship of these endpoint poses in the global coordinate system, thus forming a constraint on the global pose variables.

[0121] Meanwhile, to avoid over-parameterization or degradation during global pose optimization, appropriate prior factors such as global coordinate system origin constraints or scale constraints can be introduced to ensure that the optimization of the entire third factor graph has a unique solution and good stability. By integrating these variable nodes and factor nodes, a third factor graph for global map stitching can be constructed.

[0122] Step S1054: Graph optimization is performed based on the third factor graph to obtain the global pose of each local point cloud map.

[0123] For example, the graph optimization process for the third factor graph is similar to the optimization logic of the first and second factor graphs mentioned above. The core is to minimize the total error function composed of the error terms of each factor node through a nonlinear least squares optimization algorithm, thereby solving for the optimal global pose of each local point cloud map.

[0124] First, the global pose (position and orientation) of each local point cloud map is used as the set of state variables x'' of the third factor map. Initially, these state variables can be assigned an initial estimate. For example, the global pose of the first local point cloud map can be set as the identity matrix (i.e., its own coordinate system is defined as the initial origin of the global coordinate system). The initial global poses of other local point cloud maps can be roughly estimated based on their association with the already located maps or set as the identity matrix.

[0125] Next, the total error function F''(x'') = Σ(ω''_i×e''_i^T(x'')×Σ''_i^-1×e''_i(x'')) is constructed, where e''_i(x'') is the error term of the i-th factor node in the third factor graph (such as the relative pose constraint factor between associated map pairs, the endpoint pose prior constraint factor of the local point cloud map, etc.), Σ''_i is the covariance matrix corresponding to the error term, and ω''_i is the weight coefficient of the factor.

[0126] During the iterative optimization process, based on the current estimated value of the state variable x''_k, the Jacobian matrix J'' of the total error function F''(x''_k) with respect to the state variable x'' is calculated. Then, using the Jacobian matrix J'' and the error vector e'', a system of linear equations (J''^T×Σ''^-1×J'') Δx'' = -J''^T×Σ''^-1×e'' is constructed. Solving this system of equations yields the increment Δx'' of the state variable, and the state variable is updated according to x''_{k+1} = x''_k + Δx''.

[0127] Repeat the steps of calculating the Jacobian matrix, constructing and solving the linear equation system, and updating the state variables until the total error function F''(x'') converges to a minimum or meets the preset convergence criteria (such as the number of iterations reaching the upper limit, the magnitude of the increment Δx'' being less than a set threshold, or the change in total error between two adjacent iterations being less than a certain minimum). The state variable estimate x''_final obtained at this point is the optimized global pose of each local point cloud map in the global coordinate system.

[0128] Graph optimization using the third factor graph can effectively eliminate relative pose errors between different local point cloud maps, ensuring that all local point cloud maps can be accurately stitched together in the global coordinate system.

[0129] Step S1055: Based on the global pose of each local point cloud map, stitch together the local point cloud maps to obtain the global point cloud map.

[0130] For example, after obtaining the global poses of each local point cloud map, a global stitching can be performed to generate the final global point cloud map. The specific operation is as follows: For each local point cloud map, based on its optimized global pose, it is transformed from its own defined local world coordinate system (or the unified local coordinate system used during its construction) to the global coordinate system. This transformation process is accomplished by multiplying the coordinates of all 3D points in the local point cloud map with the global pose transformation matrix of that local point cloud map, ensuring that all points in the local point cloud map are unified into the same global coordinate system.

[0131] After completing the coordinate transformation of all local point cloud maps, the local point cloud maps are overlaid and fused in the global coordinate system. To generate a high-quality global point cloud map, point cloud denoising and simplification algorithms such as voxel filtering and statistical filtering can be further employed during the fusion process to process the massive amount of overlaid point cloud data. Furthermore, post-processing operations such as ground point segmentation and obstacle recognition can be performed on the fused point cloud according to actual needs to enhance the practicality of the global point cloud map.

[0132] In this embodiment, by introducing a third factor map for global optimization, the cumulative error and global inconsistency problems that may occur in traditional stitching methods are effectively overcome. The global point cloud map obtained by integrating various local point cloud maps can comprehensively reflect the three-dimensional structure and spatial distribution characteristics of the target area, providing a high-precision environmental model foundation for subsequent applications such as path planning, navigation and positioning, and environmental understanding.

[0133] The above describes the specific settings and implementation of the map construction method provided by the embodiments of this disclosure from different perspectives. By using the method provided by the above embodiments, the entire mapping process is decomposed into stages such as keyframe screening, local point cloud map generation, and global point cloud map stitching. Different factor graph models are introduced for optimization at each stage, so that the constructed global point cloud map can accurately reflect the local details of the environment and ensure the overall geometric accuracy and spatial consistency. This provides reliable map data support for fields such as robot navigation, autonomous driving, and virtual reality that rely on high-precision environmental perception.

[0134] Figure 6 This is a schematic flowchart of a positioning method provided in an exemplary embodiment of this disclosure. This embodiment can be applied to electronic devices, such as... Figure 6 As shown, the process includes the following steps S601-S605: Step S601: Determine the multimodal data collected by the sensor.

[0135] Multimodal data can include point cloud data, inertial measurement data, and wheel speed data. This data can be collected by sensors on the robot or vehicle and processed by electronic devices. In practical applications, the sampling frequency and data synchronization of the sensors need to be rigorously calibrated to ensure the consistency of the multimodal data in time and space. For example, timestamp alignment and coordinate system transformation can unify data from different sensors into the same reference frame, laying the foundation for subsequent fusion processing.

[0136] Step S602: Determine the initial global pose of the current point cloud frame based on the current point cloud frame and the point cloud map in the point cloud data.

[0137] For example, the point cloud map can be a global point cloud map generated using the aforementioned map construction method, or it can be a pre-constructed high-precision point cloud map. The current point cloud frame is the point cloud data collected by the sensor at the current moment, and its initial position and attitude in the global coordinate system need to be determined. In specific implementation, the current point cloud frame can be matched with the point cloud map using a point cloud registration algorithm. For example, feature points (such as corner points, planar points, etc.) can be extracted from both the current point cloud frame and the point cloud map, and then the initial pose transformation relationship can be estimated by matching these feature points. Alternatively, the RANSAC algorithm combined with feature matching can be used to quickly obtain a coarse but robust initial global pose as the starting point for subsequent optimization.

[0138] Step S603: Update the point cloud factor map based on the point cloud data, inertial measurement data, wheel speed data, and the initial global pose of the current point cloud frame.

[0139] For example, the variable nodes of the point cloud factor graph are the global poses of the current point cloud frame and historical keyframes, while the factor nodes contain constraint information on the keyframe poses obtained based on the transformation of fused multimodal data.

[0140] First, a point cloud matching factor is constructed based on point cloud data. For example, the current point cloud frame is precisely registered with the point cloud of the corresponding region in the point cloud map to obtain the pose transformation of the current point cloud frame relative to the map, which serves as a constraint factor for the global pose variables of the current point cloud frame. Second, an inertial measurement unit (IMU) pre-integration factor is constructed based on inertial measurement data. The high-frequency characteristics of the IMU provide relative motion constraints between adjacent frames. This factor can effectively compensate for the positioning drift problem of point cloud matching in dynamic environments or regions with missing features. Through pre-integration processing, the original measurement values ​​of the IMU are converted into pose increments, thereby constructing a constraint error term for the pose variables of adjacent point cloud frames. Third, a wheel speed odometry factor is constructed based on wheel speed data. Wheel speed data can provide the motion distance and direction information of the robot or vehicle. Through integration calculation, the translation and rotation estimates between adjacent frames are obtained as another kinematic constraint for pose variables. Especially when there is noise accumulation in the IMU or point cloud matching failure in a short period of time, the wheel speed factor can provide a stable motion reference. In addition, the initial global pose of the current point cloud frame can serve as a priori factor to provide an initial reference for the optimization of pose variables, thus avoiding the optimization process from getting trapped in local minima.

[0141] In this embodiment, by integrating the information contained in point cloud data, inertial measurement data, wheel speed data, and the initial global pose of the current point cloud frame into the factor graph model, a point cloud factor graph containing multi-source constraints is formed, thereby realizing the joint optimization modeling of the global pose variables of the current and historical key frames.

[0142] Step S604: Perform graph optimization based on the point cloud factor graph to obtain the relative pose between the current point cloud frame and the previous point cloud frame in the point cloud factor graph.

[0143] For example, the graph optimization process of the point cloud factor graph is similar to the factor graph optimization logic in the aforementioned map construction method. The core is to solve for the optimal relative pose between the current point cloud frame and the previous point cloud frame by minimizing the total error function composed of error terms from each factor node. The optimized relative pose between the two can be obtained by subtracting the optimized global pose of the current point cloud frame from the global pose of the previous point cloud frame. This relative pose includes not only translational components but also rotational components, enabling it to accurately describe the changes in the motion state of the robot or vehicle at adjacent time points.

[0144] Step S605: Determine the actual global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame.

[0145] For example, if the actual global pose of the previous point cloud frame is represented by the transformation matrix T_prev in the global coordinate system, and the relative pose transformation matrix between the current point cloud frame and the previous point cloud frame is ΔT (i.e., the transformation from the coordinate system of the current point cloud frame to the coordinate system of the previous point cloud frame), then the actual global pose transformation matrix T_current of the current point cloud frame can be calculated by multiplying T_prev and ΔT, that is, T_current = T_prev × ΔT.

[0146] In this embodiment, by utilizing the optimized relative pose and the known global pose from the previous moment, the high-precision global pose at the current moment can be recursively obtained, providing key data support for the real-time positioning of robots or vehicles in the global environment.

[0147] like Figure 7 As shown above, in the above Figure 6 Based on the illustrated embodiment, step S602 may include the following steps S6021-S6023: Step S6021: Determine the point cloud feature information of the current point cloud frame.

[0148] For example, the point cloud feature information can be a global feature representation of the current point cloud frame obtained by calculating the SC (Scan Context) descriptor on the processed point cloud data after distortion correction by combining IMU data and wheel speed data. This descriptor can comprehensively characterize the spatial distribution characteristics of the observed environment of the current point cloud frame, has rotation invariance and good discriminative power, and is suitable for coarse matching of point cloud frames and maps in large-scale environments.

[0149] Specifically, the SC descriptor projects a 3D point cloud onto a polar coordinate grid, statistically analyzes the point cloud height information or the number of points within each grid, and forms a two-dimensional matrix as the feature vector of that point cloud frame. This feature extraction method can effectively compress the amount of point cloud data while preserving the global structural information of the environment, enabling point cloud frames of the same area collected from different viewpoints to have a high degree of similarity.

[0150] Step S6022: Based on the point cloud feature information of the current point cloud frame, determine the target point cloud frame that matches the current point cloud frame in the mapping point cloud frame set of the point cloud map.

[0151] For example, the mapping point cloud frame set stores the point cloud feature information (such as SC descriptors) of all keyframes used in the point cloud map construction process and their corresponding global poses. During matching, the similarity between the SC descriptor of the current point cloud frame and the SC descriptor of each mapping point cloud frame in the mapping point cloud frame set is first calculated (e.g., by calculating the cosine similarity, Euclidean distance, or Hamming distance between the two descriptor matrices). Then, the top N mapping point cloud frames with the highest similarity (e.g., N=5 or N=10) are selected as candidate target point cloud frames.

[0152] Step S6023: Based on the mapping pose of the target point cloud frame, calculate the point cloud matching between the current point cloud frame and the point cloud map, and determine the initial global pose of the current point cloud frame based on the point cloud matching.

[0153] For example, for each candidate target point cloud frame, its mapping pose in the point cloud map is used as the initial reference pose, and the current point cloud frame is accurately matched with the target point cloud frame. For example, the ICP algorithm or its improved algorithm can be used for matching.

[0154] Specifically, firstly, based on the mapping pose of the target point cloud frame, a certain range of map point clouds near that pose is extracted from the point cloud map as a matching reference point set. Then, using the mapping pose of the target point cloud frame as the initial transformation, the current point cloud frame is projected onto the map coordinate system through this initial transformation. During the iteration process, the transformation parameters (translation and rotation) are continuously adjusted to minimize the sum of squared distances between points in the current point cloud frame and their corresponding points in the reference point set. This yields a more accurate pose transformation relationship between the current point cloud frame and the point cloud map, through which the initial global pose of the current point cloud frame can be obtained.

[0155] When there are multiple target point cloud frames in the map point cloud frame set that match the current point cloud frame, the above matching process can be performed separately, and the pose corresponding to the matching result with the smallest matching error (such as the smallest root mean square error) can be selected as the final initial global pose to improve the reliability of the initial pose estimation.

[0156] The method in this embodiment first uses point cloud feature information to perform global feature matching with point cloud map, which quickly narrows the search range and identifies potential target point cloud frames in the map-built point cloud frames. Then, it performs accurate point cloud matching based on the map-built pose of the target point cloud frames. This can effectively improve the efficiency and accuracy of the initial global pose estimation, providing a good starting point for subsequent multimodal data fusion positioning based on factor maps, and helping to improve the stability and accuracy of the entire positioning process.

[0157] In some embodiments, in the above Figure 6 Based on the illustrated embodiment, step S603 updates the point cloud factor map according to the point cloud data, inertial measurement data, wheel speed data, and the initial global pose of the current point cloud frame. This includes: determining the motion constraint information between the current point cloud frame and the previous point cloud frame based on the inertial measurement data and wheel speed data; performing point cloud matching on the current point cloud frame and the previous point cloud frame to determine the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame; and updating the point cloud factor map based on the initial global pose of the current point cloud frame, the motion constraint information between the current point cloud frame and the previous point cloud frame, and the fifth geometric constraint information.

[0158] For example, the principle for determining the motion constraint information and the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame in this step is similar to the principle for determining the constraint information in the first factor graph in the map construction method described above. That is, motion constraints are constructed based on the point cloud data by fusing inertial measurement data and wheel speed data to reflect the relative motion relationship between adjacent point cloud frames. After obtaining the motion constraint information and the fifth geometric constraint information, the initial global pose of the current point cloud frame is used as prior information and added to the point cloud factor graph as a new factor node together with the above two constraint information, thereby realizing the update of the point cloud factor graph.

[0159] When updating the point cloud factor graph, the initial global pose of the current point cloud frame is first used as a priori factor to provide an initial estimate of its pose variables. Then, the fused motion constraint information and fifth geometric constraint information are constructed as corresponding factor nodes and added to the point cloud factor graph, connected to the pose variable nodes of the current and previous point cloud frames. Through these newly added factor nodes, as the robot or vehicle uses sensors to detect more multimodal data, the different types of constraints provided by the multimodal data are integrated into the point cloud factor graph. This allows the point cloud factor graph to optimize the initial global pose of the current frame, outputting the optimized global pose of the current frame, and thus obtaining the relative pose between the current point cloud frame and the previous point cloud frame in the point cloud factor graph.

[0160] The method in this embodiment effectively improves the robustness and accuracy stability of pose estimation by combining motion constraint information with geometric constraint information and incorporating it into the point cloud optimization factor map.

[0161] In some embodiments, in the above Figure 6 Based on the illustrated embodiment, step S603, which updates the point cloud factor map according to point cloud data, inertial measurement data, wheel speed data, and the initial global pose of the current point cloud frame, may further include: determining motion constraint information between the current point cloud frame and the previous point cloud frame based on inertial measurement data and wheel speed data; performing point cloud matching on the current point cloud frame and the previous point cloud frame to determine the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame; responding to the existence of a current visual keyframe corresponding to the current point cloud frame in the image data of the multimodal data, determining the sixth geometric constraint information between the point cloud frame corresponding to the previous visual keyframe in the point cloud factor map and the current point cloud frame based on the current visual keyframe and the previous visual keyframe in the image data; and updating the point cloud factor map based on the initial global pose of the current point cloud frame, the motion constraint information between the current point cloud frame and the previous point cloud frame, the fifth geometric constraint information, and the sixth geometric constraint information.

[0162] For example, image data can be acquired by a visual sensor (such as a camera). The current visual keyframe is an image frame that is temporally synchronized with or adjacent to the current point cloud frame, while the previous visual keyframe is an image frame corresponding to the previous point cloud frame. When a current visual keyframe exists, constraints can be constructed through visual feature matching to further enhance the constraint dimension of the factor map. For example, the sixth geometric constraint constructed by visual features can be the third geometric constraint information determined based on the visual reprojection error of the visual keyframe, consistent with the map construction method described above. By extracting ORB feature points from the current and previous visual keyframes and matching these feature points, the matched feature points are projected from the image coordinate system to the camera coordinate system using camera intrinsics and distortion parameters. Then, combined with the global pose of the point cloud frame corresponding to the previous visual keyframe, the reprojection error constraint between the current and previous visual keyframes is constructed.

[0163] Reprojection error describes the deviation between the actually observed image feature points and the theoretical image points obtained by backprojection based on the estimated pose. Minimizing the reprojection error can provide additional visual geometric constraints for the global pose of the current point cloud frame. Adding the sixth geometric constraint information as a new factor node to the point cloud factor graph and connecting it with the pose variable nodes of the point cloud frames corresponding to the current point cloud frame and the previous visual keyframe can fully utilize the rich texture and structural information in the image data, further improving the accuracy and robustness of localization and achieving deep fusion of multimodal data.

[0164] It should be noted that if the map building stage is based on multi-source fusion of point cloud data, IMU data, and wheel velocity data to obtain a point cloud map, then when using this point cloud map for positioning and navigation, visual constraints need not be introduced to optimize the pose of the current point cloud frame. However, if visual constraints are additionally introduced during the map building stage to optimize the point cloud pose and generate a point cloud map, then when using this point cloud map for positioning and navigation, visual constraints need to be introduced to optimize the pose of the current point cloud frame, thereby improving the matching degree between the navigation and positioning side and the map building stage. In practical applications, whether or not to introduce visual constraints can also be set according to requirements, and the embodiments disclosed in this disclosure are not limited thereto.

[0165] like Figure 8 As shown above, in the above Figure 6 Based on the illustrated embodiment, step S605 may include the following steps S6051-S6053: Step S6051: Determine the optimized global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame.

[0166] For example, the actual global pose of the previous point cloud frame is represented by the transformation matrix T_prev in the global coordinate system, and the relative pose transformation matrix between the current point cloud frame and the previous point cloud frame is ΔT. Therefore, the optimized global pose transformation matrix T_current_opt of the current point cloud frame can be calculated by multiplying T_prev and ΔT, i.e., T_current_opt = T_prev × ΔT. Compared to the initial global pose, this optimized global pose has undergone joint optimization by multiple source constraints in the point cloud factor map, resulting in higher accuracy and reliability.

[0167] Step S6052: Based on optimizing the global pose, calculate the point cloud matching between the current point cloud frame and the point cloud map.

[0168] For example, using the optimized global pose as the new initial transformation parameters, the current point cloud frame is precisely matched with the point cloud map again. Utilizing the optimized pose as a better starting point, the matching relationship between the current point cloud frame and the map is further refined to examine and correct any subtle deviations that may exist in the optimized global pose. During the matching process, the transformation parameters are adjusted to further minimize the sum of squared distances between points in the current point cloud frame and their corresponding points in the map point cloud, thereby obtaining a more refined pose adjustment.

[0169] Step S6053: Adjust the optimized global pose according to the point cloud matching situation to obtain the actual global pose of the current point cloud frame.

[0170] For example, the pose adjustment obtained through the point cloud matching process described above can be understood as a fine-tuning transformation matrix. Multiplying this fine-tuning transformation matrix by the optimized global pose transformation matrix T_current_opt yields the actual global pose transformation matrix T_current_final after the final fine-tuning of the current point cloud frame. For instance, if the fine-tuning transformation matrix is ​​ΔT_fine, then T_current_final = T_current_opt × ΔT_fine.

[0171] The method in this embodiment uses the optimized pose as a more accurate initial value for secondary matching and adjustment, which can effectively eliminate minor deviations that may remain due to initial matching errors or optimization processes, further improving the absolute accuracy of the actual global pose of the current point cloud frame and ensuring the accuracy of the robot or vehicle's positioning results in complex environments.

[0172] In some embodiments, in order to provide high-frequency global pose information during the movement of a robot or vehicle, the localization method provided in this disclosure can be performed only for key point cloud frames. When it is determined that the current point cloud frame is a non-key point cloud frame, the complex optimization process based on the point cloud factor map described above is no longer performed. Instead, the actual global pose of the previous frame (which may be a key point cloud frame or a processed non-key point cloud frame) is directly used, combined with the motion constraint information (i.e., relative pose change) obtained by fusing inertial measurement data and wheel speed data between the current frame and the previous frame, and the global pose of the current non-key point cloud frame is obtained by simple pose transformation superposition calculation.

[0173] Differential localization processing triggered by key point cloud frames can significantly improve the frequency of overall localization output, meeting the real-time requirements of robots or vehicles. The selection of key point cloud frames can be based on preset time intervals, distance intervals, or dynamically determined by detecting changes in environmental features. For example, when the vehicle moves to a new environmental area or sensor data changes significantly, the current point cloud frame is marked as a key point cloud frame, and the complete optimized localization process is executed to ensure the accuracy of map matching and pose estimation.

[0174] Figure 9 This is a logical diagram illustrating map construction and real-time positioning provided in an exemplary embodiment of this disclosure, such as... Figure 9 As shown, based on the multimodal data collected by sensors, multi-source sensor data fusion is performed to first construct a high-precision global point cloud map, and then real-time and stable global positioning is achieved based on this map.

[0175] Specifically, the steps in the mapping phase include: (1) Data acquisition and preprocessing In this step, the sensors collect multimodal data, including point cloud data output by the lidar, image data output by the vision sensor, inertial measurement data output by the inertial measurement unit (IMU), and wheel speed data output by the wheel speed odometer.

[0176] The multimodal data is then preprocessed, including distortion correction and downsampling of point cloud data; zero-bias calibration and noise filtering of inertial measurement data; removal of outliers from wheel speed data; and spatiotemporal consistency calibration of multi-source data through timestamp alignment and coordinate system transformation.

[0177] (2) Point cloud pose optimization This step corresponds to steps S102-S103 and their included embodiments in the map construction method described above. Specifically, it involves constructing a first factor map by fusing point cloud data, inertial measurement data, wheel speed data, and optional image data to optimize the pose of each mapping point cloud frame. More specifically, based on preprocessed multimodal data, motion constraint information and first geometric constraint information between adjacent mapping point cloud frames are determined, as well as second and third geometric constraint information (if image data is introduced) between the mapping point cloud frame and the visual keyframe. These constraint information are then integrated into the first factor map. By optimizing and solving the first factor map, the optimized target pose of each mapping point cloud frame is obtained.

[0178] (3) Generation of local point cloud map This step corresponds to step S104 in the map construction method described above and its included embodiments. Specifically, candidate frames are filtered based on the overlap between point cloud frames (a first preset threshold). Filtering stops when the number of frames in the sequence exceeds a preset value or the overlap between the beginning and end is lower than a second preset threshold (less than the first threshold). A second factor map is constructed based on the filtered point cloud frames within the sequence, and this second factor map is optimized to obtain the precise relative pose of each point cloud frame within the sequence. Subsequently, based on these precise relative poses, all point cloud frames in the sequence are transformed to the same local coordinate system. Point cloud registration algorithms (such as ICP and its improved algorithms) are used to fuse and stitch the point cloud frames together to generate a local point cloud map. When generating the local point cloud map, it can be associated with and stored with the optimized pose information corresponding to the sequence, providing basic data units for the subsequent construction of the global point cloud map.

[0179] (4) Global point cloud map generation This step corresponds to step S105 in the map construction method described above and its included embodiments. For each local point cloud map, the poses of the first and last point cloud frames relative to themselves can be determined as the endpoint poses of the local point cloud map. By calculating the overlap between any two local point cloud maps, associated local point cloud maps are filtered. Then, with the global poses of each local point cloud map as variable nodes, and the relative pose constraints and endpoint pose prior constraints of the associated map pairs as factor nodes, a third factor map is constructed and optimized to obtain the global poses of each local point cloud map. Based on these global poses, all local point cloud maps are unified into the global coordinate system for stitching and fusion.

[0180] This global point cloud map contains 3D geometric information about the environment and the global positional relationships of various parts within it, providing a high-precision environmental model foundation for subsequent applications such as localization, navigation, and path planning. Furthermore, after the global point cloud map is generated, it can be further processed according to application requirements, such as map compression, noise removal, feature extraction, and semantic information annotation, to reduce map storage overhead, improve map usability, and support more advanced intelligent tasks.

[0181] After obtaining the global point cloud map, the robot or vehicle can store the global point cloud map offline and achieve real-time positioning based on the global point cloud map. The real-time positioning steps may include: (1) Localization data acquisition and initial global pose estimation In this step, the robot or vehicle can use sensors to collect multimodal data in real time, and use the current point cloud frame in the multimodal data to perform feature matching with the mapping point cloud frame of the global point cloud map, quickly narrowing the search range and identifying potential target point cloud frames in the mapping point cloud frame. Then, based on the mapping pose of the target point cloud frame, accurate point cloud matching is performed to quickly determine the initial global pose of the current point cloud frame.

[0182] (2) Pose optimization and relative pose acquisition This step corresponds to steps S603-S604 of the above-mentioned positioning method and the contents of its embodiments. That is, by constructing a point cloud factor graph, the initial global pose of the current point cloud frame is used as the variable node to be optimized, and various constraint information provided by multimodal data is fused as factor nodes to optimize the initial global pose. The graph optimization outputs the accurate relative pose between the current frame and the previous frame.

[0183] (3) Global pose determination This step corresponds to step S605 of the above positioning method and its included embodiments, that is, by using the relative pose between the current point cloud frame and the previous point cloud frame, combined with the actual global pose of the previous point cloud frame, the optimized global pose of the current point cloud frame is initially determined by superimposing the pose transformations.

[0184] To further improve accuracy, using the optimized global pose as a benchmark, the current point cloud frame is precisely matched again with the point clouds of the corresponding region in the global point cloud map. For example, the ICP algorithm or its improved algorithm is used to obtain a fine-tuned transformation matrix by minimizing the distance error between points or between points and surfaces. Finally, this fine-tuned transformation matrix is ​​combined with the optimized global pose to obtain the actual global pose of the current point cloud frame after fine adjustment.

[0185] Meanwhile, in order to ensure the real-time positioning, the real-time positioning steps of this scheme can be performed on the point cloud of key frames. For the point cloud of non-key frames, a simplified processing flow can be adopted, that is, the actual global pose of the previous frame and the motion constraint information obtained between the current frame and the previous frame based on inertial measurement data and wheel speed data can be directly used to quickly calculate the global pose of the current non-key frame. This ensures the positioning accuracy of key frames while meeting the system's requirement for high-frequency output.

[0186] The specific settings and implementation methods of the embodiments of this disclosure have been described above from different perspectives. Using the methods provided in the above embodiments, deep fusion of multi-source sensor data can be achieved during map construction and localization, thereby constructing a high-precision global point cloud map, and on this basis, real-time, robust, and high-precision global pose determination can be achieved. This not only improves the environmental perception capabilities of robots or autonomous vehicles in complex dynamic environments, but also overcomes the limitations of traditional single-modal localization methods in terms of accuracy and robustness, meeting the practical application requirements of real-time positioning and navigation.

[0187] Exemplary device Figure 10 This is a schematic diagram of a map building apparatus provided in an exemplary embodiment of the present disclosure, the apparatus comprising: The data determination module 1001 is used to determine the multimodal data collected by the sensor, including point cloud data, inertial measurement data, and wheel speed data. The factor graph module 1002 is used to construct a first factor graph based on point cloud data, inertial measurement data and wheel speed data, and to perform sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor graph to obtain the target pose of each point cloud frame in the point cloud data.

[0188] The local mapping module 1003 is used to generate multiple local point cloud maps based on point cloud data and the target pose of each point cloud frame in the point cloud data.

[0189] The global mapping module 1004 is used to construct a global point cloud map based on multiple local point cloud maps.

[0190] In some embodiments, the factor graph module 1002 is used to: determine the initial pose of each point cloud frame in the point cloud data based on inertial measurement data and wheel speed data; determine the constraint information between point cloud frames in the point cloud data based on point cloud data, inertial measurement data and wheel speed data in multimodal data; and construct a first factor graph based on the initial pose and constraint information of each point cloud frame in the point cloud data.

[0191] In some embodiments, the factor graph module 1002 is used to: determine motion constraint information between adjacent point cloud frames in point cloud data based on inertial measurement data and wheel speed data; perform point cloud matching on adjacent point cloud frames in point cloud data to determine first geometric constraint information between adjacent point cloud frames; and perform point cloud matching on key point cloud frames in point cloud data to determine second geometric constraint information between key point cloud frames; wherein the constraint information between point cloud frames includes motion constraint information, first geometric constraint information, and second geometric constraint information.

[0192] In some embodiments, the factor graph module 1002 is used to: determine motion constraint information between adjacent point cloud frames in point cloud data based on inertial measurement data and wheel speed data; perform point cloud matching on adjacent point cloud frames in point cloud data to determine first geometric constraint information between adjacent point cloud frames; perform point cloud matching on key point cloud frames in point cloud data to determine second geometric constraint information between key point cloud frames; determine multiple visual key frames based on image data in multimodal data, and determine third geometric constraint information between point cloud frames corresponding to adjacent visual key frames based on the multiple visual key frames and the initial pose of each point cloud frame; wherein the constraint information between point cloud frames includes motion constraint information, first geometric constraint information, second geometric constraint information, and third geometric constraint information.

[0193] In some embodiments, the factor graph module 1002 is used to: determine the feature correspondence between adjacent visual keyframes in a plurality of visual keyframes; determine the initial pose of each visual keyframe in a plurality of visual keyframes based on the initial pose of each point cloud frame in the point cloud data; determine the visual reprojection error of each visual keyframe based on the initial pose and feature correspondence of each visual keyframe; and determine the third geometric constraint information between the point cloud frames corresponding to adjacent visual keyframes based on the visual reprojection error of each visual keyframe.

[0194] In some embodiments, the factor graph module 1002 is configured to: construct a total error function based on the constraint relationships in the first factor graph; iteratively optimize the pose of each point cloud frame in the first factor graph based on the total error function; in response to the number of point cloud frames in the first factor graph exceeding the limit of the sliding window, determine marginalized point cloud frames based on the timestamps of each point cloud frame in the first factor graph; remove the marginalized point cloud frames in the first factor graph, and use the pose of the marginalized point cloud frames at the time of removal as the target pose of the marginalized point cloud frames; and in response to the marginalized point cloud frames being key point cloud frames, fix and retain the target pose of the key point cloud frames in the first factor graph.

[0195] In some embodiments, the local mapping module 1003 is configured to: determine a mapping point cloud sequence based on point cloud data; and, if the mapping point cloud sequence meets preset conditions, construct a second factor map based on the point cloud frames in the mapping point cloud sequence; perform graph optimization on the second factor map to obtain the optimized pose of each point cloud frame in the mapping point cloud sequence; and merge each point cloud frame in the mapping point cloud sequence into a local point cloud map based on the optimized pose of each point cloud frame in the mapping point cloud sequence.

[0196] In some embodiments, the local mapping module 1003 is configured to: for any point cloud frame in the point cloud data, determine the overlap between the point cloud frame and candidate point cloud frames in the mapping point cloud sequence, and add the point cloud frame as the latest candidate point cloud frame to the mapping point cloud sequence if the overlap is less than a first preset threshold; in response to the number of candidate point cloud frames in the mapping point cloud sequence being greater than a preset value or the overlap between the first and last candidate point cloud frames being less than a second preset threshold, construct a second factor map based on the point cloud frames in the mapping point cloud sequence; wherein the first preset threshold is greater than the second preset threshold.

[0197] In some embodiments, the local mapping module 1003 is configured to: perform iterative nearest-point matching between each point cloud frame in the mapping point cloud sequence and other point cloud frames in the mapping point cloud sequence to obtain fourth geometric constraint information between each point cloud frame and other point cloud frames in the mapping point cloud sequence; perform pre-integration calculation based on inertial measurement data and wheel speed data to obtain motion constraint information between adjacent point cloud frames in the mapping point cloud sequence; determine the prior constraint information of each point cloud frame in the mapping point cloud sequence in the first factor map; and construct a second factor map based on the target pose, fourth geometric constraint information, motion constraint information between adjacent point cloud frames in the mapping point cloud sequence, and prior constraint information of each point cloud frame in the mapping point cloud sequence.

[0198] In some embodiments, the global mapping module 1004 is used to: determine the endpoint poses of each local point cloud map; wherein the endpoint poses include the poses of the first and last point cloud frames in the local point cloud map relative to the local point cloud map; determine multiple sets of associated map pairs in multiple local point cloud maps, each set of associated map pairs containing two local point cloud maps with an overlap greater than a third preset threshold; construct a third factor map based on the multiple local point cloud maps, the endpoint poses of each local point cloud map, and the multiple sets of associated map pairs; perform graph optimization based on the third factor map to obtain the global poses of each local point cloud map; and stitch the local point cloud maps together based on the global poses of each local point cloud map to obtain a global point cloud map.

[0199] Figure 11 This is a schematic diagram of a positioning device provided in an exemplary embodiment of the present disclosure, the device including: The first determining module 1101 is used to determine the multimodal data collected by the sensor, including point cloud data, inertial measurement data and wheel speed data.

[0200] The second determining module 1102 is used to determine the initial global pose of the current point cloud frame based on the current point cloud frame and the point cloud map in the point cloud data.

[0201] The graph update module 1103 is used to update the point cloud factor graph based on point cloud data, inertial measurement data, wheel speed data and the initial global pose of the current point cloud frame.

[0202] The graph optimization module 1104 is used to perform graph optimization based on the point cloud factor graph to obtain the relative pose between the current point cloud frame and the previous point cloud frame in the point cloud factor graph.

[0203] The global positioning module 1105 is used to determine the actual global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame.

[0204] In some embodiments, the second determining module 1102 is configured to: determine the point cloud feature information of the current point cloud frame; based on the point cloud feature information of the current point cloud frame, determine a target point cloud frame that matches the current point cloud frame in the mapping point cloud frame set of the point cloud map; calculate the point cloud matching situation between the current point cloud frame and the point cloud map based on the mapping pose of the target point cloud frame, and determine the initial global pose of the current point cloud frame according to the point cloud matching situation.

[0205] In some embodiments, the graph update module 1103 is configured to: determine motion constraint information between the current point cloud frame and the previous point cloud frame based on inertial measurement data and wheel speed data; perform point cloud matching on the current point cloud frame and the previous point cloud frame to determine the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame; and update the point cloud factor graph based on the initial global pose of the current point cloud frame, the motion constraint information between the current point cloud frame and the previous point cloud frame, and the fifth geometric constraint information.

[0206] In some embodiments, the graph optimization module 1104 is configured to: determine motion constraint information between the current point cloud frame and the previous point cloud frame based on inertial measurement data and wheel speed data; perform point cloud matching on the current point cloud frame and the previous point cloud frame to determine the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame; in response to the existence of a current visual keyframe corresponding to the current point cloud frame in the image data of the multimodal data, determine the sixth geometric constraint information between the point cloud frame corresponding to the previous visual keyframe in the point cloud factor graph and the current point cloud frame based on the current visual keyframe and the previous visual keyframe in the image data; and update the point cloud factor graph based on the initial global pose of the current point cloud frame, the motion constraint information between the current point cloud frame and the previous point cloud frame, the fifth geometric constraint information, and the sixth geometric constraint information.

[0207] In some embodiments, the global positioning module 1105 is used to: determine the optimized global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame; calculate the point cloud matching situation between the current point cloud frame and the point cloud map based on the optimized global pose; and adjust the optimized global pose according to the point cloud matching situation to obtain the actual global pose of the current point cloud frame.

[0208] The beneficial technical effects corresponding to the exemplary embodiments of this device can be found in the corresponding beneficial technical effects in the exemplary method section above, and will not be repeated here.

[0209] Exemplary electronic devices Figure 12 The present disclosure provides a structural diagram of an electronic device 12, which includes at least one processor 121 and a memory 122.

[0210] The processor 121 may be a central processing unit (CPU) or other form of processing unit with data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 12 to perform desired functions.

[0211] The memory 122 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. Volatile memory may include, for example, random access memory (RAM) and / or cache memory. Non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 121 may execute one or more computer program instructions to implement the map building method and / or positioning method and / or other desired functions of the various embodiments of this disclosure described above.

[0212] In one example, the electronic device 12 may also include an input device 123 and an output device 124, which are interconnected via a bus system and / or other forms of connection mechanism (not shown).

[0213] The input device 123 may also include, for example, a keyboard, a mouse, etc.

[0214] The output device 124 can output various information to the outside, including, for example, a display, a speaker, a printer, and a communication network and its connected remote output devices.

[0215] Of course, for the sake of simplicity, Figure 12 Only some of the components of the electronic device 12 relevant to this disclosure are shown, omitting components such as buses, input / output interfaces, etc. In addition, the electronic device 12 may include any other suitable components depending on the specific application.

[0216] Exemplary computer program products and computer-readable storage media In addition to the methods and apparatus described above, embodiments of this disclosure may also provide a computer program product, including computer program instructions that, when executed by a processor, cause the processor to perform the steps of the map building methods and / or positioning methods of the various embodiments of this disclosure described in the "Exemplary Methods" section above.

[0217] Computer program products can be written in any combination of one or more programming languages ​​to perform the operations of embodiments of this disclosure. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on a user's computing device, partially on a user's computing device, as a standalone software package, partially on a user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0218] Furthermore, embodiments of this disclosure may also be computer-readable storage media storing computer program instructions thereon, which, when executed by a processor, cause the processor to perform the steps in the map construction methods and / or positioning methods of the various embodiments of this disclosure described in the "Exemplary Methods" section above.

[0219] Computer-readable storage media may take the form of any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may include, but is not limited to, systems, apparatuses, or devices that are electrical, magnetic, optical, electromagnetic, infrared, or semiconductor, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0220] The basic principles of this disclosure have been described above with reference to specific embodiments. However, the advantages, benefits, and effects mentioned in this disclosure are merely examples and not limitations, and should not be considered as essential features of each embodiment of this disclosure. Furthermore, the specific details disclosed above are for illustrative and facilitative purposes only, and are not limitations. These details do not limit the scope of this disclosure to the necessity of employing the aforementioned specific details for implementation.

[0221] Various modifications and variations can be made to this disclosure without departing from the spirit and scope of this application. Therefore, if such modifications and variations fall within the scope of the claims of this disclosure and their equivalents, this disclosure is also intended to include such modifications and variations.

Claims

1. A map construction method, comprising: The multimodal data collected by the sensors is determined, including point cloud data, inertial measurement data, and wheel speed data; A first factor map is constructed based on the point cloud data, the inertial measurement data, and the wheel speed data; Based on the first factor map, the pose of each point cloud frame in the point cloud data is optimized by a sliding window to obtain the target pose of each point cloud frame in the point cloud data. Based on the point cloud data and the target pose of each point cloud frame in the point cloud data, multiple local point cloud maps are generated. A global point cloud map is constructed based on the multiple local point cloud maps.

2. The method according to claim 1, wherein, The step of constructing a first factor map based on the point cloud data, the inertial measurement data, and the wheel speed data includes: The initial pose of each point cloud frame in the point cloud data is determined based on the inertial measurement data and the wheel speed data. Based on the point cloud data, the inertial measurement data, and the wheel speed data in the multimodal data, the constraint information between point cloud frames in the point cloud data is determined; Based on the initial pose of each point cloud frame in the point cloud data and the constraint information, the first factor map is constructed.

3. The method according to claim 2, wherein, The step of determining constraint information between point cloud frames in the point cloud data based on the point cloud data, the inertial measurement data, and the wheel speed data in the multimodal data includes: Based on the inertial measurement data and the wheel speed data, motion constraint information between adjacent point cloud frames in the point cloud data is determined; Perform point cloud matching on adjacent point cloud frames in the point cloud data to determine the first geometric constraint information between the adjacent point cloud frames; Point cloud matching is performed on key point cloud frames in the point cloud data to determine the second geometric constraint information between the key point cloud frames. The constraint information between the point cloud frames includes the motion constraint information, the first geometric constraint information, and the second geometric constraint information.

4. The method according to claim 2, wherein, The step of determining constraint information between point cloud frames in the point cloud data based on the point cloud data, the inertial measurement data, and the wheel speed data in the multimodal data includes: Based on the inertial measurement data and the wheel speed data, motion constraint information between adjacent point cloud frames in the point cloud data is determined; Perform point cloud matching on adjacent point cloud frames in the point cloud data to determine the first geometric constraint information between the adjacent point cloud frames; Point cloud matching is performed on key point cloud frames in the point cloud data to determine the second geometric constraint information between the key point cloud frames. Multiple visual keyframes are determined based on the image data in the multimodal data, and third geometric constraint information between the point cloud frames corresponding to adjacent visual keyframes is determined based on the multiple visual keyframes and the initial pose of each point cloud frame. The constraint information between the point cloud frames includes the motion constraint information, the first geometric constraint information, the second geometric constraint information, and the third geometric constraint information.

5. The method according to claim 4, wherein, The step of determining the third geometric constraint information between point cloud frames corresponding to adjacent visual keyframes based on the initial poses of the plurality of visual keyframes and each point cloud frame includes: Determine the feature correspondence between adjacent visual keyframes in the plurality of visual keyframes; Based on the initial pose of each point cloud frame in the point cloud data, the initial pose of each visual key frame in the plurality of visual key frames is determined. Based on the initial pose of each visual keyframe and the correspondence between the features, the visual reprojection error of each visual keyframe is determined. Based on the visual reprojection error of each visual keyframe, the third geometric constraint information between the point cloud frames corresponding to adjacent visual keyframes is determined.

6. The method according to any one of claims 1-5, wherein, The step of performing sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor map to obtain the target pose of each point cloud frame in the point cloud data includes: A total error function is constructed based on the constraint relationships in the first factor graph, and the pose of each point cloud frame in the first factor graph is iteratively optimized based on the total error function. In response to the fact that the number of point cloud frames in the first factor graph exceeds the limit of the sliding window, the marginalized point cloud frames are determined based on the timestamps of each point cloud frame in the first factor graph. In the first factor map, the marginal point cloud frame is removed, and the pose of the marginal point cloud frame at the time of removal is taken as the target pose of the marginal point cloud frame. In response to the marginalized point cloud frame being a key point cloud frame, the target pose of the key point cloud frame is fixed and preserved in the first factor map.

7. The method according to any one of claims 1-5, wherein, The step of generating multiple local point cloud maps based on the point cloud data and the target pose of each point cloud frame in the point cloud data includes: Based on the point cloud data, a mapping point cloud sequence is determined, and if the mapping point cloud sequence meets preset conditions, a second factor map is constructed based on the point cloud frames in the mapping point cloud sequence. Graph optimization is performed on the second factor graph to obtain the optimized pose of each point cloud frame in the constructed point cloud sequence; Based on the optimized pose of each point cloud frame in the mapping point cloud sequence, the point cloud frames in the mapping point cloud sequence are merged into a local point cloud map.

8. The method according to claim 7, wherein, The step of determining a mapping point cloud sequence based on the point cloud data, and constructing a second factor map based on the point cloud frames in the mapping point cloud sequence when the mapping point cloud sequence meets preset conditions, includes: For any point cloud frame in the point cloud data, determine the overlap between the point cloud frame and the candidate point cloud frames in the mapping point cloud sequence, and add the point cloud frame as the latest candidate point cloud frame to the mapping point cloud sequence if the overlap is less than a first preset threshold. In response to the number of candidate point cloud frames in the mapping point cloud sequence being greater than a preset value or the overlap between the first and last candidate point cloud frames being less than a second preset threshold, a second factor map is constructed based on the point cloud frames in the mapping point cloud sequence; wherein, the first preset threshold is greater than the second preset threshold.

9. The method according to claim 7, wherein, The step of constructing a second factor map based on point cloud frames in the point cloud mapping sequence includes: For each point cloud frame in the mapping point cloud sequence, iterative nearest point matching is performed with other point cloud frames in the mapping point cloud sequence to obtain the fourth geometric constraint information between each point cloud frame in the mapping point cloud sequence and other point cloud frames. Based on the inertial measurement data and the wheel speed data, pre-integration calculation is performed to obtain motion constraint information between adjacent point cloud frames in the mapped point cloud sequence; Determine the prior constraint information of each point cloud frame in the constructed point cloud sequence in the first factor map; The second factor map is constructed based on the target pose of each point cloud frame in the mapping point cloud sequence, the fourth geometric constraint information, the motion constraint information between adjacent point cloud frames in the mapping point cloud sequence, and the prior constraint information.

10. The method according to any one of claims 1-5, wherein, The construction of a global point cloud map based on the multiple local point cloud maps includes: Determine the endpoint pose of each of the local point cloud maps; wherein, the endpoint pose includes the pose of the first and last point cloud frames in the local point cloud map relative to the local point cloud map. Multiple sets of associated map pairs are determined in the multiple local point cloud maps, and each set of associated map pairs contains two local point cloud maps with an overlap greater than a third preset threshold. A third factor graph is constructed based on the multiple local point cloud maps, the endpoint poses of each local point cloud map, and the multiple sets of associated map pairs; Graph optimization is performed based on the third factor graph to obtain the global pose of each of the local point cloud maps; The local point cloud maps are stitched together based on their global poses to obtain the global point cloud map.

11. A positioning method, comprising: The multimodal data collected by the sensors is determined, including point cloud data, inertial measurement data, and wheel speed data; Based on the current point cloud frame and point cloud map in the point cloud data, determine the initial global pose of the current point cloud frame; Update the point cloud factor map based on the point cloud data, the inertial measurement data, the wheel speed data, and the initial global pose of the current point cloud frame; Graph optimization is performed based on the point cloud factor map to obtain the relative pose between the current point cloud frame and the previous point cloud frame in the point cloud factor map. The actual global pose of the current point cloud frame is determined based on the relative pose and the actual global pose of the previous point cloud frame.

12. The method according to claim 11, wherein, The step of determining the initial global pose of the current point cloud frame based on the current point cloud frame and the point cloud map in the point cloud data includes: Determine the point cloud feature information of the current point cloud frame; Based on the point cloud feature information of the current point cloud frame, a target point cloud frame that matches the current point cloud frame is determined from the mapping point cloud frame set of the point cloud map. Based on the mapping pose of the target point cloud frame, the point cloud matching between the current point cloud frame and the point cloud map is calculated, and the initial global pose of the current point cloud frame is determined according to the point cloud matching.

13. The method according to claim 11, wherein, The step of updating the point cloud factor map based on the point cloud data, the inertial measurement data, the wheel speed data, and the initial global pose of the current point cloud frame includes: Based on the inertial measurement data and the wheel speed data, determine the motion constraint information between the current point cloud frame and the previous point cloud frame; Perform point cloud matching on the current point cloud frame and the previous point cloud frame to determine the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame; The point cloud factor map is updated based on the initial global pose of the current point cloud frame, the motion constraint information between the current point cloud frame and the previous point cloud frame, and the fifth geometric constraint information.

14. The method according to claim 11, wherein, The step of updating the point cloud factor map based on the point cloud data, the inertial measurement data, the wheel speed data, and the initial global pose of the current point cloud frame includes: Based on the inertial measurement data and the wheel speed data, determine the motion constraint information between the current point cloud frame and the previous point cloud frame; Perform point cloud matching on the current point cloud frame and the previous point cloud frame to determine the fifth geometric constraint information between the current point cloud frame and the previous point cloud frame; In response to the existence of a current visual keyframe corresponding to the current point cloud frame in the image data of the multimodal data, the sixth geometric constraint information between the point cloud frame corresponding to the previous visual keyframe in the point cloud factor map and the current point cloud frame is determined based on the current visual keyframe and the previous visual keyframe in the image data. The point cloud factor map is updated based on the initial global pose of the current point cloud frame, the motion constraint information between the current point cloud frame and the previous point cloud frame, the fifth geometric constraint information, and the sixth geometric constraint information.

15. The method according to any one of claims 11-14, wherein, The step of determining the actual global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame in the point cloud factor map includes: Based on the relative pose and the actual global pose of the previous point cloud frame, determine the optimized global pose of the current point cloud frame; Based on the optimized global pose, calculate the point cloud matching between the current point cloud frame and the point cloud map; The optimized global pose is adjusted based on the point cloud matching to obtain the actual global pose of the current point cloud frame.

16. A map building apparatus, comprising: The data determination module is used to determine the multimodal data collected by the sensor, including point cloud data, inertial measurement data, and wheel speed data; The factor graph module is used to construct a first factor graph based on the point cloud data, the inertial measurement data and the wheel speed data, and to perform sliding window optimization on the pose of each point cloud frame in the point cloud data based on the first factor graph to obtain the target pose of each point cloud frame in the point cloud data. The local mapping module is used to generate multiple local point cloud maps based on the point cloud data and the target pose of each point cloud frame in the point cloud data. The global mapping module is used to construct a global point cloud map based on the multiple local point cloud maps.

17. A positioning device, comprising: The first determining module is used to determine the multimodal data collected by the sensor, the multimodal data including point cloud data, inertial measurement data and wheel speed data; The second determining module is used to determine the initial global pose of the current point cloud frame based on the current point cloud frame and the point cloud map in the point cloud data. The graph update module is used to update the point cloud factor graph based on the point cloud data, the inertial measurement data, the wheel speed data, and the initial global pose of the current point cloud frame. The graph optimization module is used to perform graph optimization based on the point cloud factor graph to obtain the relative pose between the current point cloud frame and the previous point cloud frame in the point cloud factor graph. The global positioning module is used to determine the actual global pose of the current point cloud frame based on the relative pose and the actual global pose of the previous point cloud frame.

18. A computer-readable storage medium storing a computer program for performing the map construction method of any one of claims 1-10, or for performing the positioning method of any one of claims 11-15.

19. An electronic device, the electronic device comprising: processor; Memory used to store the processor's executable instructions; The processor is configured to read the executable instructions from the memory and execute the instructions to implement the map construction method of any one of claims 1-10, or to implement the positioning method of any one of claims 11-15.

Images