JP6717164B2 - Operation route planning method - Google Patents

Description

The present invention relates to a motion path planning method for planning a motion path of a robot.

A plurality of relay points are set between the start position and the goal position of the robot motion path so as to avoid obstacles, intermediate points are set between adjacent relay points, and a route connecting adjacent intermediate points. An operation path planning method for smoothing is known (see Patent Document 1). For example, a midpoint located between the first relay point and the second relay point and a midpoint located between the second relay point and the third relay point are connected. Smoothing is performed by making the curved line a curved line using a predetermined function.

JP 2005-309990 A

In the above-mentioned operation route planning method, for example, the route is planned so that the intermediate point is shifted between the relay points and the route obtained by connecting the intermediate points and smoothing does not interfere with the obstacle. Therefore, there is a risk that the calculation load will be high in finding the optimum intermediate point that is smooth and does not interfere with the obstacle.

The present invention has been made to solve the above problems, and a main object of the present invention is to provide an operation path planning method for planning a path that can smoothly operate without interfering with an obstacle while reducing the calculation load. And

One aspect of the present invention for achieving the above object is
A motion path planning method for planning a motion path of a robot, comprising:
Based on the environment map information including the position and shape of the obstacle in the operating environment of the robot, a plurality of routes are provided between the preset start point and end point so that the robot does not interfere with the obstacle. Configuring the nodes of
Calculating a path of a smoothing function for the section by setting a midpoint between adjacent nodes and calculating a composite product of a section of two adjacent midpoints and a predetermined function;
If the path of the calculated smoothing function does not interfere with the obstacle, the path of the smoothing function is generated as a new path, and if the path of the calculated smoothing function interferes with the obstacle, the intermediate A division point that divides the point and the node at a predetermined ratio is reset, a path of the smoothing function between the reset division point and the intermediate point is calculated, and a path of the smoothing function is calculated. Determine whether or not to interfere with an obstacle to generate the new route,
including,
This is a motion path planning method characterized by the following.

Advantageous Effects of Invention According to the present invention, it is possible to provide an operation route planning method for planning a route that can smoothly operate without interfering with an obstacle while reducing the calculation load.

It is a figure which shows schematic structure of the robot which concerns on one Embodiment of this invention. It is a block diagram showing a schematic system configuration of an operation route planning device concerning one embodiment of the present invention. It is a figure which shows the graph of the joint angle of the joint part of a robot, and the distance of a joint vector. It is a figure for explaining an example of flattening. It is a figure for demonstrating smoothing. It is a figure which shows the area of a softening function. It is the figure which displayed an example of the softening function T(x) by the graph. It is a figure for demonstrating the generation method of the softening function shown in FIG. It is a flowchart which shows the flow of the smoothing process by a route smoothing part. It is a figure which shows schematic structure of the robot which has one arm part.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
An operation path planning device according to an embodiment of the present invention plans an operation path of a robot.
FIG. 1 is a diagram showing a schematic configuration of a robot according to an embodiment of the present invention. The robot 100 is, for example, a humanoid robot, and has a head 101, a body 102, a pair of arms 103, a moving mechanism 104, and a controller 105.

The head 101 is provided with a distance sensor such as a camera. The body portion 102 is provided with a plurality of joint portions C1 to C5. The arm 103 has a plurality of joints R1 to R4 and L1 to L4 and hands R5 and L5. The joints R1 to R4 and L1 to L4 are provided with actuators such as servo motors that rotate or slide the joints R1 to R4 and L1 to L4. Each of the joints R1 to R4 and L1 to L4 is provided with a sensor that detects rotation or slide of each of the joints R1 to R4 and L1 to L4.

The moving mechanism 104 has wheels or legs and drives them to move the robot 100. The control device 105 controls the actuators of the joints R1 to R4 and L1 to L4 and the moving mechanism 104 to move the robot 100.

