Method for Determining Immune Occlusion of Robot Platform

Abstract

The present invention discloses a method for immune occlusion positioning of a robot platform, including: the main control machine controls the robot platform to move indoors, and combines the information collected by the lidar sensor and the depth camera to obtain a two-dimensional semantic map, and performs semantic preprocessing on the two-dimensional semantic map to generate a semantic linked list; the depth camera is used to obtain observation data and perform semantic recognition. When an obstacle is detected, it is matched with the key frames saved during mapping to determine the similar key frames; the semantic information contained in the current frame is compared with the key frame semantic chain to determine the category of the occluded object, and the category of the semantic information recognized by the main control machine is compared with the semantic linked list to determine the area where the robot platform is located. Semantic positioning is performed through the depth information of the unoccluded high-confidence semantic objects, and finally, the true pose of the robot platform is obtained by synchronously combining the AMCL algorithm with the environmental map matching. The positioning of the present invention is accurate, and the robustness of the robot relocalization is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and navigation of robot platforms, and particularly relates to a positioning method for a robot platform immune to occlusion. Background Art

[0002] Due to the lack of semantic description of objects in the environment in the grid map, topological map, and feature map obtained by traditional environment modeling, mobile robots cannot well understand the environment and cannot complete some advanced and complex tasks. Semantic maps can help robots better understand the environment and greatly improve the robot's human-computer interaction ability and intelligent level. Positioning technology is currently a difficult and hot topic in the research of intelligent mobile robots. The current mainstream positioning algorithms operate well in static and unoccluded environments, but perform poorly in environments with obstacle occlusion. When semantic objects are occluded, semantic positioning cannot be achieved or multiple semantic positioning poses occur, resulting in the inability of particles to converge quickly in geometrically similar environments and a significant decrease in positioning efficiency. Therefore, it is necessary to develop a positioning method for a robot platform that can still accurately position when objects are occluded. Summary of the Invention

[0003] The purpose of the present invention is to provide a positioning method for a robot platform immune to occlusion in view of the deficiencies of the prior art. This method can still complete the precise repositioning of the robot when objects are occluded, improving the robustness of the robot's repositioning.

[0004] To solve the above technical problems, the present invention adopts the following technical solutions:

[0005] A positioning method for a robot platform immune to occlusion, the robot platform includes a robot platform chassis, a main control unit, a lidar sensor, and a depth camera disposed on the robot platform chassis. The main control unit is respectively connected to the robot platform, the lidar sensor, and the depth camera;

[0006] The positioning method includes the following steps:

[0007] Step 1: The main control unit controls the robot platform to move indoors. The lidar sensor collects indoor information in real time and transmits it to the main control unit. The main control unit obtains a two-dimensional environmental grid map through a map construction method based on the indoor information collected by the lidar sensor, and performs semantic mapping on the obtained two-dimensional environmental grid map in combination with the visual semantic information collected by the depth camera to obtain a two-dimensional grid semantic map, and performs semantic preprocessing on the two-dimensional grid semantic map to generate a semantic linked list;

[0008] Step 2: During the map building process, the master controller saves the key frames that meet the requirements obtained by the depth camera. When the master controller detects that the robot platform encounters an obstacle during indoor movement, it performs similarity matching between the current frame and the key frames saved during map building, selects the key frame with the highest similarity score as the similar frame, and compares the semantic information contained in the current frame with the semantic chain of the similar frame to determine the category of the occluded object;

[0009] Step 3: After confirming the category of the occluded object, the master controller compares the category of the semantic information obtained by the robot platform itself with the semantic linked list to determine the area where the robot platform is located. And the master controller assigns a low confidence level to the semantic object of the occluded object and a high confidence level to the semantic object recognized by the depth camera, uses the semantic information with high confidence level for semantic localization, and determines the estimated pose and direction of the robot platform through semantic localization combined with the area where the robot platform is located;

[0010] Step 4: The master controller performs iterative update of the pose according to the estimated pose obtained in Step 3 in combination with the AMCL laser localization algorithm to obtain the final true pose of the robot platform, and conducts comparison and verification under similar geometric environment occlusion and similar semantic environments.

[0011] Furthermore, the method for constructing a two-dimensional grid semantic map is as follows:

[0012] The master controller controls the robot platform to walk indoors to obtain an indoor map, constructs an environmental grid map of the indoor environment through the map construction method, extracts semantic information based on the SSD object detection algorithm and the DeeplabV2 semantic segmentation algorithm, and performs coordinate transformation and semantic mapping in combination with the laser information collected by the lidar sensor to obtain a semantic map, and then performs dilation and erosion processing on the generated semantic map to obtain a two-dimensional semantic grid map.

[0013] Furthermore, the calculation method for the mapping of semantic information is as follows:

[0014] l t,i =l t-1,i -l0+l inv,i ;

[0015] In the formula, l t,i represents the state of the i-th semantic grid at time t, l t-1,i is the state of the i-th semantic grid at time t - 1, l0 represents the prior probability, and l inv,i is the inverse observation model of the camera.

[0016] Furthermore, the method for generating a semantic linked list is as follows:

[0017] Draw the minimum enclosing contour of the semantic information in the semantic map, calculate the projection center coordinates of each semantic information, take the key objects in each area as the basic semantic information, extract the semantic information around the key objects to form a semantic chain, and the extraction range is r. Its calculation formula is:

[0018]

