# US 2010/0168950 A1 · Path Planning Device, Path Planning Method, and Computer Program

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

- Number: US 2010/0168950 A1
- Document ID: US20100168950A1
- Status: Published application
- Kind: A1
- Application number: 12/645,841
- Filed: 2009-12-23
- Published: 2010-07-01
- Earliest claimed: 2009-01-01
- Assignees: Sony Corporation
- Inventors: Masakuni NAGANO
- Web page: https://patentgrep.com/patent/US20100168950A1

## CPC

- B25J 9/1666: {Avoiding collision or forbidden zones}
- 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 path planning device includes: an initial position and posture path acquiring unit which acquires an initial position and posture path including a plurality of nodes connecting present position and posture of a control target to target position and posture and which configures a rigid body corresponding to the control target in each node in the position and posture path; an interference detector which detects interference with the control target by an obstruction; an external force calculator which calculates an external force for each rigid body; and a multi rigid body system dynamics simulation unit which performs a multi rigid body system dynamics simulation on a plurality of equations of rigid body motion and which acquires positions and postures of the control target at times corresponding to the nodes in the position and posture path.

## Claims

### Claim 1

A path planning device planning a position and posture path of a control target in a motion space from present position and posture to target position and posture, comprising:
an initial position and posture path acquiring unit which acquires an initial position and posture path including a plurality of nodes connecting the present position and posture of the control target to the target position and posture and which configures a rigid body corresponding to the control target in each node in the position and posture path;
an interference detector which detects interference with the control target in the position and posture path by an obstruction existing in the motion space;
an external force calculator which calculates an external force for each rigid body expressing a requirement for smoothing the position and posture path and avoiding interference by the obstruction; and
a multi rigid body system dynamics simulation unit which performs a multi rigid body system dynamics simulation on a plurality of equations of rigid body motion in which coupled motions of the plurality of rigid bodies on which the external forces calculated by the external force calculator act, respectively, and which acquires positions and postures of the control target at times corresponding to the nodes in the position and posture path on the basis of the positions and postures of the rigid bodies when the path is convergent to a predetermined state.

### Claim 2 (depends on 1)

The path planning device according to claim 1, wherein the initial position and posture path acquiring unit acquires a path between the present position of the control target and the target position on the basis of a search tree branched into a plurality of routes to the plurality of nodes and developed in the motion space and gives an initial posture to the rigid body located at each node.

### Claim 3 (depends on 1)

The path planning device according to claim 1, wherein the interference detector applies a GJK algorithm (Gilbert-Johnson-Keerthi distance algorithm) to detect the shortest distance between each rigid body and the obstruction.

### Claim 4 (depends on 1)

The path planning device according to claim 1, wherein the external force calculator uses a dynamical model in which a virtual rigid body plane fixed in a local coordinate system of each rigid body is defined and corresponding vertexes of the neighboring virtual rigid body planes are connected to each other by springs having a predetermined spring constant and dampers having a predetermined damping constant to calculate, as the external forces expressing the requirement for smoothing the position and posture path, an inter-rigid-body force which is the sum of spring forces and damper forces acting between the vertexes of one virtual rigid body plane and the corresponding vertexes of the neighboring virtual rigid body plane and a rotational moment, which acts on the virtual rigid body, about an origin of a global coordinate system including the sum of moments acting on the vertexes of each virtual rigid body due to the spring forces and the damper forces.

### Claim 5 (depends on 1)

The path planning device according to claim 1, wherein the interference detector detects the shortest distance between the rigid body located at each node in the position and posture path and the obstruction, and
wherein the external force calculator calculates a repulsive force acting on each rigid body depending on the shortest distance as the external force expressing the requirement for causing the position and posture path to avoid interference by the obstruction.

### Claim 6 (depends on 1)

The path planning device according to claim 1, wherein the external force calculator calculates the moment corresponding to an angle formed by a forward moving direction of each rigid body fixed in the local coordinate system of the rigid body and a direction of the position and posture path between the neighboring rigid bodies as the external force expressing a requirement for approaching an ideal position and posture path.

### Claim 7

A path planning method of planning a position and posture path of a control target in a motion space from present position and posture to target position and posture, comprising the steps of:
acquiring an initial position and posture path including a plurality of nodes connecting the present position and posture of the control target to the target position and posture and configuring a rigid body corresponding to the control target in each node in the position and posture path;
detecting interference with the control target in the position and posture path by an obstruction existing in the motion space;
calculating an external force for each rigid body expressing a requirement for smoothing the position and posture path and avoiding interference by the obstruction; and
performing a multi rigid body system dynamics simulation on a plurality of equations of rigid body motion in which coupled motions of the plurality of rigid bodies on which the calculated external forces act, respectively, and acquiring positions and postures of the control target at times corresponding to the nodes in the position and posture path on the basis of the positions and postures of the rigid bodies when the path is convergent to a predetermined state.

### Claim 8

A computer program which is described in a computer-readable manner so as to cause a computer to perform processes of planning a position and posture path of a control target in a motion space from present position and posture to target position and posture, the computer program causing the computer to serve as:
an initial position and posture path acquiring unit which acquires an initial position and posture path including a plurality of nodes connecting the present position and posture of the control target to the target position and posture and which configures a rigid body corresponding to the control target in each node in the position and posture path;
an interference detector which detects interference with the control target in the position and posture path by an obstruction existing in the motion space;
an external force calculator which calculates an external force for each rigid body expressing a requirement for smoothing the position and posture path and avoiding interference by the obstruction; and
a multi rigid body system dynamics simulation unit which performs a multi rigid body system dynamics simulation on a plurality of equations of rigid body motion in which coupled motions of the plurality of rigid bodies on which the external forces calculated by the external force calculator act, respectively, and which acquires positions and postures of the control target at times corresponding to the nodes in the position and posture path on the basis of the positions and postures of the rigid bodies when the path is convergent to a predetermined state.
