A 4D semantic slam method for indoor mobile robots based on vision

Abstract

The application discloses a kind of 4D semantic SLAM methods of indoor mobile robot based on vision, utilize binocular or RGBD camera real-time acquisition indoor scene color image and depth data, and extract human skeleton data by human posture recognition network.Further, four parallel modules of target recognition, behavior recognition, point line feature fusion odometry and loop detection are established by AI edge processor installed on robot, the model trained by calling incremental learning trainer realizes target recognition and human behavior recognition of surrounding environment of mobile robot, carries out robot own pose tracking and online 4D dynamic semantic map construction, and realizes long-time perception of robot operation state by dynamically updating 4D semantic mapping map and sample database.The application improves long-time perception and man-machine integration capability of indoor robot to complex environment, and also guarantees that robot has the ability of dynamic learning new knowledge and privacy protection in privacy environment such as family.

Description

Technical Field

[0001] This invention relates to a vision-based 4D semantic SLAM method for indoor mobile robots, belonging to the fields of visual SLAM and 3D reconstruction technology. Background Technology

[0002] The significance of achieving dynamic, long-term 3D reconstruction in unknown environments for robot scene perception and understanding is self-evident. Existing Visual Simultaneous Localization and Mapping (SLAM) is a technique that combines pose tracking and 3D reconstruction, effectively solving the problem of static mapping in unknown environments, and has attracted strong research interest from fields such as computer vision, computer graphics, and virtual reality. However, in the real world, mobile robots typically need to operate in dynamic and complex environments, which pose challenges to most existing 3D reconstruction methods. Therefore, the dynamic, real-time construction of semantic maps for unknown environments and the maintenance of long-term consistency of these semantic maps are urgent requirements for robots to achieve scene perception capabilities.

[0003] Existing visual SLAM mapping can be categorized into sparse, semi-sparse, and dense mapping. However, these maps consist of meaningless scattered points, which is detrimental to robot scene understanding. Therefore, semantic SLAM has received more attention. Current semantic SLAM mainly falls into two categories. One category is based on traditional machine learning methods, utilizing single or multiple visual features and employing methods such as SVM, Gaussian probability region analysis, and matching to classify objects and construct semantic maps containing object information, as proposed in the paper "Building multiversal semantic maps for mobile robot operation." This type of method has fast recognition speed and achieves good results in simple scenes or complex scenes with labeled objects, but its object recognition accuracy is low and it lacks adaptability in complex dynamic environments. The other category is based on semantic mapping or real-time semantic mapping methods using deep convolutional neural networks, such as the method proposed in patent CN112734845A. This method constructs a real-time semantic segmentation network based on a training dataset and builds a semantic map based on semantic feature points. These methods are inherently complex, have poor real-time computation, require a large amount of labeled data as a foundation, cannot handle dynamically added data, and have poor interpretability, making it difficult to meet the semantic understanding needs of service robots in dynamic environments. The aforementioned mapping methods only consider objects in the scene, neglecting human actions and postures. While the paper "AirDOS: Dynamic SLAM benefits from Articulated Objects" implements human representation in the map, it lacks descriptions and reasoning of human behavior, making it difficult to adapt to the "human-centered" service requirements of service robots. In summary, existing SLAM mapping methods have the following shortcomings:

[0004] (1) The maps are mainly constructed based on points, voxels or patches, and the maps are not classified according to the environment and target object attributes. The computational complexity is high, resulting in a great waste of resources and information redundancy.

[0005] (2) It focuses more on objects in the scene and fails to describe and reason about human behavior and human-object interaction, making it difficult to adapt to the needs of indoor robots to provide services for people. Summary of the Invention

[0006] To address the aforementioned technical problems, this invention provides a vision-based 4D semantic SLAM method for indoor mobile robots. It establishes four parallel modules—target recognition, behavior recognition, point-line feature fusion odometry, and loop closure detection—through an AI edge processor installed on the robot. The model trained by an incremental learning trainer enables the mobile robot to recognize targets in its surrounding environment and human behavior, performs robot pose tracking, and constructs an online 4D dynamic semantic map. Furthermore, it achieves long-term perception of the robot's operational status by dynamically updating the 4D semantic mapping map and sample database.

[0007] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0008] A vision-based 4D semantic SLAM method for indoor mobile robots includes the following steps:

[0009] Step 1: Acquire RGB image data, depth data, and human skeleton data in real time;

[0010] Step 2: Construct behavior datasets and target datasets, including a basic dataset, a personalized dataset, and a dynamically expanded dataset, respectively. Create an incremental learning trainer to pre-train the basic target recognition model and behavior recognition model based on the basic dataset and the personalized dataset. Then, based on the RGB image data and human skeleton data obtained in Step 1, implement online dynamic incremental training to achieve dynamic online updates of the target recognition model, behavior recognition model, behavior dataset, and target dataset.

[0011] Step 3: Establish three parallel processing modules: visual odometry module, target recognition module, and action recognition module; the visual odometry module extracts and tracks point and line features from the RGB image and depth data obtained in Step 1; the target recognition module inputs the RGB image obtained in Step 1 into the target recognition model pre-trained by the incremental learning trainer in Step 2 to achieve target detection and target detection box position information; the action recognition module obtains joint position information and skeleton minimum bounding box position information from the skeleton data obtained in Step 1, and inputs it into the action recognition model pre-trained by the incremental learning trainer in Step 2 to achieve human action recognition.

[0012] Step 4: Based on the target detection box position information and the skeleton minimum bounding box position information in Step 3, obtain the dynamic and static probabilities of the features in the scene respectively. Further filter the point and line features extracted by the visual odometry module in Step 3 to realize the deletion of dynamic features and the merging of static features. Update the current frame information, including target information, key points, key lines, map points, line pairs and their co-view relationships. Further calculate the pose based on the pose estimation submodule and perform local optimization.

[0013] Step 5: Based on the target detection results obtained by the target recognition module in Step 3 and the point and line features obtained by the visual odometry module in Step 3, perform loop closure frame detection. If a loop closure frame is detected, perform loop closure correction and global BA operation.

