JP2009032185A - Moving route acquisition device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a moving route acquisition device for promptly acquiring a smooth moving route according to a situation. <P>SOLUTION: A moving route acquisition device 1 acquires a moving route in advance and recalculates the moving route. There are provided a control point setting section 12 for setting control points on the moving route and for setting information related to the control points, a circular arc generation section 13 for approximating a route between neighboring control points with a circular arc based on the information related to the control points, and a recalculation section 14 for recalculating the moving route using the circular arc, to acquire the moving route. There are provided control points P<SB>A</SB>, P<SB>B</SB>on the acquired moving route, the route between the control points are approximated with a circular arc P<SB>A</SB>P<SB>C</SB>and a circular arc P<SB>C</SB>P<SB>B</SB>, based on the information related to the control points P<SB>A</SB>, P<SB>B</SB>, so that the moving route can be smoothed. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a travel route acquisition apparatus.

2. Description of the Related Art Conventionally, an apparatus that acquires an object movement path is suitably used for a mobile device such as an autonomous mobile robot or a vehicle that performs automatic driving control. Such a mobile device moves to a destination based on the input or generated movement route (see, for example, Patent Document 1). The movement route acquisition device described in Patent Literature 1 generates a zigzag movement route, and uses the movement route as a smooth movement route using a plurality of functions.
US Pat. No. 6,134,486

  However, in the conventional movement route acquisition apparatus, since the load of the calculation process of the movement speed is large, the calculation process is not performed for a sudden speed change such as when an obstacle suddenly appears in the movement path during movement. There is a risk that we cannot respond to the situation without being in time.

  Therefore, the present invention has been made to solve such a technical problem, and an object of the present invention is to provide a movement path acquisition device that can quickly acquire a smooth movement path according to the situation. .

  That is, the movement route acquisition device according to the present invention is a movement route acquisition device that acquires a movement route in advance and recalculates the movement route, sets a control point on the movement route, and stores information on the control point. Control point setting means for setting, arc approximation means for approximating a path between adjacent control points with an arc based on information on the control point, and recalculation means for recalculating the movement path using the arc And is configured.

  According to the present invention, a movement path is acquired, control points are provided in the acquired movement path, and a path between adjacent control points is approximated by an arc based on information on those control points to smooth the movement path. be able to. As a result, it is possible to recalculate the moving path of the curve into a moving path considering the state of the control point using a simple arc. Therefore, when the movement route to be acquired is changed, a smooth movement route corresponding to the change can be quickly acquired.

  Here, the movement route acquisition device may include curve approximation means for approximating the movement route acquired in advance with a curve, and the control point setting means may use the curve approximated by the curve approximation means as the movement route. .

  In the movement path acquisition device, it is preferable that the first arc approximation means approximates a movement path between adjacent control points using two arcs.

  By comprising in this way, the path | route between adjacent control points can be approximated by two circular arcs based on the information regarding those control points, and a movement path | route can be made smooth. As a result, the moving path can be approximated smoothly with high accuracy.

  In the movement route acquisition apparatus, it is preferable that the control point setting means sets a position vector and a velocity vector of the control point as information relating to the control point. With this configuration, two arcs can be determined in consideration of the speed of the control point, so that the movement path can be acquired so that the speed change of the mobile device becomes smooth.

  In the movement route acquisition apparatus, it is preferable that the arc approximating means approximates using an arc having a radius larger than a predetermined radius. By configuring in this way, a predetermined radius is set to a radius that can be actually turned by the drive portion provided in the mobile device, and an arcuate movement path corresponding to the drive capability of the mobile device can be generated. Thereby, the movement path | route corresponding to the drive capability of a mobile device can be produced | generated rapidly.

  In addition, it is preferable that the movement route acquisition device includes a recalculation unit that recalculates the movement route by selecting a straight line or an arc according to the length of the straight line portion connecting the control points of the movement route.

  By configuring in this way, it is possible to acquire a movement route and approximate it with a straight line or an arc according to the length of a straight line portion connecting the control points of the acquired movement route. For example, when a zigzag movement path is input and the movement path of a predetermined section is short, the movement path is approximated by combining a plurality of arcs, and when the movement path of the predetermined section is long, a straight line and an arc Can be combined to approximate the movement path. As a result, the zigzag moving path can be quickly recalculated into a smooth moving path composed of an arc and a straight line.

  Furthermore, the movement route acquisition apparatus includes a collision determination unit that determines whether or not to collide with an obstacle based on the movement route calculated by the recalculation unit, and the recalculation unit is configured to detect a failure with the collision determination unit. When it is determined that the object collides with an object, it is preferable to recalculate the movement path using a circular arc having a smaller radius.

  By configuring in this way, it is determined whether or not the mobile device collides with an obstacle when the mobile device travels using the approximate moving route. To re-approximate the travel path. As a result, it is possible to avoid a movement route that collides with an obstacle, so that a safe movement route can be calculated.

  According to the present invention, a smooth movement route according to a situation can be quickly acquired.

  Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the description of the drawings, the same elements are denoted by the same reference numerals, and redundant description is omitted.

