Industrial plant positioning and mapping method and system based on spatiotemporal information

Abstract

The application provides an industrial plant positioning and mapping method and system based on space-time information, comprising the following steps: S1, performing feature depth correlation on laser point cloud obtained by a laser radar and pictures obtained by a monocular camera; performing front-end point cloud registration, performing semantic instance segmentation on a video stream obtained by the monocular camera, marking environment object points, performing factor graph optimization, and performing global map splicing according to the optimized trajectory to obtain a global map of a current plant environment; setting a processing state label on the global map; constructing and maintaining a factory space-time map library; S2, performing current map prediction on the space-time information map library and prior information of a factory production plan, combining the predicted map and factory top structured information to perform extended Kalman filtering to obtain high-precision positioning information based on space-time information. The positioning based on space information and time information is subjected to extended Kalman filtering, and the positioning precision in a high-change factory environment is improved.

Description

Technical Field

[0001] This invention relates to the field of positioning, and more specifically, to a method and system for locating and mapping industrial plants based on spatiotemporal information. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) provides autonomous mobile robots with location and environmental information, making it a key technology for completing movement tasks. However, the complex indoor environment of factories processing large workpieces presents the following challenges to SLAM:

[0003] I. The front-end registration stage of SLAM is achieved by matching feature structure information in the environment. Large factory floors are relatively flat and smooth, resulting in glare. If visual sensors are used, this can cause glare in the images, making it difficult to effectively extract feature points within the scene. Furthermore, large factory floors are often spacious, making it difficult for LiDAR sensors to acquire effective point cloud information. These characteristics of factory floors prevent various sensors from obtaining effective environmental feature information from the ground, thus hindering the front-end registration stage of SLAM, leading to inaccurate localization or even complete failure. II. Large factory floors typically use assembly lines to process large workpieces, resulting in a high rate of scene change. If a map constructed by a traditional SLAM system is used as a prior map for relocalization, changes in the factory floor will increase the difference between the environment and the prior map, leading to reduced accuracy in relocalization.

[0004] Patent document CN115439601A discloses a factory management system based on 3D modeling, mainly including a factory positioning module, a factory floor plan acquisition module, a modeling module, a trajectory generation module, and an information processing module. The factory positioning module uses the AOA positioning algorithm to perform real-time positioning of equipment and / or personnel, which can promptly and accurately reflect the movement of equipment and / or personnel. The algorithm is simple and the calculation results are accurate. The modeling module generates a 3D model of the factory based on the 2D floor plan in CAD format. The trajectory generation module generates the 3D coordinates of equipment and / or personnel based on the location information and forms the movement trajectory of equipment and / or personnel based on the real-time generated 3D coordinates. However, this invention does not utilize instance segmentation networks for mapping. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the purpose of this invention is to provide a method and system for locating and mapping industrial plants based on spatiotemporal information.

[0006] A method for locating and mapping industrial plants based on spatiotemporal information according to the present invention includes:

[0007] Step S1: Perform feature depth correlation between the laser point cloud obtained by the LiDAR and the image obtained by the monocular camera; perform front-end point cloud registration; perform semantic instance segmentation on the video stream obtained by the monocular camera; mark environmental object points; perform factor map optimization; and stitch the global map based on the optimized trajectory to obtain a global map of the current factory environment; set processing status labels on the global map; and build and maintain the factory spatiotemporal map library.

[0008] Step S2: Use the spatiotemporal information map library and prior information of the factory production plan to predict the current map, and combine the predicted map with the structured information on the top of the factory to perform extended Kalman filtering to obtain high-precision positioning information based on spatiotemporal information.

[0009] Preferably, in step S1:

[0010] Step S1.1: Perform extrinsic parameter calibration on the ground monocular camera and the ground lidar to obtain the transformation matrix between the ground monocular camera and the ground lidar;

[0011] Step S1.2: Based on the sensor extrinsic parameters obtained from calibration, perform feature depth correlation between the laser point cloud obtained by the lidar and the image obtained by the monocular camera to recover the depth information in the image;

[0012] Step S1.3: Perform front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor.

[0013] Step S1.4: Perform semantic instance segmentation on the video stream obtained by the monocular camera to obtain the semantic information of the environmental objects when the factory processes the current workpiece; and obtain the pose Tobj of the current environmental objects based on feature matching;

[0014] Step S1.5: Based on the transformation matrix between the camera and the lidar, the global pose Tg, and the pose Tobj of the labeled semantic object, the environmental object points in the lidar point cloud are labeled using the feature depth association method, and denoted as Pobj.

[0015] Step S1.6: The visual odometry extracts descriptors from keyframes and uses a bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to the laser odometry. The laser odometry uses ICP for registration to obtain the pose after loop closure.

[0016] Step S1.7: Add the visual factor, lidar factor, and loop closure factor to the factor graph for graph optimization to obtain the optimized high-precision trajectory;

[0017] Step S1.8: Based on the optimized trajectory and the sensor extrinsic transformation matrix, perform global map stitching to obtain a global map of the current factory environment;

[0018] Step S1.9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map; save the data to form a spatiotemporal information map library.

[0019] Preferably, in step S1.2, in order to perform feature depth association, the lidar point cloud and image features are projected onto a sphere according to the extrinsic transformation matrix, and the visual feature coordinates are searched using kdtree to obtain the three lidar points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three lidar points.

