Real-time pose estimation method and device of robot, electronic equipment and storage medium

Abstract

The invention provides a real-time pose estimation method and device of a robot, electronic equipment and a storage medium, and relates to the technical field of automatic driving. According to the specific implementation scheme, multiple pieces of sensor data of the robot are obtained, wherein the multiple pieces of sensor data at least comprise image data and point cloud data; generating semantic point cloud data through the image data and the point cloud data; determining a relocation pose of the robot according to the semantic point cloud data and a pre-stored semantic Marker map; inputting the semantic point cloud data, the relocation poses and the multiple sensor data into a plurality of odometers of different types, and respectively obtaining estimated poses output by the odometers; and fusing the estimated poses output by the speedometers through an error state Kalman filter to obtain a fused pose of the robot. According to the technical scheme, the advantages of different sensors can be fully utilized, the error influence of a single speedometer is overcome, and more accurate pose estimation is realized.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to a method, device, electronic device, and storage medium for real-time posture estimation of a robot. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology plays a key role in fields such as autonomous driving, smart homes, and industrial robots. It can support robots in simultaneously building environmental maps and real-time positioning without prior map information. Summary of the Invention

[0003] The embodiments of the present application provide a method, device, electronic device, and storage medium for real-time posture estimation of a robot to alleviate or solve one or more technical problems existing in the prior art.

[0004] In the first aspect, an embodiment of the present application provides a real-time pose estimation method for a robot, comprising: obtaining multiple sensor data of the robot, the multiple sensor data including at least image data and point cloud data; generating semantic point cloud data through image data and point cloud data; determining the repositioning pose of the robot based on the semantic point cloud data and a pre-stored semantic marker map; inputting the semantic point cloud data, the repositioning pose and multiple sensor data into multiple odometers of different types, and obtaining the estimated pose output by each odometer respectively; fusing the estimated pose output by each odometer through an error state Kalman filter to obtain a fused pose of the robot.

[0005] In the second aspect, an embodiment of the present application provides a real-time posture estimation device for a robot, including: a data acquisition module for acquiring multiple sensor data of the robot, the multiple sensor data including at least image data and point cloud data; a semantic point cloud module for generating semantic point cloud data through image data and point cloud data; a repositioning module for determining the repositioning posture of the robot based on the semantic point cloud data and a pre-stored semantic marker map; a posture fusion module for inputting the semantic point cloud data, the repositioning posture and multiple sensor data into multiple odometers of different types, and obtaining the estimated posture output by each odometer respectively; the estimated posture output by each odometer is fused through an error state Kalman filter to obtain the fused posture of the robot.

[0006] In a third aspect, an embodiment of the present application provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory, wherein the processor implements any method of the embodiments of the present application when executing the computer program.

[0007] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the method of any one of the embodiments of the present application is implemented.

[0008] The method based on the embodiment of the present application can fully utilize the advantages of different sensors, overcome the error influence of a single odometer, and achieve more accurate pose estimation.

[0009] The above description is only an overview of the technical solution of this application. In order to more clearly understand the technical means of this application, it can be implemented in accordance with the contents of the specification. In order to make the above and other purposes, features and advantages of this application more obvious and easy to understand, the specific implementation methods of this application are listed below. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] The accompanying drawings are provided to facilitate a better understanding of the present invention and do not constitute a limitation of the present application.

[0011] Figure 1 is a schematic diagram of a system architecture provided according to an embodiment of the present application;

[0012] Figure 2 This is a flow chart of a real-time pose estimation method for a robot according to an embodiment of the present application. Figure 1 ;

[0013] Figure 3 This is a flow chart of a real-time pose estimation method for a robot according to an embodiment of the present application. Figure 2 ;

[0014] Figure 4 This is a flow chart of a real-time pose estimation method for a robot according to an embodiment of the present application. Figure 3 ;

[0015] Figure 5 is a schematic diagram of a real-time position and posture estimation device for a robot according to an embodiment of the present application;

[0016] Figure 6 is a block diagram of an electronic device used to implement the embodiments of the present application. DETAILED DESCRIPTION

[0017] Hereinafter, only certain exemplary embodiments are briefly described. As will be appreciated by those skilled in the art, the described embodiments may be modified in various ways without departing from the spirit or scope of the present application. Therefore, the drawings and description are to be regarded as illustrative in nature and not restrictive.

[0018] To facilitate understanding of the technical solutions of the embodiments of the present application, the following describes the related technologies of the embodiments of the present application. The following related technologies can be combined with the technical solutions of the embodiments of the present application as optional solutions, and all of them fall within the scope of protection of the embodiments of the present application.

[0019] Application Scenario

[0020] SLAM technology mainly relies on sensor data, such as lidar, cameras, inertial measurement units (IMUs) and global positioning systems (GPS), and fuses different data. It then achieves high-precision map construction and real-time positioning through back-end filtering or optimization, and can play an important role in multiple branches of artificial intelligence.

[0021] For example, in the field of autonomous driving, self-driving cars rely on SLAM technology, combined with data from sensors such as lidar and cameras to build maps of roads and surrounding environments. The vehicle locates its own position in real time, perceives obstacles, traffic signs and other information in advance, plans safe and efficient driving routes, assists in autonomous driving decision-making, and improves driving safety and driving experience; in the field of smart homes, robots can use SLAM technology to build indoor maps to locate their own position in real time. Sweeping robots plan efficient cleaning paths by identifying environmental features such as furniture and walls, avoid collisions with obstacles, and achieve autonomous cleaning. Service robots in restaurants or hotels can quickly find rooms or tables according to maps to provide services, thereby improving service efficiency; in the industrial field, SLAM can help robots locate in real time and achieve tasks such as material handling and equipment inspection; in the field of drone technology, drones can also use SLAM technology for auxiliary positioning when the GPS signal is weak, thereby accurately delivering items or collecting data.