[0014] Step 6: Construct a dynamic semantic map description, and establish a cognitive semantic map including a grid layer, a behavior layer and a semantic layer to realize the global 4D semantic map expression of the robot during long-term indoor operation.

[0015] A further improvement of the technical solution of the present invention is that the RGB image data, depth data and human skeleton data in step 1 are directly acquired in real time through an RGBD camera or a binocular camera.

[0016] A further improvement to the technical solution of the present invention is that the specific operation of step 2 is as follows:

[0017] Step 2.1: Construct a behavior dataset that includes a basic behavior dataset, a personalized behavior dataset, and a dynamically extended behavior dataset, as well as a target dataset that includes a basic target dataset, a personalized target dataset, and a dynamically extended target dataset;

[0018] The basic dataset consists of common basic human behaviors and common indoor objects collected from the Internet or public datasets without involving privacy; the personalized dataset consists of a small sample dataset of objects or behaviors labeled by the user before the robot runs for the first time in a personalized scene; the dynamically expanded dataset is a dataset that is dynamically maintained by the robot during long-term operation, with new samples added as the robot runs in the scene.

[0019] Step 2.2: Create the target recognition incremental learning model in the incremental learning trainer. First, train the target recognition initialization model O based on the target base dataset. 0 Next, the image information obtained in step 1 is input into the target recognition model O. t-1 The target recognition result is obtained in (t>0); set T o The confidence threshold for target identification results is defined as follows: qualified samples should meet the confidence level (≥T). o ), To The value of should satisfy:

[0020]

[0021] Where S T S represents the training dataset consisting of a basic dataset and personalized datasets. t This represents the accumulated qualified RGB image data;

[0022] The incremental learning model for target recognition labels the data that meets the requirements in the test with predicted boxes and generates a new dynamically expanded target dataset. It then checks whether the size of the dataset exceeds a set threshold Ma = 10. If this condition is met, the dynamically expanded target dataset is input into the target recognition model O. t-1 A second training is performed to obtain the target recognition model O. t If the model accuracy improves, then the original target recognition model O... t-1 Replace with target recognition model O t Otherwise, stop training.

[0023] Step 2.3: Create the behavior recognition incremental learning model in the incremental learning trainer. First, train the behavior recognition initialization model B based on the behavior base dataset. 0 Next, the human skeleton data obtained in step 1 is input into behavior recognition model B. t-1 The behavior recognition result is obtained in (t>0); set T B The confidence threshold for behavior recognition results is defined as follows: qualified samples should meet the confidence level (≥T). B ), T B The value of should satisfy:

[0024]

[0025] Where S B S represents the training dataset consisting of a basic dataset and personalized datasets. b This represents the cumulative amount of qualified skeleton data obtained.

[0026] The behavior recognition incremental learning model labels the data that meets the requirements in the test and generates a new dynamic behavior expansion dataset. It then determines whether the size of the dataset exceeds a set threshold Ma = 10. If this condition is met, the dynamic behavior expansion dataset is input into the behavior recognition model B. t-1 A second training step is performed to obtain behavior recognition model B. t If the model accuracy improves, then the original behavior recognition model B will be... t-1 Replace with behavior recognition model B t Otherwise, stop training.

[0027] A further improvement to the technical solution of the present invention is that the specific operation of step 3 is as follows:

[0028] Step 3.1: Establish the visual odometry module:

[0029] First, points and lines are extracted from the RGB image data obtained in step 1. Specifically, this includes using the EDrawing algorithm to obtain the set of edge chains E = {e1, e2, ..., e...} of the input RGB image. m Then, the edge direction code of E is calculated according to the OTLine algorithm, and it is segmented into a corner point set P = {p1, p2, ..., p}. np} and the set of linear edge chains L = {L1, L2, ..., L l};

[0030] Then, point and line features are constructed, specifically including: performing maximum suppression on the corner point set P and using its local pseudo-curvature to construct the feature point response values; to avoid feature points being too concentrated and affecting camera pose calculation, a quadtree method is further used to obtain a uniformly distributed feature point set KP = {kp1,kp2,...,kp...}. nk For a set L of straight edge chains, construct a set of straight line features KL = {kL1, kL2, ..., kL...} based on the line segment length and angle characteristics. nl};

[0031] Step 3.2: Establish the target recognition module:

[0032] Use the target recognition model pre-trained by the incremental learning trainer in step 2 to obtain the category information of the target object and the coordinate information of the bounding box;

[0033] Step 3.3: Establish the behavior recognition module:

[0034] Based on the joint positions of the human skeleton data obtained in step 1, calculate its minimum bounding box region information; use the behavior recognition model pre-trained by the incremental learning trainer in step 2 to obtain the behavior category and record the behavior label.

[0035] A further improvement to the technical solution of the present invention is that the specific operation of step 4 is as follows:

[0036] Step 4.1: During the camera pose initialization stage, if no person appears in the image, the features contained in the target detection box location region are all static features by default in this stage; if a person appears in the image, the features contained in the minimum bounding box region of the skeleton data are dynamic features; delete the dynamic features and perform camera pose estimation based on the static features.

[0037] Step 4.2: During the camera tracking stage, the dynamic probability of the target object is evaluated based on its attributes. The evaluation results are divided into three types: static, semi-static, and dynamic. For background objects and large objects, whose detection boxes occupy a large area in the image, they are considered static objects and assigned a low dynamic probability. For smaller objects, the dynamic probability of the target object can be further evaluated based on whether it interacts with a person. If a person is present in the current image, the three-dimensional spatial distance between the person's hand joint and the center of gravity of the smaller object in the image is calculated. Two distance thresholds are defined at this point. When the distance is satisfied, the object is highly likely to be associated with a person. Interactive behavior is considered dynamic and assigned a high dynamic probability. When the distance is satisfied, the object has a low probability of interacting with a person and is considered a semi-static object, assigned a medium dynamic probability. When the distance is satisfied, the object has a very low probability of interacting with a person and is considered a static object, assigned a low dynamic probability. If there is no person in the current image, all objects in the image can be considered static objects and assigned a low dynamic probability. The target detection box location region of semi-static objects is considered as the feature to be filtered, and features with a dynamic probability of 0 and features contained in the minimum bounding box region of the skeleton data are removed.