(First embodiment)
FIG. 1 is a configuration diagram of a robot 2 including a movement path acquisition device 1 according to the first embodiment. The movement route acquisition device 1 according to the present embodiment is a device that acquires movement route information indicating the route of a mobile device and updates the acquired movement route information in real time according to the surrounding environment. It is a device that is suitably used for automatic traveling control of a vehicle.

  The robot 2 provided with the movement path acquisition device 1 includes a camera 3 for recognizing the surrounding environment, an ECU 6 for controlling the operation of the robot 2, an electric motor 8 controlled by the ECU 6, and wheels driven by the electric motor 8. 9. The communication part 7 for performing communication, map DB4, and route information DB5 are provided. Here, the electronic control unit (ECU) is a computer that performs electronic control, and includes a central processing unit (CPU), a read only memory (ROM), a random access memory (RAM), and an input / output interface. ing.

  The camera 3 includes a CCD camera that acquires an image in the direction in which the robot 2 travels, and an image processing unit (not shown) that recognizes the presence or absence of an obstacle by performing image processing on the acquired image. This image processing unit has a function of extracting and recognizing an obstacle from an image acquired by a CCD camera by edge extraction or pattern recognition processing. The camera 3 has a function of outputting the acquired information to the ECU 6.

  The map DB 4 is a database in which map information is stored, and is configured to be referable from the ECU 6. The route information DB 5 is a database in which a rough route that the robot 2 is scheduled to travel is stored, and can be referred to from the ECU 6. The route information stored in the route information DB 5 is, for example, a movement route from a departure point to a destination point, and further includes target speed information and target arrival time information at a predetermined point on the movement route.

  The communication unit 7 has a function of communicating with the outside and inputting information necessary for automatic driving. For example, the communication unit 7 has a function of inputting the coordinates of the current location of the robot 2. The communication unit 7 has a function of outputting the input information to the ECU 6.

  The ECU 6 includes a movement route generation unit 61, an actuator control unit 62, and a movement route recalculation processing unit (movement route acquisition device) 1.

  The movement path generation unit 61 has a function of generating or inputting a rough path along which the robot 2 moves. For example, it has a function of generating rough route information using a departure point and a target point, route information input from the route information DB 5, and map information input from the map DB 4. The departure point, the target point, the arrival time, and the like are input via the communication unit 7, for example. The movement route indicated by the route information is formed by a continuous linear route and has a zigzag shape. The movement path generation unit 61 has a function of extracting position information of the refraction part of the movement path. In addition, the movement route generation unit 61 uses the generated route information, information on the obstacle input from the camera 3, current position information input from the communication unit 7, map information input from the map DB 4, and the like while driving. It has a function of changing route information in order to avoid an obstacle when an object is recognized. Further, the movement route generation unit 61 has a function of outputting route information to the movement route recalculation processing unit 1.

  The movement route recalculation processing unit 1 has a function of acquiring and recalculating route information, and includes a curve approximation unit (curve approximation unit) 11, a control point setting unit (control point setting unit) 12, and an arc generation unit. (Arc approximation means) 13 and a recalculation unit (recalculation means) 14 are provided.

  The curve approximation unit 11 has a function of inputting route information from the movement route generation unit 61 and approximating the movement route to a sufficiently smooth curve. The curve approximating unit 11 approximates the movement path to a curve based on the input position information at the refraction point of the zigzag movement path. Here, known approximation means with a low calculation load is used, for example, nonparametric approximation is used. Further, the curve approximation unit 11 has a function of outputting the calculated curve route information to the control point setting unit 12.

  The control point setting unit 12 has a function of setting control points on the route and setting information regarding the set control points. The control point setting unit 12 sets, for example, a point passing on the moving path of the curve as a control point, and a position vector of the control point and a velocity vector when the robot 2 passes the control point are information on the control point. Set as. As the position vector and the velocity vector, values acquired from the route information are set. The control point setting unit 12 has a function of outputting the set information to the arc generation unit 13.

  The arc generation unit 13 has a function of generating an arc that smoothly connects two points. The arc generation unit 13 calculates a circle passing through one of the two given points and a circle passing through the other point based on a predetermined condition, and uses the calculated arcs of the two circles to calculate two circles. It has a function to generate a curve that smoothly connects two points. Further, the arc generation unit 13 has a function of outputting information about the calculated curve to the recalculation unit 14.

  The recalculation unit 14 has a function of recalculating route information based on the information regarding the curve input from the arc generation unit 13. Further, it has a function of outputting the recalculated path information to the actuator controller 62.

  The actuator control unit 62 has a function of inputting route information from the recalculation unit 14 and outputting a control signal to the electric motor 8 based on the input route information. The electric motor 8 has a function of inputting a control signal from the actuator control unit 62 and driving the wheels 9 based on the control signal.

  The robot 2 configured as described above travels along the given route information, and when there is an obstacle in the traveling direction of the robot 2, the zigzag route is changed so as to avoid the obstacle. The approximated zigzag route is approximated to a smooth curve, and the approximated curve is recalculated based on the speed information, and a smooth movement path that the mobile device can move is recalculated, and the vehicle travels based on the recalculated movement route. .

  Next, operation | movement of the moving speed acquisition apparatus 1 which concerns on this embodiment is demonstrated using FIGS. FIG. 2 is a flowchart showing the operation of the robot 2 provided with the movement speed acquisition device 1 according to the present embodiment, and FIGS. 3 and 4 are schematic diagrams showing recalculated movement paths.

  The robot 2 provided with the moving speed acquisition device 1 executes the control process shown in FIG. 2 at the timing of departure toward the destination, and repeatedly executes the control process shown in FIG. 2 at a predetermined interval.

  The robot 2 starts from the information input process (S10). The process of S10 is a process that is executed by the movement route generation unit 61 and inputs information for generating a movement route. The movement route generation unit 61 inputs obstacle information from the camera 3, map information from the map DB 4, route information from the route information DB 5, and destination, target arrival time, current location, and the like from the communication unit 7. When the process of S10 ends, the process proceeds to the movement route planning process (S12).

  The process of S12 is executed by the movement path generation unit 61 and is a process for planning a movement path. The movement route generation unit 61 generates a zigzag movement route as shown in FIG. 2A based on the information obtained in the process of S10. Moreover, the movement path | route production | generation part 61 extracts the coordinate of the refractive location of the produced | generated movement path | route. When the process of S12 ends, the process proceeds to a smoothing process for the movement route (S14).

  The process of S14 is a process executed by the curve approximation unit 11 to smooth the zigzag movement path to obtain a curve. The curve approximation unit 11 approximates the zigzag movement path to a curve as shown in FIG. 2B using, for example, non-parametric approximation based on the coordinates of the refraction point of the movement path input in the process of S12. . This curve is generated without considering the speed information of the robot 2 and the restriction on the movement capability of the robot 2. When the process of S14 is completed, the process proceeds to a speed information setting process (S16).

  The process of S16 is executed by the control point setting unit 12, sets a control point on the moving route, and sets speed information corresponding to the set control point. The control point setting unit 12 sets a control point on the curve approximated in the process of S14, inputs speed information corresponding to the set position information of the control point from the route information DB 5, and sets the speed information of the control point. . When the process of S16 ends, the process proceeds to a smoothing process for the movement route (S18).