[0019] Among them, θ s is the horizontal field of view angle of the depth camera, and d s is the maximum viewing distance for semantic information extraction.

[0020] Furthermore, the conditions that the key frames in step 2 must meet are:

[0021] (1) There must be a preset sequence interval between the current frame and the previous key frame;

[0022] (2) The common viewing area between the current frame and the previous key frames is lower than the preset angular range;

[0023] (3) The key frame contains more than one type of semantic information and features.

[0024] Furthermore, the method of similarity matching in step 2 is:

[0025] Construct a dictionary containing various scene information, extract features using ORB from the key frames containing obstacles, search layer by layer in the dictionary tree to find the word w_x corresponding to the feature, and describe it in the form of a bag-of-words vector; extract the features in the current frame, determine the words in the BoW corresponding to the features and determine the usage of the words in the current frame, and convert the current frame into a bag-of-words vector description;

[0026] Determine the similarity between the key frame and the current frame by comparing the bag-of-words vectors of the key frame and the current frame. The similarity calculation is as follows:

[0027]

[0028] Among them, η n represents the weight of w n , w n represents the word, v i represents the bag-of-words vector of the key frame, v j represents the bag-of-words vector of the current frame, s(v i -v j ) represents the similarity between the current frame and the key frame, v ix represents the vector feature of the x-th key frame image, and v jx represents the vector feature of the x-th current frame image.

[0029] Furthermore, the calculation method of the word weight is:

[0030] Calculate the weight of words using the TF-IDF weighting method; among them, calculate the IDF when building the dictionary. Specifically: count the number of features in a certain leaf node w i The ratio of the number of features in w to the total number of features is used as the IDF part, as shown in the following formula:

[0031]

[0032] Among them, n is the total number of features, and n i is the number of features of w i ;

[0033] The TF part refers to the frequency of a certain feature appearing in a certain frame of image, and the calculation formula is as follows:

[0034]

[0035] Among them, m i is the number of times the word w2 appears in the image F i , m is the total number of occurrences, and the weight of the word w i is calculated as follows:

[0036] η i = TF i × IDF i

[0037] For the key frame F at a certain moment i , its feature points can be corresponding to many words w i , and the formed BoW is as follows:

[0038]

[0039] Furthermore, the method for determining the area where the robot is located is:

[0040] First, calculate the Euclidean distance between other semantic objects in the environment at the current time t and the occluded object Calculate the Euclidean distance d corresponding to the semantic objects in the similar areas in the semantic linked list 0i , and find the absolute value ΔD of the difference between its Euclidean distance and the Euclidean distance at the current time t j , compare ΔD j to determine the area where the robot is located. Select the area with the smallest ΔD j as the area where the robot is located. The specific calculation formula is:

[0041]

[0042]

[0043] In the formula, Indicates the coordinates of the occluded object recognized at the current time t. Indicates the center coordinates of other semantic objects recognized at the current time t.

[0044] Furthermore, the method for determining the estimated pose of the robot platform is as follows:

[0045] First, semantic positioning is performed using high-confidence semantic information, and its calculation formula is:

[0046]

[0047]

[0048] Among them, R1 represents the distance from one of the high-confidence semantic objects obtained by the depth camera to the robot platform, R2 represents the distance from another high-confidence semantic object obtained by the depth camera to the robot platform, d represents the distance between the high-confidence objects, h represents the distance from the possible pose of the robot platform to the line connecting the centers of the high-confidence objects, and a represents the distance from one of the high-confidence semantic objects to its foot of the perpendicular; when Indicates that the currently used semantic information cannot complete the positioning of the robot platform, and the robot platform needs to continue to move to let the robot platform recognize more semantic information for semantic positioning; when Indicates that there is only one intersection point of the circles drawn by the two semantic objects, and this point may be the true position of the robot platform; when Indicates that there are two possible positions for the robot;

[0049] Secondly, solve the accurate semantic positioning position of the robot, and its calculation formula is:

[0050] p pose = C1 + an || ± hn ⊥

[0051]

[0052] Among them, define a unit direction vector n of the line connecting the midpoints of two high-confidence semantic objects || , n ⊥ is the unit normal vector of n || , C1 represents the center distance of one of the high-confidence semantic objects, and C2 represents the center distance of the other high-confidence semantic object respectively;

[0053] After obtaining the possible position of the robot platform, determine whether this point is the actual pose (x t , y t ) of the robot by determining whether this point is in the free area of the grid in the two-dimensional grid semantic map; if this point is in the free position, then this point is the actual pose of the robot platform; otherwise, it is excluded.

[0054] Furthermore, the method for obtaining the orientation of the robot platform is as follows:

[0055] The orientation of the robot is determined by the yaw angle θ of the robot platform t and needs to be solved through the angle θ1 between the robot platform and the semantic object and the angle θ2 between the semantic object and the x-axis of the map coordinate system. The specific calculation formula is:

[0056] θ t = θ2 - θ1

[0057] θ1 = θ - 90

[0058] where θ is the angle between the center of the semantic object mapped to the robot coordinate system and the positive x-axis.

[0059] Furthermore, in step 4, the AMCL laser positioning algorithm is used for iterative update of the pose. The specific method is as follows:

[0060] (1) Initialize the particle set χ0, with the number of particles being n and the particle weight being 1 / n;

[0061] (2) Calculate the amount of movement of the robot platform from the (t - 1)th moment to the tth moment based on the motion data obtained by the robot platform at the current tth moment, and perform pose estimation to obtain the state distribution χ of the particles t ;