[0038] Step 4.3: Features of the minimum bounding box region of potential dynamic objects and skeleton data are the features to be screened; objects with higher dynamic probability are potential dynamic objects. Several feature points are sampled, and the distance between the endpoints of the selected feature points and feature lines and the epipolar lines is calculated using the reliable pose information of static points and lines. Selected feature points that do not meet the epipolar constraints are classified as dynamic points. For selected line features, if one endpoint does not meet the limit constraint, the entire line is a dynamic line feature. Then, based on the principle that points on the same object will not have two states, all feature points and lines are removed. Features of the minimum bounding box region of skeleton data are all screened. Features that do not meet the epipolar constraints are classified as dynamic features, and then the remaining features are merged with static features.

[0039] Step 4.4: After removing dynamic features, the indoor environment is modeled as the Manhattan world, and the 6D camera pose is decoupled into rotation and translation. Finally, the camera pose is estimated based on the static features.

[0040] First, based on the Manhattan hypothesis, calculate the rotation matrix R between the Manhattan world (MW) and the camera (k frames). k,MW Thus, the rotation matrix R between n frames and m frames of the camera is obtained. m,n Next, using static feature points and lines to estimate the camera's 3D translation motion, the 3D points and lines are reprojected from the last frame to the current frame. An error function is defined based on the reprojection error, where the error function for static line features can be calculated using the following steps: For line L in the nth frame... nThe corresponding matching line in the m-th frame is L. m ; make straight line L n Random sampling is performed to obtain ni pixels. Pixels with a depth greater than 0 are retained based on depth information. The position information set of the 3D point set is obtained based on camera intrinsic parameters. in Then, by projecting it onto the current frame m, we can obtain its reprojection point set. Construct a line error function based on the distance relationship from a point to a line:

[0041]

[0042] in For line L m The vector representation of the vector is given by Π(·), where Π(·) is the projection function; therefore, the least squares cost of a point and a line can be written as:

[0043]

[0044] Where ρ p With ρ l The Huber cost functions are given for points and lines, respectively, and are ultimately solved using the Levenberg-Marquardt algorithm.

[0045] A further improvement to the technical solution of the present invention is that the specific operation of step 5 is as follows:

[0046] Step 5.1: Record the object information contained in each frame of the image, and encode the object category and its corresponding dynamic probability for that frame of the image;

[0047] Step 5.2: If the current frame and the past keyframes have the same image category, use bag-of-words to accelerate matching and determine whether loop closure occurs; if the current frame does not find a case with the same category as the past keyframes, remove the object categories with high dynamic probability in the scene and then compare the categories. If they are the same, use the features of the remaining objects of the same category to perform loop closure detection. This process can make loop closure detection more robust.

[0048] A further improvement to the technical solution of this invention is as follows: Step 6 establishes a three-dimensional grid map based on the dynamic probability of the environment and objects, wherein objects and the indoor environment are described by a three-dimensional grid map combining coarse and fine granularity; the human body is described using a 3D human skeleton; objects and walls with low long-term dynamic probability are established as coarser static grid maps, while human posture and human-object interaction behavior are expressed as fine dynamic grid maps, and the map is updated in real time; the behavior layer is used to describe the long-term motion trajectory of the target, human body, and robot over time; the semantic layer establishes semantic tags based on the target detection results, behavior recognition results, and grid layer map, which are used to describe the current state of the scene and human behavior, and further, based on the above information, it is combined with an indoor scene semantic understanding network to infer the relationship between objects, the relationship between humans and objects, and the semantic reasoning of human behavior in the scene, and displays them in the form of semantic tags;

[0049] The specific steps are as follows:

[0050] Step 6.1: The entire mapping process, in chronological order, includes static mapping, dynamic mapping, and semantic mapping. Figure 3 The system is divided into several parts; static mapping includes mapping of static and semi-static objects, described through a 3D raster layer; dynamic mapping includes mapping of people and the dynamic poses of dynamic objects, described through a behavior layer; and semantic mapping includes semantic annotation and understanding of indoor scenes and people, described through a semantic layer.

[0051] Step 6.2: For static maps, including environmental background and target recognition objects; for background areas, use depth information to segment and obtain planar instances, and then describe them in the raster layer according to the size of the planar area through a combination of coarse and fine granular grids; for objects detected and recognized by target detection, describe them in the form of object centroids and 3D bounding boxes in the raster layer, and store the current object's category, ID, and dynamic probability.

[0052] Step 6.3: For dynamic maps, including camera motion trajectories and human skeleton data, when a person appears in the scene, the skeleton data is displayed in the time dimension, along with the corresponding action label.

[0053] Step 6.4: For the semantic map, store the category, ID, and dynamic probability of the current object based on the corresponding 3D position of the object in each frame; store the person ID, action category, and dynamic probability based on the recognized human skeleton and action recognition results; based on the object, human skeleton, and action category data over a period of time, combine the indoor scene semantic understanding network to infer the relationship between objects, the relationship between people and objects, and the semantic reasoning of people's behavior in the scene, and store them in the form of semantic tags by time.

[0054] The technological advancements achieved by this invention due to the adoption of the above technical solutions are as follows:

[0055] 1) The multi-CPU parallel architecture is used to synchronously realize the geometric map construction, object recognition and action recognition of the scene. The map is classified and built according to the different attributes of the environment and target objects, realizing the map expression of coarse and fine granularity fusion. Static map reconstruction is performed on walls and objects with low dynamic probability, which can effectively reduce data redundancy.

[0056] 2) The Manhattan hypothesis is used to estimate the camera rotation, which effectively reduces the camera pose drift problem caused by inter-frame estimation; the synchronous extraction of point and line features can effectively improve the efficiency of SLAM feature processing.

[0057] 3) Incremental learning is used to retrain and dynamically update the target recognition model and human behavior recognition model, so as to achieve high-accuracy target recognition and behavior recognition in the private working environment where the robot is located. While improving the robot's long-term perception ability in complex environments, it also ensures the robot's ability to protect privacy in private environments such as homes.

