Semantic-based robot simultaneous localization and mapping technology research method

Abstract

The application discloses a kind of based on semantic medical robot synchronous positioning and mapping technical research method, method includes: obtaining the environment around robot;According to the environment information of surrounding, robot positioning is carried out;The map of current environment is constructed;According to the feature of different objects, medical staff or medical equipment is segmented;Optimized map information is constructed;Design based on semantic medical robot positioning and mapping;On the basis of ORB-SLAM3, semantic information is added.The method provided by the application comprehensively considers the flow of medical staff and patient under complex hospital environment, and the use of semantic information can also enable medical robot to identify medical equipment, lay the foundation for executing other higher-level tasks.

Description

Technical Field

[0001] This invention belongs to the field of medical robot technology, specifically relating to a research method for semantic-based synchronous localization and mapping technology of medical robots. Background Technology

[0002] Traditional SLAM algorithms for AGV robots only solve the localization and mapping problems in static environments. However, when there are many moving objects in the environment, the AGV robot's localization will produce significant errors, or even lose tracking. In practical applications, AGV robots cannot operate entirely in static environments, so SLAM algorithms are particularly important in dynamic environments. Furthermore, traditional SLAM algorithms only provide AGV robots with geometric information about the environment; that is, AGV robots can only acquire the positional information of obstacles in the environment. Using this information, AGV robots can complete fixed route planning, but they cannot use environmental information to perform more advanced navigation tasks.

[0003] With the continuous development of technology, robots are widely used in modern society and have shown enormous potential. Especially in the medical field, the application of robots has become a trend. However, for medical care robots, the first step in participating in the treatment and care process is to accurately locate their position in the ward and perceive the surrounding environment. But when faced with a more complex or constantly changing environment, especially when medical care robots are dealing with a large number of medical staff and patients moving around the ward, the hospital environment is relatively complex. This leads to higher requirements for the SLAM positioning accuracy of medical care robots.

[0004] For mobile nursing service robots, the ability to locate and perceive their environment is the core foundation for them to perform other functions. The localization and environmental perception of mobile nursing robots rely on SLAM technology. The accuracy of the SLAM algorithm's localization and mapping directly determines the robot's ability to accurately determine its own position and perform assisted nursing tasks for the elderly. Moving objects in the environment can significantly interfere with the SLAM localization and mapping process. Therefore, most current research on SLAM algorithms assumes the absence of moving objects in the environment. However, this assumption is unrealistic in the real world. Summary of the Invention

[0005] Purpose of the invention: In order to solve the problems existing in the prior art, the present invention provides a research method for semantic-based synchronous localization and mapping technology of medical robots.

[0006] Technical Solution: This invention provides a semantic-based method for simultaneous localization and mapping (SLAM) of medical robots. The method is implemented using an image preprocessing module, an IMU pre-integration module, and an image feature point extraction module.

[0007] Specifically, the steps include the following:

[0008] Step 1: Obtain the robot's location coordinates. The location coordinates are obtained by localization using the SLAM algorithm to obtain the robot's current pose, and by obtaining image depth information and image information using the SALM algorithm.

[0009] Step 2: Initialize the robot pose and obtain the feature points of the map based on the current map frame. If there are too few feature points in the current frame, reconstruct the initializer. When the number of feature points in two consecutive frames is greater than 100, continue the initialization process.

[0010] Step 3: Obtain the semantic information of the map frames and perform semantic segmentation on the map using the SAM semantic segmentation model. SAM is an interactive segmentation framework that generates segmentation results based on a given prompt. The generation process is represented as follows: SAM uses a pre-trained mask autoencoder based on Vision Transformer to process the image into intermediate features and encodes the previous prompt into embedding tokens. Subsequently, a cross-attention mechanism in the mask decoder facilitates the interaction between the image features and the prompt embeddings, ultimately producing the mask output.

[0011] Step 4: Create IMU pre-integration for the current frame and from the previous keyframe. Obtain the corresponding matching relationship between the initial frame and the current frame through feature matching. Initialize using the H model and F model, where the H model uses the homography matrix H. 21 To describe the transformation relationship between feature point pairs p1 and p2, p2 = H 21 *p1, F model: The model is a feature point pair p1, p2 using the fundamental matrix F. 21 To describe the transformation relationship between feature point pairs p2 T F 21 p1=0 is used to obtain the relative motion between the two frames and initialize map points;