[0022] Currently, mainstream SLAM solutions can be divided into laser SLAM, visual SLAM and laser-vision fusion SLAM solutions according to the type of sensor used. However, these solutions fail in some specific scenarios. For example, although laser SLAM can build relatively high-precision maps, feature extraction is relatively difficult and not rich enough; visual SLAM has a low cost, but is extremely susceptible to changes in light intensity; and GPS signal strength is weak indoors or in urban canyon areas, which greatly limits the positioning function of laser-vision fusion SLAM solutions that rely on GPS-assisted positioning in these scenarios. In addition, these SLAM solutions cannot effectively eliminate moving obstacles in the scene, thereby affecting the mapping accuracy.

[0023] Based on this, this application aims to provide a real-time pose estimation method for a robot, which provides accurate robot pose estimation and synchronous mapping functions by fusing data from multiple sensors such as cameras, lidar, IMU, GPS, wheel speed and robot angle, and combining the use of inertial odometry, laser inertial odometry and robot motion model.

[0024] Exemplary Systems

[0025] Figure 1 This is a schematic diagram of the system architecture provided by an exemplary embodiment of the present application. Figure 1 As shown, the system includes two parts, the front end and the back end. The front end can include a data acquisition layer 101, a semantic segmentation layer 102 and a posture fusion layer 103, and the back end can include a relocalization layer 104, a posture optimization layer 105, a loop detection layer 106 and a construction layer 107.

[0026] The data acquisition layer 101 can utilize sensor drivers to acquire sensor data, including but not limited to images, point clouds, IMUs, wheel speeds, robot angles, and GPS. For example, image acquisition devices can be cameras, such as monocular cameras, binocular cameras, and multi-cameras. Binocular and multi-cameras can obtain depth information about objects. Point cloud acquisition devices can include millimeter-wave radars, mechanical lidars, solid-state lidars, and ultrasonic radars.

[0027] The data acquisition layer 101 can also perform time synchronization alignment of the data after collecting the sensor data, and send the time-synchronized sensor data to the semantic segmentation layer 102, the posture fusion layer 103 and the back-end relocalization layer 104 for subsequent processing.

[0028] The semantic segmentation layer 102 can receive the image data and point cloud data sent by the data acquisition layer 101, first pre-process the image data and input the processed image data into the semantic segmentation model, obtain the segmentation mask map output by the semantic segmentation model, and thus determine the part corresponding to the point cloud data in the point cloud data through timestamp matching and project the point cloud data onto the segmentation mask map to form semantic point cloud data containing semantic, color and spatial information.

[0029] The posture fusion layer 103 includes a laser inertial odometry, an inertial odometry and a motion model odometry of the robot. The inertial odometry is used to fuse GPS data and IMU data to obtain an estimated posture. The laser inertial odometry is used to determine the estimated posture of the robot based on the semantic point cloud data and the pre-stored semantic point cloud map, and to obtain the estimated posture using the estimated posture and GPS data. The motion model odometry is used to update the measurement values in the robot's motion prediction equation using GPS data, IMU data, wheel speed data and robot angle data, and to obtain the robot's estimated posture through the motion prediction equation.

[0030] After obtaining the robot's estimated pose output from the laser inertial odometry, inertial odometry, and the robot's motion model odometry, the pose fusion layer 103 can fuse the pose estimates from each odometry using an error-state Kalman filter to produce a more accurate fused pose. Furthermore, the pose fusion layer 103 can output an optimized semantic point cloud for the back-end mapping module to construct and update the semantic marker map.

[0031] The relocalization layer 104 is used to perform preliminary relocalization of the robot by combining the pre-stored semantic marker map and the semantic point cloud output by the front-end semantic segmentation layer 102 to obtain a relocalized posture.

[0032] The posture optimization layer 105 is used to obtain the fused posture output by the posture fusion layer 103 and the repositioned posture output by the repositioning layer 104, and to construct the posture optimization factor graph of the robot in combination with the GPS data, and output the optimized posture through iterative optimization.

[0033] The posture optimization layer 105 is also used to send the optimized posture to the front-end posture fusion layer 103, so that the posture fusion layer 103 uses the optimized posture to initialize each odometer, so that these odometers can accurately track the movement of the robot based on the optimized posture according to the real-time measurement data of the sensor.

[0034] The loop detection layer 106 is used to perform loop detection based on the fused pose output by the pose fusion layer 103 and the optimized semantic point cloud, and to notify the pose optimization layer 105 to add a loop factor in the process of constructing the factor graph when a loop is detected.

[0035] The construction layer 107 is used to convert the optimized semantic point cloud data from the local coordinate system at the time of acquisition to the world coordinate system according to the fusion pose output by the acquired pose fusion layer 103 and the optimized semantic point cloud, and use the converted point cloud data to update the semantic point cloud map and the marker map.

[0036] Based on this system architecture, the embodiment of the present application, on the one hand, fully utilizes the advantages of different sensors and integrates them through real-time algorithms to make the robot's posture estimation more accurate and the real-time mapping more reliable. On the other hand, the algorithm layers can be easily added or subtracted according to the configuration of different sensors, and the scalability is strong.

[0037] Exemplary Methods

[0038] Figure 2 This is a flow chart of a real-time pose estimation method for a robot provided by an exemplary embodiment of the present application. This embodiment can be applied to electronic devices and Figure 1 Systems such as Figure 2 As shown, the method includes the following steps:

[0039] Step S201 : Acquire multiple sensor data of the robot, where the multiple sensor data include at least image data and point cloud data.