[0058] 4) The reconstruction of dynamic maps takes into account human behavior in the scene, models human actions according to the timeline, describes and infers human postures or human-object interaction behavior, improves the human-robot integration capability of indoor robots, and thus better serves people. Attached Figure Description

[0059] Figure 1 This is a connection diagram of the algorithm modules of this invention;

[0060] Figure 2 This invention is a vision-based 4D semantic SLAM framework for indoor mobile robots;

[0061] Figure 3 This invention is an incremental learning model for target recognition;

[0062] Figure 4 This invention is an incremental learning model for behavior recognition. Detailed Implementation

[0063] The main objective of this invention is to disclose a vision-based 4D semantic SLAM method for indoor mobile robots. Using the TRACER robot, four parallel modules are established through an NVIDIA Jetson Orin NX high-performance edge processor installed on the robot: target recognition, behavior recognition, point-line feature fusion odometry, and loop closure detection. The model trained by an incremental learning trainer enables the mobile robot to recognize targets in its surroundings and human behavior, performs robot pose tracking, and constructs an online 4D dynamic semantic map. Long-term perception of the robot's operational status is achieved by dynamically updating the 4D semantic map and sample database.

[0064] The present invention will be further described in detail below with reference to embodiments:

[0065] like Figure 1 and 2 As shown, a vision-based 4D semantic SLAM method for indoor mobile robots includes the following steps:

[0066] Step 1: Use the Oak-D stereo camera to acquire RGB image data, depth data, and human skeleton data in real time. The human skeleton data is obtained by inputting video data composed of RGB image data into the Oak-D stereo camera's built-in skeleton extraction model.

[0067] Step 2: Construct the behavior dataset and target dataset, create an incremental learning trainer, and pre-train the basic target recognition model and behavior recognition model based on the basic dataset and the personalized dataset. Then, based on the RGB images and human skeleton data obtained in Step 1, implement online dynamic incremental training to achieve dynamic online updates of the target recognition model, behavior recognition model, behavior dataset, and target dataset.

[0068] The specific steps are as follows:

[0069] Step 2.1: Construct the behavior dataset and the target dataset.

[0070] The behavior dataset and the target dataset each contain a basic dataset, a personalized dataset, and a dynamically extended dataset. That is, the behavior dataset includes a basic behavior dataset, a personalized behavior dataset, and a dynamically extended behavior dataset; the target dataset includes a basic target dataset, a personalized target dataset, and a dynamically extended target dataset.

[0071] The basic dataset consists of common human behaviors and indoor objects collected from the Internet or public datasets without infringing on privacy; the personalized dataset is a small sample dataset of objects or behaviors labeled by the user before the robot runs for the first time in a personalized scene; the dynamically expanded dataset is a dataset that is dynamically maintained during the long-term operation of the robot, and new samples are continuously added to this dataset as the robot runs in the scene.

[0072] Step 2.2: Create an incremental learning model for target recognition in the incremental learning trainer, such as... Figure 3 As shown. First, based on the target dataset, an attention structure combining Convolutional Attention Module (CBAM) and Deformable Transformer Attention is used for offline training, or other target detection models such as the YOLO series are used for training to obtain the target recognition initialization model O. 0 Next, the image information obtained in step 1 is input into the target recognition model O. t-1The target recognition result is obtained in (t>0). Set T... o The confidence threshold for target identification results is defined as follows: qualified samples should meet the confidence level (≥T). o ), T o The value of should satisfy:

[0073]

[0074] Where S T S represents the training dataset consisting of a basic dataset and personalized datasets. t This represents the accumulated qualified RGB image data. The target recognition incremental learning model labels the qualified data in the test with predicted boxes and generates a new dynamically expanded target dataset. It then determines whether the dataset size exceeds the set threshold Ma = 10. If this condition is met, the dynamically expanded target dataset is input into the target recognition model O. t-1 A second training is performed to obtain the target recognition model O. t If the model accuracy improves, then the original target recognition model O... t-1 Replace with target recognition model O t Otherwise, stop training.

[0075] Step 2.3: Create an incremental learning model for behavior recognition in the incremental learning trainer, such as... Figure 4 As shown. First, based on the behavior dataset, a spatiotemporal graph is constructed, with skeleton joints as graph "nodes" and joint connectivity and time as graph "edges". The joint coordinate vectors on the graph nodes are input into the ST-GCN spatiotemporal graph convolutional network for offline training, thereby gradually generating higher-level feature maps on the graph. Finally, a standard Softmax classifier is used to classify them into the corresponding behavior categories and record the behavior label. Alternatively, other models such as Slowfast can be used to train and obtain the behavior recognition initialization model B. 0 Next, the human skeleton data obtained in step 1 is input into the behavior recognition model B. t-1 The behavior recognition result is obtained in (t>0). Set T... B The confidence threshold for behavior recognition results is defined as follows: qualified samples should meet the confidence level (≥T). B ), T B The value of should satisfy:

[0076]

[0077] Where S B S represents the training dataset consisting of a basic dataset and personalized datasets. bThis represents the accumulated qualified skeleton data. The behavior recognition incremental learning model labels the qualified data in the test and generates a new dynamic behavior expansion dataset. It then determines whether the number of datasets exceeds the set threshold Ma = 10. If this condition is met, the dynamic behavior expansion dataset is input into the behavior recognition model B. t-1 A second training step is performed to obtain behavior recognition model B. t If the model accuracy improves, then the original behavior recognition model B will be... t-1 Replace with behavior recognition model B t Otherwise, stop training.

[0078] Step 3: Establish three parallel processing modules: a visual odometry module, a target recognition module, and a motion recognition module. The visual odometry module extracts and tracks point and line features from the RGB images and depth data obtained in Step 1. The target recognition module inputs the RGB images obtained in Step 1 into the target recognition model pre-trained by the incremental learning trainer in Step 2 to achieve target detection and target bounding box position information. The motion recognition module obtains joint position information and minimum bounding box position information based on the skeleton data obtained in Step 1, and inputs it into the behavior recognition model pre-trained by the incremental learning trainer in Step 2 to achieve human behavior recognition. The specific steps are as follows:

[0079] Step 3.1: Visual Odometry Module. First, points and lines are extracted from the RGB image data obtained in Step 1. Specifically, this includes: using the EDrawing algorithm to obtain the set of edge chains E = {e1, e2, ..., e...} of the input RGB image. m Then, the edge direction code of E is calculated according to the OTLine algorithm, and it is segmented into a corner point set P = {p1, p2, ..., p}. np} and the set of linear edge chains L = {L1, L2, ..., L l The specific steps for constructing point and line features are as follows: Maximum suppression is applied to the corner point set P, and its local pseudo-curvature is used to construct the feature point response values. To avoid feature points being too concentrated and affecting camera pose calculation, a quadtree method is further used to obtain a uniformly distributed feature point set KP = {kp1, kp2, ..., kp...}. nk For a set L of straight edge chains, construct a straight feature set KL = {kL1, kL2, ..., kL...} based on features such as line segment length and angle. nl}

[0080] Step 3.2: Target Recognition Module. Using the target recognition model pre-trained by the incremental learning trainer in Step 2, obtain the category information of the target object and the coordinate information of the bounding box;

[0081] Step 3.3: Behavior Recognition Module. Based on the joint positions of the human skeleton data obtained in Step 1, calculate its minimum bounding box region information. Using the behavior recognition model pre-trained by the incremental learning trainer in Step 2, obtain the behavior category and record the behavior label.

[0082] Step 4: Based on the target detection bounding box position information and the skeleton minimum bounding box position information from Step 3, obtain the dynamic and static probabilities of features in the scene. Further filter the point and line features extracted by the visual odometry module in Step 3 to delete dynamic features and merge static features. Update the current frame information, including target information, key points, key lines, map points, line pairs, and their co-view relationships. Further calculate the pose based on the pose estimation submodule and perform local optimization. The specific steps are as follows:

[0083] Step 4.1: During the camera pose initialization stage, if no person appears in the image, the features contained in the target detection box location region are all considered static features by default. If a person appears in the image, the features contained in the minimum bounding box region of the skeleton data are considered dynamic features. Dynamic features are deleted, and camera pose estimation is performed based on the static features.

[0084] Step 4.2: During the camera tracking stage, the dynamic probability of the target object is evaluated based on its attributes. The evaluation results are divided into three categories: static, semi-static, and dynamic. For background objects and large objects, whose detection boxes occupy a large area of ​​the image (e.g., wardrobes, sofas), they are considered static objects and assigned a low dynamic probability. For smaller objects, the dynamic probability of the target object can be further evaluated based on whether it interacts with a person. If a person is present in the current image, the three-dimensional spatial distance between the person's hand joint and the center of gravity of the smaller object in the image is calculated. Two distance thresholds are defined: When the distance is satisfied, the object has a very high probability of interacting with a person and is considered a dynamic object, assigned a high dynamic probability; when the distance is satisfied, the object has a low probability of interacting with a person and is considered a semi-static object, assigned a medium dynamic probability; when the distance is satisfied, the object has a very low probability of interacting with a person and is considered a static object, assigned a low dynamic probability. If no person is present in the current image, all objects in the image are considered static objects and assigned a low dynamic probability. The target detection box location region of semi-static objects is regarded as the feature to be filtered, and features with dynamic probability and features contained in the minimum bounding box region of skeleton data are eliminated.

[0085] Step 4.3: Features of the minimum bounding box region of potential dynamic objects and skeleton data are the features to be screened. Objects with a high probability of dynamic motion are potential dynamic objects. Several feature points are sampled, and the distances of the endpoints of the selected feature points and feature lines relative to the epipolar lines are calculated using the reliable pose information of static points and lines. Feature points that do not meet the epipolar constraints are classified as dynamic points. For line features, if one endpoint does not meet the limit constraint, the entire line is a dynamic line feature. Then, based on the principle that points on the same object will not have two states, all feature points and lines are removed. Features of the minimum bounding box region of the skeleton data are all screened. Features that do not meet the epipolar constraints are classified as dynamic features, and then the remaining features are merged with the static features.

[0086] Dynamic feature removal can be performed using the following limit constraints, or other similar dynamic feature removal methods such as optical flow can be used:

[0087] Firstly, the epipolar geometric constraints We can obtain:

[0088]

[0089] make The above formula can be transformed into:

[0090]

[0091] Therefore, only 8 pairs of static points are needed to solve for the essential matrix E. Then, using SVD singular value decomposition, the poses (R1, t1) or (R2, t2) of the points to be selected can be obtained:

[0092] or

[0093] Where R is the rotation matrix and t is the translation vector, the points with a depth greater than 0 are the poses of the points to be selected. The epipolar line l1 = [X, Y, Z] of point x1 is obtained from the essential matrix E. T =Ex1. The formula for the distance from point x2 to the polar line l1 is:

[0094]

[0095] If D is greater than the threshold D th Then it is considered a dynamic point; if D is less than the threshold D th It is then considered a static point.

[0096] Step 4.4: After removing dynamic features, the indoor environment is modeled as a Manhattan world, and the 6D camera pose is decoupled into rotation and translation. Camera pose estimation is performed based on static features. First, the rotation matrix R from the Manhattan world MW to the camera k frames is calculated according to the Manhattan hypothesis.k,MW Thus, the rotation matrix R between n frames and m frames of the camera is obtained. m,n Next, using static feature points and lines to estimate the camera's 3D translation motion, the 3D points and lines are reprojected from the last frame to the current frame. This can be achieved through the following steps: Obtain the feature point matching set MP = {(p...} m1 ,p n1 ),...,(p mk ,p nk )}, where k is the number of matching point pairs, and the set of line matching ML = {(L m1 ,L n1 ...(L) mt ,L nt )}, where t is the number of matched line pairs;

[0097] Calculate the reprojection error of static feature points. Based on the static feature point p in the previous frame... n The depth information is used to obtain the position information of its 3D point P. n =[X ni ,Y ni Z ni ] T Then it is projected onto the current frame, and the reprojection error of the feature points that match it is calculated. Defined as:

[0098]

[0099] Where p m Let P be a static feature point in the m-th frame. n Let R be the three-dimensional spatial coordinates of the static feature point in the nth frame. m,n and t m,n Let be the camera rotation matrix and translation matrix from frame n to frame m, respectively, and Π(·) be the projection function.

[0100] Calculate the reprojection error of the static straight line feature. For the straight line L in the nth frame... n The corresponding matching line in the m-th frame is L. m For line L n Five pixels are randomly sampled, including the endpoints of the line and the midpoint of the line segment. The validity of each pixel is determined based on depth information. If any pixel has a depth less than 0, the sample is resampled. Then, the position information set of the 3D point set is obtained based on the camera intrinsic parameters. in Then, by projecting it onto the current frame m, we can obtain its reprojection point set. Construct a line error function based on the distance relationship from a point to a line:

[0101]

[0102] in For line L m The vector representation of Π(·) is the projection function. Therefore, the least squares cost of a point and a line can be written as:

[0103]

[0104] Where ρ p With ρ l The Huber cost functions are given for points and lines, respectively, and are ultimately solved using the Levenberg-Marquardt algorithm.

[0105] Step 5: Based on the target detection results obtained by the target recognition module in Step 3 and the point and line features obtained by the visual odometry module in Step 3, perform loop closure frame detection. If a loop closure frame is detected, perform loop closure correction and global BA operation. The specific steps are as follows:

[0106] Step 5.1: Record the object information contained in each frame of the image, and encode the object category and its corresponding dynamic probability for that frame of the image.

[0107] Step 5.2: If the current frame and the past keyframes have the same image category, use bag-of-words to accelerate matching and determine whether loop closure occurs; if the current frame does not find a case with the same category as the past keyframes, remove the object categories with high dynamic probability in the scene and then compare the categories. If they are the same, use the features of the remaining objects of the same category to perform loop closure detection. This process can make loop closure detection more robust.

[0108] Step 6: Construct a dynamic semantic map description, establishing a cognitive semantic map including a grid layer, a behavior layer, and a semantic layer to achieve a global 4D semantic map representation for the robot during long-term indoor operation. A 3D grid map is built based on the dynamic probabilities of the environment and objects. Objects and the indoor environment are described using a combination of coarse and fine-grained 3D grid maps; the human body is described using a 3D human skeleton; objects with low long-term dynamic probabilities and walls are represented by coarser static grid maps, while human posture and human-object interactions are represented by fine-grained dynamic grid maps, with real-time map updates; the behavior layer describes the long-term motion trajectories of targets, humans, and the robot over time; the semantic layer establishes semantic labels based on target detection results, behavior recognition results, and the grid layer map, used to describe the current state of the scene and human behavior. Furthermore, based on the above information, it is combined with an indoor scene semantic understanding network to infer the relationships between objects, the relationships between humans and objects, and the semantics of human behavior in the scene, displaying the results in the form of semantic labels. Specific steps are as follows:

[0109] Step 6.1: The entire mapping process, in chronological order, includes static mapping, dynamic mapping, and semantic mapping. Figure 3 The system is divided into several parts; static mapping includes mapping of static and semi-static objects, described through a 3D raster layer; dynamic mapping includes mapping of people and the dynamic poses of dynamic objects, described through a behavior layer; and semantic mapping includes semantic annotation and understanding of indoor scenes and people, described through a semantic layer.

[0110] Step 6.2: For static maps, including environmental background and target recognition objects. For background areas such as walls and floors, use depth information to segment and obtain planar instances. Organize points belonging to a plane into a kd-tree data structure, and then use the GST surface triangulation greedy algorithm to describe the planar region using a combination of coarse and fine granular grids based on the size of the planar region.

[0111] For objects identified by target detection, they are described in the raster layer as object centroids and 3D bounding boxes. These objects are then displayed on the map, and the current object's category, ID, and dynamic probability are stored. The specific 3D coordinates of the object centroid and the dimensions of the object's 3D bounding box are obtained through the following steps:

[0112] I. Obtain the 3D coordinates P of the feature points contained in the object detection bounding box. i =[X i ,Y i Z i ], where i = 1, ..., N O N O The number of feature points contained in the object gives the three-dimensional coordinates P of the object's centroid. m :

[0113]

[0114] II. Statistically determine the maximum X-axis value of feature points contained in the bounding box of an object. max Y max Z max Coordinate values ​​and minimum X min Y min Z min Given the coordinate values, the length L of the object's 3D bounding box is... O Width W O High H O It can be calculated using the following formula:

[0115]

[0116] Step 6.3: For dynamic maps, this includes camera motion trajectories and human skeleton data. When someone appears in the scene, their skeleton data is displayed over time, along with their corresponding action label.

[0117] Step 6.4: For the semantic map, store the category, ID, and dynamic probability of the current object based on the corresponding 3D position of the object in each frame; store the person ID, action category, and dynamic probability based on the recognized human skeleton and action recognition results; based on the object, human skeleton, and action category data over a period of time, combine the indoor scene semantic understanding network to infer the relationship between objects, the relationship between people and objects, and the semantic reasoning of people's behavior in the scene, and store them in the form of semantic tags by time.

