Path planning device, path planning method, and computer program
First Claim
1. A path planning device for planning a position and posture path of a control target in a motion space from a present position and posture to a 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 an 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 external forces for each of a plurality of rigid bodies based on a dynamic model that uses the external forces to express a requirement for smoothing the position and posture path and for avoiding the interference by the obstruction, the dynamic model modeling each of the plurality of rigid bodies as a virtue rigid body plane not having a volume; 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 representing 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 a basis of positions and postures of the rigid bodies when the path is convergent to a predetermined state.
1 Assignment
0 Petitions
Accused Products
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.
32 Citations
8 Claims
-
1. A path planning device for planning a position and posture path of a control target in a motion space from a present position and posture to a 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 an 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 external forces for each of a plurality of rigid bodies based on a dynamic model that uses the external forces to express a requirement for smoothing the position and posture path and for avoiding the interference by the obstruction, the dynamic model modeling each of the plurality of rigid bodies as a virtue rigid body plane not having a volume; 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 representing 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 a basis of positions and postures of the rigid bodies when the path is convergent to a predetermined state. - View Dependent Claims (2, 3, 4, 5, 6)
-
-
7. A path planning method of planning a position and posture path of a control target in a motion space from a present position and posture to a target position and posture, comprising the steps of:
-
acquiring, by an initial position and posture path acquiring unit, 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, by an interference detector, an interference with the control target in the position and posture path by an obstruction existing in the motion space; calculating, by a processor, external forces for each of a plurality of rigid bodies based on a dynamic model that uses external forces to express a requirement for smoothing the position and posture path and for avoiding the interference by the obstruction, the dynamic model modeling each of the plurality of rigid bodies as a virtue rigid body plane not having a volume; and performing, by the processor, a multi rigid body system dynamics simulation on a plurality of equations of rigid body motion representing 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 a basis of positions and postures of the rigid bodies when the path is convergent to a predetermined state.
-
-
8. A non-transitory computer readable storage medium encoded with a computer program which, when executed on a computer, causes the computer to perform processes of planning a position and posture path of a control target in a motion space from a present position and posture to a 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 an 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 external forces for each of a plurality of rigid bodies based on a dynamic model that uses the external forces to express a requirement for smoothing the position and posture path and for avoiding interference by the obstruction, the dynamic model modeling each of the plurality of rigid bodies as a virtue rigid body plane not having a volume; 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 representing 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 a basis of the positions and postures of the rigid bodies when the path is convergent to a predetermined state.
-
Specification