Using sensor-based observations of agents in an environment to estimate the pose of an object in the environment and to estimate an uncertainty measure for the pose

• US 10,209,063 B2
• Filed: 10/03/2015
• Issued: 02/19/2019
• Est. Priority Date: 10/03/2015
• Status: Active Grant

• Current Assignee: Intrinsic Innovation LLC (Alphabet Inc.)
• Original Assignee: X Development LLC (Alphabet Inc.)
• Inventors: Anderson-Sprecher, Peter Elving
• Primary Examiner(s): Henson, Mischita
• Assistant Examiner(s): Liao, Christine
• Application Number:
  US14/874,419
• Publication Number:
  US 20170097232A1
• Time in Patent Office: 1,235 Days
• CPC Class Codes: B25J 5/007 mounted on wheels
• CPC Class Codes: B25J 9/1697 Vision controlled systems
• CPC Class Codes: G01B 21/00 Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
• CPC Class Codes: G01C 15/00 Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
• CPC Class Codes: G01C 21/20 Instruments for performing navigational calculations G01C21/24, G01C21/26 take precedence
• CPC Class Codes: G01C 21/206 specially adapted for indoor navigation
• CPC Class Codes: G01C 21/383 Indoor data
• CPC Class Codes: G05D 1/0088 characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours using knowledge based models G06N5/00
• CPC Class Codes: G05D 1/0231 using optical position detecting means position-fixing by using electromagnetic waves other than radio waves, e.g. optical position detecting means G01S5/16
• CPC Class Codes: G05D 1/0234 using optical markers or beacons optical beacons per se G01S1/70
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0246 using a video camera in combination with image processing means
• CPC Class Codes: G05D 1/0255 using acoustic signals, e.g. ultra-sonic singals sonar systems designed for anti-collision purposes G01S15/93
• CPC Class Codes: G05D 1/0261 using magnetic plots
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G05D 1/0297 by controlling means in a control room
• CPC Class Codes: Y10S 901/01 Mobile robot
• CPC Class Codes: Y10S 901/31 Gripping jaw
• CPC Class Codes: Y10S 901/47 Optical

Abstract

Methods, apparatus, systems, and computer-readable media are provided for using sensor-based observations from multiple agents (e.g., mobile robots and/or fixed sensors) in an environment to estimate the pose of an object in the environment at a target time and to estimate an uncertainty measure for that pose. Various implementations generate a multigraph based on a group of observations from multiple agents, where the multigraph includes a reference frame node, object nodes, and a plurality edges connecting the nodes. In some implementations, a composite pose and composite uncertainty measure are generated for each of a plurality of simple paths along the edges of the multigraph that connect the reference frame node to a given object node—and a pose and uncertainty measure for an object identifier associated with the given object node is generated based on the composite poses and the composite uncertainty measures.

Claims

1. A computer-implemented method, comprising:

receiving a plurality of observations from a plurality of agents in an environment, each of the observations generated based on sensor data from one or more sensors of a corresponding agent of the agents and including;

an observation time, a source identifier of the corresponding agent, an observed object identifier of a corresponding observed object of multiple objects of the environment, a measured object pose for the corresponding observed object, and an observation uncertainty measure for the measured object pose;

identifying a group of the observations for a target time based on the observation times of the observations of the group satisfying a threshold relative to the target time;

generating, based on the group of observations, a multigraph having a plurality of nodes and edges connecting the nodes, the generating comprising defining a reference frame node of the nodes, defining object nodes of the nodes that are based on the observed object identifiers of the group, and generating the edges connecting the nodes based on the observed object identifiers, the source identifiers, and the measured object poses and observation uncertainty measures of the group,

wherein generating each of the edges comprises;

connecting the edge with a respective pair of nodes,

determining an edge measured object pose based on at least one of one or more of the measured object poses corresponding to the respective pair of nodes, and

determining an edge object uncertainty measure based on at least one of one or more of the object uncertainty measures corresponding to the respective pair of the nodes, and

wherein generating the multigraph comprises;

generating a first set of multiple of the edges, wherein the first set of the edges define a first simple path that connects the reference frame node and a given object node of the object nodes, the first set of the edges comprising a first set first edge that connects the given object node to a first object node, and a first set second edge that connects the first object node to a second object node or to the reference frame node, and

generating a second set of multiple of the edges that define a second simple path that connects the reference frame node and the given object node, the second set of edges comprising a second set first edge that is in addition to the edges of the first set of the edges that define the first simple path;

generating a composite pose and composite uncertainty measure for each of a plurality of simple paths along the edges that connect the reference frame node and the given object node of the object nodes, including the first simple path and the second simple path, wherein the composite pose and the composite uncertainty measure for each of the simple paths is based on the edge measured object pose and the edge observation uncertainty measure for each of the edges of the simple path;