[0040] For example, after acquiring multiple sensor data of the robot in step S201 , time synchronization processing may be performed on these sensor data.

[0041] It is understandable that since each sensor on the robot works independently, the start time and frequency of data collection may not be exactly the same. For example, a camera may capture images at a frequency of 30 frames per second, while the scanning frequency of a lidar may be 10 times per second, and an IMU may output data at a higher frequency (such as hundreds of times per second). To ensure the accuracy of subsequent processing, this data needs to be time-synchronized and aligned. For example, each data point can be accurately timestamped to mark its acquisition moment. These timestamps are usually based on a unified clock source. Then, a specific time synchronization algorithm is used to process the timestamped data and adjust them to a unified time scale to ensure the temporal synchronization of the data.

[0042] Step S202: Generate semantic point cloud data through image data and point cloud data.

[0043] Exemplarily, in step S202, semantic point cloud data is generated from the image data and the point cloud data, including: preprocessing the image data and inputting the image data into a semantic segmentation model to obtain a segmentation mask map output by the semantic segmentation model; determining noise points in the point cloud data based on mask IDs in the segmentation mask map, where the noise points include points representing dynamic obstacles;

[0044] Image data is used to fill in the non-noise points in the point cloud data to obtain semantic point cloud data.

[0045] It can be understood that this embodiment can determine the part corresponding to the point cloud data through timestamp matching in the point cloud data, and project the point cloud data onto the segmentation mask map through the external parameters of the lidar and camera (describing the spatial position and posture relationship between the lidar and the camera) and the internal parameters of the camera (describing the imaging characteristics of the camera). In this way, each point in the point cloud data can establish a corresponding relationship with the pixel in the image. Since the image has rich color information, the filled point cloud not only contains the three-dimensional spatial position, but also has the corresponding color information. For example, a point cloud that originally only had spatial coordinates, after filling, if it corresponds to the red table area in the image, is assigned a red RGB value. At the same time, combined with the semantic ID of the segmentation mask map, semantic point cloud data containing semantic, color and spatial information is finally formed.

[0046] In the segmentation mask, different objects or areas in the image data are identified with different IDs to achieve semantic understanding of the image. For example, people, tables, walls, etc. in the image are marked with different IDs. Therefore, in the process of projecting point cloud data onto the segmentation mask, it is possible to determine whether the point cloud projected onto the mask is a dynamic obstacle based on the segmentation mask ID. If the mask ID corresponding to the point cloud represents a dynamic object (such as a pedestrian, a moving vehicle, etc.), the point cloud is removed. For the point cloud that is not removed, the RGB information in the image data corresponding to the mask coordinates is filled into the point, thereby achieving the removal of dynamic obstacles in the process of producing semantic point cloud data.

[0047] In an embodiment of the present application, a semantic segmentation model is first used to classify the pixels in the image data, and then in the process of generating semantic point cloud data using the image data and point cloud data, dynamic obstacles are eliminated based on the semantics of the pixel points. This can prevent dynamic obstacles from interfering with the robot's perception and map construction of the static environment, improve the accuracy and stability of the generated semantic point cloud data, and enable the subsequently constructed map to better reflect the static environment structure.

[0048] In some embodiments, after generating semantic point cloud data using image data and point cloud data in step S202 , the method further includes: performing dedistortion processing on the semantic point cloud data using a laser inertial odometry.

[0049] For example, since the robot is in motion during the process of collecting point cloud data by the laser radar, the actual positions of the points collected at different times in space will deviate due to the movement of the equipment, resulting in point cloud distortion. The IMU can record the robot's acceleration, angular velocity and other information in real time during the process of collecting point cloud data. This information can characterize the robot's motion state at the moment of point cloud data collection. IMU pre-integration is a technical means developed based on the IMU measurement principle. It can pre-calculate the relative motion of the robot between adjacent moments by performing specific mathematical operations on the IMU data. Therefore, the laser inertial odometry can obtain the IMU data of the time period corresponding to the semantic point cloud data, and perform coordinate conversion on each point in each frame of point cloud data through IMU pre-integration, so that all points in each frame of point cloud data are in the coordinate system of the first point in the frame of point cloud data, so as to eliminate the distortion effect of the robot movement on the point cloud data during point cloud collection.

[0050] In this embodiment, the generated semantic point cloud data can be motion dedistorted by combining the laser inertial odometry with the IMU data, and the point cloud dedistortion based on IMU pre-integration can effectively process the measurement noise of the IMU during the I calculation process, reduce the cumulative error, and improve the accuracy of subsequent robot posture determination.

[0051] Step S203: determining the relocation pose of the robot based on the semantic point cloud data and the pre-stored semantic marker map.

[0052] Here, the pre-stored semantic marker map can be a priori map or a map temporarily created by the robot system in the current area using SLAM technology. The semantic marker map is mainly composed of representative semantic features extracted from the semantic point cloud map. It does not contain color information and only focuses on key elements with specific semantic meanings to simplify the map data.

[0053] In the process of determining the relocation pose, multiple frames of continuous point clouds (for example, 2-3 frames of point clouds) in the semantic point cloud data can be saved as point cloud data for matching to avoid the limited environmental information contained in a single frame of point cloud and affect the matching accuracy, and the iterative closest point (ICP) algorithm is used for iterative matching. In the matching process, the saved multiple frames of continuous point clouds are used as source point clouds, and the point cloud in the semantic marker map is used as the target point cloud. Each iteration of the algorithm calculates the error between the source point cloud and the target point cloud, and then adjusts the transformation matrix according to the error, so that the source point cloud gradually approaches the target point cloud. When multiple iterations make the ICP algorithm converge, the error between the source point cloud and the target point cloud reaches the set threshold, and the matching result is obtained, so that the matching result can be used to calculate the relocation pose of the robot.