The process of S18 is executed by the arc generation unit 13 and the recalculation unit 14, and is a process of smoothing the moving path of the curve in consideration of the driving capability and speed information of the robot 2. The arc generation unit 13 generates a curve connecting the two control points based on the position vector and velocity vector of the control points set in the process of S16. This curve is formed, for example, by connecting a circular arc that passes through one of the two control points and a circular arc that passes through the other control point. Hereinafter, a method for generating a curve connecting two control points will be described. For example, as shown in FIG. 3, it is assumed that a curve connecting the control point P _A and the control point P _B is generated. Position vector _{P A} and _{P B} of the control point, the velocity vector _{t A} showing the rate of passage of the control point _{P A,} the velocity vector _{t B} showing the rate of passage of the control point _{P B,} the radius _{r A} passing through the control points _{P A} circle of _{C a,} circle _{C B} of radius _{r B} through the control point _{P B,} a circle _C of intersection of the _a and the circle _{C B P C,} circle _{C a} tangent circle _{C a} at the control points _{P a} of intersection _{P C} P'an intersection where the tangents intersect at _a, circle _{C P'B} the intersection where the tangent intersects at the intersection _{P C} of the tangent and the circle _{C B} at the control point _{P B} of _B, the speed of the entire robot 2 When the vector is V and the predetermined variables are α and β, the circle C _A and the circle C _B are set so as to satisfy the following expressions (1) to (7).

Here, the circle C _A and the circle C _B satisfying the above formula are uniquely determined by setting so as to satisfy the following formula (8).

It should be noted that the radii r _A and r _B of the circle C _A and the circle C _B are set to have a radius equal to or greater than a predetermined value. The predetermined value is a radius at which the robot 2 can turn, and is a value determined by a drive system device such as the drive wheel 9.

Using arc _P C _{P B} of the circular arc _P A _{P C} and circle _{C B} of the circle _{C A} calculated in this manner, the path between the control points _P A _{P B} can be approximated. In FIG. 3, but a state of circle C _A and circle C _B overlap each other, the calculation results of the above formula is in the state circle C _A and circle C _B do not overlap each other as shown in FIG. 4 In some cases. In such a case, the movement path has an S-shape connecting the arc P _A P _C and the arc P _{C B.}

  Returning to the control process of FIG. 2, when the process of S18, that is, the smoothing process of the movement route is completed, the process proceeds to a control signal generation process (S20).

  The process of S20 is a process of controlling the actuator based on the movement path executed by the actuator control unit 62 and recalculated and smoothed in the process of S20. The actuator control unit 62 generates, for example, a control signal for the electric motor 8 based on the recalculated and smoothed movement path, and operates the wheel 9. When the process of S20 ends, the control process shown in FIG. 2 ends.

