JP2013246553A - Track planning device, track planning method and track planning program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To perform a track planning for a robot by performing interference determination between the robot and an obstacle at an optimum frequency.SOLUTION: A track planning device plans a track for a robot to travel from a start point to an end point. The track planning device includes: a distance calculation means which calculates a distance between a robot and an obstacle on a planned track; a discretization setting means which sets a discretization width for discretizing the track from the start point to the end point on the basis of the distance calculated by the distance calculation means; an interference determination means which determines whether the robot and the obstacle are interfering or not at each position on the track when discretizing in the discretization width set by the discretization setting means; and a track planning means which plans the track of the robot on the basis of the interference determination result by the interference determination means.

Description

  The present invention relates to a trajectory planning apparatus, a trajectory planning method, and a trajectory planning program that can plan a trajectory of a robot while determining interference between a robot and an obstacle.

  In recent years, various trajectory planning apparatuses that plan trajectories for moving a moving body such as a robot from a start point to an end point are known. For example, there is a motion path planning method for planning a robot trajectory by discriminating between the trajectories from the start point to the end point and providing subgoals at each discretized position, determining interference between the robot and an obstacle at each subgoal. It is known (see Patent Document 1).

JP 2002-073130 A

  However, in the motion path planning method disclosed in Patent Document 1, for example, when the discretization width between trajectories from the start point to the end point is set large, and the interference determination is performed at each discretized position, the robot is obstructed. There is a possibility that the interference judgment accuracy will be lowered. On the other hand, if the discretization width between the trajectories from the start point to the end point is set small and interference determination is performed at each discrete position, the interference determination frequency between the robot and the obstacle increases, and the calculation time increases. Occurs.

  The present invention has been made to solve such a problem, and a trajectory planning apparatus and trajectory plan capable of performing trajectory planning of a robot by performing interference determination between the robot and an obstacle at an optimum frequency. The main purpose is to provide a method and a trajectory planning program.

One aspect of the present invention for achieving the above object is a trajectory planning apparatus for planning a trajectory for a robot to move from a start point to an end point, and a distance between the robot and an obstacle on the planned trajectory. A distance calculating means for calculating a discretization, a discretization setting means for setting a discretization width for discretizing between trajectories from the start point to the end point based on the distance calculated by the distance calculation means, and the discretization setting means Interference determination means for determining whether or not the robot and the obstacle are interfering at each position on the trajectory when discretized with the discretization width set by the step, and the interference determination result by the interference determination means And a trajectory planning means for planning the trajectory of the robot based on the trajectory planning apparatus.
In this aspect, the discretization setting means may increase the discretization interval as the distance calculated by the distance calculation means increases.
In this aspect, the discretization setting unit calculates a Jacobian of the robot based on the distance calculated by the distance calculation unit and the joint angle of the robot calculated by the trajectory planning unit. A calculation unit; and a parameter calculation unit that calculates the discretization interval based on the Jacobian of the robot calculated by the Jacobian calculation unit and the distance calculated by the distance calculation unit. .
In this aspect, the distance calculation means may calculate the shortest distance between the robot and the obstacle on the planned trajectory.
In this aspect, the robot may be an arm robot, and the distance calculation unit may calculate a distance between the joint portion of the arm robot and the obstacle that is the predetermined closest to the obstacle.
In this aspect, the distance calculating unit may calculate a distance between the obstacle and a characteristic part of the robot.
In this aspect, the parameter calculation unit multiplies the robot Jacobian inverse matrix calculated by the Jacobian calculation unit, the shortest distance calculated by the distance calculation unit, and a predetermined margin constant, The discretization interval may be calculated.
In this aspect, the predetermined margin constant may be set to be large in the case of self-interference of the robot, and may be set to be small in the case of interference with other objects.
In this aspect, the characteristic portion of the robot may include at least one of a hand tip, a fingertip, a complicated shape portion, and a thin shape portion.
On the other hand, one aspect of the present invention for achieving the above object is a trajectory planning method for planning a trajectory for a robot to move from a start point to an end point, the robot and an obstacle on the planned trajectory. A step of calculating a distance, a step of setting a discretization width for discretizing between trajectories from the start point to the end point based on the calculated distance, and a discretization with the set discretization width Determining whether the robot and an obstacle are interfering at each position on the trajectory, and planning the trajectory of the robot based on the interference determination result. The trajectory planning method may be a feature.
Another aspect of the present invention for achieving the above object is a trajectory planning program for planning a trajectory for a robot to move from a start point to an end point, the robot and an obstacle on the planned trajectory. A process for calculating the distance, a process for setting a discretization width for discretizing the trajectory from the start point to the end point based on the calculated distance, and a discretization with the set discretization width Causes the computer to execute processing for determining whether or not the robot and an obstacle are interfering at each position on the trajectory, and processing for planning the trajectory of the robot based on the interference determination result , Or a trajectory planning program characterized by that.

  According to the present invention, it is possible to provide a trajectory planning apparatus, a trajectory planning method, and a trajectory planning program capable of performing trajectory planning of a robot by performing interference determination between a robot and an obstacle at an optimal frequency.