generating, based on the composite poses and the composite uncertainty measures, a pose and an uncertainty measure for the object identifier on which the given object node is based;

assigning the pose and the uncertainty measure to the object identifier on which the given object node is based; and

providing the object identifier, the pose, and the uncertainty measure to at least one robotic agent, of the agents in the environment, to influence control commands generated by the robotic agent.

2. The computer-implemented method of claim 1, wherein at least one of the observed object identifiers of the observations identifies one of the agents of the environment.

3. The computer-implemented method of claim 1, wherein the object identifier on which the given object node is based identifies a robotic agent of the agents of the environment.

4. The computer-implemented method of claim 1, wherein:

the observed object identifier of a first observation identifies a mobile robot agent of the agents and the source identifier of the first observation identifies the mobile robot agent; and

the observed object identifier of a second observation identifies a non-agent object of the agents and the source identifier of the second observation identifies the mobile robot agent.

5. The computer-implemented method of claim 4, wherein:

the observed object identifier of a third observation identifies the non-agent object of the agents and the source identifier of the third observation identifies a fixed sensor agent of the agents.

6. The computer-implemented method of claim 1, wherein generating the pose and the uncertainty measure based on the composite pose and the composite uncertainty measure comprises:

merging the composite poses and the composite uncertainty measures for the plurality of simple paths.

7. The computer-implemented method of claim 6, wherein merging the composite poses and the composite uncertainty measures for the plurality of simple paths comprises weighting, in the merging, the composite poses and the composite uncertainty measures based on the composite uncertainty measures.

8. The computer-implemented method of claim 1, wherein generating an edge of the edges along a simple path of the simple paths comprises:

generating the edge measured object pose and the edge observation uncertainty measure for the edge based on spherical linear interpolation of the edge measured object pose and the edge observation uncertainty measure of a first observation of the observations of the group and the edge measured object pose and the edge observation uncertainty measure of a second observation of the observations of the group, wherein the first observation and the second observation both identify the same observed object identifier and the same source identifier.

9. The computer-implemented method of claim 1, wherein generating the composite pose and the composite uncertainty measure for each of the plurality of simple paths comprises applying an unscented transform to the edge measured object poses and the edge observation uncertainty measures for the edges of the simple path.

10. The computer-implemented method of claim 1, wherein the first set second edge connects the first object node to the second object node, and wherein the first set of the edges further comprise a first set third edge that connects the second object node to a third object node or to the reference frame node.

11. A system comprising:

a pose and uncertainty system comprising one or more computing systems;

a plurality of agents in an environment providing sensor-based observations to the pose and uncertainty system over a period of time, wherein each of the observations is generated based on sensor data from one or more sensors of a corresponding agent of the agents and includes;

an observation time, a source identifier of the corresponding agent, an observed object identifier of a corresponding observed object of multiple objects of the environment, a measured object pose for the corresponding observed object, and an observation uncertainty measure for the measured object pose;

wherein the pose and uncertainty system comprises instructions that, when executed by the one or more computing systems, cause the computing systems to;

identify a group of the observations for a target time based on the observation times of the observations of the group satisfying a threshold relative to the target time;

generate, based on the group of observations, a multigraph having a plurality of nodes and edges connecting the nodes, the generating comprising defining a reference frame node of the nodes, defining object nodes of the nodes that are based on the observed object identifiers of the group, and generating the edges connecting the nodes based on the observed object identifiers, the source identifiers, and the measured object poses and observation uncertainty measures of the group,

wherein in generating each of the edges the instructions cause one or more of the computing systems to;

connect the edge with a respective pair of nodes,

determine an edge measured object pose based on at least one of one or more of the measured object poses corresponding to the respective pair of nodes, and

determine an edge object uncertainty measure based on at least one of one or more of the object uncertainty measures corresponding to the respective pair of the nodes, and

wherein in generating the multigraph the instructions cause one or more of the computing systems to;

generate a first set of multiple of the edges, wherein the first set of the edges define a first simple path that connects the reference frame node and a given object node of the object nodes, the first set of the edges comprising a first set first edge that connects the given object node to a first object node, and a first set second edge that connects the first object node to a second object node or to the reference frame node, and

generate a second set of multiple of the edges that define a second simple path that connects the reference frame node and the given object node, the second set of edges comprising a second set first edge that is in addition to the edges of the first set of the edges that define the first simple path;

generate a composite pose and composite uncertainty measure for each of a plurality of simple paths along the edges that connect the reference frame node and the given object node of the object nodes, including the first simple path and the second simple path, wherein the composite pose and the composite uncertainty measure for each of the simple paths is based on the edge measured object pose and the edge observation uncertainty measure for each of the edges of the simple path;

generate, based on the composite poses and the composite uncertainty measures, a pose and an uncertainty measure for the object identifier on which the given object node is based;