As described above, according to the movement route acquisition device 1 according to the first embodiment, the movement route is acquired, the control points P _A and P _B are provided on the acquired movement route, and the path between the control points is set to the control point P. _a, it is possible to smooth the movement path approximated by an arc _P a _{P C} and the arc _P C _{P B} on the basis of the information about _{P B.} As a result, it is possible to recalculate the moving path of the curve into a moving path that takes into account the states of the control points P _A and P _B using a simple arc. Therefore, when the movement route to be acquired is changed, a smooth movement route corresponding to the change can be quickly acquired.

  Further, according to the movement route acquisition device 1 according to the first embodiment, the control point setting unit 12 can provide control points on the curve approximated by the curve approximation unit 11, and thus a smooth movement route can be performed more quickly. Can be generated.

Further, according to the travel route acquisition apparatus 1 according to the first embodiment, to approximate the movement path between control points arc generating unit 13 are adjacent with two arc P _A P _C and the arc P _C P _B Therefore, the moving path can be approximated smoothly with high accuracy.

Further, according to the movement route acquisition device 1 according to the first embodiment, the position vectors P _A and P _{B of} the control points and the velocity vectors t _A and t _B are set, and the speeds of the control points P _A and P _B are taken into consideration. Thus, since two arcs can be determined, the movement path can be acquired so that the speed change of the robot 2 becomes smooth.

Furthermore, according to the movement route acquisition device 1 according to the first embodiment, the arcs are calculated using the circles C _A and C _B having radii in which the wheels 9 or the like, which are drive parts provided in the robot 2, can actually turn. As a result, a movement path corresponding to the driving capability of the robot 2 can be quickly generated.

(Second Embodiment)
FIG. 5 is a configuration diagram of the robot 2 including the movement path acquisition device 10 according to the second embodiment. The movement route acquisition device 10 according to the present embodiment is a device that acquires movement route information indicating the route of a mobile device and updates the acquired movement route information in real time according to the surrounding environment. It is comprised substantially the same as the movement path | route acquisition apparatus 1 which concerns on a form. Therefore, the same components as those in the first embodiment are denoted by the same reference numerals, description thereof is omitted, and differences will be described in detail.

  The movement route recalculation processing unit (movement route acquisition device) 10 provided in the ECU 6 does not include the curve approximation unit 11 included in the movement route acquisition device 1 of the first embodiment, and includes a collision determination unit (collision determination unit) 15. It differs in the point to prepare. Further, the arc generation unit 13 and the recalculation unit 14 included in the movement route acquisition device 10 are different in input / output destination and some functions from the arc generation unit 13 and the recalculation unit 14 included in the movement route acquisition device 1.

  The control point setting unit 12 has a function of inputting the coordinates of the refraction point in the zigzag movement path from the path information generation unit 61. The input coordinates are set as control points and output to the arc generation unit 13.

  The arc generation unit 13 has a function of approximating the movement path between the refraction points with a smooth arc based on the coordinates of the control points input from the control point setting unit 12. Further, it has a function of generating an arc composed of a circle having a smaller radius based on a determination result of a collision determination unit 15 described later. In addition, a function of outputting information about the calculated arc to the collision determination unit 15 is provided.

  The collision determination unit 15 has a function of inputting a smooth movement path calculated by the arc generation unit 13 and determining whether or not the robot 2 collides with an obstacle when traveling along the movement path. For example, the collision determination unit 15 has a function of determining whether or not to collide with an obstacle based on the radius of the circle generated by the arc generation unit 13. The collision determination unit 15 has a function of outputting a determination result on whether or not to collide to the arc generation unit 13. Further, the collision determination unit 15 has a function of determining whether or not to collide with an obstacle based on the route information input from the recalculation processing unit 14.

  The recalculation unit 14 has a function of recalculating route information based on the information about the arc input from the arc generation unit 13 and the coordinates of the refraction part. Further, it has a function of outputting the recalculated path information to the actuator controller 62.

  As in the first embodiment, the robot 2 configured in this way travels along the given route information, and when there is an obstacle in the traveling direction of the robot 2, a zigzag is used to avoid the obstacle. The route is changed, the changed zigzag route is recalculated into a smooth movement route that the mobile device can travel, and the vehicle travels based on the recalculated movement route.

  Next, operation | movement of the movement path | route acquisition apparatus 10 which concerns on this embodiment is demonstrated using FIGS. 6 to 8 are flowcharts showing the operation of the robot 2 provided with the movement route acquisition apparatus 10 according to the present embodiment, and FIGS. 9 to 14 are schematic views showing the recalculated movement routes.

  The robot 2 provided with the moving speed acquisition device 10 executes the control process shown in FIG. 6 at the timing of departure toward the destination, and repeatedly executes the control process shown in FIG. 6 at a predetermined interval.

  The robot 2 starts from the information input process (S30). The process of S30 is a process that is executed by the movement route generation unit 61 and inputs information for generating a movement route. The movement route generation unit 61 inputs obstacle information from the camera 3, map information from the map DB 4, route information from the route information DB 5, and destination, target arrival time, current location, and the like from the communication unit 7. When the process of S30 ends, the process proceeds to the movement route planning process (S32).

  The process of S32 is a process that is executed by the movement route generation unit 61 and plans a movement route. The movement path generation unit 61 generates a zigzag movement path as shown in FIG. 6A based on the information obtained in the process of S30. Moreover, the movement path | route production | generation part 61 extracts the coordinate of the refractive location of the produced | generated movement path | route. When the process of S32 ends, the process proceeds to a smoothing process for the movement route (S34).

  The process of S34 is a process of smoothing the zigzag moving path. Here, the process of S34 will be described with reference to the flowcharts shown in FIGS. The flowcharts shown in FIGS. 7 and 8 are the details of the process of S34 shown in FIG. 6, and are repeatedly executed until all the refraction points input in the process of S32 are processed.

  As illustrated in FIG. 7, the movement route acquisition apparatus 10 starts with control point information input processing for smoothing the movement route (S <b> 50).