[0020] In step S1.4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects with probabilistic changes in the factory environment. The focus is on training items that can reflect the current processing state of the factory. The semantic information is used to determine the current processing state of the factory.

[0021] In step S1.5, the semantic information in the visual image is labeled onto the corresponding LiDAR point cloud using the feature depth association method in step S1.2.

[0022] In step S1.8, the obtained global map is the global map created within this SLAM running cycle, in which the objects segmented from the instance are specially labeled and additional semantic information is added;

[0023] In step S1.9, labels are set on the map according to the semantic labels of the workpieces being processed, the production plan for the day, or by manual annotation; the labels reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed.

[0024] Preferably, in step S2:

[0025] Step S2.1: Use LiDAR to extract structured feature information from the ceiling truss and walls of the factory building. When a loop is detected, register the LiDAR point cloud of the current frame with the local map using the nearest point search and normal distribution transformation method to obtain the loop information. Add the positioning factor and loop factor to the factor map for optimization to obtain spatial positioning information Ts.

[0026] Step S2.2: Use a monocular camera and lidar responsible for acquiring ground information to implement the odometry function, and perform point cloud stitching in real time during operation to obtain the currently running local map Ml;

[0027] Step S2.3: Using the provided factory production operation plan as prior information, perform ICP matching between the local map in step S2.2 and the map Ms in the spatiotemporal information map library that matches the production plan. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp. If there is no map with a matching degree exceeding the threshold, cancel the time information positioning and proceed to step S2.5.

[0028] Step S2.4: Relocate the predicted map Mp obtained in step S2.3 with the current local map Ml to obtain the location information Tt based on time information;

[0029] Step S2.5: Perform extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

[0030] Preferably, in step S2.2, during the process of stitching the laser odometry and the local map, the error of the laser odometry and the residual of the factor map optimization are saved as the confidence level of spatial positioning;

[0031] In step S2.3, the matching process between the local map M1 and the map Ms in the map library is as follows: First, maps in the map library whose labels match the daily production plan are retrieved for ICP matching, and their matching degree is saved; if the matching degree exceeds the threshold, only maps that match the production plan are compared, and the map with the highest score is selected as the prediction map; if the matching degree does not exceed the threshold, maps whose labels are other production states are compared; the matching degree of the prediction map is saved as the confidence level of time positioning, providing data for the subsequent determination of Kalman gain; if there is a map whose matching degree exceeds the threshold, it is used as the prediction map; if there is no map whose matching degree exceeds the threshold, the positioning using time information is canceled, and step S2.4 is not performed.

[0032] In step S2.4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map.

[0033] In step S2.5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning; the Kalman gain is determined based on the covariance of the two, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

[0034] An industrial plant positioning and mapping system based on spatiotemporal information provided by the present invention includes:

[0035] Module M1: Performs feature depth correlation between the laser point cloud obtained by the LiDAR and the image obtained by the monocular camera; performs front-end point cloud registration; performs semantic instance segmentation on the video stream obtained by the monocular camera; marks environmental object points; performs factor graph optimization; performs global map stitching based on the optimized trajectory to obtain a global map of the current factory environment; sets processing status labels on the global map; and builds and maintains the factory spatiotemporal map library.

[0036] Module M2: It uses the spatiotemporal information map library and prior information of the factory production plan to predict the current map, and combines the predicted map with the structured information on the top of the factory to perform extended Kalman filtering to obtain high-precision positioning information based on spatiotemporal information.

[0037] Preferably, in module M1:

[0038] Module M1.1: Performs extrinsic parameter calibration on the ground monocular camera and ground lidar to obtain the transformation matrix between the ground monocular camera and ground lidar;

[0039] Module M1.2: Based on the sensor extrinsic parameters obtained from calibration, the laser point cloud obtained by the lidar is correlated with the image obtained by the monocular camera to recover the depth information in the image;

[0040] Module M1.3: Performs front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor;

[0041] Module M1.4: Performs semantic instance segmentation on the video stream obtained by the monocular camera to obtain semantic information of environmental objects when the factory processes the current workpiece; and obtains the pose Tobj of the current environmental objects based on feature matching;

[0042] Module M1.5: Based on the transformation matrix between the camera and the lidar, the global pose Tg, and the pose Tobj of the labeled semantic objects, the environmental object points in the lidar point cloud are labeled using the feature depth association method, and denoted as Pobj.

[0043] Module M1.6: The visual odometry extracts descriptors from keyframes and uses a bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to the laser odometry. The laser odometry uses ICP for registration to obtain the pose after loop closure.

[0044] Module M1.7: Visual factors, lidar factors, and loop closure factors are added to the factor graph for graph optimization to obtain an optimized high-precision trajectory;

[0045] Module M1.8: Based on the optimized trajectory and sensor extrinsic transformation matrix, perform global map stitching to obtain a global map of the current factory environment;

[0046] Module M1.9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map; save the data to form a spatiotemporal information map library.

[0047] Preferably, in module M1.2, in order to perform feature depth association, the lidar point cloud and image features are projected onto a sphere according to the extrinsic transformation matrix, and the visual feature coordinates are searched using kdtree to obtain the three lidar points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three lidar points.

[0048] In module M1.4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects with probabilistic changes in the factory environment. The focus is on training items that can reflect the current processing state of the factory, and the semantic information is used to determine the current processing state of the factory.

[0049] In module M1.5, the feature depth association method in module M1.2 is used to annotate the semantic information in the visual image onto the corresponding LiDAR point cloud;