[0054] In step S204 , the semantic point cloud data, the relocalized pose, and the multiple sensor data are input into a plurality of odometers of different types, and the estimated pose output by each odometer is obtained respectively.

[0055] For example, Figure 3 As shown, in step S204, the semantic point cloud data, the relocalized pose, and multiple sensor data are input into multiple odometers of different types, and the estimated pose output by each odometer is obtained respectively, including:

[0056] Step S2041, using the repositioned pose to update the initial poses of multiple odometers;

[0057] In step S2042 , the semantic point cloud data and multiple sensor data are input into corresponding odometers, and the estimated pose output by each odometer is obtained respectively.

[0058] It is understandable that the odometry often requires an initial pose and combines it with subsequent odometry information to estimate the pose of the robot after movement. For example, the laser inertial odometry usually uses the environmental point cloud information obtained at the initial moment in combination with the existing map information, and determines the position and pose that matches the point cloud information at the initial moment through feature matching as the initial pose; the inertial odometry mainly relies on the acceleration and angular velocity information of the robot measured at the initial moment to determine the initial pose; the motion model odometry uses the wheel speed and angle information at the initial moment to infer the initial pose.

[0059] However, the initial pose determined by a single odometry often fails to meet the accuracy requirements of pose estimation. Using the relocalization pose to update the initial pose of the odometry can provide an accurate starting point for the subsequent calculations of the odometry, enabling them to accurately track the movement of the robot based on the real-time measurement data of the sensor, helping to reduce the accumulation of errors and improve the accuracy of pose estimation.

[0060] In step S205 , the estimated poses output by each odometer are fused through an error state Kalman filter to obtain a fused pose of the robot.

[0061] Exemplarily, multiple odometers of different types may include inertial odometers, laser inertial odometers, and motion model odometers. Since the robot's pose estimation based on odometer information from a single odometer may be affected by environmental factors and error accumulation, the error state Kalman filtering (ESKF) may be used to fuse the estimated poses output by multiple odometers. The core idea of ESKF is to decompose the true state into a nominal state and an error state. The nominal state may be provided by each odometer, while the error state is estimated and corrected through Kalman filtering. By modeling and estimating the errors of each odometer, ESKF can comprehensively utilize the information of these odometers to reduce the impact of the error of a single odometer, thereby obtaining a more accurate and stable robot pose, i.e., a fused pose.

[0062] It is understandable that during the fusion process, ESKF will assign different weights to each odometer based on the error characteristics and reliability of each odometer. The weight ratio can be fixed according to the characteristics of each odometer, or it can be set to be dynamically adjusted by detecting the environment in which the robot is located. For example, in an environment with good lidar data quality, the weight of the laser inertial odometer can be set higher, and in the case of lost or inaccurate lidar data, the weights of other odometers can be adjusted to increase.

[0063] In some embodiments, the multiple odometers of different types include at least an inertial odometer, a laser inertial odometer, and a motion model odometer, and the multiple sensor data also include GPS data, IMU data, wheel speed data, and robot angle data. After the initial pose of each odometer is updated using the relocalization pose, in step S204, the semantic point cloud data and the multiple sensor data are input into the corresponding odometer, and the estimated pose output by each odometer is obtained respectively, including:

[0064] For the inertial odometry, GPS data and IMU data are input into the inertial odometry, and the inertial odometry is used to fuse GPS data and IMU data to obtain an estimated pose.

[0065] It is understandable that due to the weak GPS signal in areas such as indoors or in urban canyons, the IMU will collect data in real time at a higher frequency, generating a series of timestamped measurements. By checking the timestamp of the current IMU data, the time elapsed since the last GPS update can be determined. If the interval between the current IMU time and the last GPS update time exceeds a set threshold, or other preset conditions for updating are met, it can be set that a GPS update is required, and after the update, the IMU data and GPS data are fused using an extended Kalman filter to obtain the estimated position of the robot in the world coordinate system by the inertial odometer.

[0066] For the laser inertial odometry, GPS data and semantic point cloud data are input into the laser inertial odometry. The laser inertial odometry is used to determine the estimated posture of the robot based on the semantic point cloud data and the pre-stored semantic point cloud map, and the estimated position is obtained using the estimated posture and GPS data.

[0067] It can be understood that the semantic point cloud data here can be the semantic point cloud data that has been dedistorted by the laser inertial odometry, and the pre-stored semantic point cloud map can be a priori map or a map temporarily created by the robot in the current area using SLAM technology. The semantic point cloud map integrates the spatial coordinates, color and semantic information of the point cloud. By matching the semantic point cloud data with the pre-stored semantic point cloud map, the part that is most similar to the semantic point cloud data (the current frame semantic point cloud) can be determined in the semantic point cloud map, thereby determining the relative position and posture of the robot in the area represented by the semantic point cloud map.

[0068] In addition, due to the influence of noise and uncertainty in the point cloud matching process, in this embodiment, iterative Kalman filtering can be used to combine the previous motion state of the robot and the current point cloud matching results to output the estimated posture. On this basis, the GPS data is combined to perform coordinate transformation to obtain the estimated posture of the robot in the world coordinate system using the laser inertial odometry.

[0069] It should be emphasized that if the data volume of the semantic point cloud map is large, before performing point cloud matching, the current Kalman filter status can be obtained first, so that the laser inertial odometer can use the previously accumulated information to determine the approximate position of the current robot in the semantic point cloud map, and then a local point cloud map can be extracted from the semantic point cloud map. Point cloud matching can be performed between the local point cloud map and the semantic point cloud data to reduce the amount of calculation and improve positioning efficiency.