[0062] (3) First, update the particle weights according to the state distribution χ t in combination with the confidence level of the robot pose matching in step 3, the data collected by the lidar, and the matching confidence level of the two-dimensional grid semantic map to obtain the particle weights w at the tth moment t , then update the particle weights based on the estimated pose of the robot platform obtained in step 3, and then traverse the entire particle set to weight all the particles in the weighted particle set to obtain the particle set after weight update

[0063] (4) Resample the particle set after weight update and perform normalization processing on the weights, and repeatedly iterate until the particles converge to obtain the actual pose of the robot.

[0064] Compared with the prior art, the beneficial effects of the present invention are as follows: In view of the problem that semantic objects are blocked by obstacles during the semantic localization process, resulting in semantic localization failure and the inability to correctly obtain the robot pose, the present invention proposes a localization method for a robot platform immune to occlusion. During the relocalization process, first, observation data is obtained through a Kinect v2 depth camera, and semantic recognition is performed using SSD object detection. When an obstacle is detected, similarity matching is performed with the key frames saved during mapping to determine the similar key frames. Secondly, the semantic information contained in the current frame is weighted and subtracted from the semantic chain of the key frames to determine the category of the occluded object. The category of the semantic information recognized by the robot is compared with the semantic chain of the semantic map to determine the area where the robot is located. Semantic localization is performed through the depth information of the unoccluded high-confidence semantic objects. Finally, by synchronously combining the AMCL algorithm with the method of matching with the environmental map, a more accurate and faster localization effect is achieved, and the robustness of the robot relocalization is improved. Description of the Drawings

[0065] Figure 1 It is a flowchart of the method for the robot platform immune to occlusion in the embodiment of the present invention;

[0066] Figure 2 It is the map constructed in the embodiment of the present invention, where (a) is the experimental environment, and (b) is the two-dimensional grid map;

[0067] Figure 3 It is the effect diagram of SSD object detection in the embodiment of the present invention;

[0068] Figure 4 It is the semantic segmentation result diagram in the embodiment of the present invention, where (a) is the visual diagram collected by the depth camera, and (b) is the semantic segmentation effect diagram of (a);

[0069] Figure 5 It is the semantic mapping diagram in the embodiment of the present invention, where (a) is the corner mapping diagram, (b) is other semantic mapping diagrams, and (c) is the semantic mapping color correspondence diagram;

[0070] Figure 6 It is the two-dimensional grid semantic map constructed in the embodiment of the present invention;

[0071] Figure 7 It is the semantic information search diagram in the embodiment of the present invention, where (a) is the semantic search area, (b) is the environmental grid map, and (c) is the search area division and semantic labels;

[0072] Figure 8 It is the key frame diagram in different environments in the embodiment of the present invention;

[0073] Figure 9 It is the pose solution diagram in the embodiment of the present invention, where (a) is the robot pose; (b) is the robot perspective view;

[0074] Figure 10 This is the heading angle solution diagram of the embodiment of the present invention, where (a) is the heading angle in the two-dimensional grid semantic map; (b) is the mapping diagram in the robot coordinate system;

[0075] Figure 11 This is the semantic localization map of the occluded trash can in the embodiment of the present invention, where (a) is the diagram of the recognized obstacle, (b) is the key frame diagram, (c) is to determine the semantic object category occluded by the obstacle, (d) is the semantic localization, and (e) is the semantic localization result;

[0076] Figure 12 This is the semantic localization map of the occluded chair in the embodiment of the present invention, where (a) is the diagram of the recognized obstacle, (b) is the key frame diagram, (c) is to determine the semantic object category occluded by the obstacle, (d) is the semantic localization, and (e) is the semantic localization result;

[0077] Figure 13 This is the semantic localization map of the occluded corner in the embodiment of the present invention, where (a) is the diagram of the recognized obstacle, (b) is the key frame diagram, (c) is to determine the semantic object category occluded by the obstacle, (d) is the semantic localization, and (e) is the semantic localization result;

[0078] Figure 14 This is the similar semantic real environment of the embodiment of the present invention;

[0079] Figure 15 This is the map in the similar environment of the embodiment of the present invention, where (a) is the two-dimensional environmental grid map, (b) is the semantic mapping map, and (c) is the two-dimensional grid semantic map;

[0080] Figure 16 This is the semantic localization map in the similar environment of the embodiment of the present invention, where (a) is the position where the robot is located, (b) is the target detection diagram, (c) is the similar semantics, (d) is the pose solution result of the robot platform, and (e) is the preliminary semantic localization;

[0081] Figure 17 This is the semantic localization result diagram in the similar environment of the embodiment of the present invention, where (a) is the distance between the semantic object and the corner, (b) is the semantic estimated pose, and (c) is the real pose of the AMCL localization. Detailed implementation manners

[0082] Next, the technical solutions in the embodiments of the present invention will be described clearly and completely in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without making creative efforts belong to the scope of protection of the present invention.

[0083] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0084] The present invention will be further described below in conjunction with specific embodiments, but it is not limited to the present invention.

[0085] An embodiment of the present invention discloses a positioning method for a robot platform immune to occlusion. The robot platform includes a robot platform chassis, a main control unit, a lidar sensor, and a depth camera disposed on the robot platform chassis. The main control unit is respectively connected to the robot platform, the lidar sensor, and the depth camera through wires; in this embodiment, the main control unit selected is a Core M4I7-D mini host; the robot platform chassis selected is an Arduino drive board chassis of the pibot brand; the lidar sensor selected is a SICK lms111 lidar with stable performance; the depth camera selected is a kinect v2.