[0050] In module M1.8, the global map obtained is the global map created within this SLAM cycle, in which objects segmented from instances are specially labeled and have additional semantic information added;

[0051] In module M1.9, tags are set on the map based on the semantic tags of the workpieces being processed, the daily production plan, or manual annotation. The tags reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed.

[0052] Preferably, in module M2:

[0053] Module M2.1: Utilizes LiDAR to extract structured feature information from the ceiling trusses and walls of the factory building. When a loop is detected, the LiDAR point cloud of the current frame is registered with the local map using the nearest point search and normal distribution transformation method to obtain the loop information. The positioning factor and loop factor are added to the factor map for optimization to obtain spatial positioning information Ts.

[0054] Module M2.2: Utilizes a monocular camera and lidar responsible for acquiring ground information to realize the odometry function, and performs point cloud stitching in real time during operation to obtain the currently running local map M1;

[0055] Module M2.3: Using the provided factory production operation plan as prior information, the local map of module M2.2 is first matched with the map Ms in the spatiotemporal information map library that matches the production plan by ICP. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp; if there is no map with a matching degree exceeding the threshold, the time information positioning is canceled and module M2.5 is executed.

[0056] Module M2.4: Relocate the predicted map Mp obtained from module M2.3 with the current local map Ml to obtain location information Tt based on time information;

[0057] Module M2.5: Performs extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

[0058] Preferably, in module M2.2, during the process of stitching together the laser odometry and the local map, the error of the laser odometry and the residual of the factor map optimization are saved as the confidence level of spatial positioning;

[0059] In module M2.3, the matching process between the local map ML and the map Ms in the map library is as follows: First, maps whose labels match the daily production plan are retrieved from the map library and subjected to ICP matching, and their matching degree is saved; if the matching degree exceeds the threshold, only maps that match the production plan are compared, and the map with the highest score is selected as the prediction map; if the matching degree does not exceed the threshold, maps whose labels are other production states are compared; the matching degree of the prediction map is saved as the confidence level of time positioning, providing data for the subsequent determination of Kalman gain; if there is a map whose matching degree exceeds the threshold, it is used as the prediction map; if there is no map whose matching degree exceeds the threshold, the use of time information for positioning is canceled, and module M2.4 is not performed.

[0060] In module M2.4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map.

[0061] In module M2.5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning. The Kalman gain is determined based on the covariance of the two, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] 1. In the case of high environmental change rate in multi-category workpiece processing plant, this invention uses instance segmentation network to build a map; based on semantic information in the map, daily production plan and manual annotation, labels are added to the map to indicate the factory production status during the mapping period and stored in the map library to provide time information for future positioning.

[0064] 2. In the positioning process of this invention, to avoid the influence of degraded ground scenes (smooth ground causing reflection, and open ground lacking feature information) and high environmental change rates (changes in large workpieces leading to high environmental change rates), structured environments such as factory ceilings and walls are utilized for feature extraction, which serves as spatial information to improve positioning accuracy. Simultaneously, by combining factory production plans and time information from the map database, the current factory production status is predicted, and the corresponding map is obtained as the predicted map for repositioning.

[0065] 3. This invention applies extended Kalman filtering to positioning based on spatial and temporal information, thereby improving positioning accuracy in highly variable factory environments. Attached Figure Description

[0066] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0067] Figure 1 Here is a flowchart of the overall process for location services based on spatiotemporal information.

[0068] Figure 2 Flowchart for the mapping and spatiotemporal information map database construction process;

[0069] Figure 3 This is a flowchart of the positioning process based on spatiotemporal information. Detailed Implementation

[0070] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0071] Example 1:

[0072] This invention relates to a method for locating and mapping industrial plants based on spatiotemporal information, specifically targeting the characteristics of large plants with few structural features and high rate of change in the ground.

[0073] This invention proposes a method for locating and mapping industrial plants based on spatiotemporal information. It utilizes the spatial structured features of the plant's roof and walls for high-precision positioning, and leverages the temporal information during SLAM operation and the semantic information of the plant's production line for mapping. This method achieves robust positioning and mapping even when the plant floor lacks structured features and has a high rate of change.

[0074] According to the present invention, an industrial plant location and mapping method based on spatiotemporal information is provided, such as... Figures 1-3 As shown, it includes:

[0075] Step S1: Perform feature depth correlation between the laser point cloud obtained by the LiDAR and the image obtained by the monocular camera; perform front-end point cloud registration; perform semantic instance segmentation on the video stream obtained by the monocular camera; mark environmental object points; perform factor map optimization; and stitch the global map based on the optimized trajectory to obtain a global map of the current factory environment; set processing status labels on the global map; and build and maintain the factory spatiotemporal map library.

[0076] Preferably, in step S1:

[0077] Step S1.1: Perform extrinsic parameter calibration on the ground monocular camera and the ground lidar to obtain the transformation matrix between the ground monocular camera and the ground lidar;

[0078] Step S1.2: Based on the sensor extrinsic parameters obtained from calibration, perform feature depth correlation between the laser point cloud obtained by the lidar and the image obtained by the monocular camera to recover the depth information in the image;

[0079] Step S1.3: Perform front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor.

[0080] Step S1.4: Perform semantic instance segmentation on the video stream obtained by the monocular camera to obtain the semantic information of the environmental objects when the factory processes the current workpiece; and obtain the pose Tobj of the current environmental objects based on feature matching;