1 is a block diagram showing a schematic system configuration of a trajectory planning apparatus according to an embodiment of the present invention. It is a figure for demonstrating an arm robot and the shortest distance. It is a figure for demonstrating the discretization width | variety according to the shortest distance of an arm robot and an obstruction. It is a figure for demonstrating the interference determination frequency according to the shortest distance of an arm robot and an obstruction. It is a flowchart which shows an example of the trajectory plan process by the trajectory plan apparatus which concerns on one embodiment of this invention.

  Embodiments of the present invention will be described below with reference to the drawings. In the trajectory planning apparatus 1 according to an embodiment of the present invention, for example, an arm robot 100 having a plurality of links 101 and a plurality of joint portions 102 that rotatably connect the links 101 is an end point from a start point (start point). The trajectory for moving to (goal point) is optimally planned (FIG. 2). FIG. 1 is a block diagram showing a schematic system configuration of a trajectory planning apparatus according to an embodiment of the present invention.

  The trajectory planning apparatus 1 includes an environment restoration unit 2, a distance calculation unit 3, a Jacobian calculation unit 4, a parameter calculation unit 5, an interference determination unit 6, and a trajectory planning unit 7.

  For example, environment information such as position information, shape information, and map information of the obstacle X is input to the environment restoration unit 2. The environmental information is stored in advance in the storage device 8 such as a magnetic disk device, an optical disk device, a ROM, or a RAM, and may be output from the storage device 8. The environment restoration unit 2 may calculate environment information around the arm robot 100 using a detection sensor 9 (an ultrasonic sensor, a millimeter wave sensor, a camera, or the like) mounted on the arm robot 100.

  Based on the calculated or input environment information and the joint angles (time series data, etc.) of each joint unit 102 of the arm robot 100 calculated by the trajectory planning unit 7, the environment restoration unit 2 applies to the arm robot 100. The relative position of the obstacle X and the posture of the arm robot 100 are calculated. The environment restoration unit 2 outputs the calculated relative position of the obstacle X and the posture of the arm robot 100 to the distance calculation unit 3 and the interference determination unit 6.

  The distance calculation unit 3 is a specific example of a distance calculation unit, and is based on the relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100, and the shortest distance between the arm robot 100 and the obstacle X. The distance Lmin is calculated. The distance calculation unit 3 calculates the shortest distance Lmin between the arm robot 100 and the obstacle X using, for example, a GJK algorithm (Gillbert-Johnson-Keerthi Algorithm). Lmin may be calculated. The distance calculation unit 3 outputs the calculated shortest distance Lmin between the arm robot 100 and the obstacle X to the Jacobian calculation unit 4 and the parameter calculation unit 5.

  The distance calculation unit 3 is based on the relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100, instead of the shortest distance, a predetermined number (for example, the second) Alternatively, the distance between the joint portion 102 close to the third) and the obstacle X may be calculated. Thereby, contact with arm robot 100 and obstacle X can be prevented more certainly.

  In addition, the distance calculation unit 3 uses the relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100 based on the characteristic part of the arm robot 100 and the obstacle X instead of the shortest distance. The distance may be calculated. Note that the characteristic portions of the arm robot 100 include, for example, a hand tip, a fingertip, a complicated shape portion (such as an elbow joint), and a thin shape portion that easily slips through. Thereby, according to the characteristic of the characteristic site | part of the arm robot 100, an interference determination can be performed optimally.

  The Jacobian calculation unit 4 includes the shortest distance Lmin between the arm robot 100 and the obstacle X calculated by the distance calculation unit 3 and the joint angle of each joint unit 102 of the arm robot 100 calculated by the trajectory planning unit 7. Based on this, the Jacobian J (θ) of the arm robot 100 is calculated. The Jacobian calculation unit 4 outputs the calculated Jacobian J (θ) of the arm robot 100 to the parameter calculation unit 5.

  By the way, conventionally, for example, when the discretization width between trajectories from the start point to the end point is set to be large and interference determination is performed at each discretized position, there is a possibility that the robot may pass through the obstacle, and the interference determination accuracy This causes a problem of lowering. On the other hand, when the discretization width between trajectories from the start point to the end point is set small and interference determination is performed at each discretized position, the interference determination frequency between the robot and the obstacle increases, and the calculation time increases. Problems arise.

  Therefore, in the trajectory planning apparatus 1 according to the present embodiment, the parameter calculation unit 5 increases the shortest distance Lmin between the arm robot 100 and the obstacle X calculated by the distance calculation unit 3, and the arm robot 100 and the obstacle. The discretization width Δθ is increased as X is separated (FIG. 3). On the other hand, the parameter calculation unit 5 decreases the shortest distance Lmin between the arm robot 100 and the obstacle X calculated by the distance calculation unit 3 and increases the discretization width Δθ as the arm robot 100 and the obstacle X approach each other. .

  Thereby, when the distance between the arm robot 100 and the obstacle X is large in the environment (when the possibility of contact is low), the interference between the arm robot 100 and the obstacle X is determined with low frequency (FIG. 4). ). On the other hand, when the distance between the arm robot 100 and the obstacle X is small (when the possibility of contact is high), the interference between the arm robot 100 and the obstacle X is determined with high frequency. Therefore, according to the possibility of interference between the arm robot 100 and the obstacle X, it is possible to plan the trajectory of the arm robot 100 by performing the interference determination between the arm robot 100 and the obstacle X at an optimum frequency.