[0086] As Figure 1 shown, the positioning method includes the following steps:

[0087] Step 1: The main control unit controls the robot platform to move indoors. The lidar sensor collects indoor information in real time and transmits it to the main control unit. The main control unit obtains a two-dimensional environmental grid map through a map construction method based on the indoor information collected by the lidar sensor, and performs semantic mapping on the obtained two-dimensional environmental grid map combined with the visual semantic information collected by the depth camera to obtain a two-dimensional grid semantic map, and performs semantic preprocessing on the two-dimensional grid semantic map to generate a semantic linked list;

[0088] The main control unit controls the robot platform to move and walk indoors. When walking, the lidar sensor collects the distance between indoor objects and the robot platform and the direction angle between indoor objects and the robot platform in real time, and transmits them to the main control unit. The main control unit obtains a two-dimensional environmental grid map through a map construction method based on the distance between indoor objects and the robot platform and the direction angle between indoor objects and the robot platform (as Figure 2 ), at the same time, the depth camera collects visual information of the indoor environment in real time and transmits it to the main control unit. The main control unit extracts semantic information from the collected visual information based on the SSD (Single Shot MultiBox Detector) object detection algorithm and the DeeplabV2 semantic segmentation algorithm. In this embodiment, that is, indoor objects including chairs, cabinets, trash cans, and doors are segmented to obtain a semantic segmentation effect diagram. The SSD object detection effect diagram is as Figure 3 , the deeplabv2 image segmentation effect diagram is as Figure 4 ; then, multi-sensor fusion coordinate transformation and semantic mapping are performed to obtain a corner semantic mapping diagram and other semantic information mapping diagrams ( Figure 5) Finally, fuse the previously constructed two-dimensional environmental grid map with the semantic mapping map to generate a semantic map, and perform dilation and erosion processing on the generated semantic map to obtain a two-dimensional grid semantic map (such as Figure 6 ). Among them, the mapping calculation formula for semantic information in the semantic map is:

[0089] l t,i = l t-1,i - l0 + l inv,i ;

[0090] In the formula, l t,i represents the state of the i-th semantic grid at time t, l t-1,i is the state of the i-th semantic grid at time t-1, l0 represents the prior probability. It is assumed that the occupancy probability and free probability of the semantic grid at the startup moment of the mobile robot platform are both 0.5, that is, l0 = 0, and l inv,i is the inverse observation model of the camera.

[0091] Perform semantic preprocessing on the two-dimensional grid semantic map to generate a semantic linked list, which is used for subsequent immune occlusion localization research. The specific method is: use the function in the Opencv library to draw the minimum enclosing contour of the semantic information in the semantic map (such as Figure 7 ), and calculate the projection center coordinates of each semantic information. Since each area must contain corner semantic information, take the key corners of each area as the basic semantic information, and extract the semantic information around the corners to form a semantic linked list, as shown in Table 1. The extraction range is r(m), and the calculation formula is:

[0092]

[0093] Among them, θ s is the horizontal field of view angle of the camera, d s is the maximum viewing distance for semantic information extraction, which is the best observation distance of the Kinectv2 camera in this embodiment;

[0094] Table I is the semantic linked list

[0095]

[0096]

[0097] Step 2: The master control machine saves the key frames that meet the requirements obtained by the depth camera during the mapping process. When the master control machine detects that the robot platform encounters an obstacle during indoor movement, it performs similarity matching between the current frame and the key frames saved during mapping, selects the key frame with the highest similarity score as the similar frame, and compares the semantic information contained in the current frame with the semantic chain of the similar frame to determine the category of the occluded object;

[0098] The master controller controls the robot platform to move indoors in real time. The Kinect v2 depth camera obtains observation data in real time. The master controller takes the frames that meet the key-frame selection rules from the depth camera as key frames (such as Figure 8 ) and stores them. In this embodiment, the key-frame selection rules are as follows:

[0099] (1) There must be a certain sequence interval between the current frame and the previous key frame. In this embodiment, the sequence time interval is greater than 30 s;

[0100] (2) The co-visible area between the current frame and the previous key frames is lower than a certain range. The specific range depends on the actual experimental environment;

[0101] (3) The key frame has sufficient semantic information and features, that is, it has more than one kind of semantic information and features. For example, in a frame of picture, there can be a chair, a cabinet, a trash can and a corner at the same time, rather than only a chair and a cabinet or only a chair, to ensure the richness of features.

[0102] When an obstacle is detected, the master controller performs similarity matching between the current frame obtained by the depth camera and the key frames saved during mapping, selects the key frame with the highest similarity score as the similar frame, and then compares the semantic linked lists of the current frame and the similar frame to determine the category of the occluded object;

[0103] This embodiment selects the method based on the bag-of-words model for similarity matching. Specifically:

[0104] A dictionary containing various scene information is created, with a scale of K = 10 and d = 5. Features are extracted from the key frames containing obstacles using the Oriented FAST and Rotated BRIE algorithm (ORB), and searched layer by layer in the dictionary tree to find the word w x corresponding to the feature, which is described in the form of a vector. For example, a frame of image has words such as "person", "car", "cat", etc., which can be recorded as the following formula:

[0105] F i = 1·w1 + 1·w2 + 0·w3