[0012] Step 5: Extract the semantic information of the map frame based on Step 3, such as doctors and patients. Obtain the feature points of the map frame based on Step 4. Delete the feature points on the doctors or patients using the SAM algorithm and delete the matching points that cannot be triangulated. Then, generate 3D points through triangulation and then generate map points. Send the map points to the LOCALMAPPING thread of SLAM.

[0013] Step 6: The thread completes the three-stage initialization of the IMU and processes the keyframes in the queue.

[0014] Step 7: Obtain the newly added map points to be checked. Set different observation thresholds according to the camera type. Remove map points whose observation values ​​exceed the threshold based on the observation data. Let the probability distribution (cumulative distribution function) be α = F(x). Given an α, an interval [0, F...] can be defined. -1 (α)]. We can consider γ as an interior point if it falls within this interval, and an exterior point if it falls outside this interval. F -1 (α) is the threshold to be found. Then new map points are generated to make the tracking more stable, and finally, the redundancy of co-view keyframes in the current keyframe is checked;

[0015] Step 8: Add the current keyframe to the loop closure detection queue. By calculating the similarity score between the current keyframe and the keyframes in the keyframe database, candidate keyframes with possible loop closure or sub-map fusion can be obtained. Perform geometric similarity verification and temporal geometric verification on the candidate keyframes.

[0016] Step 9: Determine whether there is a loop or map fusion possibility in the current area based on the verification results, and perform loop correction or map fusion according to different situations.

[0017] Furthermore, the H model in step 4 can be written in matrix form.

[0018]

[0019] Expanding the calculation yields

[0020] υ2=(h4*u1+h5*υ1+h6) / (h7*u1+h8*υ1+h9)

[0021] υ2=(h1*u1+h2*υ1+h3) / (h7*u1+h8*υ1+h9)

[0022] Transform into matrix form

[0023]

[0024] Let A and X represent the two terms on the left side of the equation, respectively. Then, AX = 0 provides two constraint equations for each pair of points. The homography matrix H has a total of 9 elements and 8 degrees of freedom (scale equivalence), so 4 pairs of points are needed. The 8 constraint equations can then be solved.

[0025] The F-model in step 4 can be written in matrix form:

[0026]

[0027] Expanding, we get:

[0028] f1*u1*u2+f2*υ1*u2+f3*u2+f4*u1+υ2+f5*υ1*υ2+f6*υ2+f7*u1+f8*υ1+f9=0

[0029] Transform into matrix form:

[0030]

[0031] Let A and f represent the two terms on the left side of the equation, then we have

[0032] Af = 0

[0033] Each pair of points provides a constraint equation. The fundamental matrix F has a total of 9 elements and 7 degrees of freedom (scale equivalence, the intrinsic property of the essential matrix E is rank 2). Therefore, 8 pairs of points provide 8 constraint equations to solve F.

[0034] Furthermore, the triangulation and map point generation in step 5 specifically involve:

[0035] First, feature points are matched: projection matrices are applied to x, x', P, and P' respectively, projecting the same spatial point X onto the pixel coordinates of F1 and F2 respectively.

[0036] x=λ*P*X

[0037] x′=λ*P′*X

[0038] The two expressions are similar, and we describe them using a general equation:

[0039]

[0040] For ease of derivation, let's simply call it as

[0041]

[0042] To transform it into a homogeneous equation, we cross-multiply both sides simultaneously, obtaining...

[0043]

[0044]

[0045]

[0046]

[0047] The third row is equivalent to the linear relationship of the first two rows, therefore only the first two rows are needed. Also, since a pair of matching relationships has two points, F1 and F2, then:

[0048]

[0049] Let A and X represent the two terms on the left side of the equation, respectively. Then we have AX = 0. Solving by singular value decomposition (SVD), the last row of the right singular matrix is ​​the final solution.

[0050] The LOCAL MAPPING thread works by taking keyframes as input, removing useless keypoints, initializing with local BA and IMU, removing local useless keyframes, and refining the scale of the IMU.

[0051] Furthermore, step 6 involves processing keyframes in the queue, including computing the bag-of-words (BOW) model, updating observations, descriptors, co-views, and inserting them into the map.

[0052] Further, step 8, loop closure detection, specifically involves: detecting whether there are common words between keyframes and previously created keyframes. If so, further analysis is conducted to determine whether the detected shared keyframes belong to other submaps or the current submap. If the detected shared keyframes belong to other submaps, the two submaps are merged. If the detected shared keyframes belong to the current submap, a loop closure relationship may exist. If a loop closure relationship exists, the similarity transformation between the loop closure keyframes is calculated to obtain the degree of drift. If the similarity transformation calculation is successful, the cumulative error at the loop closure is corrected based on the degree of drift.