[0081] Step S1.5: Based on the transformation matrix between the camera and the lidar, the global pose Tg, and the pose Tobj of the labeled semantic object, the environmental object points in the lidar point cloud are labeled using the feature depth association method, and denoted as Pobj.

[0082] Step S1.6: The visual odometry extracts descriptors from keyframes and uses a bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to the laser odometry. The laser odometry uses ICP for registration to obtain the pose after loop closure.

[0083] Step S1.7: Add the visual factor, lidar factor, and loop closure factor to the factor graph for graph optimization to obtain the optimized high-precision trajectory;

[0084] Step S1.8: Based on the optimized trajectory and the sensor extrinsic transformation matrix, perform global map stitching to obtain a global map of the current factory environment;

[0085] Step S1.9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map; save the data to form a spatiotemporal information map library.

[0086] Preferably, in step S1.2, in order to perform feature depth association, the lidar point cloud and image features are projected onto a sphere according to the extrinsic transformation matrix, and the visual feature coordinates are searched using kdtree to obtain the three lidar points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three lidar points.

[0087] In step S1.4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects with probabilistic changes in the factory environment. The focus is on training items that can reflect the current processing state of the factory. The semantic information is used to determine the current processing state of the factory.

[0088] In step S1.5, the semantic information in the visual image is labeled onto the corresponding LiDAR point cloud using the feature depth association method in step S1.2.

[0089] In step S1.8, the obtained global map is the global map created within this SLAM running cycle, in which the objects segmented from the instance are specially labeled and additional semantic information is added;

[0090] In step S1.9, labels are set on the map according to the semantic labels of the workpieces being processed, the production plan for the day, or by manual annotation; the labels reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed.

[0091] Step S2: Use the spatiotemporal information map library and prior information of the factory production plan to predict the current map, and combine the predicted map with the structured information on the top of the factory to perform extended Kalman filtering to obtain high-precision positioning information based on spatiotemporal information.

[0092] Preferably, in step S2:

[0093] Step S2.1: Use LiDAR to extract structured feature information from the ceiling truss and walls of the factory building. When a loop is detected, register the LiDAR point cloud of the current frame with the local map using the nearest point search and normal distribution transformation method to obtain the loop information. Add the positioning factor and loop factor to the factor map for optimization to obtain spatial positioning information Ts.

[0094] Step S2.2: Use a monocular camera and lidar responsible for acquiring ground information to implement the odometry function, and perform point cloud stitching in real time during operation to obtain the currently running local map Ml;

[0095] Step S2.3: Using the provided factory production operation plan as prior information, perform ICP matching between the local map in step S2.2 and the map Ms in the spatiotemporal information map library that matches the production plan. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp. If there is no map with a matching degree exceeding the threshold, cancel the time information positioning and proceed to step S2.5.

[0096] Step S2.4: Relocate the predicted map Mp obtained in step S2.3 with the current local map Ml to obtain the location information Tt based on time information;

[0097] Step S2.5: Perform extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

[0098] Preferably, in step S2.2, during the process of stitching the laser odometry and the local map, the error of the laser odometry and the residual of the factor map optimization are saved as the confidence level of spatial positioning;

[0099] In step S2.3, the matching process between the local map M1 and the map Ms in the map library is as follows: First, maps in the map library whose labels match the daily production plan are retrieved for ICP matching, and their matching degree is saved; if the matching degree exceeds the threshold, only maps that match the production plan are compared, and the map with the highest score is selected as the prediction map; if the matching degree does not exceed the threshold, maps whose labels are other production states are compared; the matching degree of the prediction map is saved as the confidence level of time positioning, providing data for the subsequent determination of Kalman gain; if there is a map whose matching degree exceeds the threshold, it is used as the prediction map; if there is no map whose matching degree exceeds the threshold, the positioning using time information is canceled, and step S2.4 is not performed.

[0100] In step S2.4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map.

[0101] In step S2.5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning; the Kalman gain is determined based on the covariance of the two, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

[0102] Example 2:

[0103] Example 2 is a preferred embodiment of Example 1, and is used to illustrate the present invention in more detail.

[0104] The present invention also provides an industrial plant positioning and mapping system based on spatiotemporal information. The industrial plant positioning and mapping system based on spatiotemporal information can be implemented by executing the process steps of the industrial plant positioning and mapping method based on spatiotemporal information. That is, those skilled in the art can understand the industrial plant positioning and mapping method based on spatiotemporal information as a preferred embodiment of the industrial plant positioning and mapping system based on spatiotemporal information.

[0105] An industrial plant positioning and mapping system based on spatiotemporal information provided by the present invention includes:

[0106] Module M1: Performs feature depth correlation between the laser point cloud obtained by the LiDAR and the image obtained by the monocular camera; performs front-end point cloud registration; performs semantic instance segmentation on the video stream obtained by the monocular camera; marks environmental object points; performs factor graph optimization; performs global map stitching based on the optimized trajectory to obtain a global map of the current factory environment; sets processing status labels on the global map; and builds and maintains the factory spatiotemporal map library.

[0107] Preferably, in module M1:

[0108] Module M1.1: Performs extrinsic parameter calibration on the ground monocular camera and ground lidar to obtain the transformation matrix between the ground monocular camera and ground lidar;

[0109] Module M1.2: Based on the sensor extrinsic parameters obtained from calibration, the laser point cloud obtained by the lidar is correlated with the image obtained by the monocular camera to recover the depth information in the image;