The process of S50 is executed by the control point setting unit 12 and inputs the coordinates of the refraction spot as the coordinates of the control point. In the following description, the total number of control points in the moving route to be smoothed is N, and the position vector of the _i-th control point (i is a natural number from 1 to N−2) from the head of the route will be described as P _i . When the process of S50 ends, the process proceeds to a control point selection process (S52).

The process of S52 is a process that is executed by the control point setting unit 12 and selects three control points that are continuous on the movement path in a zigzag path that connects the control points with a straight line. The control point setting unit 12 selects three control points: a control point P _i , a control point P _{i + 1,} and a control point P _{i + 2} . For example, the control point P _A, the control point P _B and the control point P _C is the are arranged on the movement path successively in this order, the control point P _i to the control point P _A, controls the control point P _B point P _{i + 1,} to select the control points _{P C} as the control point _{P i + 2.} In the following, the length of the line segment P _i P _{i + 1} (line segment P _A P _B ) sandwiched between the control point P _i and the control point P _{i + 1} is sandwiched between l _A , the control point P _{i + 1} and the control point P _{i + 2.} The length of the line segment P _{i + 1} P _{i + 2} (line segment P _B P _C ) is set to 1 _B. When the process of S62 ends, the process proceeds to the first circle generation process (S64).

Processing of step S54 is performed by the arc generating unit 13 calculates a first circle C _i in contact with the two segments consists of three control points, it is a process that approximates a movement path in an arc. That is, the first circle C _i is set so that the line segment P _A P _B and the line segment P _B P _C are common tangents. At this time, the radius of the first circle C _i is set to be maximum. For example, when l _A is smaller than l _B , the contact point between the line segment P _A P _B and the first circle C _i is the control point P _A , and the line segment P _B P _C and the first circle C _i are contact _'as, the point P _a to satisfy equation (9) _below' the point P _a of setting a.

When Expression (9) is satisfied, for example, a circle C _i as shown in FIG. 9 is set, and a zigzag-shaped movement path composed of the line segment P _A P _B and the line segment P _B P _C is set to a smooth arc P _A P _A ', it can be approximated by an arc _P A'P _C.

On the other hand, for example, if _{l A} is not less than _{l B} is the contact point between the line segment _P B _{P C} and the circle _{C i} and the control point _{P C,} the contact point of the line segment _P A _{P B} and circle _{C i} As the point P _A ′, the point P _A ′ is set so as to satisfy the following expression (10).

When the expression (10) is satisfied, for example, a circle C _i as shown in FIG. 10 is set, and a zigzag-shaped movement path composed of the line segment P _A P _B and the line segment P _B P _C is set to a smooth arc P _A P _A ', it can be approximated by an arc _P A'P _C. When the process of S54 is completed, the process proceeds to a collision determination process (S56).

The process of S56 is executed by the collision determination unit 15 and is a process of determining whether or not the robot 2 collides with an obstacle when the robot 2 travels along an arc that is a part of the circle set in the process of S54. is there. The determination of whether or not to collide is made, for example, based on whether or not the generated circle overlaps an obstacle. In the process of S56, for example, as shown in FIG. 11, when it is determined that the circle C _i generated in the process of S54 does not overlap the obstacle O, the circle C _i is approximated by the arc P _A P _A ′. Assuming that it is safe for the robot 2 to travel along the travel path, the process proceeds to the next control point selection process (S64 shown in FIG. 8).