[0106] where F i represents a frame of image at the i-th moment, and w1, w2, and w3 correspond to the words "person", "car", and "cat" respectively.

[0107] When the frequency of a certain word in an image is relatively high or low, the weight it occupies will also be correspondingly different. In this embodiment, the TF-IDF weighting method is used (the idea of the TF part is that the higher the frequency of a certain word in the key frame, the higher its discrimination; the idea of the IDF is that the lower the frequency of a certain word in the dictionary, the higher the discrimination when classifying images). When building the dictionary, calculate the IDF: Count the proportion of the number of features in a certain leaf node w i in all features as the IDF part, as shown in the following formula:

[0108]

[0109] where n is the number of all features, n i is the number of features of w i ;

[0110] The TF part refers to the frequency of a certain feature in a certain frame of the image, and the calculation formula is as follows:

[0111]

[0112] where m i is the number of times the word w2 appears in the image F i , m is the total number of occurrences, and the weight of the word w i is calculated as follows:

[0113] η i = TF i × IDF i

[0114] For the key frame F i at a certain moment, its feature points can correspond to many words w i , and the resulting bag-of-words model BoW is as follows:

[0115]

[0116] The key frame F i is represented by the bag-of-words vector v i ; Then use ORB to extract the features of the current frame, determine the words in the BoW corresponding to the features, and then determine the usage of the words in the current frame to convert the current frame into a bag-of-words vector description; Compare the bag-of-words vectors of the key frame and the current frame to determine the similarity between the two, and the similarity calculation is as follows:

[0117]

[0118] where η n represents the weight of w n , v i represents the bag-of-words vector of the key frame, vj represents the bag-of-words vector of the current frame, s(v i -v j ) represents the similarity between the current frame and the key frame, v ix represents the vector feature of the x-th key frame image, v jx represents the vector feature of the x-th current frame image.

[0119] Select the key frame with the highest similarity score as the similar frame through the above method. After determining the similar frame, convert the semantic chain information contained in the similar frame and the current frame into vector form, assign different weights to each object (in this embodiment, corner = 1, door = 10, trash = 100, chair = 1000, cabinet = 10000), perform comparison of semantic chains, and determine the category of the occluded semantic object. The calculation formulas for the similar frame and the current frame are as follows:

[0120] A = 1·door + 0·chair + 1·corner + 0·trash + 1·cabinet

[0121] B = 1·door + 0·chair + 0·corner + 0·trash + 1·cabinet

[0122] T = A - B

[0123] In the formula, A represents the similar frame, and B represents the current frame; determine the category of the occluded object according to the value of T in the above formula. When T = 0, it means that no semantic object is occluded; when T = 1, the occluded object category is the corner; when T = 10, the occluded object category is the door; when T = 100, the occluded object category is the trash can; when T = 1000, the occluded object category is the chair; when T = 10000, the occluded object category is the cabinet.

[0124] Step 3: After confirming the category of the occluded object, the main control unit compares the semantic information category obtained by the robot platform itself with the semantic linked list to determine the area where the robot platform is located. And the main control unit assigns a low confidence to the semantic object of the occluded object and a high confidence to the semantic object recognized by the depth camera, uses the semantic information with high confidence for semantic localization, and determines the estimated pose and direction of the robot platform through semantic localization combined with the area where the robot platform is located;

[0125] In this embodiment, the method for determining the area where the robot platform is located is as follows:

[0126] First, calculate the Euclidean distance between other semantic objects in the environment at the current time t and the corner Calculate the Euclidean distance d corresponding to the semantic objects in the possible similar areas in the semantic linked list0i , find the absolute value ΔD of the difference in Euclidean distance from it at the current time t j , compare ΔD j in terms of magnitude to determine the area where the robot is located, and select the area with the smallest ΔD j as the area where the robot is located. The calculation formula is:

[0127]

[0128]

[0129] In the formula, represents the coordinates of the corner recognized at the current time t, represents the center coordinates of other semantic objects recognized at the current time t.

[0130] After confirming the category of the occluded semantic object in step 2, assign a low confidence level to the semantic information of the occluded semantic object, and assign a high confidence level to the semantic information of the semantic object directly recognized from the visual information collected by the depth camera. Use the semantic information with high confidence level for semantic localization. For example Figure 9 in, when the corner is occluded, use the door and the cabinet for semantic localization. Solve the intersection points of the two circles to obtain the position of the robot. Among them, C1 is the center coordinate of the door obtained by querying the semantic linked list, R1 is the relative distance value from the robot to the door obtained by the depth camera, C2 is the center coordinate of the cabinet, and R2 is the relative distance value from the robot to the cabinet obtained by the depth camera. The geometric relationship of its radius is as shown in the calculation formula:

[0131]

[0132]

[0133] And convert it into expressions of a and h to judge the corresponding semantic information to solve the pose of the robot. The conversion formula is as follows:

[0134]

[0135]

[0136] Among them, R1 and R2 represent the distances from the high-confidence semantic objects to the robot, d represents the distance between the high-confidence objects, h represents the distance from the possible pose of the robot to the center connection line of the high-confidence objects, and a represents the distance from one of the high-confidence semantic objects to its foot of the perpendicular. When indicates that the currently used semantic information cannot complete the localization of the robot, and the robot needs to continue to move to enable the robot to recognize more semantic information for semantic localization; when Indicates that there is only one intersection point of the circles drawn by two semantic objects, and this point may be the real position of the robot; when Indicates that there are two points for the possible positions of the robot.