[0110] Module M1.3: Performs front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor;

[0111] Module M1.4: Performs semantic instance segmentation on the video stream obtained by the monocular camera to obtain semantic information of environmental objects when the factory processes the current workpiece; and obtains the pose Tobj of the current environmental objects based on feature matching;

[0112] Module M1.5: Based on the transformation matrix between the camera and the lidar, the global pose Tg, and the pose Tobj of the labeled semantic objects, the environmental object points in the lidar point cloud are labeled using the feature depth association method, and denoted as Pobj.

[0113] Module M1.6: The visual odometry extracts descriptors from keyframes and uses a bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to the laser odometry. The laser odometry uses ICP for registration to obtain the pose after loop closure.

[0114] Module M1.7: Visual factors, lidar factors, and loop closure factors are added to the factor graph for graph optimization to obtain an optimized high-precision trajectory;

[0115] Module M1.8: Based on the optimized trajectory and sensor extrinsic transformation matrix, perform global map stitching to obtain a global map of the current factory environment;

[0116] Module M1.9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map; save the data to form a spatiotemporal information map library.

[0117] Preferably, in module M1.2, in order to perform feature depth association, the lidar point cloud and image features are projected onto a sphere according to the extrinsic transformation matrix, and the visual feature coordinates are searched using kdtree to obtain the three lidar points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three lidar points.

[0118] In module M1.4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects with probabilistic changes in the factory environment. The focus is on training items that can reflect the current processing state of the factory, and the semantic information is used to determine the current processing state of the factory.

[0119] In module M1.5, the feature depth association method in module M1.2 is used to annotate the semantic information in the visual image onto the corresponding LiDAR point cloud;

[0120] In module M1.8, the global map obtained is the global map created within this SLAM cycle, in which objects segmented from instances are specially labeled and have additional semantic information added;

[0121] In module M1.9, tags are set on the map based on the semantic tags of the workpieces being processed, the daily production plan, or manual annotation. The tags reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed.

[0122] Module M2: It uses the spatiotemporal information map library and prior information of the factory production plan to predict the current map, and combines the predicted map with the structured information on the top of the factory to perform extended Kalman filtering to obtain high-precision positioning information based on spatiotemporal information.

[0123] Preferably, in module M2:

[0124] Module M2.1: Utilizes LiDAR to extract structured feature information from the ceiling trusses and walls of the factory building. When a loop is detected, the LiDAR point cloud of the current frame is registered with the local map using the nearest point search and normal distribution transformation method to obtain the loop information. The positioning factor and loop factor are added to the factor map for optimization to obtain spatial positioning information Ts.

[0125] Module M2.2: Utilizes a monocular camera and lidar responsible for acquiring ground information to realize the odometry function, and performs point cloud stitching in real time during operation to obtain the currently running local map M1;

[0126] Module M2.3: Using the provided factory production operation plan as prior information, the local map of module M2.2 is first matched with the map Ms in the spatiotemporal information map library that matches the production plan by ICP. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp; if there is no map with a matching degree exceeding the threshold, the time information positioning is canceled and module M2.5 is executed.

[0127] Module M2.4: Relocate the predicted map Mp obtained from module M2.3 with the current local map Ml to obtain location information Tt based on time information;

[0128] Module M2.5: Performs extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

[0129] Preferably, in module M2.2, during the process of stitching together the laser odometry and the local map, the error of the laser odometry and the residual of the factor map optimization are saved as the confidence level of spatial positioning;

[0130] In module M2.3, the matching process between the local map ML and the map Ms in the map library is as follows: First, maps whose labels match the daily production plan are retrieved from the map library and subjected to ICP matching, and their matching degree is saved; if the matching degree exceeds the threshold, only maps that match the production plan are compared, and the map with the highest score is selected as the prediction map; if the matching degree does not exceed the threshold, maps whose labels are other production states are compared; the matching degree of the prediction map is saved as the confidence level of time positioning, providing data for the subsequent determination of Kalman gain; if there is a map whose matching degree exceeds the threshold, it is used as the prediction map; if there is no map whose matching degree exceeds the threshold, the use of time information for positioning is canceled, and module M2.4 is not performed.

[0131] In module M2.4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map.

[0132] In module M2.5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning. The Kalman gain is determined based on the covariance of the two, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

[0133] Example 3:

[0134] Example 3 is a preferred example of Example 1, and is used to illustrate the present invention in more detail.

[0135] The technical solution adopted in this invention is as follows:

[0136] A method for locating and mapping industrial plants based on spatiotemporal information, with an overall flowchart as follows: Figure 1 As shown, it can be divided into two stages: mapping and localization.

[0137] The robot is equipped with two LiDARs and one monocular camera. One LiDAR and monocular camera are used to acquire ground environment information, which is then used in the mapping and localization stages to obtain and match the factory's location information in the time domain. The other LiDAR is used to acquire structured environmental information about the factory's roof, which is then used in the localization stage to obtain the factory's location information in the spatial domain. The system initially performs only the localization stage; subsequent system runs will run both mapping and localization stages. The mapping stage is responsible for constructing a global semantic map of the factory and building and maintaining a spatiotemporal map library for the factory. The localization stage uses the spatiotemporal map library and prior information about the factory's production plan to predict the current map. Combined with the predicted map and the structured information about the factory's roof, an extended Kalman filter is applied to obtain high-precision localization information based on spatiotemporal information.