[0053] Furthermore, the SAM semantic segmentation model in step 3 includes: an image encoder module, a prompt encoder module, and a mask decoder module. The image encoder module maps the image to be segmented to the image feature space using an image encoder; based on a scalable and powerful pre-training method, it uses ViT pre-trained with MAE, which is minimally suitable for processing high-resolution inputs. The prompt encoder module maps the input prompt to the prompt's feature space. The mask decoder module integrates the two embeddings output by the image encoder and prompt encoder, respectively, and then decodes the final segmentation mask from the feature map of this embedding.

[0054] Furthermore, the image preprocessing module segments the movable object using the SAM algorithm and removes the movable object and its feature points; the image feature point extraction module creates an image pyramid using a feature extraction function, extracts features from each pyramid layer, performs uniformization processing on the features based on a quadtree, calculates the orientation of the fast feature points, and calculates the descriptors corresponding to the feature points; the IUM pre-integration module initializes the pose using an estimated integration method.

[0055] Beneficial Effects: This invention proposes a semantic-based method for simultaneous localization and mapping (SLAM) of medical robots. We compared our proposed method with current state-of-the-art methods using the public TUM RGB-D dataset. By adding a semantic segmentation thread before feature extraction, we can handle dynamic scene problems. Furthermore, our algorithm design is based on the best current visual SLAM algorithm, thus achieving an optimal balance between speed and accuracy. Attached Figure Description

[0056] Figure 1 This is a flowchart of a method according to an embodiment of the present invention.

[0057] Figure 2 This is a schematic diagram of the semantic segmentation SAM of the present invention. Detailed Implementation

[0058] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0059] like Figure 1 As shown, this embodiment provides a research method for semantic-based synchronous localization and mapping technology for medical robots. The method is implemented based on an image preprocessing module, an IMU pre-integration module, and an image feature point extraction module.

[0060] Specifically, the steps include the following:

[0061] Step 1: Obtain the robot's location coordinates. These coordinates are determined using the SLAM algorithm to locate the robot's current pose. The SLAM algorithm is then used to obtain image depth and image information. In this case, the medical robot is located in a hospital. The robot first needs to obtain the image depth information to determine its position.

[0062] Step 2: Initialize the robot pose and obtain the feature points of the map based on the current map frame. If there are too few feature points in the current frame, reconstruct the initializer. When the number of feature points in two consecutive frames is greater than 100, continue the initialization process.

[0063] Step 3: As Figure 2 As shown, semantic information of map frames is obtained, and semantic segmentation of the map is performed using the SAM semantic segmentation model. SAM is an interactive segmentation framework that generates segmentation results based on a given prompt. The generation process is represented as follows: SAM uses a pre-trained mask autoencoder based on Vision Transformer to process the image into intermediate features and encodes the previous prompt into embedding tokens. Subsequently, a cross-attention mechanism in the mask decoder facilitates the interaction between the image features and the prompt embeddings, ultimately producing the mask output.

[0064] Step 4: Create IMU pre-integration for the current frame and from the previous keyframe. Obtain the corresponding matching relationship between the initial frame and the current frame through feature matching. Initialize using the H model and F model, where the H model uses the homography matrix H. 21 To describe the transformation relationship between feature point pairs p1 and p2, p2 = H 21 *p1, F model: The model is a feature point pair p1, p2 using the fundamental matrix F. 21 To describe the transformation relationship between feature point pairs p2 T F 21 p1=0 is used to obtain the relative motion between the two frames and initialize map points;

[0065] In step 4, the H model is written in matrix form.

[0066]

[0067] Expanding the calculation yields

[0068] v2=(h4*u1+h5*υ1+h6) / (h7*u1+h8*υ1+h9)

[0069] u2=(h1*u1+h2*υ1+h3) / (h7*u1+h8*υ1+h9)

[0070] Transform into matrix form

[0071]

[0072] Let A and X represent the two terms on the left side of the equation, respectively. Then, AX = 0 provides two constraint equations for each pair of points. The homography matrix H has a total of 9 elements and 8 degrees of freedom (scale equivalence), so 4 pairs of points are needed. The 8 constraint equations can then be solved.

[0073] The F-model in step 4 can be written in matrix form:

[0074]

[0075] Expanding, we get:

[0076] f1*u1*u2+f2*υ1*u2+f3*u2+f4*u1+υ2+f5*υ1*υ2+f6*υ2+f7*u1+f8*υ1+f9=0

