# US 9,435,911 B2 · Visual-based obstacle detection method and apparatus for mobile robot

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

- Number: US 9,435,911 B2
- Document ID: US9435911B2
- Status: Granted
- Kind: B2
- Application number: 14/699,165
- Filed: 2015-04-29
- Published: 2015-11-05
- Granted: 2016-09-06
- Earliest claimed: 2014-04-30
- Assignees: Shenzhen Mercury Optoelectronics Research Institute
- Applicants: Shenzhen Mercury Optoelectronics Research Institute
- Inventors: Shuda Yu
- Web page: https://patentgrep.com/patent/US9435911B2

## CPC

- G01V 8/10: Detecting, e.g. by using light barriers (by reflection from the object G01S17/00)
- B25J 9/1666: {Avoiding collision or forbidden zones}
- B25J 9/1697: {Vision controlled systems}
- G05D 1/00: Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots (drive control systems specially adapted for autonomous road vehicles B60W60/00)
- G05D 1/0246: {using a video camera in combination with image processing means}
- G05D 1/0274: {using mapping information stored in a memory device (navigation using map-matching G01C21/30)}
- G06T 7/579: from motion
- G06T 7/73: using feature-based methods
- G06V 20/20: in augmented reality scenes
- G05B 2219/39082: Collision, real time collision avoidance
- G05B 2219/40519: Motion, trajectory planning
- G06T 2207/30244: Camera pose
- G06T 2207/30261: Obstacle
- Y10S 901/01: Mobile robot
- G01V: GEOPHYSICS; GRAVITATIONAL MEASUREMENTS; DETECTING MASSES OR OBJECTS; TAGS (means for indicating the location of accidentally buried, e.g. snow-buried, persons A63B29/02)
- 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})
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G05B: CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS (systems for controlling or regulating non-electric variables G05D; systems for regulating electric or magnetic variables G05F; control devices or systems insofar as characterised by mechanical features only G05G)
- Y10S: TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS

## Abstract

A visual-based obstacle detection method is provided. The method includes receiving image information of an environment containing a mobile robot captured by at least one visual sensor to obtain one of a three-dimensional space and a two-dimensional plane, and extracting and matching feature geometric primitives based on the received image information. The method also includes reconstructing coordinates of matched extracted feature geometric primitives and positions and orientations of the visual sensor and optimizing the reconstructed coordinates to obtain a calculation result, wherein the calculation result includes the feature geometric primitives, the positions and orientations of the visual sensor, as well as visible constraint relationships between the feature geometric primitives and the positions and orientations. Further, the method includes detecting obstacles in the environment containing the mobile robot based on the obtained calculation result and plotting a path for automatic motion of the mobile robot based on the detected obstacles.

## Claims

### Claim 1

A visual-based obstacle detection method, comprising:
receiving image information of an environment containing a mobile robot captured by at least one visual sensor to obtain a three-dimensional (3D) space;
based on the received image information, extracting feature geometric primitives;
based on a certain matching criteria, matching the extracted feature geometric primitives to obtain matched extracted feature geometric primitives;
reconstructing coordinates of the matched extracted feature geometric primitives and positions and orientations of the visual sensor;
optimizing the reconstructed coordinates to obtain a calculation result, wherein the calculation result includes the feature geometric primitives, the positions and orientations of the visual sensor, as well as visible constraint relationships between the feature geometric primitives and the positions and orientations of the visual sensor;
based on the obtained calculation result, detecting obstacles in the environment containing the mobile robot through dividing the 3D space of the environment containing the mobile robot into a plurality of subspaces, marking obstacle subspaces in the plurality of divided subspaces based on at least one of the feature geometric primitives, and the visible constraint relationships, and combining the obstacle subspaces into an obstacle space; and
plotting a path for automatic motion of the mobile robot based on the detected obstacles.

### Claim 2 (depends on 1)

The method according to claim 1, wherein dividing a 3D space of the environment containing the mobile robot into a plurality of subspaces includes:
dividing the 3D space containing the mobile robot into a plurality of subspaces using one of a regular grid format and an irregular combination structure.

### Claim 3 (depends on 2)