[0138] The process of mapping and building a spatiotemporal information map database is as follows: Figure 2 As shown:

[0139] Step 1: Perform extrinsic parameter calibration on the ground monocular camera and the ground lidar to obtain the transformation matrix between them.

[0140] Step 2: Based on the sensor extrinsic parameters obtained from calibration, perform feature depth correlation between the laser point cloud obtained by the lidar and the image obtained by the monocular camera to recover the depth information in the image.

[0141] Step 3: Perform front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor.

[0142] Step 4: Perform semantic instance segmentation on the video stream obtained from the monocular camera to obtain the semantic information of environmental objects when the factory processes the current workpiece. Then, obtain the pose (Tobj) of the current environmental objects based on feature matching.

[0143] Step 5: Based on the transformation matrix between the camera and the LiDAR, the global pose Tg, and the pose Tobj of the labeled semantic objects, the environmental object points in the LiDAR point cloud are labeled using the feature deep association method. These labeled points are denoted as Pobj.

[0144] Step 6: Visual odometry extracts descriptors from keyframes and uses a bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to laser odometry, which uses ICP for registration to obtain the pose after loop closure.

[0145] Step 7: Add the visual factor, lidar factor, and loop closure factor to the factor graph for graph optimization to obtain the optimized high-precision trajectory.

[0146] Step 8: Based on the optimized trajectory and the sensor extrinsic parameter transformation matrix, perform global map stitching to obtain a global map of the current factory environment.

[0147] Step 9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map. Save the data to create a spatiotemporal information map library.

[0148] In step 2, to perform feature depth association, the LiDAR point cloud and image features are projected onto a sphere using an extrinsic transformation matrix. A kdtree search is then used to find the coordinates of the visual feature, identifying the three LiDAR points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three LiDAR points.

[0149] In step 4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects that may be changing in the factory environment. The training focuses on items that can reflect the current processing status of the factory, such as large workpieces and the related equipment for processing them. This semantic information is used to determine the current processing status of the factory.

[0150] In step 5, the semantic information in the visual image is labeled onto the corresponding LiDAR point cloud using the feature depth association method in step 2.

[0151] In step 8, the resulting global map is the global map created during this SLAM cycle. The objects segmented from the instances are specially labeled and have additional semantic information added to facilitate the setting of map labels later.

[0152] In step 9, labels are set on the map based on the semantic tags of the workpieces being processed, the daily production plan, or manual annotation. These labels primarily reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed. This facilitates subsequent map library management and map prediction.

[0153] The positioning process based on spatiotemporal information is as follows: Figure 3 As shown:

[0154] Step 1: Utilize LiDAR to extract structured feature information from the factory ceiling trusses and walls, facilitating subsequent point cloud registration and improving positioning accuracy. When a loop closure is detected, register the current frame's LiDAR point cloud with the local map using nearest-neighbor search and normal distribution transformation methods to obtain accurate loop closure information. Optimize the positioning factor and loop closure factor by adding them to the factor map to obtain high-precision spatial positioning information Ts.

[0155] Step 2: Use a monocular camera and lidar responsible for acquiring ground information to implement the odometry function, and perform point cloud stitching in real time during operation to obtain the currently running local map Ml.

[0156] Step 3: Using the provided factory production plan as prior information, perform ICP matching between the local map from Step 2 and the map Ms in the spatiotemporal information map library that matches the production plan. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp. If there is no map with a matching degree exceeding the threshold, cancel the time information positioning and proceed to Step 5.

[0157] Step 4: Relocate the predicted map Mp obtained in Step 3 with the current local map Ml to obtain the time-based positioning information Tt.

[0158] Step 5: Perform extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

[0159] In step 2, during the process of stitching together the laser odometry and the local map, it is necessary to save the error of the laser odometry and the residual of the factor map optimization as the confidence level of spatial positioning, and provide data for the subsequent determination of Kalman gain.

[0160] In step 3, the matching process between the local map ML and the map Ms in the map library is as follows: First, maps in the map library whose labels match the daily production plan are retrieved and subjected to ICP matching, and their matching degree is saved. If the matching degree exceeds a threshold, only maps that match the production plan are compared, and the map with the highest score is selected as the prediction map. If the matching degree does not exceed the threshold, maps with labels of other production states are compared. The matching degree of the prediction map is saved as the confidence level of time positioning, providing data for the subsequent determination of Kalman gain. If a map with a matching degree exceeding the threshold is found, it is used as the prediction map; if no map with a matching degree exceeding the threshold is found, the use of time information for positioning is canceled, and step 4 is skipped.

[0161] In step 4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map, providing data for the subsequent determination of Kalman gain.

[0162] In step 5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning. The Kalman gain is then determined based on these two covariances, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