For example, the parameter calculation unit 5 calculates the shortest distance Lmin between the arm robot 100 and the obstacle X calculated by the distance calculation unit 3 and the Jacobian J (θ) of the arm robot 100 calculated by the Jacobian calculation unit 4. Based on this, the discretization width Δθ is calculated using the following equation (1). The parameter calculation unit 5 outputs the calculated discretization width Δθ to the interference determination unit 6.
Δθ = J (θ) ^# δLmin (1) where J (θ) ^# is a pseudo inverse matrix of Jacobian J (θ), and δ is a margin constant for multiplying the shortest distance Lmin.

  For example, when the self-interference of the arm robot 100 is assumed, the margin constant δ is set large because the interference can be predicted to some extent (a value close to 1 is set). On the other hand, when the interference with other objects is assumed, the margin constant δ is set small because the interference cannot be predicted. As described above, the margin constant δ is set according to the predictability of interference. However, the present invention is not limited to this, and any setting method can be applied. Further, the discretization width Δθ obtained by the above equation (1) is, for example, a first-order approximation value that is δ times the width between the obstacle X and the shortest point on the arm robot 100. It is good also as an n-th order approximate value.

  The interference determination unit 6 is a specific example of the interference determination unit. The relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100, and the discretization width Δθ calculated by the parameter calculation unit 5 are used. Based on the above, it is determined whether or not the arm robot 100 and the obstacle X interfere with each other. For example, the interference determination unit 6 determines whether or not the arm robot 100 and the obstacle X are interfering at each position on the trajectory discretized with the discretization width Δθ calculated by the parameter calculation unit 5. . In the present embodiment, the interference between the arm robot 100 and the obstacle X includes self-interference of the arm robot 100. The interference determination unit 6 outputs an interference determination result between the arm robot 100 and the obstacle X to the trajectory planning unit 7.

  The trajectory planning unit 7 is a specific example of the trajectory planning unit, and plans the trajectory of the arm robot 100 based on the interference determination result by the interference determination unit 6. For example, the trajectory planning unit 7 plans a trajectory so that the arm robot 100 and the obstacle X do not interfere with each other.

  The trajectory planning apparatus 1 includes, for example, a CPU (Central Processing Unit) that performs arithmetic processing, a ROM (Read Only Memory) that stores arithmetic programs executed by the CPU, and a RAM (temporarily storing processing data). Random Access Memory) and other microcomputers are mainly used for hardware configuration. The CPU, ROM, and RAM are connected to each other via a data bus or the like.

  Next, an example of a trajectory planning method by the trajectory planning apparatus according to the present embodiment will be described in detail. FIG. 5 is a flowchart showing an example of a trajectory planning process by the trajectory planning apparatus according to the present embodiment. Note that the trajectory planning process shown in FIG. 5 is repeatedly executed, for example, every predetermined time.

  Based on the input environment information and the joint angles of the joints 102 of the arm robot 100 calculated by the trajectory planning unit 7, the environment restoration unit 2 determines the relative position of the obstacle X to the arm robot 100 and the arm. The posture of the robot 100 is calculated (step S101).

  The distance calculation unit 3 calculates the shortest distance Lmin between the arm robot 100 and the obstacle X based on the relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100 (step S102).

  The Jacobian calculation unit 4 is based on the shortest distance Lmin between the arm robot 100 and the obstacle X calculated by the distance calculation unit 3 and the joint angle of the arm robot 100 calculated by the trajectory planning unit 7. The Jacobian J (θ) is calculated (step S103).

  The parameter calculation unit 5 is based on the shortest distance Lmin between the arm robot 100 and the obstacle X calculated by the distance calculation unit 3 and the Jacobian J (θ) of the arm robot 100 calculated by the Jacobian calculation unit 4. The discretization width Δθ is calculated using the above equation (1) (step S104).

  Based on the relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100, and the discretization width Δθ calculated by the parameter calculation unit 5, the interference determination unit 6 It is determined whether or not the obstacle X interferes (step S105).

  The trajectory planning unit 7 plans a trajectory so that the arm robot 100 and the obstacle X do not interfere based on the interference determination result by the interference determination unit 6 (step S106).

  In (Step S105), the interference determination unit 6 causes the arm robot 100 and the obstacle X to interfere with each other based on the relative position of the obstacle X calculated by the environment restoration unit 2 and the posture of the arm robot 100. It may be determined whether or not. In this case, in (Step S106), the trajectory planning unit 7 determines the interference determination result by the interference determination unit 6 based on the discretization width Δθ calculated by the parameter calculation unit 5, and the arm robot 100 and the obstacle X are separated. Plan a trajectory that will not interfere.

  As described above, in the trajectory planning apparatus 1 according to the present embodiment, the discretization width is set according to the shortest distance between the arm robot and the obstacle. As a result, when the distance between the arm robot and the obstacle is large and the possibility of interference is low, the interference between the arm robot and the obstacle is determined at a low frequency. On the other hand, when the distance between the arm robot and the obstacle is small and the possibility of interference is high, the interference between the arm robot and the obstacle is determined with high frequency. Therefore, it is possible to determine the interference between the arm robot and the obstacle at an optimum frequency according to the possibility of the interference between the arm robot and the obstacle. That is, the robot trajectory can be planned by determining the interference between the arm robot and the obstacle at an optimum frequency.

Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, in the above-described embodiment, the case where the robot is an arm robot has been described.
In addition, the present invention can also realize, for example, the processing shown in FIG. 5 by causing the CPU to execute a computer program.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

  The present invention can be used for a trajectory planning apparatus that plans a trajectory when moving a moving body such as an arm robot, a biped robot, a humanoid robot, and the like.