The method according to claim 2, wherein dividing the 3D space containing the mobile robot into a plurality of subspaces using a regular grid format further includes:
when a uniform grid structure is used to divide the 3D space, dividing the 3D space into a plurality of subspaces with a preset size; and
when a self-adaptive grid structure is used to divide the 3D space, dividing the 3D space into a plurality of subspaces with a same size, wherein when the size of the subspace is greater than a preset lower limit, and the divided subspace includes the feature geometric primitives, the divided subspace is further divided; otherwise, the division of the subspace stops.

### Claim 4 (depends on 3)

The method according to claim 3, wherein:
the feature geometric primitives include feature geometric points, as well as feature geometric points and feature geometric line segments.

### Claim 5 (depends on 2)

The method according to claim 2, wherein dividing the 3D space containing the mobile robot into a plurality of subspaces using an irregular combination structure further includes:
when the feature geometric primitives are the feature geometric points, creating the irregular combination structures using the feature geometric points as vertexes to divide the 3D space into a plurality of irregular polyhedrons; and
when the feature geometric primitives include the feature geometric points and the feature geometric line segments, creating the irregular combination structures by using the feature geometric points as vertexes and using the feature geometric line segments as edges to divide the 3D space into a plurality of irregular polyhedrons.

### Claim 6 (depends on 1)

The method according to claim 1, wherein marking obstacle subspaces in the plurality of divided subspaces includes:
marking the subspaces that meet a first preset condition as obstacle subspaces, wherein the first preset condition includes one of the following:
the subspace includes the feature geometric primitive; and
the subspace does not include the feature geometric primitive and the subspace is not intersected with any constraint primitive, wherein the constraint primitive is the primitive constituted by the feature geometric primitive and the position and orientation of the visual sensor which has the visible constraint relationship with the feature geometric primitive.

### Claim 7 (depends on 1)

The method according to claim 1, wherein marking obstacle subspaces in the plurality of divided subspaces further includes:
marking the subspaces that meet a second preset condition as obstacle subspaces, wherein the second preset condition is that the subspace is not intersected with any constraint primitive; and the constraint primitive is the primitive constituted by the feature geometric primitive and the position and orientation of the visual sensor which has the visible constraint relationship with the feature geometric primitive.

### Claim 8 (depends on 1)

The method according to claim 1, when a moving space of the mobile robot is a two-dimensional (2D) plane, further including:
based on the feature geometric primitives and positions and orientations of the visual sensor, calculating a plurality of 2D plane equations;
selecting a 2D plane equation from the plurality of 2D plane equations to determine a 2D plane;
selecting the feature geometric primitives to be projected from the feature geometric primitives;
projecting the selected feature geometric primitives to be projected and the positions and orientations of the visual sensor to the 2D plane corresponding to the 2D plane equation;
obtaining 2D feature geometric primitives, 2D positions and orientations of the visual sensor, and 2D visible constraint relationships;
dividing the 2D plane into a plurality of plane subspaces; and
based on at least one of the 2D feature geometric primitives and the 2D visible constraint relationships, marking 2D plane obstacle subspaces in the plurality of divided 2D plane subspaces.

### Claim 9 (depends on 1)

The method according to claim 1, further including:
when the environment containing the mobile robot changes, determining an updated calculation result and the changed subspaces based on updated image information and the calculation result, wherein the updated calculation result includes updated feature geometric primitives and updated visible constraint relationships; and
based on at least one of the updated feature geometric primitives and the updated visible constraint relationships, re-marking the obstacle subspaces in the changed subspaces.

### Claim 10

A visual-based obstacle detection apparatus, comprising:
a receiving module configured to receive image information of an environment containing a mobile robot captured by at least one visual sensor to obtain a three-dimensional (3D) space;
a calculation module configured to calculate the image information received by the receiving module using a Visual Simultaneous Localization and Mapping (Visual-SLAM) algorithm to obtain a calculation result, wherein the calculation result includes feature geometric primitives, positions and orientations of the visual sensor, and visible constraint relationships between the feature geometric primitives and the positions and orientations of the visual sensor; and
a processing module configured to, based on the obtained calculation result calculated by the calculation module, detect obstacles in the environment containing the mobile robot,
wherein the processing module further includes:
a space division unit configured to divide the 3D space of the environment containing the mobile robot into a plurality of subspaces;
a space marking unit configured to, based on at least one of the feature geometric primitives and the visible constrain relationships, mark obstacle subspaces among the plurality of subspaces divided by the space division unit; and
a combining unit configured to combine the obstacle subspaces marked by the marking unit into an obstacle space.