[0118] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made by those skilled in the art to the technical solutions of the present invention without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A vision-based 4D semantic SLAM method for indoor mobile robots, characterized in that: Includes the following steps: Step 1: Acquire RGB image data, depth data, and human skeleton data in real time; Step 2: Construct behavior datasets and target datasets, including a basic dataset, a personalized dataset, and a dynamically expanded dataset, respectively. Create an incremental learning trainer to pre-train the basic target recognition model and behavior recognition model based on the basic dataset and the personalized dataset. Then, based on the RGB image data and human skeleton data obtained in Step 1, implement online dynamic incremental training to achieve dynamic online updates of the target recognition model, behavior recognition model, behavior dataset, and target dataset. Step 3: Establish three parallel processing modules: visual odometry module, target recognition module, and action recognition module; the visual odometry module is used to extract and track point and line features from the RGB image and depth data obtained in Step 1; the target recognition module includes inputting the RGB image obtained in Step 1 into the target recognition model pre-trained by the incremental learning trainer in Step 2 to realize target detection and target detection box position information. The action recognition module includes obtaining joint position information and skeleton minimum bounding box position information based on the skeleton data obtained in step 1, and inputting them into the behavior recognition model pre-trained by the incremental learning trainer in step 2 to realize human behavior recognition. Step 4: Based on the target detection box position information and the skeleton minimum bounding box position information in Step 3, obtain the dynamic and static probabilities of the features in the scene respectively. Further filter the point and line features extracted by the visual odometry module in Step 3 to realize the deletion of dynamic features and the merging of static features. Update the current frame information, including target information, key points, key lines, map points, line pairs and their co-view relationships. Further calculate the pose based on the pose estimation submodule and perform local optimization. Step 5: Based on the target detection results obtained by the target recognition module in Step 3 and the point and line features obtained by the visual odometry module in Step 3, perform loop closure frame detection. If a loop closure frame is detected, perform loop closure correction and global BA operation. Step 6: Construct a dynamic semantic map description, and establish a cognitive semantic map including a grid layer, a behavior layer, and a semantic layer to realize the global 4D semantic map expression of the robot during long-term indoor operation.

2. The vision-based 4D semantic SLAM method for indoor mobile robots according to claim 1, characterized in that: The RGB image data, depth data, and human skeleton data in step 1 are acquired directly in real time using an RGBD camera or a binocular camera.

3. The 4D semantic SLAM method for a vision-based indoor mobile robot according to claim 1, characterized in that: The specific steps for step 2 are as follows: Step 2.1: Construct a behavioral dataset that includes a basic behavioral dataset, a personalized behavioral dataset, and a dynamically extended behavioral dataset, as well as a target dataset that includes a basic target dataset, a personalized target dataset, and a dynamically extended target dataset; The basic dataset consists of common basic human behaviors and common indoor objects collected from the Internet or public datasets without involving privacy; the personalized dataset consists of a small sample dataset of objects or behaviors labeled by the user before the robot runs for the first time in a personalized scene; the dynamically expanded dataset is a dataset that is dynamically maintained by the robot during long-term operation, with new samples added as the robot runs in the scene. Step 2.2: Create the target recognition incremental learning model in the incremental learning trainer. First, train the target recognition initialization model O based on the target base dataset. 0 Next, the image information obtained in step 1 is input into the target recognition model O. t -1 The target recognition result is obtained in (t>0); set T o The confidence threshold for target identification results is defined as follows: qualified samples should meet the confidence level (≥T). o ), T o The value of should satisfy: Where S T S represents the training dataset consisting of a basic dataset and personalized datasets. t This represents the accumulated qualified RGB image data; The incremental learning model for target recognition labels the data that meets the requirements in the test with predicted boxes and generates a new dynamically expanded target dataset. It then checks whether the size of the dataset exceeds a set threshold Ma = 10. If this condition is met, the dynamically expanded target dataset is input into the target recognition model O. t-1 A second training step is performed to obtain the target recognition model O. t If the model accuracy improves, then the original target recognition model O... t-1 Replace with target recognition model O t ; Otherwise, stop training; Step 2.3: Create the behavior recognition incremental learning model in the incremental learning trainer. First, train the behavior recognition initialization model B based on the behavior base dataset. 0 Next, the human skeleton data obtained in step 1 is input into behavior recognition model B. t-1 The behavior recognition result is obtained in (t>0); set T B The confidence threshold for behavior recognition results is defined as follows: qualified samples should meet the confidence level (≥T). B ), T B The value of should satisfy: Where S B S represents the training dataset consisting of a basic dataset and personalized datasets. b This represents the cumulative amount of qualified skeleton data obtained. The behavior recognition incremental learning model labels the data that meets the requirements in the test and generates a new dynamic behavior expansion dataset. It then determines whether the size of the dataset exceeds a set threshold Ma = 10. If this condition is met, the dynamic behavior expansion dataset is input into the behavior recognition model B. t-1 A second training iteration is performed to obtain behavior recognition model B. t If the model accuracy improves, then the original behavior recognition model B will be... t-1 Replace with behavior recognition model B t ; Otherwise, stop training.

4. The vision-based 4D semantic SLAM method for indoor mobile robots according to claim 1, characterized in that: The specific steps for step 3 are as follows: Step 3.1: Establish the visual odometry module: First, points and lines are extracted from the RGB image data obtained in step 1. Specifically, this includes using the EDrawing algorithm to obtain the set of edge chains E = {e1, e2, ..., e...} of the input RGB image. m Then, the edge direction code of E is calculated according to the OTLine algorithm, and it is segmented into a corner point set P = {p1, p2, ..., p}. np } and the set of linear edge chains L = {L1, L2, ..., L l }; Then, point and line features are constructed, specifically including: performing maximum suppression on the corner point set P and using its local pseudo-curvature to construct the feature point response values; to avoid feature points being too concentrated and affecting camera pose calculation, a quadtree method is further used to obtain a uniformly distributed feature point set KP = {kp1,kp2,...,kp...}. nk For a set L of straight edge chains, construct a set of straight line features KL = {kL1, kL2, ..., kL...} based on the line segment length and angle characteristics. nl }; Step 3.2: Establish the target recognition module: Use the target recognition model pre-trained by the incremental learning trainer in step 2 to obtain the category information of the target object and the coordinate information of the bounding box; Step 3.3: Establish the behavior recognition module: Based on the joint positions of the human skeleton data obtained in step 1, calculate its minimum bounding box region information; use the behavior recognition model pre-trained by the incremental learning trainer in step 2 to obtain the behavior category and record the behavior label.