The control device 105 associates, for example, an environment map vector (X, Y) indicating the position of the environment map with a robot map vector (x, y) indicating the position of the robot 100 (such as the center of gravity or the center position of the robot 100). In addition, the movement of the robot 100 is controlled. The configuration of the robot 100 is an example, and the present invention is not limited to this.

The motion path planning apparatus 1 according to the present embodiment includes joint vectors R1 to R4 and L1 to L4 indicating the positions of the joints R1 to R4 and L1 to L4 of the robot 100, and the position of the robot 100 ( A motion route (hereinafter, motion route) of at least one of the robot map vectors indicating x, y) is planned.

FIG. 2 is a block diagram showing a schematic system configuration of the operation route planning apparatus according to the present embodiment. The operation route planning apparatus 1 according to the present embodiment includes an external information acquisition unit 2, a route generation unit 3, a route flattening unit 4, a route smoothing unit 5, and a route output unit 6.

The operation path planning device 1 includes, for example, a CPU (Central Processing Unit) that performs control processing, arithmetic processing, and the like, a ROM (Read Only Memory) and a RAM (Random Access) that store arithmetic programs executed by the CPU, control programs, and the like. Each of the hardware components is mainly composed of a microcomputer including a memory including a memory) and an interface unit (I/F) that inputs and outputs signals to and from the outside. The CPU, memory, and interface section are connected to each other via a data bus or the like.

The external information acquisition unit 2 uses a distance sensor such as a head camera to acquire external information such as the position and shape of an obstacle in the external environment. The external information acquired by the external information acquisition unit 2 is stored in a memory or the like as environment map information.

The route generation unit 3 may, for example, based on the preset start position (start end point) and goal position (end point) and the environment map information stored in the memory, obstruct the robot 100 and the environment map information. An operation path of the robot body and joints R1 to R4 and L1 to L4 that does not interfere with objects is generated. The route generation unit 3 generates an action route by using a probabilistic search method capable of searching the action route even in an environment in which obstacles are complicated.

The details of the probabilistic search method are described in, for example, Non-Patent Document: LaValle Steven K, Kuffner Jr. James J. (2001). “Randomized kinodynamic planning”. The International Journal of Robotics Research (IJRR)20(5 )., which can be incorporated by reference. Further, the method of generating the operation path described above is an example, and the present invention is not limited to this.

The path generation unit 3, for example, as illustrated in FIG. 3, generates an operation path of joint vectors of the joints R1 to R4 and L1 to L4 of the robot 100. In FIG. 3, the vertical axis represents the angles θ _i (θ _i (0), θ _i (1), θ _i (2),..., θ _i (k)) of the joints R1 to R4 and L1 to L4. Yes, the horizontal axis is the distance S of the joint vector. The joint vector is expressed as follows, for example.
θ=(θ ₁ , θ ₂ , θ ₃ ,..., θ _n )
[Θ _i (0)=(θ ₁ (0), θ ₂ (0), θ ₃ (0),..., θ _n (0)), θ _i (1)=(θ ₁ (1), θ ₂ (1), θ ₃ (1),..., θ _n (1)),..., θ i (k)=(θ ₁ (k), θ ₂ (k), θ ₃ (k), ..., θ _n (k))]

The joint vector distance S is the distance between the joint vectors. Note that the route generation unit 3 may generate the motion route of the robot map vector (x, y) with the vertical axis at the y coordinate and the horizontal axis at the x coordinate, as in FIG.

A plurality of relay points are set between the start position and the goal position of the operation route generated by the above generation method so that the robot 100 does not interfere with the obstacle. The route generation unit 3 generates a zigzag motion route connecting these relay points between the start position and the goal position.

The route flattening unit 4 flattens the operation route generated by the route generation unit 3. Here, the flattening is to extract an optional relay point from the relay points set as described above and delete the extracted relay point. By flattening, the movement path is simplified, and unnecessary movement of the robot 100 is reduced (or eliminated).