[0137] The calculation formula for solving the accurate semantic positioning position of the robot is:

[0138] p pose = C1 + an || ± hn ⊥

[0139]

[0140] Among them, a unit direction vector n of the line connecting the midlines of two high-confidence semantic objects is defined || , n ⊥ is the unit normal vector of n || .

[0141] After obtaining the possible positions pose1 and pose2 of the robot (such as Figure 9 ), it is judged whether this point is the estimated pose (x t , y t ) of the robot by determining whether this point is in the grid free area of the two-dimensional grid semantic map. If this point is in the free position, this point is the estimated pose of the robot; otherwise, it is excluded.

[0142] After determining the estimated pose of the robot platform, it is necessary to determine the direction of the robot platform, that is, to determine the yaw angle θ t of the robot platform, which needs to be solved through the included angle θ1 between the robot platform and the semantic object and the included angle θ2 between the semantic object and the x-axis of the map coordinate system (such as Figure 10 ), and its calculation formula is:

[0143] θ t = θ2 - θ1

[0144] θ1 = θ - 90

[0145] Among them, θ is the included angle between the center of the semantic object mapped to the robot coordinate system and the positive x-axis.

[0146] Step 4: The master control machine performs iterative update of the pose according to the estimated pose and direction obtained in step 3 in combination with the AMCL laser positioning algorithm, and conducts comparative experiments and verifications in similar geometric environment occlusion and similar semantic environments to obtain the final real pose of the robot platform, and conducts comparative verifications in similar geometric environment occlusion and similar semantic environments.

[0147] In this embodiment, the method for the master control machine to perform iterative update of the pose and direction of the robot platform by using the AMCL laser positioning algorithm is:

[0148] (1) Initialize the particle set: Since there is no prior information on the robot platform, the pose of the robot platform in the 2D grid semantic map is completely unknown. Define a particle set of size n at the initial moment. Randomly distribute n particles in the 2D grid semantic map, and the weights of each particle are equal, all being where represents the state estimate represented by the i-th particle in the particle set χ0 at the initial moment;

[0149] (2) Particle pose prediction: Calculate the movement amount of the robot platform from the (t - 1)-th moment to the t-th moment according to the movement data obtained by the robot platform at the current t-th moment, and convert it from the robot platform coordinate system to the world coordinate system. Use the particle state estimate and the movement amount u t to predict the current particles at the t-th moment to obtain their state estimates i.e., p(x t |x t-1 , u t ), which is specifically expressed as:

[0150]

[0151] In the formula, p(x t |x t-1 , u t ) represents the state estimate of the particles at the current t-th moment, bel(x t-1 ) represents the prior of the particles at the (t - 1)-th moment, represents the prior of the particles at the t-th moment;

[0152] (3) Particle weight update: Use the observed data z t input from the lidar to correct the weights of the particles. The process is as follows: Convert the lidar observed data z t at the t-th moment to the map coordinate system according to the poses of the current respective particles, match the current observed data with the 2D grid semantic map, return the matching scores of each particle, update the particle weights according to the scores. After traversing the entire particle set, weight all the particles in the weighted particle set to obtain the particle set after weight update

[0153] (4) Particle resampling: The weights of the particles in the particle set have been updated. To make the posterior probability distribution closer to the true pose of the mobile robot platform and converge the initially randomly distributed particles, the roulette wheel method is adopted. First, all the particles in the particle set occupy the interval from 0 to 1 according to the weight ratio. Then, a random number is generated, the corresponding interval of the random number is calculated, and then the particles corresponding to the random number interval are copied. Finally, the weights of the particles are normalized. During the resampling process, particles with large weights are copied multiple times, and particles that contribute little to the posterior probability density are gradually eliminated. After multiple resamplings, the particles gradually gather and converge, completing the positioning process of the robot.

[0154] After obtaining the true pose of the robot platform, a comparative experiment is completed under the occlusion of a similar geometric environment. Combining the two-dimensional grid semantic map and semantic chain constructed in the steps, the true pose obtained in this embodiment is experimentally verified in a similar geometric environment. When there is a dynamic obstacle occluding a semantic object, the method of this embodiment is used. First, the obstacle is recognized through object detection, such as Figure 11 (a). Secondly, through the similarity matching based on the bag-of-words model with the previously saved key frames, the similarity matching score between the current frame and key frame 3 is the highest. It is initially judged that the similar frame is key frame 3, as shown in Figure 11 (b). By matching and verifying the semantic chain information contained in the current frame and the key frame, it is judged whether the similar frame is correctly matched. The current frame contains a cabinet and a corner, and the similar frame also contains a cabinet and a corner, so it is determined that the similar match is correct. Thirdly, the semantic chains of the key frame and the current frame are assigned weights. According to the formula in step 3, T = 100 is calculated. Therefore, it is determined that the semantic object occluded by the obstacle is a trash can, as shown in Figure 11 (c). Then, by searching the semantic linked list Figure 8 to find the area containing the semantics of the corner, cabinet, and trash can, it is determined that the robot is in area 3. Finally, when performing the accurate semantic positioning pose solution, since the trash can is occluded and its depth information is not accurate, the positioning confidence of the trash can is set to 0, and semantic positioning is performed through the corner and the cabinet, as shown in Figure 11 (d), and the semantic positioning result is as shown in Figure 11 (e); when a chair is occluded, the final semantic positioning result is obtained by applying the method of this embodiment ( Figure 12 ); when a corner is occluded, the final semantic positioning result is obtained by applying the method of this embodiment ( Figure 13 );