[0163] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0164] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for locating and mapping industrial plants based on spatiotemporal information, characterized in that, include: Step S1: Perform feature depth correlation between the laser point cloud obtained by the lidar and the image obtained by the monocular camera; Perform front-end point cloud registration, semantic instance segmentation on the video stream obtained by the monocular camera, mark environmental object points, optimize factor graph, and stitch global map based on the optimized trajectory to obtain a global map of the current factory environment. Set processing status labels for the global map; build and maintain a spatiotemporal map library for the factory; Step S2: Use the spatiotemporal information map library and the prior information of the factory production plan to predict the current map, and combine the predicted map with the structured information on the top of the factory to perform extended Kalman filtering to obtain high-precision positioning information based on spatiotemporal information. Step S2.1: Use LiDAR to extract structured feature information from the ceiling truss and walls of the factory building. When a loop is detected, register the LiDAR point cloud of the current frame with the local map using the nearest point search and normal distribution transformation method to obtain the loop information. Add the positioning factor and loop factor to the factor map for optimization to obtain spatial positioning information Ts. Step S2.2: Use a monocular camera and lidar responsible for acquiring ground information to implement the odometry function, and perform point cloud stitching in real time during operation to obtain the currently running local map Ml; During the process of stitching together laser odometry and local maps, the error of laser odometry and the residual of factor map optimization are saved as the confidence level of spatial positioning. Step S2.3: Using the provided factory production operation plan as prior information, perform ICP matching between the local map in step S2.2 and the map Ms in the spatiotemporal information map library that matches the production plan. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp. If no map matches the threshold, cancel the time-based location and proceed to step S2.

5. In step S2.3, the matching process between the local map M1 and the map Ms in the map library is as follows: First, the maps in the map library whose labels match the daily production plan are retrieved and ICP matched, and their matching degree is saved; if the matching degree exceeds the threshold, only the maps that match the production plan are compared, and the map with the highest score is selected as the predicted map; if the matching degree does not exceed the threshold, the maps whose labels are other production states are compared. The matching degree of the predicted map is saved as the confidence level of the time positioning, providing data for the subsequent determination of the Kalman gain; if there is a map with a matching degree exceeding the threshold, it is used as the predicted map; if there is no map with a matching degree exceeding the threshold, the positioning using time information is canceled and step S2.4 is not performed. Step S2.4: Relocate the predicted map Mp obtained in step S2.3 with the current local map Ml to obtain the location information Tt based on time information; In step S2.4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map.

2. The method for locating and mapping industrial plants based on spatiotemporal information according to claim 1, characterized in that, In step S1: Step S1.1: Perform extrinsic parameter calibration on the ground monocular camera and the ground lidar to obtain the transformation matrix between the ground monocular camera and the ground lidar; Step S1.2: Based on the sensor extrinsic parameters obtained from calibration, perform feature depth correlation between the laser point cloud obtained by the lidar and the image obtained by the monocular camera to recover the depth information in the image; Step S1.3: Perform front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor. Step S1.4: Perform semantic instance segmentation on the video stream obtained by the monocular camera to obtain the semantic information of the environmental objects when the factory processes the current workpiece; and obtain the pose Tobj of the current environmental objects based on feature matching as a visual factor. Step S1.5: Based on the transformation matrix between the camera and the lidar, the global pose Tg, and the pose Tobj of the labeled semantic object, the environmental object points in the lidar point cloud are labeled using the feature depth association method, and denoted as Pobj. Step S1.6: The visual odometry extracts descriptors from keyframes and uses the bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to the laser odometry. The laser odometry uses ICP for registration to obtain the pose after loop closure, which is used as the loop closure factor. Step S1.7: Add the visual factor, lidar factor, and loop closure factor to the factor graph for graph optimization to obtain the optimized high-precision trajectory; Step S1.8: Based on the optimized trajectory and the sensor extrinsic transformation matrix, perform global map stitching to obtain a global map of the current factory environment; Step S1.9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map; save the data to form a spatiotemporal information map library.

3. The method for locating and mapping industrial plants based on spatiotemporal information according to claim 2, characterized in that: In step S1.2, in order to perform feature depth association, the LiDAR point cloud and image features are projected onto a sphere according to the extrinsic transformation matrix. The kdtree is used to search the visual feature coordinates to obtain the three LiDAR points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three LiDAR points. In step S1.4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects with probabilistic changes in the factory environment. The focus is on training items that can reflect the current processing state of the factory. The semantic information is used to determine the current processing state of the factory. In step S1.5, the semantic information in the visual image is labeled onto the corresponding LiDAR point cloud using the feature depth association method in step S1.

2. In step S1.8, the obtained global map is the global map created within this SLAM running cycle, in which the objects segmented from the instance are specially labeled and additional semantic information is added; In step S1.9, labels are set on the map according to the semantic labels of the workpieces being processed, the production plan for the day, or by manual annotation; the labels reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed.

4. The method for locating and mapping industrial plants based on spatiotemporal information according to claim 1, characterized in that, In step S2: Step S2.5: Perform extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

5. The method for locating and mapping industrial plants based on spatiotemporal information according to claim 4, characterized in that: In step S2.5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning; the Kalman gain is determined based on the covariance of the two, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