[0070] For the motion model odometry, GPS data, IMU data, wheel speed data and robot angle data are input into the motion model odometry. The motion model odometry is used to update the measurement values in the robot's motion prediction equation using GPS data, IMU data, wheel speed data and robot angle data, and obtain the estimated posture of the robot through the motion prediction equation.

[0071] It is understandable that the motion model odometry pre-builds the robot's motion model based on the IMU data, wheel speed data, and robot angle data of the robot's motion state. Based on the motion model, a motion prediction equation for the robot can be obtained, thereby substituting the real-time IMU data, wheel speed data, and robot angle data into the motion prediction equation as measurement values, and obtaining a predicted value for the robot's state vector based on the motion prediction equation. Considering that the robot's motion model is generally nonlinear, in the process of predicting the robot's current state based on the motion prediction equation, the robot's current state can be predicted by combining the robot's state estimate at the previous moment and the currently input measurement value through an extended Kalman filter. The measurement value is compared with the predicted state and the Kalman gain is calculated to correct the predicted state, so that the motion model odometry outputs the robot's estimated position in the world coordinate system.

[0072] Therefore, in this embodiment, the estimated posture of the robot can be obtained based on different types of odometers, and the posture estimated by each odometer can be fused in combination with the error state Kalman filter, so as to make full use of the advantages of different sensors, overcome the error influence of a single odometer, and obtain a more accurate fused posture.

[0073] It should be noted that this embodiment uses the fusion of inertial odometry, laser inertial odometry and motion model odometry to schematically describe how to use the fusion of multiple odometry to estimate the real-time posture of the robot, but does not limit the number of odometry of different types. In actual applications, the types of odometry can be deleted or added, and this application is not limited to this.

[0074] In some embodiments, as Figure 4 As shown, after obtaining the fusion posture of the robot in step S205, the following steps are further included:

[0075] Step S206: Create target nodes corresponding to the relocated pose and the fused pose in the robot's pose optimization factor graph based on the GPS data, and associate the target nodes with other nodes in the pose optimization factor graph. The pose optimization factor graph includes nodes with time stamps and edges between nodes. Each node represents the relocated pose or fused pose of the robot at a certain historical moment, and each edge represents the association relationship between the poses corresponding to the two nodes connected to the edge.

[0076] Step S207: Iteratively optimize the posture optimization factor graph to obtain the optimized posture of the robot.

[0077] For example, a robot's posture optimization factor graph utilizes the factor graph-based C++ library GTSAM (Generalized Trajectory Sampling and Mapping). This factor graph is constructed by combining robot posture data from different sources at different times with nodes and relationships between different postures (such as relative displacement and rotation angle) as edges. Due to measurement errors between different sensors, the information within each node and edge in the factor graph constructed by GTSAM also experiences accumulated errors. The GTSAM optimization library employs advanced optimization algorithms, such as factor graph optimization, to iteratively optimize the posture graph. In each iteration, the algorithm calculates the error of each node in the posture graph based on a preset error model and adjusts the node's posture values to minimize the sum of the errors across the entire graph. After multiple rounds of iterative optimization, when the posture graph's errors converge to an acceptable range, GTSAM outputs the optimized posture information for each node. This posture information is then uniformly converted to the world coordinate system. The resulting posture data comprehensively considers information from multiple sensors and, through optimization, reduces errors, making it more accurate and reliable than posture data from a single sensor. For example, in a complex indoor environment, various factors may cause deviations in sensor measurements during the robot's movement. After multiple iterative optimizations, GTSAM can gradually correct these deviations, making the posture graph more consistent with the robot's actual motion trajectory.

[0078] In some embodiments, in addition to using the error Kalman filter to fuse the output information of multiple odometers to obtain the robot's fused pose in step S205, the error Kalman filter can also output optimized semantic point cloud data that has undergone precise posture adjustment. After obtaining the robot's fused pose in step S205, the method also includes: inputting the optimized semantic point cloud data and the fused pose into the mapping model, and using the output information of the mapping model to update the pre-stored semantic point cloud map; based on the updated semantic point cloud map, updating the pre-stored semantic marker map.

[0079] For example, the robot can use the fusion pose optimized by the error state Kalman filter to transform the optimized semantic point cloud data from the local coordinate system at the time of acquisition to the world coordinate system through a specific transformation matrix. After completing the coordinate conversion, these semantic point clouds are saved in the semantic point cloud map, gradually building a map containing rich environmental details and semantic information. As the robot continues to move and collect data, the semantic point cloud map is continuously updated and improved, and the covered environmental area is getting larger and larger. Based on the updated semantic point cloud map and semantic ID, the semantic tags that need to be updated in the semantic marker map can be extracted to achieve the update of the semantic marker map. The updated semantic marker map can serve as the map basis for subsequent relocation.

[0080] In this embodiment, during the real-time pose estimation of the robot, the optimized pose and optimized semantic point cloud are used to perform real-time map updates. Compared with the existing SLAM solutions, more accurate semantic point cloud maps and semantic marker maps can be constructed, thereby improving the efficiency and accuracy of repositioning.

[0081] In some embodiments, before associating the target node with other nodes in the pose optimization factor graph in step S206, the method further includes:

[0082] In response to detecting that historical semantic point cloud data matching the semantic point cloud data exists in the historical semantic point cloud database, a historical pose node corresponding to the historical semantic point cloud data is determined in the pose optimization factor graph.

[0083] In step S206, the target node is associated with other nodes in the pose optimization factor graph, including: establishing an association relationship between the target node corresponding to the fusion pose and the historical pose nodes.

