JP2011086099A - Method of generating movement path data of movable body - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a method of generating movement path data of a movable body allowing the movable body to pass through a command value without passing through a unnecessary route. <P>SOLUTION: The method of generating movement path data of a movable body by interpolating command values serially given to specify a moving position by use of a cubic function includes: a step of interpolating between a first command value and a second command value to be passed through later than the first command value, based on the first command value and the second command value; and a step of, when a third command value to be passed through later than the second command value is acquired while the movable body moves between a first position specified by the first command value and a second position specified by the second command value, interpolating between a present command value to specify a present value and the second command value, based on the present command value, the second command value and the third command value. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a method for generating movement path data of a moving object.

  In the control of the moving body, various command values represented by the command value for autonomous movement are given to the moving body. However, it is rare that all the command values are given to the moving body from the beginning, and the command values are often given sequentially. Furthermore, the cycle in which the command value is given is often a low cycle. Therefore, a method for interpolating command values given sequentially is required.

  Spline interpolation is widely known as a method of interpolating a given command value. Spline interpolation is characterized in that differential values in command values are continuous.

  In addition, Ferguson Coons interpolation is known as a method of interpolating command values that arrive sequentially, in which interpolation is performed with a cubic function using only two command values.

Patent Documents 1 to 4 disclose a method of interpolating a movement path of a robot with a three-dimensional function.
Further, Patent Document 5 discloses a movement path control method by spline interpolation that can move flexibly from an initial position to a target position in a movement path even if the target position of an autonomous mobile robot changes dynamically. Is disclosed.

Japanese Patent Laid-Open No. 08-136415 JP 05-324036 A JP 09-244725 A JP-A-10-228306 JP 2001-060112 A

However, in spline interpolation, an interpolation function cannot be determined unless all command values are given. That is, when command values are given sequentially, it is not assumed that command values are interpolated.
In addition, Ferguson Coons interpolation does not clearly define the function coefficient determination conditions. Therefore, for example, in FIG. 9, if a command value is given at a practical circle, a path by interpolation passes through a useless path that is largely detoured between two points like a part surrounded by a dotted line. It may be a thing.

  The present invention has been made to solve such problems, and an object of the present invention is to provide a method for generating movement route data that can pass a command value without passing through a useless route.

The method for generating moving path data of a moving body according to the present invention is a method for generating moving path data of a moving body by interpolating a command value given sequentially and specifying a moving position of the moving body using a cubic function. And interpolating between the first command value and the second command value based on the first command value and the second command value passing after the first command value; When the moving body is moving between the first position specified by the first command value and the second position specified by the second command value, the second command When the third command value that passes after the value is acquired, the current command value is determined based on the current command value that identifies the current value, the second command value, and the third command value. And a step of interpolating between the second command values, and Is an adult way.
Thereby, when the command value is sequentially added and given, the path can be interpolated so that the moving body passes the command value without passing through a useless path.

  According to the present invention, it is possible to provide a method of generating movement route data that can pass a command value without passing through a useless route.

It is a figure which shows the interpolation object area concerning Embodiment 1. FIG. 3 is an interpolation function calculation flowchart according to the first embodiment; It is a flowchart in case the new command value concerning Embodiment 1 is added. It is a figure which shows the state before the 3rd command value concerning Embodiment 1 is given. It is a figure which shows the state to which the 3rd command value concerning Embodiment 1 was given. It is a figure which shows the state by which the recalculation of the interpolation function concerning Embodiment 1 was performed. FIG. 3 is a diagram illustrating an example of a movement route according to the first embodiment. It is a figure which shows the first interpolation object area concerning Embodiment 2. FIG. It is a figure which shows the path | route calculated | required by the conventional interpolation method.

Embodiment 1.
Embodiments of the present invention will be described below with reference to the drawings.
In this embodiment, a case where an autonomous robot is used as a moving body will be described. Note that the command values are not all given to the robot from the beginning, but are given sequentially as time passes.