[0155] The obtained true pose is compared in a similar semantic environment. To verify that the semantic linked list constructed by the method of this embodiment can improve the robustness of the mobile robot platform in positioning in a similar semantic environment, in Figure 14The semantic localization experiment is conducted in the environment shown in the figure. There are two areas that contain both chair and cabinet semantics, forming similar semantic scenes. The environment grid map, semantic map and two-dimensional grid semantic map ( Figure 15 ), according to the experimental environment, we apply step 2 to obtain the semantic list in this environment, see Table 2.

[0156] Table 2 is the semantic link list in the experimental environment

[0157]

[0158] The main control machine controls the robot platform to move in the above environment for semantic relocation. Figure 16 When the robot is at the position shown in (a), based on target detection, it recognizes semantic information such as corners, cabinets, and chairs, as shown in Figure 2. Figure 16 (b); however, the robot platform can also recognize the same semantic information near the corner of 2○, such as Figure 16 As shown in (c); the position of the robot platform is solved based on the relative distance between the semantic object and the robot platform, as shown in Figure 16 As shown in (d), the robot platform’s posture may exist in both areas; the preliminary semantic positioning is as follows Figure 16 As shown in (e), both semantic positioning poses are likely to be the real poses of the robot platform. Combined with the generated semantic linked list (Table 2), the distance between the corners near the possible poses and the semantic objects they contain is calculated, as shown in Figure 17 (a) The distance d between the cabinet and the wall corner 01 The distance d between the chair and the corner 04 , calculate the distance between the semantic object recognized by the robot platform through target detection and the corner of the wall at the current time t and Then, it is determined that the robot platform is near the corner of wall 7○. The semantic positioning pose is shown in 17(b). The semantic positioning pose is combined with AMCL, the weight of the current particle set is updated, the weight of the particles near the current semantic positioning pose is increased, and it is updated iteratively. Finally, the particles converge, as shown in Figure 17 (c) shown.

[0159] The above are only preferred embodiments of the present invention and do not limit the implementation mode and protection scope of the present invention. For those skilled in the art, it should be aware that all solutions obtained by equivalent substitutions and obvious changes made using the contents of the present invention specification should be included in the protection scope of the present invention.

Claims

1. A method for determining a robot platform immune to occlusion, characterized in that, The robot platform includes a robot platform chassis, a main control unit, a lidar sensor, and a depth camera disposed on the robot platform chassis. The main control unit is respectively connected to the robot platform, the lidar sensor, and the depth camera; The positioning method includes the following steps: Step 1: The main control unit controls the robot platform to move indoors. The lidar sensor collects indoor information in real time and transmits it to the main control unit. The main control unit obtains a two-dimensional environmental grid map through a map construction method based on the indoor information collected by the lidar sensor, and performs semantic mapping on the obtained two-dimensional environmental grid map combined with the visual semantic information collected by the depth camera to obtain a two-dimensional grid semantic map, and performs semantic preprocessing on the two-dimensional grid semantic map to generate a semantic linked list; Step 2: The main control unit saves the key frames that meet the requirements obtained by the depth camera during the map construction process. When the main control unit detects that the robot platform encounters an obstacle during indoor movement, it performs similarity matching between the current frame and the key frames saved during map construction, selects the key frame with the highest similarity score as the similar frame, and compares the semantic information contained in the current frame with the semantic chain of the similar frame to determine the category of the occluded object; Step 3: After confirming the category of the occluded object, the main control unit uses the category of semantic information obtained by the robot platform itself to compare with the semantic linked list to determine the area where the robot platform is located. And the main control unit assigns a low confidence level to the semantic object of the occluded object and a high confidence level to the semantic object recognized by the depth camera, and uses the semantic information with high confidence level for semantic positioning. The estimated pose and direction of the robot platform are determined by combining semantic positioning with the area where the robot platform is located; Step 4: The main control unit performs iterative update of the pose based on the estimated pose obtained in Step 3 combined with the AMCL laser positioning algorithm to obtain the final true pose of the robot platform, and conducts comparative verification in similar geometric environment occlusion and similar semantic environments.

2. The method for determining the immune occlusion of the robot platform according to claim 1, characterized in that, The method for constructing a two-dimensional grid semantic map is: The main control unit controls the robot platform to walk indoors to obtain an indoor map, constructs an environmental grid map of the indoor environment through a map construction method, extracts semantic information based on the SSD object detection algorithm and the DeeplabV2 semantic segmentation algorithm, and performs coordinate transformation and semantic mapping combined with the laser information collected by the lidar sensor to obtain a semantic map, and then performs dilation and erosion processing on the generated semantic map to obtain a two-dimensional semantic grid map.

3. The method for determining the immune occlusion of the robot platform according to claim 2, characterized in that, The calculation method for the mapping of semantic information is: l t,i ＝l t-1,i -l0+l inv,i ； where, \(l\) t,i represents the state of the \(i\)-th semantic grid at time \(t\), and \(l\) t-1,i is the state of the \(i\)-th semantic grid at time \(t - 1\), \(l_0\) represents the prior probability, and \(l\) inv,i is the inverse observation model of the camera.

