A robot target search method based on association probability semantic map

Abstract

The present invention discloses a robot target search method based on an association probability semantic map. The present invention is based on a two-dimensional grid map, and uses objects with fixed positions and easy to identify as landmark objects. The semantic information and spatial position information of the landmark objects are mapped to the grid map to construct a grid-semantic map. Then, the association relationship between the landmark objects and ordinary objects is extracted, and the association probability between them is updated according to real-time sensor data. The association probability is added to the grid-semantic map to construct a probabilistic grid semantic map, which narrows the search space and improves the efficiency of the robot's target search in complex environments. The present invention proposes to construct an association probability semantic map using the association relationship between objects, and continuously update the association relationship between objects using real-time sensor information, thereby improving the efficiency of the robot in long-term target search in complex and changing environments.

Description

Technical Field

[0001] The present invention belongs to the technical field of fusion map construction and target search of mobile robots, and relates to a robot target search method based on an association probability semantic map, including a method for constructing an association probability semantic map based on real-time association information update, and a robot target search method based on an association probability semantic map. Background Art

[0002] With the advancement of robotics technology and the decreasing price of related sensors, indoor service robots are increasingly entering public spaces and homes, providing convenience for people. However, the use of indoor service robots still faces many challenges, one of which is target search. The targets that indoor service robots search for are often dynamic and small in size. Therefore, not only do robots face complex and changing environments when performing search tasks, but the target's position can also move without the robot's knowledge, resulting in inefficient search and even failure.

[0003] Traditional search strategies for complex and ever-changing indoor environments rely on zigzag or zigzag searches of the entire environment based on two-dimensional grid or topological maps. However, this strategy is time-consuming and inefficient. To improve target search efficiency, researchers have summarized common sense knowledge about the typical spatial relationships between objects and rooms in real life into a probability distribution model for objects in indoor environments. Combining this probability distribution model with grid and topological maps to form a probability map, they use this probability distribution model to infer the likelihood of an object being found in various areas of the work environment. This provides prior knowledge for service robots to perform search tasks and improves target search efficiency. However, this typical spatial relationship between objects and rooms only helps service robots quickly determine the room where the target object is located; it does not address the problem of robots searching in complex indoor environments. Existing target search methods, such as a mobile robot target search method that mimics human memory (CN114397894A) and a robot minimum expected time target search method based on probability maps (CN111427341A), utilize probability to improve target search efficiency. However, these methods fail to consider the impact of environmental dynamics on search efficiency. Therefore, the problem to be solved by the present invention is how to enable the mobile robot to perform efficient target search in a complex and changeable environment. Summary of the Invention

[0004] The purpose of the present invention is to address the low search efficiency of existing search strategies in complex and changing environments. This invention proposes a method for constructing a probabilistic semantic map based on real-time association information updates, and uses the constructed association probabilistic semantic map to help robots perform efficient target search. Specifically, the present invention uses a two-dimensional grid map as a basis, uses objects with fixed positions and easy to identify as landmark objects, and maps the semantic information and spatial position information of the landmark objects into the grid map to construct a grid-semantic map. Then, the association relationship between the landmark objects and ordinary objects is extracted, and the association probability between them is updated based on real-time sensor data. The association probability is added to the grid-semantic map to construct a probabilistic grid semantic map, thereby narrowing the search space and improving the efficiency of the robot's target search in complex environments.

[0005] In a first aspect, the present invention proposes a robot target search method based on an association probability semantic map, comprising the following steps:

[0006] Step 1: Construction of raster map

[0007] Use LiDAR to describe the structural information of the current environment, generate a two-dimensional grid map as the underlying architecture, and use a positioning algorithm to determine the robot's position in the grid map;

[0008] The positioning algorithm is a prior art and therefore will not be explained in detail;

[0009] Step 2: Construction of grid-semantic map

[0010] 2-1 Target Identification

[0011] Use the target recognition algorithm to detect the RGB color image obtained by the depth camera, obtain the rectangular box surrounding the detected object, and use the rectangular box to obtain the midpoint pixel coordinates of the object;

[0012] 2-2 Three-dimensional coordinate solution

[0013] The depth image obtained by the depth camera is combined to obtain the depth value corresponding to the midpoint pixel coordinates. The midpoint coordinates in the pixel plane coordinate system are converted into coordinates in the camera coordinate system through the coordinate conversion of formula (1).

[0014]

