# US 2021/0239472 A1 · System and Method for Probabilistic Multi-Robot Positioning

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2021/0239472 A1
- Document ID: US20210239472A1
- Status: Published application
- Kind: A1
- Application number: 16/777,445
- Filed: 2020-01-30
- Published: 2021-08-05
- Earliest claimed: 2020-01-30
- Assignees: Mitsubishi Electric Research Laboratories, Inc.
- Applicants: Mitsubishi Electric Research Laboratories, Inc.
- Inventors: Milutin Pajovic; Philip Orlik
- Web page: https://patentgrep.com/patent/US20210239472A1

## CPC

- G01C 21/32: Structuring or formatting of map data
- G05D 1/0274: {using mapping information stored in a memory device (navigation using map-matching G01C21/30)}
- B25J 9/1682: {Dual arm manipulator; Coordination of several manipulators}
- G01C 21/20: Instruments for performing navigational calculations (G01C21/24, G01C21/26 take precedence)
- G05D 1/0246: {using a video camera in combination with image processing means}
- 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)}
- G05D 1/0291: {Fleet control (monitoring fleets in traffic control systems for road vehicles G08G1/127, G08G1/127)}
- G01C: MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY (measuring liquid level G01F; radio navigation, determining distance or velocity by use of propagation effects, e.g. Doppler effects, propagation time, of radio waves, analogous arrangements using other waves G01S)
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- B25J: MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES ({manipulators specially adapted for use in surgery A61B34/70; manipulators used in cleaning hollow articles B08B9/04}; manipulators associated with rolling mills B21B39/20; manipulators associated with forging machines B21J13/10; {manipulators associated with picking-up and placing mechanisms B23P19/007}; means for holding wheels or parts thereof B60B30/00; {vehicles with ground-engaging propulsion means, e.g. walking members B62D57/02, B62D57/032; devices for picking-up and depositing articles or materials between conveyors B65G47/90, B65G47/91; manipulators with gripping or holding means for transferring packages B65H67/065}; cranes B66C; {manipulators used in the protection or supervision of pipe-line installations F17D5/00; walking equipment adapted for nuclear steam-generators F22B37/006}; manipulators specially adapted for, or associated with, nuclear reactors G21C; {apparatus used for handling wafers during manufacture or treatment of semiconductor H10P72/50})

## Abstract

A robot configured for multi-robot pose estimation execute iteratively the primary particle filter and the secondary particle filter independently from each other to update the pose estimations in the primary and the secondary particles. In response to each encounter of the robot with a neighboring robot, the robot receives neighboring particles of a particle filter of the neighboring robot identifying poses and likelihoods of the poses of the neighboring robot and replaces the primary particles of the primary particle filter with the secondary particles fused with the neighboring particles. The robot outputs its pose according to the primary particles of the primary particle filter.

## Claims

### Claim 1

A robot configured for multi-robot pose estimation, comprising:
a set of sensors for measuring movements of the robot in an environment to produce sensor measurements;
a memory configured to store a primary particle filter maintaining a set of primary particles and a secondary particle filter maintaining a set of secondary particles, each particle in the primary and secondary set of particles includes an estimation of a pose of the robot and a weight indicative of a probability of the pose estimation;
a processor configured to
execute iteratively the primary particle filter and the secondary particle filter independently from each other to update the pose estimations in the primary and the secondary particles based on the sensor measurements;
receive, in response to each encounter of the robot with a neighboring robot, neighboring particles of a particle filter of the neighboring robot identifying poses and likelihoods of the poses of the neighboring robot;
replace, in response to each encounter of the robot with the neighboring robot, the primary particles of the primary particle filter with the secondary particles fused with the neighboring particles; and
an output interface configured to output the pose of the robot according to the primary particles of the primary particle filter of the robot.

### Claim 2 (depends on 1)

The robot of claim 1, wherein the memory stores a particle tuner configured, upon execution by the processor, to
pair a secondary particle with a neighboring particle;
determine a weight of the paired particles in reverse proportion to a pose error between a relative pose defined by the paired particles and a relative pose between the robot and the neighboring robot measured at the encounter; and
update the secondary particles in accordance to the weights of corresponding paired particles.

### Claim 3 (depends on 2)

The robot of claim 2, wherein the particle tuner stores values of the measured relative pose and values of the neighboring particles for each of the encounter of the robot with the neighboring robot and determines the weight of the paired particles based on a product the pose errors of the paired particles at each of the encounter.