FIG. 1 is a diagram showing an interpolation target section according to the present embodiment. In FIG. 1, the horizontal axis represents time, and the vertical axis represents the command value.
The section between the command value 2 and the command value 3 is set as the interpolation target section, and the interpolation function of the curve portion indicated by the dotted line is obtained. At this time, the command value 2 is the first command value, and the command value 3 is the second command value. The command value 1 is a command value that has already passed.
When a command value is given for a time after the second command value, the command value 4 is set as the third command value. When a plurality of command values are given for the time after the command value 3 as the second command value, the command value at the earliest time among them is set as the third command value.

により行う。
ここでｔは時間を示し、ｆ_ｋは時間ｋからｋ＋１における関数である。係数ａ_ｋ０、ａ_ｋ１、ａ_ｋ２、及びａ_ｋ３は、１つの補間対象区間において、再計算が行われる場合を除いて、それぞれ１つの値が用いられる。ａ_ｋ０、ａ_ｋ１、ａ_ｋ２、及びａ_ｋ３は、次の補間関数計算フローにより定める。 Interpolation in the interpolation target section is a cubic function

To do.
Here, t represents time, and f _k is a function from time k to k + 1. The coefficients a _k0 , a _k1 , a _k2 , and a _k3 each have one value, except when recalculation is performed in one interpolation target section. a _k0 , a _k1 , a _k2 , and a _k3 are determined by the following interpolation function calculation flow.

An interpolation function calculation flow will be described. FIG. 2 is an interpolation function calculation flowchart. _{Hereinafter, f k (t) f k} ' the first-order derivative of the (t), represents the second derivative with _{f k} "(t).

ここでｘは指令値を表す。なお、第１の指令値以前の時間における補間関数計算（ｆ_ｋ-１,ｆ_ｋ-２,・・・）は、すでに行われているものとする。 First, when the robot passes through the first command value and enters a new complementary section, the robot sets three calculation conditions (2) to (4) (step S11).

Here, x represents a command value. It is assumed that the interpolation function calculation (f _k-1 , f _k-2 ,...) In the time before the first command value has already been performed.

第３の指令値が与えられていない場合には（ステップＳ１２でＮＯ）、（６）式を４つ目の計算条件として設定する（ステップＳ１４）。

Next, the robot checks whether or not a third command value is given (step S12). If the third command value is given (YES in step S12), equation (5) is set as the fourth calculation condition (step S13).

If the third command value is not given (NO in step S12), equation (6) is set as the fourth calculation condition (step S14).

Next, from the above-described three conditions (2) to (4) and any one of the conditions (5) or (6), a _k0 , a _k1 , a _k2 , And a _k3 are calculated (step S15).
As a result, the interpolation function f _k in the interpolation target section can be obtained.

  Next, a flow when a command value is newly added will be described. FIG. 3 is a flowchart when a new command value is added to the robot.

  When a new command value is added, the robot determines whether there is already a third command value (step S21).

If the third command value does not exist (NO in step S21), the robot newly sets the interpolation value f _k (T) at the current time T as the first command value (step S22). In addition, the robot sets the newly added command value as the third command value (step S23).
Next, using the first and third command values specified in step S22 and step S23, the interpolation function calculation is performed based on the above-described interpolation function calculation flow (steps S11 to S15) (step S24). .

Next, the operation according to the flow when a new command value is given will be described.
FIG. 4 is a diagram illustrating a state where the third command value is not given. In FIG. 4, the horizontal axis is the time axis, and the vertical axis indicates the command value. Also, the dotted curve portion in FIG. 4 indicates the path calculated by the interpolation function calculation at the time before the new command value is given.
At this time, the command value 11 at (t _k , x _k ) is the first command value, and the command value 12 at (t _{k + 1} , x _{k + 1} ) is the second command value. Point 13 is the interpolation value f _k (T) at the current time T, that is, the current value of the robot.

FIG. 5 is a diagram illustrating a state where a new command value is given to (t _{k + 2} , x _{k + 2} ) and the new command value becomes the third command value. The horizontal axis in FIG. 5 is the time axis, and the vertical axis indicates the command value. The dotted curve portion in FIG. 5 indicates the path calculated by the interpolation function calculation at the time before the new command value is given, as in FIG.
In FIG. 5, the robot determines that a new command value 14 has been added (step S21), and if there is no other third command value (NO in step S21), the robot moves the point 13 to the new first value. The command value is set (step S22), and the command value 14 is set as a third command value (step S23).