[0084] For example, in this scheme, the semantic point cloud data obtained at historical moments can be saved to the historical semantic point cloud database. When it is detected that the difference between the current semantic point cloud data and the semantic point cloud data at a certain historical moment is less than the set threshold, it can be considered that the robot has returned to the position at that historical moment, and the loop is penalized. Therefore, when the fused pose corresponding to the current moment is added to the pose optimization factor graph for optimization, it is necessary to additionally associate the target node corresponding to the fused pose with the historical pose node corresponding to the historical moment in the factor graph to ensure the optimization effect of the factor graph.

[0085] In some embodiments, after obtaining the optimized posture of the robot in step S207, the method further includes: using the optimized posture to update the initial postures of multiple odometers.

[0086] It can be understood that using the optimized pose to update the initial pose of multiple odometry meters can provide a more accurate initial pose for the subsequent movement of the robot, improve the effect of the laser inertial odometry on the subsequent dedistortion of semantic point cloud data, and provide a more accurate information basis for the subsequent use of the error state Kalman filter for odometry information fusion.

[0087] In some embodiments, the specific steps of performing pose optimization using the pose optimization factor graph are as follows:

[0088] (1) Create nodes and build factor graphs

[0089] The pose obtained at each moment (relocalized pose or fused pose) is used as a new node in the factor graph, and the data information output by each odometry (inertial odometry, laser inertial odometry, motion model odometry, etc.) is used as the constraint factor between the poses. When loop detection is triggered, a loop factor is added to increase the constraint.

[0090] (2) Factor Graph Optimization

[0091] The optimization process includes: marginalization processing, for real-time optimization, old nodes can be marginalized to reduce the size of the graph; nonlinear optimization, using the nonlinear optimization methods provided by GTSAM (such as Levenberg-Marquardt or Gauss-Newton algorithms) to solve the factor graph and obtain the optimal pose estimate; incremental update, using GTSAM's incremental graph optimization to update and re-optimize the graph to achieve real-time processing.

[0092] (3) Filtering

[0093] A Kalman filter is used to smooth the odometer information to reduce noise errors, and real-time smoothing updates are performed based on the filtered information to remove redundant old data, thereby improving the real-time performance of the pose optimization factor graph.

[0094] (4) Iterative optimization result output

[0095] When the error of the factor graph converges to an acceptable range through iterative optimization, the optimized posture of the robot is output.

[0096] In this embodiment, after using the error state Kalman filter to fuse the outputs of multiple odometers to obtain a fused posture, the obtained robot posture can be optimized in real time by constructing the robot's posture optimization factor graph to further eliminate the impact of the odometer error and achieve accurate estimation of the robot's real-time posture.

[0097] The above describes the specific configuration and implementation of the embodiments of the present application from different perspectives. Using the methods provided in the above embodiments, the interference of dynamic obstacles in the scene can be eliminated during the process of estimating the robot's pose, and the advantages of various odometers can be integrated to output the robot's real-time pose. On this basis, it is also possible to construct a pose optimization factor graph by using the robot's pose at each historical moment, and perform secondary optimization on the fused output of the robot's real-time pose, further improving the accuracy of the real-time pose estimation.

[0098] In addition, the embodiment of the present application can also update the semantic marker map used for repositioning in real time based on the optimized robot posture and semantic point cloud data, thereby improving the efficiency and accuracy of repositioning.

[0099] Exemplary devices

[0100] As the implementation of the above methods, Figure 5 As shown, the embodiment of the present application further provides a real-time posture estimation device for a robot, which may include:

[0101] A data acquisition module 501 is used to acquire multiple sensor data of the robot, where the multiple sensor data include at least image data and point cloud data;

[0102] A semantic point cloud module 502 is configured to generate semantic point cloud data using image data and point cloud data;

[0103] The relocation module 503 is used to determine the relocation pose of the robot based on the semantic point cloud data and the pre-stored semantic marker map;

[0104] The pose fusion module 504 is used to input semantic point cloud data, relocalization pose and multiple sensor data into multiple odometers of different types, and obtain the estimated pose output by each odometer respectively; the estimated pose output by each odometer is fused through the error state Kalman filter to obtain the fused pose of the robot.

[0105] In some embodiments, as Figure 5 As shown, the device also includes a posture optimization module 505, which is used to create target nodes corresponding to the repositioning posture and the fusion posture in the posture optimization factor graph of the robot according to the GPS data, and associate the target nodes with other nodes in the posture optimization factor graph. The posture optimization factor graph includes nodes with time identifiers and edges between nodes. Each node represents the repositioning posture or fusion posture of the robot at a certain historical moment, and each edge represents the association relationship between the postures corresponding to the two nodes connected to the edge; the posture optimization factor graph is iteratively optimized to obtain the optimized posture of the robot.

[0106] In some embodiments, the relocation module 503 is further configured to:

[0107] Use the relocalization pose to update the initial poses of multiple odometry units;

[0108] The semantic point cloud data and multiple sensor data are input into the corresponding odometry, and the estimated pose output by each odometry is obtained respectively.

[0109] In some embodiments, the multiple sensor data further include IMU data, wheel speed data, and robot angle data, and the multiple odometers of different types include at least an inertial odometer, a laser inertial odometer, and a motion model odometer. The relocalization module 503 is further configured to:

[0110] Input GPS data and IMU data into the inertial odometry, which is used to fuse GPS data and IMU data to obtain the estimated pose.

[0111] The GPS data and semantic point cloud data are input into the laser inertial odometry. The laser inertial odometry is used to determine the robot's estimated posture based on the semantic point cloud data and the pre-stored semantic point cloud map, and the estimated posture and GPS data are used to obtain the estimated position.

[0112] The GPS data, IMU data, wheel speed data and robot angle data are input into the motion model odometry. The motion model odometry is used to update the predicted value of the state vector in the robot's motion prediction equation using the GPS data, IMU data, wheel speed data and robot angle data, and obtain the estimated posture of the robot through the motion prediction equation.