[0015] Where, P uv =[uv 1] T is the homogeneous coordinate of the object in the pixel coordinate system, P c =[X c Y c Z c ] T is the coordinate of the object in the camera coordinate system, Z cis the object depth information measured by the depth camera, f x and f y are the focal lengths of the camera in the x and y directions, c x and c y are the translation relationships between the origin of the pixel coordinate system and the optical axis, respectively. The matrix K represents the intrinsic parameter matrix of the camera, which can be obtained through calibration.

[0016] According to the coordinates of the object in the camera coordinate system, the coordinates of the object in the global grid coordinate system are obtained through the coordinate transformation of formula (2);

[0017]

[0018] Where, P w =[X w Y w Z w ] T is the coordinate of the object in the global coordinate system, R and T are the rotation and translation transformation relationship of the camera coordinate system relative to the global coordinate system;

[0019] 2-3 Based on the coordinates of the object in the global map coordinate system, the object is annotated with semantic information, and the object semantics is mapped to the two-dimensional grid map, thereby obtaining a grid-semantic map;

[0020] Step 3: Construction of association probability semantic map

[0021] To determine whether objects are associated, we can analyze their properties such as function and distance. Using function as the basis for judgment is more in line with people's common sense, but for robots to perform search tasks, distance is more effective information. For example, if a robot needs to search for keys, from a functional point of view, the robot should go to the location of the door to search, but this is unrealistic. We should judge from the perspective of distance and look for tables, cabinets and other places where keys are often placed. Therefore, the present invention chooses to use distance as the basis for judgment to extract associated information of objects in the environment.

[0022] 3-1 Object association extraction:

[0023] 4) When a landmark object A is first recognized, its position in the global map coordinate system is expressed as (X l ,Y l ), and mark the coordinates of landmark object A on the grid-semantic map;

[0024] 5) When a new landmark object B is recognized again, first determine whether the new landmark object B has the same semantic information as the existing landmark objects. If not, directly mark the coordinates and semantic information of the new landmark object B on the grid-semantic map. If so, traverse all existing landmark objects again in turn to solve the global coordinates of the new landmark object B and the semantic information of each existing landmark object (X i ,Y i )’s global coordinates to determine whether the object is in (X i ,Y i ) as the center and within the neighborhood with the spatial threshold R as the radius, if yes, the position and semantic information of the new landmark object B will not be recorded. Otherwise, the coordinates and semantic information of the new landmark object B will be marked on the grid-semantic map.

[0025] 6) When other objects C are identified besides the landmark object, the Euclidean distance between the global coordinates of the other object C and the global coordinates of each landmark object is calculated, and the landmark object (X j ,Y j ), determine whether the object C is in (X j ,Y j ) as the center and in a neighborhood with the spatial threshold R as the radius, if yes, it is considered as an association, otherwise it is considered as a non-association.

[0026] 3-2 Bayesian-based association probability update

[0027] There are only two states between objects: associated or unassociated. However, due to the randomness of object placement, we cannot simply assume that two objects are associated based on the fact that they meet a distance threshold in a certain observation. In contrast, using probability to describe the association between objects is more rigorous. Therefore, after extracting the association information between objects, we need to convert this information into probabilities to describe the association between objects.

[0028] For two objects in the environment, a is used to represent the association state, p(a=1) represents the probability that the two objects are associated, and p(a=0) represents the probability that the two objects are not associated, and they satisfy formula (3);

[0029] p(a=1)+p(a=0)=1 (3)

[0030] For two objects that have not yet been observed, the initial probability of association and the probability of non-association are both set to 0.5. Using p(a) to represent the probability of association between objects is intuitive and easy to understand, but it is not easy to process. Therefore, a different expression is used, and the ratio of the two probabilities A(a) is used to describe the association relationship between the objects; if A(a)>1, the association between the objects is strong, and A(a)<1, the association between the objects is weak.

[0031]

[0032] Assume that at time t, the association state A between objects has been obtained t (a) At the next moment t+1, a new observation w is obtained t+1 ∈{0,1},w t+1 Indicates the result of extracting the association relationship of the depth camera information, that is, whether the objects are associated or not. t (a) is updated and written in the form of conditional probability:

[0033]

[0034] where w 1:t+1 Represents the observation superposition from the initial time to time t+1.

[0035] Then the update formula of the association probability is:

[0036]

[0037] According to the Bayesian formula, we can get:

[0038]

[0039]

[0040] where p(a=1|w 1:t ) represents the prior probability, p(a=1|w 1:t+1 ) represents the posterior probability, It is an intermediate variable in the calculation and has no specific meaning;

[0041] Substituting formula (7) and formula (8) into formula (5) yields the updated model of association probability:

[0042]

[0043] in It represents the update coefficient, which is set manually and affects how fast the probability increases or decreases;