[0077] Transform into matrix form:

[0078]

[0079] Let A and f represent the two terms on the left side of the equation, then we have

[0080] Af = 0

[0081] Each pair of points provides a constraint equation. The fundamental matrix F has a total of 9 elements and 7 degrees of freedom (scale equivalence, the intrinsic property of the essential matrix E is rank 2). Therefore, 8 pairs of points provide 8 constraint equations to solve F.

[0082] Step 5: Extract the semantic information of the map frame based on Step 3, such as doctors and patients. Obtain the feature points of the map frame based on Step 4. Delete the feature points on the doctors or patients using the SAM algorithm and delete the matching points that cannot be triangulated. Then, generate 3D points through triangulation and then generate map points. Send the map points to the LOCALMAPPING thread of SLAM.

[0083] The triangulation and map point generation in step 5 specifically involve:

[0084] First, feature points are matched: projection matrices are applied to x, x', P, and P' respectively, projecting the same spatial point X onto the pixel coordinates of F1 and F2 respectively.

[0085] x=λ*P*X

[0086] x′=λ*P′*X

[0087] The two expressions are similar, and we describe them using a general equation:

[0088]

[0089] For ease of derivation, let's simply call it as

[0090]

[0091] To transform it into a homogeneous equation, we cross-multiply both sides simultaneously, obtaining...

[0092]

[0093]

[0094]

[0095]

[0096] The third row is equivalent to the linear relationship of the first two rows, therefore only the first two rows are needed. Also, since a pair of matching relationships has two points, F1 and F2, then:

[0097]

[0098] Let A and X represent the two terms on the left side of the equation, respectively. Then we have AX = 0. Solving by singular value decomposition (SVD), the last row of the right singular matrix is ​​the final solution.

[0099] The LOCALMAPPING thread involves taking keyframes as input, removing useless keypoints, initializing with local BA and IMU, removing local useless keyframes, and refining the scale of the IMU.

[0100] Step 6: The thread completes the three-stage initialization of the IMU and processes the keyframes in the queue.

[0101] Step 6 involves processing keyframes in the queue, including computing the bag-of-words (BOW) model, updating observations, descriptors, co-views, and inserting them into the map.

[0102] Step 7: Obtain the newly added map points to be checked. Set different observation thresholds according to the camera type. Remove map points whose observation values ​​exceed the threshold based on the observation data. Let the probability distribution (cumulative distribution function) be α = F(x). Given an α, an interval [0, F...] can be defined. -1 (α)]. We can consider γ as an interior point if it falls within this interval, and an exterior point if it falls outside this interval. F -1 (α) is the threshold to be found. Then new map points are generated to make the tracking more stable, and finally, the redundancy of co-view keyframes in the current keyframe is checked;

[0103] Step 8: Add the current keyframe to the loop closure detection queue. By calculating the similarity score between the current keyframe and the keyframes in the keyframe database, candidate keyframes with possible loop closure or sub-map fusion can be obtained. Perform geometric similarity verification and temporal geometric verification on the candidate keyframes.

[0104] Step 8, loop closure detection, specifically involves: detecting whether there are common words between keyframes and previously created keyframes. If so, further analysis is performed to determine whether the detected shared keyframes belong to other submaps or the current submap. If the detected shared keyframes belong to other submaps, the two submaps are merged. If the detected shared keyframes belong to the current submap, a loop closure relationship may exist. If a loop closure relationship exists, the similarity transformation between the loop closure keyframes is calculated to obtain the degree of drift. If the similarity transformation calculation is successful, the cumulative error at the loop closure is corrected based on the degree of drift.

[0105] The loop closure detection method is based on bag-of-words vectors. By calculating the similarity score between the current keyframe and the keyframes in the keyframe database, candidate keyframes with possible loop closures or sub-map fusion can be obtained. By performing geometric similarity verification and temporal geometric verification on the candidate keyframes, it is determined whether there is a loop closure or map fusion possibility in the current region based on the verification results, and loop closure correction or map fusion is performed according to different situations.

[0106] Step 9: Determine whether there is a loop or map fusion possibility in the current area based on the verification results, and perform loop correction or map fusion according to different situations.

[0107] Step 3 of the SAM semantic segmentation model includes: an image encoder module, a cue information encoder module, and a mask decoder module; specifically as follows: Figure 2 As shown.

[0108] The image encoder module maps the image to be segmented to the image feature space using an image encoder. Based on a scalable and powerful pre-training method, it uses ViT pre-trained in MAE, making it minimally suitable for processing high-resolution inputs. The image encoder runs once for each image and is applied before the cue model.