[0113] In some embodiments, the device further includes a dedistortion module for performing dedistortion processing on the semantic point cloud data using a laser inertial odometry.

[0114] In some embodiments, the relocalization module 503 is further configured to update the initial poses of the multiple odometers using the optimized poses.

[0115] In some embodiments, the device also includes a loop detection module for determining a historical pose node corresponding to the historical semantic point cloud data in the pose optimization factor graph in response to detecting that there is historical semantic point cloud data matching the semantic point cloud data in the historical semantic point cloud database.

[0116] The posture optimization module 505 is further used to establish an association relationship between the target node corresponding to the fusion posture and the historical posture node.

[0117] In some embodiments, the device also includes a mapping module for obtaining optimized semantic point cloud data output by the error state Kalman filter; inputting the optimized semantic point cloud data and fusion pose into a mapping model, and using the output information of the mapping model to update the pre-stored semantic point cloud map; based on the updated semantic point cloud map, updating the pre-stored semantic marker map.

[0118] The functions of each unit, module or sub-module in each device in the embodiments of the present application can be referred to the corresponding description in the above method embodiments, and have corresponding beneficial effects, which will not be repeated here.

[0119] Exemplary electronic devices and computer-readable storage media

[0120] Figure 6 FIG. 1 is a block diagram of an electronic device for implementing an embodiment of the present application. Figure 6 As shown, the electronic device includes: a memory 601 and a processor 602. The memory 601 stores a computer program that can be run on the processor 602. When the processor 602 executes the computer program, the method in the above embodiment is implemented. The number of the memory 601 and the processor 602 can be one or more.

[0121] The electronic device also includes:

[0122] The communication interface 603 is used to communicate with external devices and perform data exchange transmission.

[0123] If the memory 601, processor 602, and communication interface 603 are implemented independently, the memory 601, processor 602, and communication interface 603 can be connected to each other via a bus and communicate with each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 6 Only one thick line is used in the diagram, but this does not mean that there is only one bus or one type of bus.

[0124] Optionally, in a specific implementation, if the memory 601, the processor 602 and the communication interface 603 are integrated on a chip, the memory 601, the processor 602 and the communication interface 603 can communicate with each other through an internal interface.

[0125] An embodiment of the present application also provides a chip, which includes a processor for calling and executing instructions stored in the memory from the memory, so that a communication device equipped with the chip executes the method provided in the embodiment of the present application.

[0126] An embodiment of the present application also provides a chip, including: an input interface, an output interface, a processor and a memory. The input interface, the output interface, the processor and the memory are connected through an internal connection path. The processor is used to execute the code in the memory. When the code is executed, the processor is used to execute the method provided in the embodiment of the present application.

[0127] It should be understood that the processor may be a CPU, or other general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), an FPGA or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc. It is worth noting that the processor may be a processor that supports the Advanced RISC Machines (ARM) architecture.

[0128] Furthermore, optionally, the above-mentioned memory may include a read-only memory and a random access memory. The memory may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may include a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may include a random access memory (RAM), which is used as an external cache. By way of example but not limitation, many forms of RAM are available. For example, static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link DRAM (SLDRAM) and direct memory bus random access memory (DR RAM).

[0129] In the above embodiments, all or part of the embodiments may be implemented using software, hardware, firmware, or any combination thereof. When implemented using software, all or part of the embodiments may be implemented in the form of a computer program product. A computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions according to the present application are generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions may be stored in a computer-readable storage medium or transferred from one computer-readable storage medium to another.

[0130] In addition to the above-mentioned methods and devices, embodiments of the present disclosure may also provide a computer program product, including computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the image processing method of various embodiments of the present disclosure described in the above-mentioned "Exemplary Method" section.

[0131] The computer program product may be written in any combination of one or more programming languages to implement the operations of the disclosed embodiments, including object-oriented programming languages such as Java, C++, and conventional procedural programming languages such as C or similar programming languages. The program code may be executed entirely on the user's computing device, partially on the user's computing device, as a stand-alone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0132] In the description of this specification, the reference terms "one embodiment," "some embodiments," "example," "specific example," or "some examples" mean that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. Moreover, the specific features, structures, materials, or characteristics described may be combined in any appropriate manner in any one or more embodiments or examples. In addition, those skilled in the art may combine and integrate different embodiments or examples described in this specification, as well as features of different embodiments or examples, unless they are mutually inconsistent.

[0133] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. Throughout the description of this application, "plurality" means two or more, unless otherwise specifically defined.

[0134] Any process or method described in the flowchart or otherwise described herein can be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a specific logical function or process. The scope of the preferred embodiments of the present application includes other implementations in which the functions may be performed in a different order than shown or discussed, including performing the functions substantially simultaneously or in reverse order depending on the functions involved.

[0135] The logic and / or steps described in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, apparatus or device (such as a computer-based system, a system including a processor or other system that can fetch instructions from an instruction execution system, apparatus or device and execute instructions), or used in combination with such instruction execution systems, apparatuses or devices.

[0136] It should be understood that various parts of the present application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. All or part of the steps of the above embodiment method can be completed by instructing the relevant hardware through a program, which can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0137] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing module, or each unit may exist physically separately, or two or more units may be integrated into a single module. The aforementioned integrated modules may be implemented in the form of hardware or in the form of software functional modules. If the aforementioned integrated modules are implemented in the form of software functional modules and sold or used as independent products, they may also be stored in a computer-readable storage medium. The storage medium may be a read-only memory, a magnetic disk, or an optical disk, etc.