Based on the above-described interpolation function calculation flow (steps S11 to S15), the interpolation function calculation is performed (step S24). Since the command value 14 is the third command value, YES is determined in step S12.
FIG. 6 shows a state where the interpolation function calculation is performed with the new command value 14 as the third command value. In addition, the horizontal axis of FIG. 6 is a time axis, and a vertical axis | shaft shows command value. That is, the dotted curve portion in FIG. 6 is a new path calculated by interpolation function calculation after newly setting the first and third command values.

  If it is determined in step S21 that the third command value already exists (YES in step S21), the interpolation function currently being executed is not changed.

Next, an example of performing interpolation calculation according to the present invention will be described with reference to FIG. FIG. 7 is a diagram illustrating a moving path of a moving body in which the horizontal axis is time and the vertical axis is a command value.
In FIG. 7, circles indicate given command values given sequentially. A black square mark (point) indicates a point when a new command value is given to the robot and the newly given command value becomes the third command value.

First, the robot operates between the command value 21 and the command value 22. At this time, it is assumed that the robot has been continuously operated before the command value 21. Further, the command value 23 is already given to the robot. Therefore, the robot calculates the interpolation function f ₁ according to the above-described interpolation function calculation flow, with the command value 21 as the first command value, the command value 22 as the second command value, and the command value 23 as the third command value. It can be carried out. Since the command value 23 exists as the third command value, the robot determines YES in step S12 and performs the interpolation function calculation. The robot executes an operation between the command value 21 and the command value 22 according to f ₁ .

Next, the robot operates between the command value 22 and the command value 23. At this time, the command value 24 is not given to the robot until the point 31 is passed. That is, the command value 22 is the first command value and the command value 23 is the second command value, but there is no third command value. Accordingly robot interpolation function calculation of f _2, carried out determined as NO in step S12 in the interpolation function calculation flow. Robot performs the operation between the command value 22 and the command value 23 in accordance with f _2.

Next, it is assumed that a command value 24 is given to the robot at point 31. In this case, the robot executes a flow (steps S21 to S24) when the above-described new command value is added. Here, since there is no command value after the command value 23 and before the command value 24, the robot determines NO in step S21.
Therefore, the point 31 is newly set as the first command value (step S23), and the command value 24 is set as the third command value (step S24). The second command value remains the command value 23. The robot performs an interpolation function calculation (steps S11 to S15) based on these first to third command values, and sets the calculated interpolation function to f ₂ (new). Since the command value 24 is the third command value, YES is determined in step S12. The robot executes an operation between the point 31 and the command value 23 according to f ₂ (new).

Next, it is assumed that the command value 25 is already given when the robot passes the command value 23. Therefore, the command value 23 first command value, the command value 24 second command value, the command value 25 as a third command value, the calculation of the interpolation function f ₃ in accordance with the interpolation function calculation flow described above. Since the command value 25 exists as the third command value, the robot determines YES in step S12 and performs the interpolation function calculation. Robot performs the operation between the command value 24 and the command value 23 in accordance with f _3.

Next, the robot operates between the command value 24 and the command value 25. However, the robot is not given the command value 26 until it passes the point 32. That is, the command value 24 is the first command value and the command value 25 is the second command value, but there is no third command value. Thus, the robot, the interpolation function calculation of f _4, carried out by determination of NO in step S12 of interpolation function calculation flow. Robot performs the operation between the command value 24 and point 32 in accordance with f _4.

Next, it is assumed that the command value 26 is given to the robot at the point 32. In this case, the robot executes a flow (steps S21 to S24) when the above-described new command value is added. Here, since there is no command value after the command value 25 and before the command value 26, the robot determines NO in step S21.
Therefore, the point 32 is newly set as the first command value (step S23), and the command value 26 is set as the third command value (step S24). The second command value remains the command value 25. The robot performs an interpolation function calculation (steps S11 to S15) based on the first to third command values, and sets the calculated interpolation function to f ₄ (new). Since command value 26 is the third command value, YES is determined in step S12. The robot executes an operation between the point 32 and the command value 25 according to f ₄ (new).