4. The method for determining the immune occlusion of the robot platform according to claim 2, wherein, The method for generating a semantic linked list is: Draw the minimum bounding contour of the semantic information in the semantic map, calculate the projection center coordinates of each semantic information, take the key object in each area as the basic semantic information, extract the semantic information around the key object to form a semantic chain, and the extraction range is r, and its calculation formula is: where θ s is the horizontal field of view angle of the depth camera, and d s is the maximum viewing distance for semantic information extraction.

5. The method for determining the immune occlusion of the robot platform according to claim 1, wherein, The conditions that the key frames in Step 2 meet are: (1) There must be a preset sequence interval between the current frame and the previous key frame; (2) The common viewing area between the current frame and the previous key frames is lower than the preset angular range; (3) The key frame contains more than one type of semantic information and features.

6. The method for determining the robot platform immune to occlusion according to claim 1, characterized in that The method for similarity matching in Step 2 is: Construct a dictionary containing various scenario information, extract features using ORB from key frames containing obstacles, search layer by layer in the dictionary tree to find the word w corresponding to the feature x , and describe it in the form of a bag-of-words vector; extract the features in the current frame, determine the words in the BoW corresponding to the features and determine the usage of the words in the current frame, and convert the current frame into a bag-of-words vector description; The similarity between the key frame and the current frame is determined by comparing their bag-of-words vectors, and the similarity calculation is as follows: Among them, η n represents the weight of w n , w n represents a word, v i represents the bag-of-words vector of the key frame, v j represents the bag-of-words vector of the current frame, s(v i - v j ) represents the similarity between the current frame and the key frame, v ix represents the vector feature of the x-th key frame image, v jx represents the vector feature of the x-th current frame image.

7. The method for determining immune occlusion of the robot platform according to claim 1, characterized in that, The method for determining the area where the robot is located is as follows: First, calculate the Euclidean distance between other semantic objects in the environment at the current time t and the occluded object Calculate the Euclidean distance d corresponding to the semantic objects in the similar regions in the semantic linked list 0i , and find the absolute value ΔD of the difference between its Euclidean distance at the current time t j , and compare ΔD j in size to determine the area where the robot is located, and select the area with the smallest ΔD j as the area where the robot is located. The specific calculation formula is as follows: In the formula, represents the coordinates of the occluded object recognized at the current time t, represents the center coordinates of other semantic objects recognized at the current time t.

8. The method for determining the immune occlusion of the robot platform according to claim 1, wherein The method for determining the estimated pose of the robot platform is as follows: First, semantic localization is performed using semantic information with high confidence, and its calculation formula is: Among them, R1 represents the distance from one of the high-confidence semantic objects obtained by the depth camera to the robot platform, R2 represents the distance from another high-confidence semantic object obtained by the depth camera to the robot platform, d represents the distance between the high-confidence objects, h represents the distance from the possible pose of the robot platform to the line connecting the centers of the high-confidence objects, and a represents the distance from one of the high-confidence semantic objects to its foot of the perpendicular; when indicates that the current semantic information used cannot complete the positioning of the robot platform, and it is necessary to move the robot platform continuously to enable the robot platform to recognize more semantic information for semantic positioning; when indicates that there is only one intersection point of the circles drawn by the two semantic objects, and this point may be the real position of the robot platform; when indicates that there are two possible positions for the robot; Second, the accurate semantic localization position of the robot is solved, and its calculation formula is: p pose = C1 + an || ± hn ⊥ Among them, a unit direction vector n connecting the midpoints of the lines of two high-confidence semantic objects is defined || , n ⊥ is the unit normal vector of n || , C1 represents the central distance of one high-confidence semantic object, and C2 represents the central distance of the other high-confidence semantic object respectively; After obtaining the possible positions of the robot platform, it is determined whether this point is the actual pose (x t , y t ) of the robot by determining whether this point is in the free area of the grid in the two-dimensional grid semantic map; if this point is in the free position, this point is the actual pose of the robot platform; otherwise, it is excluded.

9. The method for determining the immune occlusion of the robot platform according to claim 1, characterized in that, The method for obtaining the direction of the robot platform is as follows: The direction of the robot is determined by the yaw angle θ of the robot platform, t which needs to be solved through the included angle θ1 between the robot platform and the semantic object and the included angle θ2 between the semantic object and the x-axis of the map coordinate system. The specific calculation formula is as follows: θ t = θ2 - θ1 θ1 = θ - 90 where θ is the angle between the center of the semantic object mapped to the robot coordinate system and the positive x-axis.

10. The method for determining the immune occlusion of the robot platform according to claim 1, wherein In step 4, the AMCL laser localization algorithm is used for iterative update of the pose, and the specific method is as follows: (1) Initialize the particle set χ0, with the number of particles being n and the particle weight being 1 / n; (2) Calculate the amount of movement of the robot platform from time t - 1 to time t based on the motion data obtained at the current time t of the robot platform, and perform pose estimation to obtain the state distribution χ of the particles t ; (3) First, according to the state distribution χ t Combine the confidence of the robot pose matching in step 3, the data collected by the lidar, and the matching confidence of the two-dimensional grid semantic map to update the particle weights, and obtain the particle weights w at time t t , then update the particle weights by combining the estimated pose of the robot platform obtained in step 3, and then traverse the entire particle set, weight all the particles in the weighted particle set, and obtain the particle set after weight update (4) After the weights of the particle set are updated resample and normalize the weights, iterate repeatedly until the particle set converges, and obtain the actual pose of the robot.

Images