5. The vision-based 4D semantic SLAM method for indoor mobile robots according to claim 1, characterized in that: The specific operations for step 4 are as follows: Step 4.1: During the camera pose initialization stage, if no person appears in the image, the features contained in the target detection box location area are all assumed to be static features during this stage. If a person is present in the image, the features contained in the minimum bounding box region of the skeleton data are dynamic features; remove the dynamic features and perform camera pose estimation based on the static features. Step 4.2: During the camera tracking stage, the dynamic probability of the target object is evaluated based on its attributes. The evaluation results are divided into three types: static, semi-static, and dynamic. For background objects and large objects, whose detection boxes occupy a large area in the image, they are considered static objects and assigned a low dynamic probability. For smaller objects, the dynamic probability of the target object can be further evaluated based on whether it interacts with a person. If a person is present in the current image, the three-dimensional spatial distance between the person's hand joint and the center of gravity of the smaller object in the image is calculated. Two distance thresholds are defined at this point. When the distance is satisfied, the object is highly likely to be associated with a person. Interactive behavior is considered dynamic and assigned a high dynamic probability. When the distance is satisfied, the object has a low probability of interacting with a person and is considered a semi-static object, assigned a medium dynamic probability. When the distance is satisfied, the object has a very low probability of interacting with a person and is considered a static object, assigned a low dynamic probability. If there is no person in the current image, all objects in the image can be considered static objects and assigned a low dynamic probability. The target detection box location region of semi-static objects is considered as the feature to be filtered, and features with a dynamic probability of 0 and features contained in the minimum bounding box region of the skeleton data are removed. Step 4.3: The features of the minimum bounding box region of the potential dynamic object and skeleton data are the features to be screened; Objects with a high probability of dynamic movement are considered potential dynamic objects. Several feature points are sampled, and the distances between the endpoints of the selected feature points and feature lines and the epipolar lines are calculated using the reliable pose information of static points and lines. Feature points that do not conform to the epipolar constraints are classified as dynamic points. For selected line features, if one endpoint does not conform to the limit constraint, the entire line is considered a dynamic line feature. Then, based on the principle that points on the same object will not have two states, all feature points and lines are removed. Features of the minimum bounding box region of the skeleton data are all screened. Features that do not conform to the epipolar constraints are classified as dynamic features. The remaining features are then merged with the static features. Step 4.4: After removing dynamic features, the indoor environment is modeled as the Manhattan world, and the 6D camera pose is decoupled into rotation and translation. Finally, the camera pose is estimated based on the static features. First, based on the Manhattan hypothesis, calculate the rotation matrix R between the Manhattan world (MW) and the camera (k frames). k,MW Thus, the rotation matrix R between n frames and m frames of the camera is obtained. m,n Next, using static feature points and lines to estimate the camera's 3D translation motion, the 3D points and lines are reprojected from the last frame to the current frame. An error function is defined based on the reprojection error, where the error function for static line features can be calculated using the following steps: For line L in the nth frame... n The corresponding matching line in the m-th frame is L. m ; make straight line L n Random sampling is performed to obtain ni pixels. Pixels with a depth greater than 0 are retained based on depth information. The position information set of the 3D point set is obtained based on camera intrinsic parameters. Where P li n =[X li ,Y li Z li ] T ; Then, by projecting it onto the current frame m, we can obtain its reprojection point set. Construct a line error function based on the distance relationship from a point to a line: in For line L m The vector representation of the vector is given by Π(·), where Π(·) is the projection function; therefore, the least squares cost of a point and a line can be written as: Where ρ p With ρ l The Huber cost functions are given for points and lines, respectively, and are ultimately solved using the Levenberg-Marquardt algorithm.

6. The vision-based 4D semantic SLAM method for indoor mobile robots according to claim 1, characterized in that: The specific operation of step 5 is as follows: Step 5.1: Record the object information contained in each frame of the image, and encode the object category and its corresponding dynamic probability for that frame of the image; Step 5.2: If the current frame and the past keyframes have the same image category, use bag-of-words to accelerate matching and determine whether loop closure occurs; if the current frame does not find a case with the same category as the past keyframes, remove the object categories with high dynamic probability in the scene and then compare the categories. If they are the same, use the features of the remaining objects of the same category to perform loop closure detection. This process can make loop closure detection more robust.

7. The vision-based 4D semantic SLAM method for indoor mobile robots according to claim 1, characterized in that: Step 6 establishes a 3D grid map based on the dynamic probabilities of the environment and objects. Objects and the indoor environment are described using a combination of coarse and fine-grained 3D grid maps; the human body is described using a 3D human skeleton; objects and walls with low long-term dynamic probabilities are represented by coarser static grid maps, while human postures and human-object interactions are represented by fine-grained dynamic grid maps, with real-time map updates; the behavior layer describes the long-term motion trajectories of targets, humans, and robots over time; the semantic layer establishes semantic tags based on target detection results, behavior recognition results, and grid map layers, used to describe the current state of the scene and human behavior, and further, based on the above information, it combines with an indoor scene semantic understanding network to infer the relationships between objects, the relationships between humans and objects, and the semantics of human behavior in the scene, displaying them in the form of semantic tags; The specific steps are as follows: Step 6.1: The entire mapping process consists of three parts in chronological order: static mapping, dynamic mapping, and semantic mapping. Static mapping includes mapping static and semi-static objects, described by a 3D raster layer. Dynamic mapping includes mapping the dynamic poses of people and dynamic objects, described by a behavior layer. Semantic mapping includes semantic annotation and understanding of indoor scenes and people, described by a semantic layer. Step 6.2: For static maps, including environmental background and target recognition objects; for background areas, use depth information to segment and obtain planar instances, and then describe them at the raster layer according to the size of the planar area using a combination of coarse and fine granular gratings; For objects identified by target detection, they are described in the form of object centroid and 3D bounding box in the grid layer, and the current object's category, ID and dynamic probability are stored. Step 6.3: For dynamic maps, including camera motion trajectories and human skeleton data, when a person appears in the scene, the skeleton data is displayed in the time dimension, along with the corresponding action label. Step 6.4: For the semantic map, store the category, ID, and dynamic probability of the current object based on the corresponding 3D position of the object in each frame; store the person ID, action category, and dynamic probability based on the recognized human skeleton and action recognition results. Based on object, human skeleton, and action category data over a period of time, an indoor scene semantic understanding network is used to infer the relationships between objects, the relationships between people and objects, and the semantics of human behavior in the scene, and the data is stored in the form of semantic tags over time.

Images