### Claim 4 (depends on 3)

The robot of claim 3, wherein in response to pairing the secondary particle with the neighboring particle at the second encounter, the particle tuner is configured to
select first poses estimated in the paired particles at the first encounter and second poses estimated in the paired particles at the second encounter;
estimate a first relative pose between first poses of the paired particles and a second relative pose between the second poses;
determine a first pose error as a difference between the first relative pose and the relative pose measured at the first encounter and determines a second pose error as a difference between the second relative pose and the relative pose measured at the second encounter; and
update the weight of the secondary particle in the paired particles in reverse proportion to the product of the first pose error and the second pose error.

### Claim 5 (depends on 2)

The robot of claim 2, wherein pairing the secondary particle with the neighboring particle includes sampling the secondary particle from the set of neighboring particles and sampling the neighboring particle from the set of neighboring particles.

### Claim 6 (depends on 5)

The robot of claim 5, wherein the secondary and the neighboring particles have equal weights and the sampling is arbitrarily.

### Claim 7 (depends on 6)

The system of claim 6, wherein the particle tuner samples the secondary and the neighboring particles uniformly randomly.

### Claim 8 (depends on 5)

The robot of claim 5, wherein the sampling is based on the weights of the secondary and the neighboring particles.

### Claim 9 (depends on 8)

The system of claim 8, wherein the particle tuner samples the secondary and the neighboring particles non-uniformly randomly according to their weights, such as a particle with a larger weight is more likely to be sampled than a particle with a smaller weight.

### Claim 10 (depends on 1)

The system of claim 1, wherein the particle tuner, to update the secondary particles of the robot, is configured to
resample the secondary particles according to their weights to produce a set of secondary particles representing the pose of the robot with equal weights.

### Claim 11 (depends on 1)

The system of claim 1, wherein the particle tuner, to update the secondary particles of the robot, is configured to
normalize weights of the secondary particles to produce a set of secondary particles representing the pose of the robot with different weights.

### Claim 12 (depends on 1)

The robot of claim 1, wherein fusion of the secondary particles with the neighboring particles includes updating the weights of the secondary particles based on reverse proportion to a product of pose errors of the secondary and neighboring particles at each encounter.

### Claim 13 (depends on 1)

The robot of claim 1, further comprising:
an input interface configured to receive the neighboring particles and the relative pose between the robot and the neighboring robot at each encounter.

### Claim 14 (depends on 1)

The robot of claim 1, further comprising:
a camera and an image processor configured to estimate the relative pose between the robot and the neighboring robot at each encounter.

### Claim 15

A method for estimating a pose of a robot, wherein the pose includes one or combination of a location of the robot and an orientation of the robot, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out steps of the method, comprising:
receiving sensor measurements of movements of the robot in an environment;
executing iteratively a primary particle filter maintaining a set of primary particles and a secondary particle filter maintaining a set of secondary particles independently from each other to update the pose estimations in the primary and the secondary particles based on the sensor measurements, wherein each particle in the primary and secondary set of particles includes an estimation of a pose of the robot and a weight indicative of a probability of the pose estimation;
receiving, in response to each encounter of the robot with a neighboring robot, neighboring particles of a particle filter of the neighboring robot identifying poses and likelihoods of the poses of the neighboring robot;
replacing, in response to each encounter of the robot with the neighboring robot, the primary particles of the primary particle filter with the secondary particles fused with the neighboring particles; and
outputting the pose of the robot according to the primary particles of the primary particle filter of the robot.

### Claim 16

A non-transitory computer readable storage medium embodied thereon a program executable by a processor for performing a method, the method comprising:
receiving sensor measurements of movements of the robot in an environment;
execute iteratively a primary particle filter maintaining a set of primary particles and a secondary particle filter maintaining a set of secondary particles independently from each other to update the pose estimations in the primary and the secondary particles based on the sensor measurements, wherein each particle in the primary and secondary set of particles includes an estimation of a pose of the robot and a weight indicative of a probability of the pose estimation;
receiving, in response to each encounter of the robot with a neighboring robot, neighboring particles of a particle filter of the neighboring robot identifying poses and likelihoods of the poses of the neighboring robot;
replacing, in response to each encounter of the robot with the neighboring robot, the primary particles of the primary particle filter with the secondary particles fused with the neighboring particles; and
outputting the pose of the robot according to the primary particles of the primary particle filter of the robot.