### Claim 11 (depends on 10)

The apparatus according to claim 10, wherein the space 1division unit further includes:
when a uniform grid structure is used to divide the 3D space, a regular grid division subunit configured to divide the 3D space into a plurality of subspaces with a preset size; and
when a self-adaptive grid structure is used to divide the 3D space, the regular grid division subunit configured to divide the 3D space into a plurality of subspaces with a same size, wherein when the size of the subspace is greater than a preset lower limit, and the divided subspace includes the feature geometric primitives, the divided subspace is further divided; otherwise, the division of the subspace stops.

### Claim 12 (depends on 10)

The apparatus according to claim 10, wherein:
the feature geometric primitives include feature geometric points, as well as feature geometric points and feature geometric line segments.

### Claim 13 (depends on 10)

The apparatus according to claim 10, wherein the space division unit further includes:
an irregular combination structure division subunit configured to, when the feature geometric primitives are the feature geometric points, create irregular combination structures by using the feature geometric points as vertexes, and divide the 3D space into a plurality of irregular polyhedrons; and
the irregular combination structure division subunit also configured to, when the feature geometric primitives include the feature geometric points and the feature geometric line segments, create the irregular combination structures by using the feature geometric points as vertexes and using the feature geometric line segments as edges, and divide the 3D space into a plurality of irregular polyhedrons.

### Claim 14 (depends on 10)

The apparatus according to claim 10, wherein the space marking unit further includes:
a first space marking subunit configured to mark the subspace that meets a first preset condition as the obstacle subspace, wherein the first preset condition includes one of the following:
the subspace contains the feature geometric primitives; and
the subspace does not contain the feature geometric primitives and the subspace has no intersection with any constraint primitive, wherein the constraint primitive is the primitive constituted by the feature geometric primitive and the position and orientation of the visual sensor which has the visible constraint relationship with the feature geometric primitive.

### Claim 15 (depends on 10)

The apparatus according to claim 10, wherein the space marking unit further includes:
a second space marking subunit configured to mark the subspace that meets a second preset condition as the obstacle subspace, wherein the second preset condition is that the subspace has no intersection with any constraint primitive; and the constraint primitive is the primitive constituted by the feature geometric primitive and the position and orientation of the visual sensor which has the visible constraint relationship with the feature geometric primitive.

### Claim 16 (depends on 10)

The apparatus according to claim 10, wherein the processing module further includes:
a plane equation calculation unit configured to, based on the feature geometric primitives and the positions and orientations of the visual sensor, calculate 2D plane equations;
a selection unit configured to select the feature geometric primitives to be projected from the feature geometric primitives; and
a projecting unit configured to project the feature geometric primitives to be projected and the positions and orientations of the visual sensor to the 2D plane corresponding to the 2D plane equation to obtain 2D feature geometric primitives, 2D positions and orientations of the visual sensor, and 2D visible constraint relationships.

### Claim 17 (depends on 10)

The apparatus according to claim 10, wherein:
the space division unit is further configured to divide the 2D plane corresponding to the 2D plane equation calculated by the plane equation calculation unit into a plurality of plane subspaces; and
the space marking unit is further configured to, based on at least one of the 2D feature geometric primitives and the 2D visible constraint relationships, mark the plane obstacle subspaces in the plane subspaces divided by the space division unit.

### Claim 18 (depends on 10)

The apparatus according to claim 10, wherein:
the calculation unit is further configured to, when the environment containing the mobile robot changes, determine an updated calculation result and changed subspaces based on updated image information and the calculation result, wherein the updated calculation result includes updated feature geometric primitives and updated visible constraint relationships; and
the processing module is further configured to, based on at least one of the updated feature geometric primitives and the updated visible constraint relationships, re-mark the obstacle subspaces in the changed subspaces.