Next, the robot operates between the command value 25 and the command value 26. However, the robot is not given the command value 27 until the point 33 is passed. That is, the command value 25 is the first command value and the command value 26 is the second command value, but there is no third command value. Thus, the robot, the interpolation function calculation of f _5, carried out by determination of NO in step S12 of interpolation function calculation flow. Robot performs the operation between the command value 25 and point 33 in accordance with f _5.

Next, it is assumed that a command value 27 is given to the robot at point 33. In this case, the robot executes a flow when the above-described new command value is added. Here, since there is no command value after command value 26 and before command value 27, the robot determines NO in step S21.
Therefore, the point 33 is newly set as the first command value (step S23), and the command value 27 is set as the third command value (step S24). The second command value remains the command value 26. The robot performs an interpolation function calculation (steps S11 to S15) based on these first to third command values, and sets the calculated interpolation function to f ₅ (new). Since command value 27 is the third command value, YES is determined in step S12. The robot executes an operation between the point 33 and the command value 26 according to f ₅ (new).

Next, the robot operates between the command value 26 and the command value 27. However, the robot is not given the command value 28 until the point 34 is passed. That is, the command value 26 is the first command value and the command value 27 is the second command value, but there is no third command value. Thus, the robot, the interpolation function calculation of f _6, carried out by determined as NO in step S12 in the interpolation function calculation flow. Robot performs the operation between the command value 26 and point 34 in accordance with f _6.

Next, it is assumed that a command value 28 is given to the robot at point 34. In this case, the robot executes a flow when the above-described new command value is added. Here, since there is no command value after the command value 27 and before the command value 28, the robot determines NO in step S21.
Therefore, the point 34 is newly set as the first command value (step S23), and the command value 28 is set as the third command value (step S24). The second command value remains the command value 27. The robot performs an interpolation function calculation (steps S11 to S15) based on the first to third command values, and sets the calculated interpolation function as f ₆ (new). Since command value 28 is the third command value, YES is determined in step S12. The robot executes an operation between the point 34 and the command value 27 according to f ₆ (new).

Next, the robot operates between the command value 27 and the command value 28. In this case, the command value 27 is the first command value, and the command value 28 is the second command value. Since the third command value does not exist, the robot determines that the NO in step S12 of the interpolation function calculation flow, performs interpolation function calculation, calculates a f _7. Robot, the operation between the command value 27 and the command value 28, performed according to f _7.

  Accordingly, the robot can calculate a new route when the command value is added, and can pass the command value without passing through a useless route.

Embodiment 2. FIG.
Also in the present embodiment, a case where an autonomous robot is used as a moving body will be described as in the first embodiment.

FIG. 8 shows a state in the first interpolation section. In addition, the horizontal axis of FIG. 8 is a time axis, and a vertical axis | shaft shows command value.
The command value 41 given at t ₁ is the command value given first, and the interpolation function f _k−1 does not exist in the previous section. Therefore, in the above-described interpolation function calculation flow, even if the command value 41 is the first command value, the command value 42 is the second command value, and the command value 43 is the third command value, in step S11 The equation (4) cannot be used.

Therefore, instead of the formula (4), for example, the formula (7) or the formula (8) can be used.

Thereby, the interpolation function calculation in the interval between t ₁ and t ₂ can be performed according to the above-described interpolation function calculation flow (step S11 to step S15). Therefore, the interpolation function f ₁ of the path indicated by the dotted curve in FIG. 8 can be calculated, and the robot can execute the operation between the command value 27 and the command value 28 according to f ₁ .

  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.

11, 12, 14, 21-28, 41-43 Command value 13, 31-34 points

Claims (1)

A method of generating a movement path data of the moving body by interpolating a command value given sequentially and specifying a moving position of the moving body using a cubic function,
Interpolating between the first command value and the second command value based on the first command value and the second command value passing after the first command value;
When the movable body is moving between the first position specified by the first command value and the second position specified by the second command value, the second command value If the third command value that passes later is acquired, the current command value based on the current command value that identifies the current value, the second command value, and the third command value Interpolating between the second command values;
A method for generating moving path data of a moving body.

Images