1 Trajectory planning device 1
2 Environment Restoration Unit 3 Distance Calculation Unit 4 Jacobian Calculation Unit 5 Parameter Calculation Unit 6 Interference Determination Unit 7 Trajectory Planning Unit 7
8 Storage device 9 Detection sensor

Claims (11)

A trajectory planning device for planning a trajectory for a robot to move from a start point to an end point,
A distance calculating means for calculating a distance between the robot and the obstacle on the planned trajectory;
Based on the distance calculated by the distance calculation means, a discretization setting means for setting a discretization width for discretizing between trajectories from the start point to the end point;
Interference determining means for determining whether or not the robot and an obstacle interfere at each position on the trajectory when discretized with the discretization width set by the discretization setting means;
A trajectory planning means for planning a trajectory of the robot based on an interference judgment result by the interference judgment means;
A trajectory planning apparatus comprising: The trajectory planning device according to claim 1,
The trajectory planning device, wherein the discretization setting means increases the discretization interval as the distance calculated by the distance calculation means increases. The trajectory planning apparatus according to claim 1 or 2,
The discretization setting means includes
A Jacobian calculator that calculates the Jacobian of the robot based on the distance calculated by the distance calculator and the joint angle of the robot calculated by the trajectory planning unit;
A trajectory planning apparatus, comprising: a parameter calculation unit that calculates the discretization interval based on the Jacobian of the robot calculated by the Jacobian calculation unit and the distance calculated by the distance calculation unit . The trajectory planning device according to any one of claims 1 to 3,
The trajectory planning apparatus, wherein the distance calculating means calculates the shortest distance between the robot and the obstacle on the planned trajectory. The trajectory planning device according to any one of claims 1 to 3,
The robot is an arm robot;
The distance calculation unit calculates a distance between a joint portion of the arm robot and the obstacle that is the predetermined closest from the obstacle. The trajectory planning device according to any one of claims 1 to 3,
The distance calculation unit calculates a distance between the obstacle and a characteristic part of the robot. A trajectory planning apparatus according to claim 3,
The parameter calculating unit multiplies the discretization interval by multiplying the Jacobian inverse matrix of the robot calculated by the Jacobian calculating unit, the shortest distance calculated by the distance calculating unit, and a predetermined margin constant. A trajectory planning apparatus characterized by calculating. The trajectory planning apparatus according to claim 7,
The trajectory planning apparatus, wherein the predetermined margin constant is set to be large in the case of self-interference of the robot and is set to be small in the case of interference with another object. The trajectory planning apparatus according to claim 6,
The trajectory planning apparatus characterized in that the characteristic part of the robot includes at least one of a hand tip, a fingertip, a complicated shape part, and a thin shape part. A trajectory planning method for planning a trajectory for a robot to move from a start point to an end point,
Calculating a distance between the robot and an obstacle on the planned trajectory;
Setting a discretization width for discretizing between trajectories from the start point to the end point based on the calculated distance;
Determining whether the robot and an obstacle are interfering at each position on the trajectory when discretized with the set discretization width; and
Planning a trajectory of the robot based on the interference determination result;
A trajectory planning method comprising: A trajectory planning program for planning a trajectory for a robot to move from a start point to an end point,
Processing to calculate the distance between the robot and the obstacle on the planned trajectory;
Based on the calculated distance, a process of setting a discretization width for discretizing between trajectories from the start point to the end point;
A process of determining whether or not the robot and an obstacle interfere at each position on a trajectory when discretized with the set discretization width;
A process of planning a trajectory of the robot based on the interference determination result;
A trajectory planning program characterized by causing a computer to execute.

Images