assign the pose and the uncertainty measure to the object identifier on which the given object node is based; and

provide the object identifier and the pose to at least one robotic agent, of the agents in the environment, to influence control commands generated by the robotic agent.

12. The system of claim 11, wherein at least one of the observed object identifiers of the observations identifies one of the agents of the environment.

13. The system of claim 11, wherein the object identifier on which the given object node is based identifies a robotic agent of the agents of the environment.

14. The system of claim 11, wherein:

the observed object identifier of a first observation identifies a mobile robot agent of the agents and the source identifier of the first observation identifies the mobile robot agent; and

the observed object identifier of a second observation identifies a non-agent object of the agents and the source identifier of the second observation identifies the mobile robot agent.

15. The system of claim 11, wherein the instructions that, when executed by the one or more computing systems, cause the computing systems to generate the pose and the uncertainty measure based on the composite pose and the composite uncertainty measure comprise instructions that cause the computing systems to:

merge the composite poses and the composite uncertainty measures for the plurality of simple paths.

16. The system of claim 15, wherein the instruction that, when executed by the one or more computing systems, cause the computing systems to merge the composite poses and the composite uncertainty measures for the plurality of simple paths comprises instructions that cause the computing systems to weight, in the merge, the composite poses and the composite uncertainty measures based on the composite uncertainty measures.

17. The system of claim 11, wherein the instruction that, when executed by the one or more computing systems, cause the computing systems to generate an edge of the edges along a simple path of the simple paths comprise instructions that cause the computing systems to:

generate the edge measured object pose and the edge observation uncertainty measure for the edge based on spherical linear interpolation of the edge measured object pose and the edge observation uncertainty measure of a first observation of the observations of the group and the edge measured object pose and the edge observation uncertainty measure of a second observation of the observations of the group, wherein the first observation and the second observation both identify the same observed object identifier and the same source identifier.

18. The system of claim 11, wherein the instruction that, when executed by the one or more computing systems, cause the computing systems to generate the composite pose and the composite uncertainty measure for each of the plurality of simple paths comprise instructions that cause the computing systems to apply an unscented transform to the edge measured object poses and the edge observation uncertainty measures for the edges of the simple path.

19. The system of claim 11, wherein the first set second edge connects the first object node to the second object node, and wherein the first set of the edges further comprise a first set third edge that connects the second object node to a third object node or to the reference frame node.

20. At least one non-transitory computer-readable medium comprising instructions that, in response to execution of the instructions by one or more processors, cause the one or more processors to perform the following operations:

receiving a plurality of observations from a plurality of agents in an environment, each of the observations generated based on sensor data from one or more sensors of a corresponding agent of the agents and including;

an observation time, a source identifier of the corresponding agent, an observed object identifier of a corresponding observed object of multiple objects of the environment, a measured object pose for the corresponding observed object, and an observation uncertainty measure for the measured object pose;

identifying a group of the observations for a target time based on the observation times of the observations of the group satisfying a threshold relative to the target time;

generating, based on the group of observations, a multigraph having a plurality of nodes and edges connecting the nodes, the generating comprising defining a reference frame node of the nodes, defining object nodes of the nodes that are based on the observed object identifiers of the group, and generating the edges connecting the nodes based on the observed object identifiers, the source identifiers, and the measured object poses and observation uncertainty measures of the group,

wherein generating each of the edges comprises;

connecting the edge with a respective pair of nodes,

determining an edge measured object pose based on at least one of one or more of the measured object poses corresponding to the respective pair of nodes, and

determining an edge object uncertainty measure based on at least one of one or more of the object uncertainty measures corresponding to the respective pair of the nodes, and

wherein generating the multigraph comprises;

generating a first set of multiple of the edges, wherein the first set of the edges define a first simple path that connects the reference frame node and a given object node of the object nodes, the first set of the edges comprising a first set first edge that connects the given object node to a first object node, and a first set second edge that connects the first object node to a second object node or to the reference frame node, and

generating a second set of multiple of the edges that define a second simple path that connects the reference frame node and the given object node, the second set of edges comprising a second set first edge that is in addition to the edges of the first set of the edges that define the first simple path;

generating a composite pose and composite uncertainty measure for each of a plurality of simple paths along the edges that connect the reference frame node and the given object node, including the first simple path and the second simple path, wherein the composite pose and the composite uncertainty measure for each of the simple paths is based on the edge measured object pose and the edge observation uncertainty measure for each of the edges of the simple path;

generating, based on the composite poses and the composite uncertainty measures, a pose and an uncertainty measure for the object identifier on which the given object node is based;

assigning the pose and the uncertainty measure to the object identifier on which the given object node is based; and

providing the object identifier and the pose to at least one robotic agent, of the agents in the environment, to influence control commands generated by the robotic agent.