The process of S64 shown in FIG. 8 is executed by the control point setting unit 12, and in the zigzag path connecting the control points with a straight line as in the process of S52, three consecutive control points on the moving path are selected. It is processing to do. Assuming that the control points selected in the process of S52 are the control point P _i , the control point P _{i + 1} , and the control point P _{i + 2} , the control point P _{i + 1} , the control point P _{i + 2} and the control point P _{i + 3} are selected in the process of S64. For example, as shown in FIG. 11, the control point P _B, following the control point P _C, when the control point P _D are arranged on the movement path in succession, the control point P _D as the control point P _{i + 3} select. Hereinafter, the length of the line segment _{P i + 2 P} i + ₃ sandwiched between the control point _{P i + 2} and a control point _{P i + 3} (the line segment _P C _{P D)} and _{l C.} When the process of S64 is completed, the process proceeds to the second circle generation process (S66).

The process of S66 is executed by the arc generation unit 13, calculates a second circle C _{i + 1} in contact with the two line segments composed of the three control points selected in the process of S64, and approximates the movement path by the arc. It is processing. That is, it sets the circle _{C i + 1} as the line _P B _{P C} and the line segment _P C _{P D} becomes common tangent. The setting method is the same as the process of S54. When the process of S66 ends, the process proceeds to a collision determination process (S68).

The process of S68 is executed by the collision determination unit 15, and when the robot 2 travels along an arc that is a part of the circle set in the process of S66, it is a process of determining whether to collide with an obstacle. is there. The determination of whether or not to collide is made, for example, based on whether or not the generated circle overlaps an obstacle. In the process of S68, if it is determined that the second circle C _{i + 1} generated in the process of S66 does not overlap the obstacle O, it is assumed that it is safe for the robot 2 to travel along the movement path approximated by an arc. The process proceeds to the overlap determination process (S76).

The process of S76 is executed by the recalculation unit 14, and is a process of determining whether or not the first circle C _i calculated in the process of S54 and the second circle C _{i + 1} calculated in the process of S66 overlap. is there. If it is determined in the process of S76 that the first circle C _i and the second circle C _{i + 1} overlap, the process proceeds to the third and fourth circle generation processes (S78).

The process of S78 is executed by the arc generation unit 13, and when the first circle C _i and the second circle C _{i + 1} overlap, the arc of the first circle C _{i and} the arc of the second circle C _{i + 1} Is a process of generating a third circle C _i ′ and a fourth circle C _{i + 1} ′ that satisfy the condition for smoothly connecting the two. For example, the third circle C _i ′ is set so that the line segment P _A P _B and the line segment P _B P _C are common tangent lines, and the contact point with the line segment P _B P _C is I ′. 4 of the circle _{C i + 1} ', as the I'contacts segment _P B _{P C,} and set in contact with the line segment _P C _{P D.} That is, when the fourth circle _{C i + 1} 'and the line segment _P C _{P D} and the control point contacts contacting the _{P D'',} set to satisfy the following equation.

When Expression (11) is satisfied, for example, a third circle C _i ′ and a fourth circle C _{i + 1} ′ as shown in FIG. 12 can be set. When the process of S78 ends, the process proceeds to an approximation process (S80).

The processing of S80 is executed by the recalculation unit 14, and is a process of approximating a moving path portion having a zigzag shape by combining the calculated circles to a smooth curve. The first circle C _i generated in the process of S54, the second circle C _{i + 1} generated in the process of S66, the third circle C _i ′ generated in the process of S78, and the fourth circle C _{i + 1} ′ are combined. For example, as shown in FIG. 12, a zigzag path composed of a line segment P _A P _B , a line segment P _B P _C, and a line segment P _C P _D is represented by an arc P _A P _{A ″} and an arc P _{A ″.} I', approximates the path comprising the arc I'p _D'' and arc _{P D''P} D. When the process of S80 is completed, the process proceeds to a process for recording the changed part (S82).

  The process of S82 is executed by the ECU 6, and is a process of recording a place where a zigzag path is approximated by a curve. For example, it records on the memory with which ECU6 is provided. When the process of S82 ends, the control process shown in FIGS. 7 and 8 ends.