[0138] The above is merely an exemplary embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any person skilled in the art can easily conceive of various modifications or substitutions within the technical scope described in this application, and such modifications or substitutions should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A real-time pose estimation method for a robot, characterized in that: include: Acquire multiple sensor data of the robot, wherein the multiple sensor data include at least image data and point cloud data; generating semantic point cloud data using the image data and the point cloud data; Determining a relocation pose of the robot based on the semantic point cloud data and a pre-stored semantic marker map; Inputting the semantic point cloud data, the relocalized pose, and the plurality of sensor data into a plurality of odometers of different types, and respectively obtaining an estimated pose output by each of the odometers; The estimated poses output by each of the odometers are fused through an error state Kalman filter to obtain a fused pose of the robot.

2. The method according to claim 1, characterized in that The plurality of sensor data further includes GPS data, and the method further includes: Creating target nodes corresponding to the relocated pose and the fused pose in the pose optimization factor graph of the robot according to the GPS data, and associating the target nodes with other nodes in the pose optimization factor graph, wherein the pose optimization factor graph includes nodes with time stamps and edges between nodes, each node representing the relocated pose or fused pose of the robot at a certain historical moment, and each edge representing the association relationship between the poses corresponding to the two nodes connected to the edge; The posture optimization factor graph is iteratively optimized to obtain the optimized posture of the robot.

3. The method according to claim 2, characterized in that Inputting the semantic point cloud data, the relocalized pose, and the plurality of sensor data into a plurality of different types of odometry devices, and respectively obtaining an estimated pose output by each of the odometry devices, comprising: Updating the initial poses of the plurality of odometers using the relocated poses; The semantic point cloud data and the multiple sensor data are input into corresponding odometers, and the estimated pose output by each odometer is obtained respectively.

4. The method according to claim 3, characterized in that The multiple sensor data further include IMU data, wheel speed data, and robot angle data. The multiple odometers of different types include at least an inertial odometer, a laser inertial odometer, and a motion model odometer. The semantic point cloud data and the multiple sensor data are input into corresponding odometers, and the estimated pose output by each odometer is obtained, including: Inputting the GPS data and the IMU data into the inertial odometer, wherein the inertial odometer is used to fuse the GPS data and the IMU data to obtain the estimated pose; Inputting the GPS data and the semantic point cloud data into the laser inertial odometry, the laser inertial odometry being used to determine an estimated posture of the robot based on the semantic point cloud data and a pre-stored semantic point cloud map, and obtaining the estimated position pose using the estimated posture and the GPS data; The GPS data, the IMU data, the wheel speed data and the robot angle data are input into the motion model odometer, and the motion model odometer is used to update the measurement values in the motion prediction equation of the robot using the GPS data, the IMU data, the wheel speed data and the robot angle data, and obtain the estimated posture of the robot through the motion prediction equation.

5. The method according to claim 4, characterized in that After generating semantic point cloud data using the image data and the point cloud data, the method further includes: The semantic point cloud data is dedistorted by the laser inertial odometry.

6. The method according to claim 2, characterized in that After obtaining the optimized posture of the robot, the method further includes: The optimized poses are used to update the initial poses of the multiple odometers.

7. The method according to claim 2, characterized in that The method further comprises: In response to detecting that historical semantic point cloud data matching the semantic point cloud data exists in the historical semantic point cloud database, determining a historical pose node corresponding to the historical semantic point cloud data in the pose optimization factor graph; Associating the target node with other nodes in the pose optimization factor graph includes: Establish an association relationship between the target node corresponding to the fusion posture and the historical posture node.

8. The method according to claim 1, characterized in that After obtaining the fusion posture of the robot, the method further includes: Obtaining optimized semantic point cloud data output by the error state Kalman filter; Inputting the optimized semantic point cloud data and the fused pose into a mapping model, and using the output information of the mapping model to update a pre-stored semantic point cloud map; Based on the updated semantic point cloud map, the pre-stored semantic marker map is updated.

9. The method according to claim 1, characterized in that Generating semantic point cloud data using the image data and the point cloud data, comprising: Preprocessing the image data and inputting the data into a semantic segmentation model to obtain a segmentation mask output by the semantic segmentation model; Determining noise points in the point cloud data based on the mask ID in the segmentation mask image, wherein the noise points include points representing dynamic obstacles; The image data is used to perform image filling on non-noise points in the point cloud data to obtain the semantic point cloud data.

10. A real-time posture estimation device for a robot, characterized in that: The device comprises: A data acquisition module, configured to acquire a plurality of sensor data of the robot, wherein the plurality of sensor data includes at least image data and point cloud data; A semantic point cloud module, configured to generate semantic point cloud data using the image data and the point cloud data; A relocation module, configured to determine a relocation pose of the robot based on the semantic point cloud data and a pre-stored semantic marker map; The pose fusion module is used to input the semantic point cloud data, the repositioning pose and the multiple sensor data into multiple odometers of different types, and obtain the estimated pose output by each odometer respectively; the estimated pose output by each odometer is fused through an error state Kalman filter to obtain the fused pose of the robot.

11. The device according to claim 10, characterized in that The device further comprises: A posture optimization module is used to create target nodes corresponding to the repositioning posture and the fusion posture in the posture optimization factor graph of the robot according to the GPS data, and associate the target nodes with other nodes in the posture optimization factor graph. The posture optimization factor graph includes nodes with time identifiers and edges between nodes. Each node represents the repositioning posture or fusion posture of the robot at a certain historical moment. Each edge represents the association relationship between the postures corresponding to the two nodes connected to the edge; the posture optimization factor graph is iteratively optimized to obtain the optimized posture of the robot.

12. An electronic device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory, wherein the processor implements the method according to any one of claims 1 to 9 when executing the computer program.

13. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.

Images