[0044] 3-3 Constructing Association Probability Semantic Map

[0045] Using a rapid random tree (RRT) algorithm, observation points are randomly generated within the raster-semantic map. The observation ranges of these observation points can be superimposed to cover the entire map. Once generated, semantic points and observation points in the environment are classified based on Euclidean distance, assigned semantic information, and the object-to-object association probabilities are combined with the observation points based on the semantic information of the objects. This results in a semantic map with association probabilities that is updated in real time based on this information.

[0046] The process of combining the object-object association probability with the observation point based on the semantic information of the object is as follows: finding the observation point that matches the semantic information of the landmark object and assigning the association probability to the observation point.

[0047] Step 4: Target search based on probabilistic semantic map

[0048] 4-1 In the association probability semantic map, based on the current position of the robot, the ratio of the association probability of the observation point to the distance between the robot and the observation point is calculated according to formula (10), which can also be called the gain value; based on the gain value of each observation point, a greedy strategy is used to determine the traversal sequence of the observation point.

[0049]

[0050] Among them, p o is the association probability of the observation point. If the target has never been found at the observation point during training, the probability of the observation point is set to 0.1. ro is the distance between the robot and the observation point.

[0051] 4-2 After determining the traversal sequence of observation points, a discrete search method is used to search for targets at the selected observation points. Target detection is not performed on the way from the current observation point to the next observation point. When the robot reaches the observation point, it rotates in place for one circle to search for the target.

[0052] When the robot searches for a target in a room, it continuously updates the association probability between objects based on the information obtained by the depth camera, so that the probability distribution model of objects in the room gradually conforms to the actual environment, and the robot will also perform more intelligent search behavior.

[0053] In a second aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to execute the method described above.

[0054] In a third aspect, the present invention proposes a computing device comprising a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, the method described is implemented.

[0055] The beneficial effects of the present invention are:

[0056] The present invention proposes to use the association relationship between objects to construct an association probability semantic map, and use real-time sensor information to continuously update the association relationship between objects, thereby improving the efficiency of the robot in performing long-term target search in complex and changing environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1Flowchart of the method of the present invention. DETAILED DESCRIPTION

[0058] The following will provide a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0059] The robot platform in this example is P3DX, equipped with a Realsense D435i depth camera and a Ruichi Zhiguang LakiBeam1 lidar.

[0060] A robot target search method based on an association probability semantic map comprises the following steps:

[0061] Step 1: Use the Cartographer algorithm to process the information obtained by the lidar sensor, generating a two-dimensional grid map as the underlying framework, describing the structure of the current environment. After the map is built, a localization algorithm (AMCL (Adaptive Monte Carlo Localization) or Cartographer) is used to locate the robot and obtain its coordinates in the global map coordinate system.

[0062] Step 2: Use the YOLO-v5 target recognition algorithm to detect the RGB color image obtained by the depth camera, obtain the coordinates of the midpoint of the detected object in the pixel coordinate system, and combine the Depth image obtained by the depth camera to obtain the depth value corresponding to the midpoint pixel coordinate. After coordinate conversion, the coordinates of the detected object in the global map coordinate system are obtained, and the semantic and position information of the object is recorded and combined with the grid map to construct a grid-semantic map.

[0063] Step 3: The robot observes the working environment and extracts the relationships between objects. The main process of object relationship extraction is as follows:

[0064] 1) When a landmark object A is first recognized, its position in the global map coordinate system is expressed as (X l ,Y l ), and mark the coordinates of landmark object A on the grid-semantic map.

[0065] 2) When a new landmark object B is recognized again, first determine whether the new landmark object B has the same semantic information as the existing landmark objects. If not, directly mark the coordinates of the new landmark object B on the grid-semantic map. If so, traverse all existing landmark objects in turn to solve the global coordinates of the new landmark object B and the existing landmark objects (X i ,Y i)’s global coordinates, to determine whether the object is in (X i ,Y i ) as the center and within the neighborhood with the spatial threshold R as the radius, if yes, the position and semantic information of the new landmark object B will not be recorded. Otherwise, the coordinates of the new landmark object B will be marked on the grid-semantic map.

[0066] 3) When other objects C are identified besides the landmark object, the Euclidean distance between the global coordinates of the other object C and the global coordinates of each landmark object is calculated, and the landmark object (X j ,Y j ), determine whether the object C is in (X j ,Y j ) as the center and in a neighborhood with the spatial threshold R as the radius, if yes, it is considered as an association, otherwise it is considered as a non-association.

[0067] After extracting the association relationship between objects, the Bayesian method is used to update the association probability between objects based on real-time data. The probability update process is as follows:

[0068] For two objects in the environment, a is used to represent the associated state, p(a=1) represents the probability that the two objects are associated, and p(a=0) represents the probability that the two objects are not associated, and

[0069] p(a=1)+p(a=0)=1 (1)

[0070] For two objects that have not been observed, the initial association probability and non-association probability are both set to 0.5. Using p(a) to represent the association probability between objects is intuitive and easy to understand, but it is not easy to process. Therefore, we use a different expression to take the ratio of the two probabilities A(a) to describe the association relationship between objects.

[0071]

[0072] Assume that at time t, the association state A between objects has been obtained t (a) At the next moment t+1, a new observation w is obtained t+1 ∈{0,1},w t+1 Indicates the result of extracting the association relationship of the depth camera information, that is, whether the objects are associated or not. t (a) is updated and written in the form of conditional probability:

[0073]

[0074] w 1:t+1 Represents the observation superposition from the initial time to time t+1.

[0075] Then the update formula of the association probability is:

[0076]

[0077] According to the Bayesian formula, we can get:

[0078]

[0079]

[0080] p(a=1|w 1:t ) represents the prior probability, p(a=1|w 1:t+1 ) represents the posterior probability, Represents the possibility function. Finally, substituting formula (5) and formula (6) into formula (3) yields the updated model of the association probability:

[0081] Represents the update coefficient. The value of this update coefficient is 1.22.

[0082] Using a rapid randomized tree (RRT) algorithm, observation points are randomly generated within the raster-semantic map. The observation ranges of these observation points can be superimposed to cover the entire map. Once generated, semantic points and observation points in the environment are classified based on Euclidean distance. Based on the semantic information of the objects, the association probabilities between objects are combined with the observation points to construct a probabilistic semantic map that updates with this association information in real time.

[0083] Step 4: In the association probability semantic map, according to the current position of the robot, calculate the ratio of the association probability of the observation point to the distance between the robot and the observation point according to formula (8), which can also be called the gain value; based on the gain value of each observation point, use the greedy strategy to determine the traversal sequence of the observation point.

[0084]

[0085] Among them, p o is the association probability of the observation point. If the target has never been found at the observation point during training, the probability of the observation point is set to 0.1. ro is the distance between the robot and the observation point.

[0086] After determining the traversal sequence of observation points, a discrete search method is used to search for targets at the selected observation points. No target detection is performed on the journey from the current observation point to the next observation point. When the robot reaches the observation point, it will rotate in place for one circle to search for the target.

[0087] When the robot searches for a target in a room, it continuously updates the association probability between objects based on the information obtained by the sensors, so that the probability distribution model of objects in the room gradually conforms to the actual environment, and the robot will also perform more intelligent search behavior.

[0088] The present invention proposes to use the association relationship between objects to construct an association probability semantic map, and use real-time sensor information to continuously update the association relationship between objects, thereby improving the efficiency of the robot in performing long-term target search in complex and changing environments.

Claims