On the other hand, in the process step S56 shown in FIG. 7, for example, as shown in FIG. 13, when it is determined that a movement path that approximates an arc of the first circle C _i is the robot 2 collides with the obstacle O when traveling, The process proceeds to the control point changing process (S58). Process step S58 is executed by the control point setting unit 12, in order to regenerate the circle C _i generated in the process of S54 to a more smaller radius of the circle C _i'', a process of changing the control point. For example, the control point is changed to satisfy the following expression.

  As shown in Expression (12), by reducing the control point, the radius of a circle in contact with two continuous line segments can be reduced. When the process of S58 ends, the process proceeds to a limit value confirmation process (S60).

  The process of S60 is a process that is executed by the control point setting unit 12 and determines whether or not the number of processes of S58 is within an allowable range. For example, it is confirmed whether or not the process of S58 has been repeated a predetermined number of times, that is, more than the limit value. In the process of S60, when it is determined that the calculation process is not performed more than the set limit value, the process proceeds to the first circle generation process, and the first circle is again generated based on the control point set in S58. Generation processing is performed (S54). On the other hand, in the process of S60, when it is determined that the calculation process has been performed more than the set limit value, the process proceeds to the current state maintenance process (S62).

  The process of S62 is a process that is executed by the ECU 6 and determines that the path between the control points to be approximated is not approximated by a circular arc, so that the path between the control points is not approximated. When the process of S62 ends, the control process of FIGS. 7 and 8 ends.

On the other hand, in the process of S68 shown in FIG. 8, when it is determined that the robot 2 collides with the obstacle O when the robot 2 travels on the movement path approximated by the arc of the second circle C _{i + 1} , the process proceeds to the control point change process ( S70). The process of S70 is executed by the control point setting unit 12 and is a process of changing the control point in order to regenerate the second circle C _{i + 1} generated in the process of S68 into a circle with a smaller radius. The processing content is the same as the processing of S58 shown in FIG. When the processing of S70 ends, the process proceeds to limit value confirmation processing (S72).

  The process of S72 is executed by the control point setting unit 12 and determines whether or not the number of processes of S70 is within an allowable range. For example, it is confirmed whether or not the process of S70 has been repeated a predetermined number of times, that is, more than the limit value. In the process of S72, when it is determined that the calculation process is not performed more than the set limit value, the process proceeds to the second circle generation process, and the second circle is again generated based on the control point set in S70. Generation processing is performed (S66). On the other hand, in the process of S72, when it is determined that the calculation process has been performed more than the set limit value, the process proceeds to the current state maintenance process (S74).

  The process of S74 is a process that is executed by the ECU 6 and determines that the path between the control points to be approximated is not approximated by a circular arc, so that the path between the control points is not approximated. When the process of S74 ends, the control process of FIGS. 7 and 8 ends.

In the process of S76, if it is determined that the first circle C _i calculated in the process of S54 and the second circle C _{i + 1} calculated in the process of S66 do not overlap, the process proceeds to a replacement process ( S84).

The process of S84 is executed by the recalculator 14 and approximates the movement path using a straight line and an arc, and replaces the zigzag movement path with the approximate movement path. For example, as shown in FIG. 14, a zigzag path composed of a line segment P _A P _B , a line segment P _B P _C, and a line segment P _C P _D is represented by an arc P _A P _A ′ and a straight line P _A ′ P _B. ′, Approximating to a path consisting of arc P _B ′ P _D. When the process of S84 is completed, the process proceeds to the recording process of the changed portion (S82).

  As described above, by executing the control process shown in FIG. 7 and FIG. 8, the refraction point of the zigzag movement path constituted by line segments can be approximated by two arcs or arcs and straight lines, By executing the control processing shown in FIGS. 7 and 8 at all control points, the entire movement path can be approximated to a smooth movement path having no refraction spot, as shown in FIG. 6B.

  Returning to the process of S34 in FIG. 6, when the process of smoothing the zigzag movement path is completed, the process proceeds to the collision determination process (S36).

  The process of S36 is a process that is executed by the collision determination unit 15 and determines whether or not the robot 2 collides with an obstacle when traveling on the movement route obtained in the process of S34. In the process of S36, when it is determined that the robot 2 collides with an obstacle, the process of S34 is executed again. At that time, in the part determined to collide, approximation is performed by further reducing the radius of the circle in the same manner as the process of S58 and the process of S70. On the other hand, if it is determined in the process of S36 that the robot 2 does not collide with an obstacle, the process proceeds to a speed information setting process (S38).

  The process of S38 is executed by the control point setting unit 12, and sets speed information on each movement route. For example, as shown in FIG. 6C, the control point setting unit 12 sets speed information on the movement route approximated in the process of S38. When the process of S38 is completed, the process proceeds to a control signal generation process (S40).

  The process of S40 is a process of controlling the actuator based on the movement path executed by the actuator control unit 62 and recalculated and smoothed in the process of S34. The actuator control unit 62 generates, for example, a control signal for the electric motor 8 based on the recalculated and smoothed movement path, and operates the wheel 9. When the process of S40 ends, the control process shown in FIG. 6 ends.

  Thus, by executing the control process shown in FIG. 6, the zigzag movement path can be approximated to a smooth movement path using straight lines and arcs.

As described above, according to the movement route acquisition apparatus 10 according to the second embodiment obtains the travel route acquired control point P _A to the movement _pathway, provided the P _B and P _C, the path between the control points control point _P a, it is possible to smooth the movement path approximated by an arc _P a _{P a} 'and the arc _P a'P _C based on the information about the _{P B} and _{P C.} Thereby, the movement path | route of a curve can be recalculated using a simple circular arc. Therefore, when the movement route to be acquired is changed, a smooth movement route corresponding to the change can be quickly acquired.