FIG. 4 is a diagram for explaining an example of the flattening. For example, in FIG. 4, p1 is a start point, p2 to p5 are relay points, and p6 is a goal point. First, the path flattening unit 4 specifies how many points (from two points or more to the end point) to be considered for flattening from the start point p1. This designation may be input to the route flattening unit 4 by the user, or the route flattening unit 4 may use a preset default value. Here, description will be made by taking as an example the case where three points ahead (p4) are designated. At this time, the route flattening unit 4 connects p1 and p4 with a line segment, and determines whether points on the line segment at a certain fixed interval interfere with the obstacle based on the environment map information in the memory. Verify. As a result of the verification, if there is no interference with the obstacle, the route flattening unit 4 sets the route connecting p1 and p4 as the new route.

On the other hand, if the result of the verification is that the path flattening unit 4 interferes with the obstacle, the path flattening unit 4 performs the same verification for the path connecting p1 and p3, and if there is no interference with the obstacle, it connects p1 and p3. The new route is defined as the new route. When the route connecting p1 and p3 interferes with an obstacle, the route flattening unit 4 does not perform flattening in this section. Then, the path flattening unit 4 performs the next flattening again from the end point after the flattening. For example, when the route connecting p1 and p4 is the new route, the route flattening unit 4 executes the next flattening process with p4 as the start point. In this way, the path flattening unit 4 sequentially flattens the paths and flattens all the movement paths from the start position to the goal position. The above-described flattening method is an example, and the present invention is not limited to this.

The path smoothing unit 5 smoothes the zigzag operation path flattened by the path flattening unit 4. Here, smoothing refers to making the route set as described above into a smooth curve. The operation path at the time when the flattening is completed is a so-called zigzag path in which the direction changes abruptly at each relay point. By making this a smooth curve, the operation of the robot 100 becomes smooth. Here, since the smoothed path is approximated by an infinitely differentiable function, smoothness of the moving speed and acceleration on the path is guaranteed in addition to the smoothness of the path itself. During smoothing, it is verified whether the smoothed movement path interferes with the obstacle, and if it does, the smoothing is re-implemented until the interference is avoided.

By the way, in a conventional operation route planning apparatus, for example, a route is planned so that the intermediate point is shifted between relay points and the smoothed route is connected to the intermediate point so as not to interfere with an obstacle. Therefore, there is a risk that the calculation load will be high in finding the optimum intermediate point that is smooth and does not interfere with the obstacle.

On the other hand, in the operation route planning apparatus according to the present embodiment, the route smoothing unit 5 sets intermediate points between adjacent nodes, respectively, and synthesizes a product of the two adjacent intermediate points and the softening function. By calculating, the path of the smoothing function for the section is calculated. When the calculated smoothing function path does not interfere with the obstacle, the path smoothing unit 5 generates the smoothing function path as a new path, and the calculated smoothing function path is an obstacle. In the case of interference, a dividing point that divides the intermediate point and the node at a predetermined ratio is reset, a path of the smoothing function between the reset dividing point and the intermediate point is calculated, and the smoothing function is calculated. A new route is generated by determining whether or not the route of 1 interferes with the obstacle.

With this, it is possible to automatically set the intermediate points and the dividing points between each node, calculate the smoothing function for the set intermediate points and the dividing points, and determine the interference of the obstacles. Optimal midpoints and division points that do not interfere can be set automatically. Therefore, it is possible to plan a route that can smoothly operate without interfering with an obstacle while reducing the calculation load.

The path smoothing unit 5 smoothes the operation path by, for example, performing a composite product of the operation path flattened by the path flattening unit 4 and the softening function (predetermined function) of the polynomial.