1. A robot target search method based on association probability semantic map, characterized in that The method comprises the following steps: Step 1: Construction of raster map Use LiDAR to describe the structural information of the current environment, generate a two-dimensional grid map as the underlying architecture, and use a positioning algorithm to determine the robot's position in the grid map; Step 2: Construction of grid-semantic map 2-1 Target identification; Use the target recognition algorithm to detect the RGB color image obtained by the depth camera, obtain the rectangular box surrounding the detected object, and use the rectangular box to obtain the midpoint pixel coordinates of the object; 2-2 Three-dimensional coordinate solution to obtain the coordinates of the object in the global map coordinate system; 2-3 Based on the coordinates of the object in the global map coordinate system, the object is annotated with semantic information, and the object semantics is mapped to the two-dimensional grid map, thereby obtaining a grid-semantic map; Step 3: Construction of association probability semantic map 3-1 Extract the relationship between two objects; 3-2 Bayesian-based association probability update; For two objects in the environment, a is used to represent the associated state, p(a=1) represents the probability of the two objects being associated, and p(a=0) represents the probability of the two objects being unassociated, and they satisfy formula (3); p(a=1)+p(a=0)=1 (3) For two objects that have not been observed, the initial associated probability and unassociated probability are both 0.5; The association state between objects is described by the ratio A(a) of the probability of association p(a=1) to the probability of non-association p(a=0); if A(a)>1, the association between objects is strong, and A(a)<1, the association between objects is weak. Assume that at time t the associated state A between the two objects t (a) At the next moment t+1, a new observation w is obtained t+1 ∈{0,1},w t+1 Indicates the result of extracting the association relationship of the depth camera information, that is, whether the objects are associated or not. At this time, the association relationship A between the related objects t (a) is updated and written in the form of conditional probability: where w 1:t+1 Represents the observation superposition from the initial moment to the moment t+1; Then the update formula of the association probability is: According to the Bayesian formula, we can get: where p(a=1|w 1:t ) represents the prior probability, p(a=1|w 1:t+1 ) represents the posterior probability, is an intermediate variable in the calculation; Substituting formula (7) and formula (8) into formula (5) yields the updated model of association probability: in represents the update coefficient; 3-3 Constructing Association Probability Semantic Map An observation point generation method based on a fast random tree algorithm is used to randomly generate observation points in the raster-semantic map. After the observation points are generated, semantic points and observation points in the environment are classified according to Euclidean distance, and semantic information is assigned to the observation points. Based on the semantic information of the objects, the association probability between objects and the observation points is combined to obtain an association probability semantic map based on real-time association information updates; Step 4: Target search based on probabilistic semantic map 4-1 In the association probability semantic map, according to the current position of the robot, the association probability of the observation point and the distance d between the robot and the observation point are calculated according to formula (10) ro The ratio of is taken as the gain of the current observation point; according to the gain of each observation point, the traversal sequence of the observation point is determined using a greedy strategy; Among them, p o is the association probability of the observation point; 4-2 Based on the traversal sequence of observation points, a discrete search method is used to search for targets at the selected observation points. Target detection is not performed on the way from the current observation point to the next observation point. When the robot reaches the observation point, it rotates in place for one circle before searching for targets again. Based on the information obtained by the depth camera during the robot's target search, combined with formula (9), the association probability between objects is updated and step 4 is repeated.

2. The method according to claim 1, characterized in that Step 2-2 is specifically: Combined with the Depth image obtained by the depth camera, the depth value corresponding to the midpoint pixel coordinate is obtained. The midpoint coordinate in the pixel plane coordinate system is converted into the coordinate in the camera coordinate system through the coordinate conversion of formula (1); Where, P uv =[uv 1] T is the homogeneous coordinate of the object in the pixel coordinate system, P c =[X c Y c Z c ] T is the coordinate of the object in the camera coordinate system, Z c is the object depth information measured by the depth camera, f x and f y are the focal lengths of the camera in the x and y directions, c x and c y are the translation relationships between the origin of the pixel coordinate system and the optical axis, respectively. The matrix K represents the intrinsic parameter matrix of the camera. According to the coordinates of the object in the camera coordinate system, the coordinates of the object in the global grid coordinate system are obtained through the coordinate transformation of formula (2); Where, P w =[X w Y w Z w ] T is the coordinate of the object in the global coordinate system, R and T are the rotation and translation transformation relationship of the camera coordinate system relative to the global coordinate system.

3. The method according to claim 1, characterized in that In step 3-1, 1) When a landmark object A is first recognized, its position in the global map coordinate system is expressed as (X l ,Y l ), and mark the coordinates of landmark object A on the grid-semantic map; 2) When a new landmark object B is recognized again, first determine whether the new landmark object B has the same semantic information as the existing landmark objects. If not, directly mark the coordinates and semantic information of the new landmark object B on the grid-semantic map. If so, traverse all existing landmark objects again in turn to solve the global coordinates of the new landmark object B and the semantic information of each existing landmark object (X i ,Y i )’s global coordinates, to determine whether the object is in (X i ,Y i ) as the center and within the neighborhood with the spatial threshold R as the radius, if yes, then the position and semantic information of the new landmark object B will not be recorded; otherwise, the coordinates and semantic information of the new landmark object B will be marked on the grid-semantic map; 3) When other objects C are identified besides the landmark object, the Euclidean distance between the global coordinates of the other object C and the global coordinates of each landmark object is calculated, and the landmark object (X j ,Y j ), determine whether the object C is in (X j ,Y j ) as the center and in a neighborhood with the spatial threshold R as the radius, if yes, it is considered as an association, otherwise it is considered as a non-association.

4. The method according to claim 1, characterized in that In step 3-3, the process of combining the object-object association probability with the observation point based on the semantic information of the object is: finding the observation point that matches the semantic information of the landmark object and assigning the association probability to the observation point.

5. The method according to claim 1, characterized in that In step 4-1, if the target has never been found at the observation point during training, the observation point probability p o Set to 0.

1.

6. A computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to execute the method according to any one of claims 1 to 5.

7. A computing device comprising a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, the method according to any one of claims 1 to 5 is implemented.

Images