Further, according to the movement path acquisition device 10 according to the second embodiment, the arc generation unit 13 approximates the movement path between the control points using the two circles C _i and C _{i + 1} , and the four circles C _i. By approximating the movement path using _i , circle C _{i + 1} , circle C _i ′, and C _{i + 1} ′, the movement path can be approximated smoothly with high accuracy.

Further, according to the movement route acquisition apparatus 10 according to the second embodiment, the circles C _i , C _{i + 1} , C _i ′, C _i having radii that the wheels 9 that are driving parts provided in the robot 2 can actually turn are provided. By calculating the arc using ″ and C _{i + 1} ′, a movement path corresponding to the driving capability of the robot 2 can be quickly generated.

  Moreover, according to the movement path | route acquisition apparatus 10 which concerns on 2nd Embodiment, a movement path | route can be acquired and it can approximate with a straight line or a circular arc according to the length of the straight line part which connects the control points of the acquired movement path | route. For example, when a zigzag movement path is input and the movement path of a predetermined section is short, the movement path is approximated by combining a plurality of arcs, and when the movement path of the predetermined section is long, a straight line and an arc Can be combined to approximate the movement path. As a result, the zigzag moving path can be quickly recalculated into a smooth moving path composed of an arc and a straight line.

  Furthermore, according to the movement route acquisition device 10 according to the second embodiment, when the robot 2 travels using an approximate movement route, it is determined whether or not it collides with an obstacle. The moving path can be approximated again using a circular arc having a smaller radius. As a result, it is possible to avoid a movement route that collides with an obstacle, so that a safe movement route can be calculated.

  Each embodiment mentioned above shows an example of the movement course acquisition device concerning the present invention. The movement route acquisition device according to the present invention is not limited to the movement route acquisition device according to each of these embodiments, and the movement route acquisition device according to each embodiment is within a range not changing the gist described in each claim. May be modified or applied to others.

  For example, although the example which employ | adopted the movement path | route acquisition apparatus which concerns on each said embodiment as the robot 2 was demonstrated, you may employ | adopt as a vehicle, a robot arm, etc. In short, it moves based on a zigzag-like target movement locus. Any mobile device may be used. By adopting such a mobile device, it is possible to dynamically generate a smooth movement path in consideration of the driving capability of the mobile device.

It is a block diagram of the robot provided with the movement path | route acquisition apparatus which concerns on 1st Embodiment. It is a flowchart which shows operation | movement of the robot provided with the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a block diagram of the robot provided with the movement path | route acquisition apparatus which concerns on 2nd Embodiment. It is a flowchart which shows operation | movement of the robot provided with the movement path | route acquisition apparatus of FIG. It is a flowchart which shows operation | movement of the robot provided with the movement path | route acquisition apparatus of FIG. It is a flowchart which shows operation | movement of the robot provided with the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG. It is a schematic diagram which shows the movement path | route recalculated by the movement path | route acquisition apparatus of FIG.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1,10 ... Movement route recalculation process part (movement route acquisition apparatus), 11 ... Curve approximation part (curve approximation means), 12 ... Control point setting part (control point setting means), 13 ... Arc generation part (arc approximation means) 14... Recalculation unit (recalculation unit), 15. Collision determination unit (collision determination unit).

Claims (7)

A movement path acquisition device that acquires a movement path in advance and recalculates the movement path,
Control point setting means for setting a control point on the movement path and setting information on the control point;
An arc approximation means for approximating a path between the adjacent control points with an arc based on information on the control points;
Recalculation means for recalculating the movement path using the arc;
A travel route acquisition apparatus comprising: A curve approximation means for approximating the movement path acquired in advance with a curve;
The movement path acquisition apparatus according to claim 1, wherein the control point setting unit uses the curve approximated by the curve approximation unit as the movement path.   3. The movement path acquisition apparatus according to claim 1, wherein the first arc approximation unit approximates a movement path between adjacent control points using two arcs. 4.   The said control point setting means sets the position vector and speed vector of the said control point as information regarding the said control point, The movement path | route acquisition apparatus as described in any one of Claims 1-3 characterized by the above-mentioned.   The movement path acquisition apparatus according to claim 1, wherein the arc approximating unit approximates using an arc having a radius larger than a predetermined radius.   6. The movement according to claim 1, further comprising a recalculation unit that recalculates the movement path by selecting a straight line or an arc according to a length of a straight line portion connecting control points of the movement path. Route acquisition device. Based on the movement route calculated by the recalculation means, comprising a collision determination means for determining whether or not to collide with an obstacle,
The recalculation means recalculates the movement path using a circular arc having a smaller radius when the collision determination means determines that the object collides with an obstacle;
The movement route acquisition apparatus according to claim 6.

Images