FIG. 5 is a diagram for explaining the smoothing. In FIG. 5, n0 to n4 are relay points and nodes of the operation route set as described above. sn_0 to sn4 are joint vector distances corresponding to the nodes n0 to n4. m1 to m4 are intermediate points between adjacent nodes in the operation path. s_1 to s_4 are joint vector distances corresponding to the intermediate points m1 to m4. m11 to m31 are division points that divide the intermediate point of the operation path and the adjacent nodes at a predetermined ratio (for example, 1:1 or 2:3). As the predetermined ratio, for example, an optimum value experimentally obtained in consideration of the physique of the robot 100 and the environmental condition (number of obstacles, size, etc.) is preset in a memory or the like. sm_11 to sm_31 are joint vector distances corresponding to the division points m11 to m31.

The route smoothing unit 5 first performs smoothing in the vicinity of the node n1, as shown in FIG. 5, for example.
The route smoothing unit 5, the midpoint m1, m2 section of the movement path (solid line (1)) [s_1, s_2 ] and the interval of the softening function shown in FIG. _{6 [-a n / 2, a} n / 2] And, For example, the route smoothing unit 5 selects the shorter one of the distance [s_1, sn_1] between the intermediate point m1 and the node n1 and the distance [sn_1, s_2] between the node n1 and the intermediate point m2. In this case, the section [s_1, sn_1]) is selected. The route smoothing unit 5, the selected interval [s_1, sn_1] to that doubled, so that the distance interval _{[-a n / 2, a n} / 2] are equal, by setting the _{a n} , The section [s_1, s_2] and the section [-a _n /2, a _n /2] are combined (s_1 = -a _n /2, s_2 = a _n /2). Then, the route smoothing unit 5, generates the interval [s_1, s_2] a, and the interval _{[-a n / 2, a n} / 2], the focus score was calculated for the smoothing function (dotted line (2)) To do.

The route smoothing unit 5 confirms whether the route of the generated smoothing function interferes with an obstacle based on the environment map information in the memory. In this case, since the path of the smoothing function interferes with the obstacle, the path smoothing unit 5 discards the generated smoothing function.

Here, when the distance between the division point m11 and the node n1 is larger than the predetermined distance ε, the route smoothing unit 5 continues the next smoothing process. For the predetermined distance ε, for example, an optimum value experimentally obtained in consideration of the physique of the robot 100 and the environmental state (number of obstacles, size, etc.) is preset in a memory or the like. The route smoothing unit 5, the dividing point of the motion path m11, m @ 2 section of [s_11, s_2] and the interval of the softening function _{[-a n / 2, a n} / 2] and, the combined (s_11 = -a _n /2, s_2 = a _n /2), the result is calculated, and a smoothing function (solid line (3)) is generated. In this case, since the smoothing function does not interfere with the obstacle, the path smoothing unit 5 sets the smoothing function as a new operation path. On the other hand, when the distance between the division point m11 and the node n1 is less than or equal to the predetermined distance ε, the route smoothing unit 5 stops the smoothing process and sets the section [s_1, s_2] of the intermediate points m1, m2 of the operation route. , Leave the zigzag condition as it is. In this case, the operation paths are m1, n1 and m2.

Similarly, the path smoothing unit 5 also performs smoothing on the sections [s_2, s_3] and [s_3, s_4] of the operation path to generate a new operation path.

Here, the softening function described above will be described.
7A to 7D are graphs showing an example of the softening function T(x).
The softening function is, for example, a Ternary Polynomial function. The tenary polynomial function is a polynomial function that has three areas, that is, an increase area, a constant area, and a decrease area of the function value, and the increase area and the decrease area are symmetric with respect to the constant area. The details of the tenary polynomial are disclosed in Japanese Patent Application Laid-Open No. 2009-053926 already filed by the present applicant, which can be incorporated by reference.

式（１） The softening function T(x) is represented by the following equation (1), for example.

Formula (1)

In this formula (1), T ₀ (x) of degree 0 is T ₀ (x)=C(−a ₀ /2<x<a ₀ /2), T ₀ (x)=0 (x≦−) a ₀ /2, x≧a ₀ /2). C and a ₀ are constants. i is 0 or a positive integer.