6. An industrial plant positioning and mapping system based on spatiotemporal information, characterized in that, include: Module M1: Performs feature depth correlation between the laser point cloud obtained by the lidar and the image obtained by the monocular camera; Perform front-end point cloud registration, semantic instance segmentation on the video stream obtained by the monocular camera, mark environmental object points, optimize factor graph, and stitch global map based on the optimized trajectory to obtain a global map of the current factory environment. Set processing status labels for the global map; build and maintain a spatiotemporal map library for the factory; Module M2: It uses the spatiotemporal information map library and prior information of the factory production plan to predict the current map, and combines the predicted map with the structured information on the top of the factory to perform extended Kalman filtering to obtain high-precision positioning information based on spatiotemporal information. Module M2.1: Utilizes LiDAR to extract structured feature information from the ceiling trusses and walls of the factory building. When a loop is detected, the LiDAR point cloud of the current frame is registered with the local map using the nearest point search and normal distribution transformation method to obtain the loop information. The positioning factor and loop factor are added to the factor map for optimization to obtain spatial positioning information Ts. Module M2.2: Utilizes a monocular camera and lidar responsible for acquiring ground information to realize the odometry function, and performs point cloud stitching in real time during operation to obtain the currently running local map M1; During the process of stitching together laser odometry and local maps, the error of laser odometry and the residual of factor map optimization are saved as the confidence level of spatial positioning. Module M2.3: Using the provided factory production operation plan as prior information, the local map of module M2.2 is first matched with the map Ms in the spatiotemporal information map library that matches the production plan by ICP. The map with the highest matching degree exceeding the threshold is used as the current predicted map Mp; if there is no map with a matching degree exceeding the threshold, the time information positioning is canceled and module M2.5 is executed. In module M2.3, the matching process between the local map M1 and the map Ms in the map library is as follows: First, the maps in the map library whose labels match the daily production plan are retrieved and ICP matched, and their matching degree is saved; if the matching degree exceeds the threshold, only the maps that match the production plan are compared, and the map with the highest score is selected as the prediction map; if the matching degree does not exceed the threshold, the maps whose labels are other production states are compared. The matching degree of the predicted map is saved as the confidence level of the time positioning, providing data for the subsequent determination of the Kalman gain; if there is a map with a matching degree exceeding the threshold, it is used as the predicted map; if there is no map with a matching degree exceeding the threshold, the positioning using time information is canceled and module M2.4 is not performed. Module M2.4: Relocate the predicted map Mp obtained from module M2.3 with the current local map Ml to obtain location information Tt based on time information; In module M2.4, the confidence level of time positioning is determined based on the degree of matching between the local map and the predicted map, as well as the confidence level of the predicted map.

7. The industrial plant positioning and mapping system based on spatiotemporal information according to claim 6, characterized in that, In module M1: Module M1.1: Performs extrinsic parameter calibration on the ground monocular camera and ground lidar to obtain the transformation matrix between the ground monocular camera and ground lidar; Module M1.2: Based on the sensor extrinsic parameters obtained from calibration, the laser point cloud obtained by the lidar is correlated with the image obtained by the monocular camera to recover the depth information in the image; Module M1.3: Performs front-end point cloud registration on the point cloud acquired by the lidar to obtain the global pose Tg of the mobile robot in the lidar coordinate system, which is used as the lidar factor; Module M1.4: Performs semantic instance segmentation on the video stream obtained by the monocular camera to obtain semantic information of environmental objects when the factory processes the current workpiece; and obtains the pose Tobj of the current environmental objects based on feature matching as a visual factor. Module M1.5: Based on the transformation matrix between the camera and the lidar, the global pose Tg, and the pose Tobj of the labeled semantic objects, the environmental object points in the lidar point cloud are labeled using the feature depth association method, and denoted as Pobj. Module M1.6: The visual odometry extracts descriptors from keyframes and uses the bag-of-words model for loop closure detection. When a loop closure candidate frame is found, the timestamp is sent to the laser odometry. The laser odometry uses ICP for registration to obtain the pose after loop closure, which is used as the loop closure factor. Module M1.7: Visual factors, lidar factors, and loop closure factors are added to the factor graph for graph optimization to obtain an optimized high-precision trajectory; Module M1.8: Based on the optimized trajectory and sensor extrinsic transformation matrix, perform global map stitching to obtain a global map of the current factory environment; Module M1.9: Based on the semantics of objects in the map, the daily production plan, and manual annotations, set processing status labels for the global map; save the data to form a spatiotemporal information map library.

8. The industrial plant positioning and mapping system based on spatiotemporal information according to claim 7, characterized in that: In module M1.2, in order to perform feature depth association, the LiDAR point cloud and image features are projected onto a sphere according to the extrinsic transformation matrix. The kdtree is used to search the visual feature coordinates to obtain the three LiDAR points closest to this feature. The depth of this visual feature is the distance from the feature point to the plane formed by these three LiDAR points. In module M1.4, the Yolact Edge deep learning network is used to perform semantic instance segmentation on objects with probabilistic changes in the factory environment. The focus is on training items that can reflect the current processing state of the factory, and the semantic information is used to determine the current processing state of the factory. In module M1.5, the feature depth association method in module M1.2 is used to annotate the semantic information in the visual image onto the corresponding LiDAR point cloud; In module M1.8, the global map obtained is the global map created within this SLAM cycle, in which objects segmented from instances are specially labeled and have additional semantic information added; In module M1.9, tags are set on the map based on the semantic tags of the workpieces being processed, the daily production plan, or manual annotation. The tags reflect the current state of the factory map in the spatiotemporal domain and the type of objects being processed.

9. The industrial plant positioning and mapping system based on spatiotemporal information according to claim 6, characterized in that, In module M2: Module M2.5: Performs extended Kalman filtering on the temporal positioning information Tt and the spatial positioning information Ts to obtain positioning based on spatiotemporal information.

10. The industrial plant positioning and mapping system based on spatiotemporal information according to claim 9, characterized in that: In module M2.5, the spatial positioning covariance is determined based on the confidence level of spatial positioning, and the temporal positioning covariance is determined based on the confidence level of temporal positioning. The Kalman gain is determined based on the covariance of the two, and Kalman filtering is performed to obtain the final high-precision positioning based on spatiotemporal information.

Images