[0109] The prompt encoder module maps the input prompt to the prompt's feature space. Two types of prompts are defined: sparse and dense. The sparse prompt refers to the points, bounding boxes, or text describing the target that can be input in the demo. The dense prompt is not currently available in the online demo; the paper only mentions that it corresponds to a mask-type prompt. Judging from the code, it seems to be used more during training, typically as a coarsely segmented mask predicted in the previous iteration, roughly indicating the target region to be segmented. Two sets of prompts are considered: sparse (points, boxes, text) and dense (masks). Researchers represent points and boxes using positional encoding and add the learned embeddings for each prompt type and free-form text to the existing text encoding in CLIP. Dense prompts (i.e., masks) are embedded using convolution and element-wise summed using image embeddings.

[0110] The mask decoder module is used to integrate the two embeddings output by the image encoder and the prompt encoder respectively, and then decode the final segmentation mask from the feature map of this embedding.

[0111] The image preprocessing module segments the movable object using the SAM algorithm and removes the movable object and its feature points.

[0112] The image feature point extraction module creates an image pyramid using a feature extraction function, extracts features from each pyramid layer, performs homogenization processing on the features based on a quadtree, calculates the orientation of the fast feature points, and calculates the descriptors corresponding to the feature points.

[0113] The IUM pre-integration module initializes the pose by predicting the integral.

[0114] The embodiments of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those skilled in the art, various changes can be made without departing from the spirit of the present invention.

Claims

1. A research method for semantic-based synchronous localization and mapping technology of medical robots, the method being implemented based on an image preprocessing module, an IMU pre-integration module, and an image feature point extraction module, characterized in that... Specifically, the steps include the following: Step 1: Obtain the robot's location coordinates. The location coordinates are obtained by localization using the SLAM algorithm to obtain the robot's current pose, and by obtaining image depth information and image information using the SLAM algorithm. Step 2: Initialize the robot pose and obtain the feature points of the map based on the current map frame. If there are too few feature points in the current frame, reconstruct the initializer. When the number of feature points in two consecutive frames is greater than 100, continue the initialization process. Step 3: Obtain the semantic information of the map frames and perform semantic segmentation on the map using the SAM semantic segmentation model. SAM is an interactive segmentation framework that generates segmentation results based on a given prompt. The generation process is represented as follows: SAM uses a pre-trained mask autoencoder based on Vision Transformer to process the image into intermediate features and encodes the previous prompt into embedding tokens; subsequently, the cross-attention mechanism in the mask decoder facilitates the interaction between the image features and the prompt embeddings, ultimately producing the mask output. Step 4: Create IMU pre-integration for the current frame and from the previous keyframe. Obtain the corresponding matching relationship between the initial frame and the current frame through feature matching. Initialize using the H model and F model, where the H model uses the homography matrix H. 21 To describe the transformation relationship between feature point pairs p1 and p2, p2=H 21 *p1, F model: The model is a feature point pair p1, p2 using the fundamental matrix F. 21 To describe the transformation relationship between feature point pairs p2 T F 21 p1=0 is used to obtain the relative motion between the two frames and initialize map points; Step 5: Extract the semantic information of the doctor and patient from the map frame based on Step 3. Obtain the feature points of the map frame based on Step 4. Remove the feature points on the doctor and patient using the SAM algorithm, and remove matching points that cannot be triangulated. Then, generate 3D points through triangulation to generate map points. Send the map points to the SLAM locale. MAPPING thread; Step 6: The thread completes the three-stage initialization of the IMU and processes the keyframes in the queue; Step 7: Obtain the newly added map points that need to be checked, set different observation thresholds according to the camera type, and remove map points that exceed the threshold based on the observation data. Denote the probability distribution as follows: Given a An interval can be determined It can be considered that, If a point falls within this interval, it is an interior point; if it falls outside this interval, it is an exterior point. The threshold to be found Then new map points are generated to make tracking more stable, and finally, the co-view keyframes of the current keyframe are checked for redundancy. Step 8: Add the current keyframe to the loop closure detection queue. By calculating the similarity score between the current keyframe and the keyframes in the keyframe database, candidate keyframes with possible loop closure or sub-map fusion can be obtained. Perform geometric similarity verification and temporal geometric verification on the candidate keyframes. Step 9: Determine whether there is a loop or map fusion possibility in the current area based on the verification results, and perform loop correction or map fusion according to different situations.

Images