FIG. 7A shows T ₀ (x) of order 0 of the softening function, which has a rectangular wave profile. FIG. 7B shows T ₁ (x) of degree 1 of the softening function, which has a trapezoidal profile. FIG. 7C shows T ₂ (x) of degree 2 of the softening function, and has a trapezoidal profile that smoothly changes. FIG. 7D shows T ₃ (x) of degree 3 of the softening function, which has a trapezoidal profile that smoothly changes.

As shown in FIG. 8, the softening function _T i (x) _is, T i (x) the waveform of the _T i (x) in the coordinate system of -x by distributing in point symmetry around the origin _{T 'i} + 1 (x) Is generated and T′ _i+1 (x) is integrated, it is possible to generate T _i+1 (x) having a higher order.

For _{example, T} 0 when generating _T 2 (x) is from _{(x), T} 0 (x) is symmetrical division (symmetrical distribution) integrated to generate the _T 1 _(x), symmetrical _T 1 (x) divided may generate the _T 2 (x) by _{integrating, T} 0 (x) is after symmetrical division, _"generates a 2 _{(x), T"} further symmetrically divide and _T 2 (x) May be integrated twice to generate T ₂ (x).

The constants such as C and a ₀ in the softening function T(x) may be set according to the limit values (upper limit value and lower limit value) of the motion in the moving part of the robot 100. For example, when the velocity profile of the moving part is generated using the softening function T(x), if T ₂ (x) is set as the velocity profile in FIG. 8, T ₂ ′(x) indicates the acceleration state, and T ₂ ′(x) indicates the acceleration state. ₂ ″(x) indicates a jerk state.

_Therefore, limit value C of the T 2 "(x), jerk upper limit value of the moving portion as -C, set the lower limit _value, T 2 'limit value _C · a 0 for (x), as -C · _{a 0} By setting the upper and lower limits of the acceleration of the moving part and setting the speed limit value of the moving part as the limit value C·(a ₁ −a ₀ )·a ₀ of T ₂ (x), a constant is uniquely set. The values of C, a ₀ , and a ₁ are determined.

Such a softening function T(x) can quickly perform an operation satisfying the limitation of differential coefficients such as velocity, acceleration and jerk. Therefore, by calculating the motion path using the softening function T(x), it is possible to quickly calculate the movement path that satisfies a predetermined limit such as speed.

式（２） Next, the method of calculating the composite product of the motion path and the polynomial softening function will be described in detail.
The route smoothing unit 5 uses the following equation (2) to synthesize a product based on the motion route f(y) generated by the route generation unit 3 and the above-described polynomial softening function T _n (x). Perform f _d (x).

Formula (2)

The softening function T _n (x) is a function that can be differentiated n−1 times, and has parameters a ₀ , a ₁ ,..., _An , and C. It is defined as d=(a ₀ , a ₁ ,..., _An , C). C is adjusted so that the integral value of the softening function becomes 1. f _d (x) is a function that can be differentiated n−1 times, and converges uniformly as f _d →f.

The route output unit 6 outputs the operation route smoothed by the route smoothing unit 5. For example, the route output unit 6 outputs, for each component of the joint vector of the robot 100, as a smoothed movement route, graph data showing the relationship between the joint angle and the distance of the joint vector as shown in FIG. Further, the route output unit 6 outputs the trajectory of the robot map vector (x, y) as a smoothed movement route. The path output unit 6 outputs the smoothed operation path to the control device 105 that controls the robot 100 and the display unit that displays the graph data.

Next, the operation route planning method according to this embodiment will be described in detail. FIG. 9 is a flowchart showing the flow of the operation route planning method. The path generation unit 3 sets the nodes n _i based on the environment map information in the memory so that the robot 100 has an operation path that does not interfere with an obstacle between the preset start point and end point. The route flattening unit 4 flattens the node n _i of the operation route set by the route generation unit 3.

Subsequently, the route smoothing unit 5 smoothes the node n _i of the operation route flattened by the route flattening unit 4. When smoothing the node n _i of the motion path, the route smoothing unit 5 determines whether the distance between the intermediate point m _i and the node n _i and the distance between the intermediate point m _i+1 and the node n _i are larger than the predetermined distance ε. It is determined whether or not (step S101). i is a natural number.

When the route smoothing unit 5 determines that one of the distance between the intermediate point m _i and the node n _i and the distance between the intermediate point m _i+1 and the node n _i is equal to or less than the predetermined distance ε (NO in step S101), Smooth the next node n _i+1 . In this case, the operation path is a zigzag path that connects the midpoint m _i , the node n _i , and the midpoint m _i+1 .

On the other hand, when the route smoothing unit 5 determines that the distance between the intermediate point m _i and the node n _i and the distance between the intermediate point m _i+1 and the node n _i are larger than the predetermined distance ε (YES in step S101), the intermediate point is determined. The section between m _i and the intermediate point m _i+1 and the corresponding softening function T _n are combined to calculate the result f _d (x), and the smoothing function is generated (step S102). Then, the route smoothing unit 5 determines whether the route of the generated smoothing function interferes with the obstacle (step S103).

When the route smoothing unit 5 determines that the route of the generated smoothing function does not interfere with the obstacle (NO in step S103), the route of the smoothing function is set as a new route, and the smoothing of the next node n _i+1 is performed. To convert. On the other hand, when the path smoothing unit 5 determines that the path of the generated smoothing function interferes with the obstacle (YES in step S103), p=1 is set, and the path between the node _ni and the intermediate point _mi is set. It is determined whether or not the distance between the division point m _ip and the node n _i is larger than the predetermined distance ε (step S104).

When the path smoothing unit 5 determines that the distance between the division point m _ip and the node n _i is equal to or less than the predetermined distance ε (NO in step S104), the original zigzag paths m1, n1, and m2 are set as the operation paths. Then, the next node n _i+1 is smoothed. On the other hand, when the route smoothing unit 5 determines that the distance between the division point m _ip and the node n _i is larger than the predetermined distance ε (YES in step S104), the section between the division point m _ip and the intermediate point m _i+1 , The corresponding softening function T _n and the corresponding softening function T _n are combined to calculate the result f _d (x) to generate a smoothing function (step S105). The route smoothing unit 5 determines whether the route of the generated smoothing function interferes with an obstacle (step S106).

When the route smoothing unit 5 determines that the route of the generated smoothing function does not interfere with the obstacle (NO in step S106), the route of the smoothing function is set as a new route, and the smoothing of the next node n _i+1 is performed. To convert. On the other hand, when the route smoothing unit 5 determines that the route of the generated smoothing function interferes with the obstacle (YES in step S106), p=1 and q=1 are set, and the node n _i and the intermediate point m _{2 are set.} It is determined whether or not the distance between the division point m _i+1q between and and the node n _i is greater than the predetermined distance ε (step S107).

When the path smoothing unit 5 determines that the distance between the division point m _i+1q and the node n _i is equal to or smaller than the predetermined distance ε (NO in step S107), the original zigzag paths m1, n1, and m2 are set as the operation paths. Then, the next node n _i+1 is smoothed. On the other hand, when the route smoothing unit 5 determines that the distance between the division point m _i+1q and the node n _i is larger than the predetermined distance ε (YES in step S107), the section between the division point m _ip and the division point m _i+1q. And the corresponding softening function T _n are combined to calculate a result f _d (x), and a smoothing function is generated (step S108). Then, the route smoothing unit 5 determines whether or not the route of the generated smoothing function interferes with the obstacle (step S109).

When the route smoothing unit 5 determines that the route of the generated smoothing function does not interfere with the obstacle (NO in step S109), the route of the smoothing function is set as a new route, and the smoothing of the next node n _i+1 is performed. To convert. On the other hand, when the route smoothing unit 5 determines that the route of the generated smoothing function interferes with the obstacle (YES in step S109), it divides the dividing point m _ip and the node n _i at a predetermined ratio. It is determined whether or not the distance between the division point and the node n _i is larger than the predetermined distance ε (step S110).

When the path smoothing unit 5 determines that the distance between the division point and the node n _i is equal to or less than the predetermined distance ε (NO in step S110), the original zigzag paths m1, n1, and m2 are set as operation paths, Smooth the next node n _i+1 . On the other hand, when the route smoothing unit 5 determines that the distance between the division point and the node n _i is larger than the predetermined distance ε (YES in step S110), it sets p=p+1 and q=q+1 (step S111), and It returns to (step S108).

As described above, in the present embodiment, the route generation unit 3 uses the environment map information including the position and shape of the obstacle in the operating environment of the robot 100 to set the robot 100 between the preset start point and end point. Set multiple nodes so that the route does not interfere with the obstacle. The path smoothing unit 5 sets a midpoint between adjacent nodes and calculates a composite product of a softening function and a section of two adjacent midpoints, thereby determining a path of the smoothing function for the section. calculate. Further, when the calculated smoothing function path does not interfere with the obstacle, the path smoothing unit 5 generates the smoothing function path as a new path, and the calculated smoothing function path is an obstacle. In the case of interference, a dividing point that divides the intermediate point and the node at a predetermined ratio is reset, a path of the smoothing function between the reset dividing point and the intermediate point is calculated, and the smoothing function is calculated. A new route is generated by determining whether or not the route of 1 interferes with the obstacle.
With this, it is possible to automatically set the intermediate points and the dividing points between the respective nodes, calculate the smoothing function for the set intermediate points and the dividing points, and judge the interference of the obstacles. Optimal midpoints and division points that do not interfere with objects can be set automatically. Therefore, it is possible to plan a route that can smoothly operate without interfering with an obstacle while reducing the calculation load.

The present invention is not limited to the above-mentioned embodiment, but can be modified as appropriate without departing from the spirit of the present invention.
In the above embodiment, the operation route planning device 1 may be configured without the route flattening unit 4. In this case, the route smoothing unit 5 smoothes the operation route generated by the route generation unit 3.
In the above embodiment, the robot 100 may be configured as a one-arm robot having one arm unit 103 (FIG. 10) or may be an arm-type robot having only the arm unit 103. Furthermore, the robot 100 may be a mobile body such as an autonomous vehicle.

The present invention can also be realized by causing the CPU to execute a computer program, for example, the processing shown in FIG. 9.

The program can be stored using various types of non-transitory computer readable media and supplied to the computer. Non-transitory computer readable media include various types of tangible storage media. Examples of the non-transitory computer-readable medium include a magnetic recording medium (for example, flexible disk, magnetic tape, hard disk drive), magneto-optical recording medium (for example, magneto-optical disk), CD-ROM (Read Only Memory), CD-R, It includes a CD-R/W and a semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)).

The program may 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 transitory 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.

1 operation route planning device, 2 external information acquisition unit, 3 route generation unit, 4 route flattening unit, 5 route smoothing unit, 6 route output unit

Claims (1)

A motion path planning method for planning a motion path of a robot, comprising:
Based on environment map information including the position and shape of obstacles in the operating environment of the robot, a plurality of routes are set between the preset start point and end point so that the robot does not interfere with the obstacles. Configuring the nodes of
The midpoint set respectively between adjacent nodes, by calculating the convolution of the two one and interval and a predetermined function of the other intermediate point adjacent to the node, the path of the smoothing function for inter-compartment A step of calculating,
If the path of the calculated smoothing function does not interfere with the obstacle, the path of the smoothing function is generated as a new path, and if the path of the calculated smoothing function interferes with the obstacle, the one of the an intermediate point of said one and reconfigure the other and a node adjacent to the intermediate point, the division points for dividing between a predetermined ratio, wherein between the middle point of the other and divided point as該再set Calculating a path of the smoothing function, determining whether the path of the smoothing function interferes with the obstacle, and generating the new path;
including,
A motion path planning method characterized by the above.

Images