JP2014026516A - Target route generation device and target route generation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To prevent reduction in followability of an automatic guided vehicle and vibration thereof.SOLUTION: A target route generation device and a target route generation method generate a target route from an initial position Ns of an automatic guided vehicle to a target position Ne. The target route generation device and the target route generation method approximate minimum jerk trajectory, where a rate of change in curvature ρ from the initial position Ns to the target position Ne is zero, in minimum jerk trajectories connecting the initial position Ns and the target position Ne, with a trigonometric function, to calculate the target route.

Description

  The present invention relates to a target route generation device and a target route generation method.

  Conventionally, a method of generating a vehicle route by combining a clothoid curve, a straight line, and an arc is known (see Patent Document 1). The clothoid curve is a curve in which the curvature ρ is proportional to the curve length L. Each of the clothoid curve, the straight line, and the arc has a constant change rate of curvature, so that the vehicle has good trackability. For this reason, it is used for road alignment and route generation of an automatic parking system.

JP 2010-18074 A

  However, in the conventional method, the path is calculated by combining a clothoid curve, a straight line, and an arc, so that the rate of change of curvature is discontinuous at the connection point of these lines. Therefore, if a route combining a clothoid curve, a straight line, and a curve is used as a target route for automatic steering, a large torque is required for the steering servo system at the connection point, the vehicle followability is reduced, and the vehicle is vibrated. May occur and give the passenger a sense of incongruity.

  The present invention has been made in view of the above-described problems, and an object thereof is to provide a target route generation device and a target route generation method that suppress a decrease in followability of an automatic operation vehicle and vibration of the automatic operation vehicle. Is

  In order to achieve the above object, in the target route generation device and the target route generation method according to one aspect of the present invention, the curvature change at the initial position and the target position in the jerk minimizing trajectory connecting the initial position to the target position. A target path is calculated by approximating a jerk minimizing trajectory with a rate of zero with a trigonometric function.

  According to the target route generation device and the target route generation method according to one aspect of the present invention, compared with a route combining a clothoid curve, a straight line, and a curve, a decrease in followability of the automatic operation vehicle and vibration of the automatic operation vehicle are suppressed. can do.

FIG. 1 is a block diagram showing the overall configuration of a target route generation apparatus according to an embodiment of the present invention. FIG. 2 is a block diagram illustrating a detailed configuration of the route generation unit 33 of FIG. FIG. 3 is a flowchart showing a procedure of a target route generation method executed by the route generation unit 33 of FIG. FIG. 4 is a flowchart showing a detailed procedure of step S01 in FIG. FIG. 5 is a plan view showing an example of an obstacle map illustrating a region where the obstacle 101 exists. FIG. 6 is a plan view showing an example in which the obstacle 101 shown in the obstacle map of FIG. 5 is divided by a square convex region 102. FIG. 7 is a Voronoi diagram in which the center of gravity of the convex region 102 in FIG. FIG. 8 is a Delaunay diagram with the generating point 103 of FIG. 7 as the Delaunay point. FIG. 9 is a plan view illustrating the initial position Ns and the target position Ne selected in step S107 of FIG. FIG. 10 is a plan view showing route candidates (R ₁ , R ₂ ) connecting the initial position Ns to the target position Ne. FIG. 11 is a plan view showing a circle Cr passing through _three consecutive waypoints (N ₁ , N ₂ , N ₃ ) and a vector (V ₁ , V ₂ , V ₃ ) in contact with the circle Cr. Figure 12 is a plan view showing three vectors calculated for transit point _{N 3 (Va, Vb, Vc} ) and the search range GN attitude angle of the vehicle. Figure 13 is a plan view showing a corridor KL until the vehicle position CA3 through point _{N 3} from the vehicle position CA2 through point _{N 2.} FIG. 14 is a graph for explaining the line search method (golden section method). FIG. 15 is a plan view for explaining a method for searching for a route by moving a waypoint in Delaunay side 105. FIG. 16A shows a flowchart for searching for the optimum value of the posture angle using the straight line search method within the search range GN of the posture angle, and FIG. 16B shows a straight line search using the Delaunay side 105 as the search range. 6 shows a flowchart for searching for an optimum value of a waypoint using a method. FIG. 17 is a plan view for explaining a method of approximating a segment path by two arcs in contact with each other. FIG. 18 is a plan view showing a combinable range of the circumference C ₁ and the circumference C ₂ . FIG. 19 is a plan view showing the relationship between the posture angle θ _P and the stroke angle φ _P in a circular orbit. FIG. 20A is a graph showing the curvature ρ of each curve of “triangular wave”, “judgement minimizing trajectory”, and “trigonometric function approximation”, and FIG. It is a top view which shows each curve of "minimized orbit" and "trigonometric function approximation". FIG. 21A is a graph showing the curvature ρ of each curve of “trapezoidal wave”, “jumpiness minimizing trajectory”, and “trigonometric function approximation”, and FIG. It is a top view which shows each curve of a jerk minimum trajectory "and a trigonometric function approximation. FIG. 22 is a plan view showing a target path that connects via points W _i (i = 1 to n−1) arbitrarily set between the initial position S and the target position E. FIG. FIG. 23A is a graph showing target paths in a three-dimensional space projected onto the horizontal plane HP and the vertical plane VP, respectively, and FIG. 23B uses two spheroids E ₁ and E ₂ that are in contact with each other. It is a three-dimensional view showing a route approximation model.

  Embodiments of the present invention will be described below with reference to the drawings. In the description of the drawings, the same parts are denoted by the same reference numerals and description thereof is omitted.

[Target route generator]
With reference to FIG. 1, the overall configuration of a target route generation apparatus according to the embodiment will be described. The target route generation device calculates a target route from an initial position to a target position in a two-dimensional space or a three-dimensional space in a movable body (hereinafter referred to as “automatic operation vehicle”) including a vehicle, a ship, and an aircraft. It is a device to generate. In the embodiment, a vehicle capable of automatic steering with wheels will be described as an example of an automatic operation vehicle.

  As shown in FIG. 1, the target route generation device according to the embodiment includes an ECU (electronic control device) 11 that controls the entire target route generation device, a communication unit 12 that performs data communication with the outside, and external recognition including a camera and sonar. A sensor 13, a monitor 14 for providing image information to a vehicle occupant, a speaker 15 for providing audio information, an input unit 16 for receiving an instruction input from the occupant, and a vehicle speed from a rotational speed of a vehicle wheel. A wheel speed sensor 17, a steering angle sensor 18 for measuring a steering angle, a steering actuator 19 for steering the wheel, a driving force control unit 20 for controlling the driving force of the vehicle, and a driving actuator 21 for driving the vehicle are provided.

  The ECU 11 includes a microcomputer including a CPU (Central Processing Unit), ROM, RAM, and an external storage device, and implements the functional configuration shown below by executing a computer program stored in the external storage device. The ECU 11 recognizes or determines an obstacle existing in the region from the initial position to the target position and the position of the obstacle from the image information of the region from the initial position to the target position obtained from the communication unit 12 and the external recognition sensor 13. 31, a steering control unit 32 that transmits a control signal for controlling steering so that the vehicle passes a target route generated by a route generation unit 33, which will be described later, is recognized or determined by a recognition determination unit 31. A path generation unit 33 that generates a target path based on the obstacle and its position, a self-position estimation unit 34 that estimates the current position of the vehicle from the wheel speed and the steering angle obtained from the wheel speed sensor 17 and the steering angle sensor 18, and A positioning control unit 35 that transmits a control signal for determining the position of the vehicle to the driving force control unit 20 is realized.

  The communication unit 12 receives image data captured by a fixed point camera installed outside the vehicle. Image information that cannot be obtained by the external recognition sensor 13 including the camera and the sonar can be obtained via the communication unit 12. For this reason, the recognition determination part 31 can obtain | require the obstruction and its position in a wider range. The route generation unit 33 creates a map of obstacles (obstacle map) existing around the vehicle from the information on the obstacles and their positions.

  The monitor 14 displays the obstacle map created by the route generation unit 33. Furthermore, the monitor 14 can display the target route generated by the route generation unit 33 and the current position of the vehicle estimated by the self-position estimation unit 34 superimposed on the obstacle map. The type of the monitor 14 is not particularly limited, but when a touch panel display is applied, the input unit 16 and the monitor 14 can be integrated if a soft keyboard or other character string is displayed on the screen of the monitor 14. The speaker 15 outputs a sound for guiding a procedure of driving operation when the vehicle is automatically operated along the target route.

  The self-position estimating unit 34 integrates the vehicle speed obtained by the wheel speed sensor 17 to obtain the moving distance of the vehicle, and integrates the rudder angle obtained by the rudder angle sensor 18 by the moving distance, thereby calculating the yaw angle of the vehicle. Can be sought.

  With reference to FIG. 2, a detailed configuration of the route generation unit 33 in FIG. 1 will be described. The route generation unit 33 includes a waypoint setting unit 41, a circumference approximation unit 42, and a trigonometric function approximation unit 43.

  The waypoint setting unit 41 sets at least one or more waypoints that pass along the target route that connects the initial position to the target position, and the attitude angle of the vehicle at the waypoint. The waypoint setting unit 41 divides an obstacle existing between the initial position and the target position into a plurality of convex areas, and obtains the position of the barycentric point of each convex area. Then, at least one waypoint and a vehicle attitude angle at the waypoint are set using a Voronoi diagram and a Delaunay diagram having the center of gravity as a base point. The attitude angle of the vehicle includes, for example, an angle formed by the traveling direction of the vehicle. Details of processing performed by the waypoint setting unit 41 will be described later with reference to FIGS.

  The circumferential approximation unit 42 starts from the initial position based on the position of the via point, the initial position and the target position set by the via point setting unit 41, and the posture angle of the vehicle at each of the via point, the initial position, and the target position. A path connecting to the target position via the via point is approximated by a combination of an arc, a clothoid curve, or a line segment. When the initial position is set as the first waypoint and the target position is set as the last waypoint, the circumference approximating unit 42 determines the waypoint from the waypoint to the next waypoint for each of the other waypoints except the last waypoint. The segmented path is approximated by one arc or two arcs in contact with each other. The section route corresponds to one section when a route connecting from the initial position to the target position is divided by waypoints. A specific example of a “two-circle model” that approximates a section route from one way point to the next way point by two arcs that contact each other will be described later with reference to FIGS. 17 to 18. When the segmented path is approximated by two arcs in contact with each other, the circumferential approximation unit 42 sets the contact point of the two arcs as a new via point, and sets the angle formed by the tangential directions of the two arcs to the vehicle at the new via point. Set as the attitude angle.

  The trigonometric function approximating unit 43 has a minimum jerk from a transit point to the next transit point based on the position of the transit point set by the transit point setting unit 41 and the circumference approximating unit 42 and the attitude angle of the vehicle at the transit point. The target segmented path that approximates the normalized trajectory with a trigonometric function is calculated. The trigonometric function approximating unit 43 calculates a target segment path approximated by a trigonometric function for each of the waypoints from the initial position to the target position, and connects these target segment paths to obtain a target from the initial position to the target position. Generate a route. Details of processing performed by the trigonometric function approximating unit 43 will be described later with reference to FIGS.

[Target route generation method]
With reference to the flowchart of FIG. 3, the procedure of the target route generation method executed by the route generation unit 33 of FIG. 2 will be described.

  First, in step S01, the waypoint setting unit 41, based on the obstacle map, the initial position and the target position, the position of the waypoint along which the route from the initial position to the target position passes, and the initial position, the target position, and the waypoint Sets the vehicle attitude angle at. The detailed procedure of step S01 will be described later with reference to the flowchart of FIG.

  Proceeding to step S03, the circumference approximating unit 42 approximates the segmented route from one waypoint to the next waypoint by two arcs that touch each other. Thereafter, the process proceeds to step S05, and the trigonometric function approximating unit 43 approximates the jerk minimizing trajectory from one waypoint to the next waypoint using a trigonometric function based on the position of the waypoint and the attitude angle of the vehicle at the waypoint. A target segment route is calculated.

[Set via points and attitude angles]
The detailed procedure of step S01 in FIG. 3 will be described with reference to FIGS. First, in step S101, the waypoint setting unit 41 creates an obstacle map based on the obstacle obtained by the recognition determination unit 31 and its position. An example of the created obstacle map is shown in FIG. The obstacle map shows an area where the obstacle 101 exists and other areas where the obstacle 101 does not exist and the vehicle can pass.

  In step S103, the waypoint setting unit 41 divides the obstacle existing between the initial position and the target position into a plurality of convex areas. As the shape of the convex region, in the case of a two-dimensional space, a regular plane filling shape including a regular triangle, a square, and a regular hexagon can be used. In the case of a three-dimensional space, a space-filling polyhedron including a parallelepiped, a truncated octahedron, and a rhomboid dodecahedron can be used as the shape of the convex region. FIG. 6 shows an example in which the obstacle 101 shown in the obstacle map of FIG.

  In step S <b> 105, the waypoint setting unit 41 obtains the position of the barycentric point of each convex region 102, and creates a Voronoi diagram and a Delaunay diagram using each barycentric point as a generating point 103. FIG. 7 shows a Voronoi diagram in which the center of gravity of the convex region 102 in FIG. The Voronoi diagram includes Voronoi boundaries 104 and Voronoi points 106. The Voronoi boundary 104 is a part of a bisector of two adjacent generating points 103, and the Voronoi point 106 is an intersection of two or more Voronoi boundaries 104. The Voronoi boundary 104 and the Voronoi point 106 are uniquely determined with respect to the generating point 103.

  Next, in step S105, the waypoint setting unit 41 creates the Delaunay diagram shown in FIG. 8 from the Voronoi diagram of FIG. The Delaunay diagram includes a Delaunay point and a Delaunay side 105. The Delaunay point corresponds to the generating point 103 in the Voronoi diagram. A Delaunay side 105 is a line segment connecting Delaunay points 103.

  In step S107, the waypoint setting unit 41 selects an initial position and a target position. The initial position and the target position are determined from among the Voronoi points 106 obtained in step S105 based on the current position of the vehicle estimated by the self-position estimation unit 34 and the target arrival position input by the user via the input unit 16. Is selected from each. For example, the Voronoi point 106 nearest to each of the current position and the target arrival position of the vehicle can be set as the initial position Ns and the target position Ne. FIG. 9 illustrates the initial position Ns and the target position Ne selected in step S107. Further, the waypoint setting unit 41 can set the posture angle at each of the initial position and the target position.

In step S109, the waypoint setting unit 41 calculates a route candidate (R ₁ , R ₂ ) connecting the initial position Ns to the target position Ne as shown in FIG. However, the route candidate (R ₁ , R ₂ ) uses the Voronoi point 106 in the area where the obstacle 101 does not exist as a transit point (node) and the Voronoi boundary 104 in the area where the obstacle 101 does not exist as a branch. The waypoint setting unit 41 can create a list (node list) of waypoints through which the route candidates (R ₁ , R ₂ ) pass for each of the calculated route candidates (R ₁ , R ₂ ).

In step S111, when there are a plurality of route candidates (R ₁ , R ₂ ), the waypoint setting unit 41 searches for the shortest route from the plurality of route candidates (R ₁ , R ₂ ). Is determined as the optimum route. For the search of the shortest route, a route search algorithm including a Dijkstra method or an A ^* (Aster) search algorithm can be used. In the example shown in FIG. 10, through point setting unit 41, from among a plurality of candidate routes _(R 1, _{R 2),} determining a path candidate _{R 1} as the optimal path.

The process proceeds to step S113, waypoint setting unit 41 calculates the search range GN attitude angle of the vehicle at each route point on the optimum route R _1. Specifically, first, the waypoint setting unit 41 calculates a circle Cr that passes through _three consecutive waypoints (N ₁ , N ₂ , N ₃ ) as shown in FIG. Then, vectors (V ₁ , V ₂ , V ₃ ) in contact with the circle Cr at _three waypoints (N ₁ , N ₂ , N ₃ ) are calculated. However, when three consecutive waypoints exist on the same straight line, the waypoint setting unit 41 calculates the straight line as a vector at the three waypoints. By calculating the circle Cr and the vectors (V ₁ , V ₂ , V ₃ ) for each waypoint, as shown in FIG. 12, three vectors (Va, Vb) for each waypoint (route point N ₃ ) are shown. , Vc). The waypoint setting unit 41 calculates an angle range including the three vectors (Va, Vb, Vc) as the vehicle attitude angle search range GN.

The process proceeds to step S115, waypoint setting section 41, between the through successive points on the optimal path R _1, as the change in attitude angle is minimized, calculates the combination of the attitude angle at each route point. At this time, the initial posture angle at the initial position Ns and the target posture angle at the target position Ne are set as constraint conditions. The calculated posture angle at each waypoint is set as the initial posture angle solution. The waypoint setting unit 41 connects each waypoint with a smooth route that the vehicle can follow based on the initial solution of the posture angle at each waypoint.

In step S117, the waypoint setting unit 41 calculates the trajectory (corridor) of the moving vehicle with respect to the smooth route that the vehicle can follow, and the corridor KL and the obstacle exist as shown in FIG. It is determined whether or not the projecting region 102 collides with the projecting region. Figure 13 shows a corridor KL until the vehicle position CA3 through point _{N 3} from the vehicle position CA2 through point _{N 2.} When the corridor KL does not collide with the convex area 102 and reaches the next via point (YES in S119), the process proceeds to step S123, and the corridor KL is calculated and the collision is determined for the next via point. On the other hand, when the corridor KL and the convex area 102 collide (NO in S119), the waypoint setting unit 41 changes the attitude angle within the vehicle attitude angle search range GN (NO in S121), and again the corridor. KL calculation and collision determination are performed (S117). In this way, a route in which no collision occurs is searched while changing the posture angle in the posture angle search range GN. This operation is performed between each via point from the initial position Ns to the target position Ne.

  Further, when there are a plurality of smooth paths to the next via point without colliding the corridor KL with the convex area 102, the movement path length of the sum of the distances between the six points for calculating the corridor KL and the nearest point of the convex area 102 Select the path with the maximum integral value for.

  If a smooth route to the next via point without colliding with the convex region 102 is not found within the vehicle attitude angle search range GN (YES in S121), it is determined that the route cannot be generated ( S125), the flowchart of FIG. 4 ends.

Through the above steps, via point setting unit 41 can determine the position of the route point that optimal route R ₁ from the initial position Ns to the target position Ne passes through, and the attitude angle of the vehicle at each route point.

[Line search on Delaunay side]
As a modified example of the flowchart of FIG. 4, if a smooth route that does not collide with the convex region 102 and reaches the next via point is not found within the vehicle attitude angle search range GN (YES in S121), Delaunay A route can be searched by moving the waypoint in the side 105.

  In this search, a straight line search method (golden section method) is used. Finding a solution that maximizes or minimizes an objective function under given constraints is generally called an optimization problem. In this optimization problem, a straight line search method (golden section method) is one of the methods for repeatedly searching for the optimum value within the search range.

The straight line search method (golden section method) will be described with reference to FIG. When searching for x that minimizes the objective function f (x) within a given search range (x _min ≦ x ≦ x _{max in} FIG. 9), the search range (x _min , x _max ) is set to the golden division ratio 1 : (1 + 5 ^1/2) to calculate the value _{x 1} divided by / 2, calculates a search range _{(x min, x max)} value _{x 2} obtained by dividing the golden division ratio. By repeating this process, the minimum value (optimum value) of f (x) is searched. The line search method (golden section method) is often used when dealing with optimization problems because the algorithm is simple and efficient.

With reference to FIG. 15, a method for searching for a route by moving a waypoint in Delaunay side 105 will be described. In transit point N _2, if it can not find the smooth path without colliding with the convex region 102 to the next route point N _3, Delaunay edges crossing between the via-point N ₂ and the next waypoint N ₃ 105 over the search range, searching for a route by moving the waypoint N _2. At this time, the optimum value is searched by dividing the search range using the straight line search method described above. Code CA2 in FIG. 15 shows the outline of the vehicle in the via-point _{N 2,} reference numeral CA21 shows the outline of the vehicle through the point _{N 21,} reference numeral CA22 shows the outline of the vehicle through the point _{N 22.}

  FIG. 16A shows a flowchart for searching for the optimum value of the posture angle using the straight line search method within the search range GN of the posture angle, and FIG. 16B shows a straight line search method using the Delaunay side 105 as the search range. The flowchart which searches the optimal value of a via point using is shown. The range of the smooth route is calculated within the search range GN for the attitude angle (S201), and when there is a route (solution) that does not collide with the convex region 102 (YES in S203), the vehicle is obtained by performing a straight line search. A smooth path that can be followed is searched (S205). At this time, the via point is not moved on the Delaunay side.

On the other hand, if a smooth route that does not collide with the convex region 102 and reaches the next via point is not found in the vehicle attitude angle search range GN (YES in S305), the process proceeds to step S309 and shown in FIG. as described above, by moving the route point N ₂ as the search range on Delaunay edges 105 crossing between the via-point N ₂ and the next waypoint N _3, to the next route point without colliding with the convex region 102 Search for a smooth route.

Incidentally, the straight line on Delaunay edges 105 if the loop shown in FIG. 16 (a) to determine the route to the line search within the search range GN posture angle at waypoint N _2, the path passing through the via-point N ₂ does not exist a loop of Figure 16 to change over the point N ₂ search to (b) are nested structure. In other words, (in the case of solutions without the loop of FIG. 16 (a)) If the solution by line search within the search range GN posture angle not obtained, move waypoint N ₂ as the search range on Delaunay edges 105 ((The loop of FIG. 16B is executed.) Then, returning to the loop of FIG. 16A again, a straight line search is performed in the posture angle search range GN. And the process shown in FIG.16 (b) respond | corresponds to the process from step S117 of FIG. 4 to S125.

[Approximate sectioned path by 2-circle model]
Next, with reference to FIGS. 17 and 18, a specific example of a “two-circle model” that approximates a segmented route from one waypoint to the next waypoint by two arcs that contact each other will be described.

As shown in FIG. 17, the via point is the origin O (0, 0) on the XY coordinate axes, and the vehicle attitude angle at the via point is θ ₀ = 0. On the other hand, the following waypoint coordinates Q (X _{Q, Y Q)} and the attitude angle of the vehicle in the via-point and theta _Q. Here, the attitude angle θ of the vehicle indicates an angle formed with respect to the positive direction of the X axis. In FIG. 17, the arrows extending from the origin O and the coordinates Q are vectors (posture angle vectors) indicating the posture angle of the vehicle.

A path from the origin O to the coordinate Q can be approximated by an arc OP and an arc PQ that constitute a part of _two circles C ₁ and C _{2 that} are in contact with the point P. Circle C ₁ is in contact with the vehicle attitude angle vector at origin O, and circle C ₂ is in contact with the vehicle attitude angle vector at coordinate Q.

When the radius of the circle C ₁ is R ₁ , the center R of the circle C ₁ is a point R (0, R ₁ ) on the Y axis. If the radius of the circle C ₂ is R ₂ , the coordinates of the center S of the circle C ₂ can be expressed as (X _Q ± R ₂ sin θ _Q , Y _Q ± R ₂ cos θ _Q ). In the coordinates of the center S, when the X coordinate ± is a + sign, the Y coordinate ± is a − sign, and when the X coordinate ± is a − sign, the Y coordinate ± is a + sign. Therefore, when the two circles C ₁ and C ₂ are in contact with each other at the point P, the equation (1) holds for the distance (RS) from the center R of the circle C ₁ to the center S of the circle C ₂ .

When formula (1) is summarized with respect to R ₁ and R ₂ and the formula is transformed, formula (2) and formula (3) are obtained.

If the minimum turning radius of the vehicle was _{R min,} (2) equation by substituting _{R min} on the right side of the _{R 2} of (3) and substituting the right side of the _{R 1 R min} of formula, _{R 1} and _{R 2} is The possible range can be expressed by the equation (4).

If the radius _{R 2} of the circumference _{C 2} of substituting _{R min,} it is possible to approximate the path from the origin O to the coordinates Q by a combination of circumferential _{C 1} and the circumferential _{C 2} shown in FIG. 18. Contacts the circumference _{C 1} and the circumference _{C 2} and _{P 2.} On the other hand, when substituting R _min to a radius R ₁ of the circumference C _1, it is possible to approximate the path from the origin O to the coordinates Q by a combination of circumferential C _{1 'and} the circumferential C _2' shown in FIG. 18 . A contact point between the circumference C ₁ ′ and the circumference C ₂ ′ is P ₁ . The circumference approximation unit 42 approximates the path from the origin O to the coordinate Q within the range from the combination of the circumference C ₁ and the circumference C _{2 to} the combination of the circumference C ₁ ′ and the circumference C ₂ ′. be able to. A line l _p having both ends of the contact P ₁ and the contact P ₂ is called a solution curve of the contact P. The circumferential approximation unit 42 can set the contact point P on the solution curve l _p .

When R ₁ / (R ₁ + R ₂ ) = β, the expression (5) can be obtained by substituting the expression (3) into this expression.

Further, by substituting the equation (3) into the coordinates of the center S of the circle C ₂ (X _S = X _Q ± R ₂ sin θ _Q , Y _S = Y _Q ± R ₂ cos θ _Q ), the equation (6) is obtained. It is done. In the coordinates of the center S, when the X coordinate ± is a + sign, the Y coordinate ± is a − sign, and when the X coordinate ± is a − sign, the Y coordinate ± is a + sign.

Therefore, the coordinates (βX _S , βY _S + (1−β) R ₁ ) of the contact point P between the circumference C ₁ and the circumference C ₂ can be obtained by using the formulas (5) and (6) to calculate the radius R ₁ Can be expressed as a function of

Further, as shown in FIG. 19, there is a relationship of θ _P = 2φ _P between the attitude angle θ _P and the stroke angle φ _P in the circular orbit. By utilizing this relationship, the equation (7) can be obtained from the coordinates (βX _S , βY _S + (1−β) R ₁ ) of the contact P. By substituting the equation (5) into the equation (7), the posture angle θ _P of the vehicle at the contact point P between the circumference C ₁ and the circumference C ₂ can be expressed as a function of the radius R ₁ .

In FIG. 17, when the via point is the origin O (0, 0) on the XY coordinate axis and the attitude angle of the vehicle at the via point is θ ₀ = 0, the coordinates Q (X _Q , The case where Y _Q ) is located in the first quadrant of the XY coordinate axis has been described. In this case, among the two circumferences in contact with the posture angle vector at the origin O, the circumference in the first quadrant or the second quadrant is used, and among the two circumferences in contact with the posture angle vector in the coordinate Q, the third quadrant Alternatively, the circumference on the fourth quadrant side is used. The same applies when the coordinates Q (X _Q , Y _Q ) are located in the second quadrant of the XY coordinate axes. On the other hand, when the coordinate Q (X _Q , Y _Q ) is located in the third quadrant or the fourth quadrant of the XY coordinate axis, the third quadrant or the second quadrant out of two circumferences in contact with the attitude angle vector at the origin O Using the circumference on the fourth quadrant side, the circumference on the first quadrant side or the second quadrant side among the two circumferences in contact with the attitude angle vector at the coordinate Q can be used.

As described above, the circumference approximating unit 42 approximates the segmented path from the via point to the next via point by the two arcs that contact each other, and the position of the contact point P of the two arcs and the attitude angle of the vehicle at the contact point P. θ _P can be set. The circumferential approximation unit 42 sets the contact point P as a new route point, sets the tangential directions of the two arcs as the posture angle theta _P of the vehicle in a new way point.

  However, in the combination of two circular arc tracks, the curvature of the track changes discontinuously at the contact point P. Therefore, when a segmented path combining two circular arc tracks is used as a target segmented path for automatic steering, a large torque is required for the steering servo system at the connection point, the followability of the vehicle is reduced, and the vehicle is vibrated. May occur and give the passenger a sense of incongruity. Therefore, the trigonometric function approximating unit 43 connects between the coordinates O and the contact P and between the contact P and the coordinate Q with the curve approximating the jerk minimizing trajectory with a trigonometric function according to the procedure described below. To do. Thereby, the path | route from the coordinate O to the coordinate Q can be represented by the curve which the vehicle can track and the curvature changes smoothly.

[Calculation of target segment route approximated by trigonometric function]
The problem that the rate of curvature change at the contact point P is discontinuous is solved by using a jerk minimizing trajectory to which a constraint condition that the curvature rate of change at the coordinate O and the contact point P is zero is added. However, as a price, a trajectory that turns greatly is generated, and the trajectory swells compared to an arc or clothoid curve. This problem narrows the degree of freedom of the route to the target position without touching the obstacle. Therefore, the trigonometric function approximating unit 43 changes the curvature at the transit point and the next transit point in the jerk minimizing trajectory connecting the transit point (for example, coordinate O) to the next transit point (for example, contact point P). A target segment path is calculated by approximating a jerk minimizing trajectory with a rate of zero with a trigonometric function. As a result, it is possible to avoid the deterioration of the followability and the occurrence of vibration, and to prevent the trajectory from expanding in a trajectory with a large turn.

Origin O in FIG. 19, the contact from the attitude angle theta _{O =} 0 ° at the origin O P, of the curve leading to the attitude angle theta _{P =} 90 ° at the contact point P, the steering angle of the vehicle is zero at the origin O and point P Think of a curve. FIG. 20B is a graph showing the curvature ρ of the curve with respect to the curve length L of these curves. FIG. 20A is a graph showing these curves.

  The “triangular wave” in FIG. 20B indicates the curvature of a combination of two clothoid curves in which the curvature ρ is proportional to the curve length L. Specifically, the “triangular wave” in FIG. 20B is a combination of a clothoid curve between “B1” and “B2” and a clothoid curve between “B2” and “B3”. is there. In “B1”, “B2”, and “B3”, the rate of change in curvature is discontinuous. However, as shown in FIG. 20A, the trajectory is smaller than the other two curves described below.

  20B shows the curvature of the jerk minimized trajectory in which the rate of change in curvature is zero at the origin O and the contact point P. In FIG. Although the curvature change rate is continuous at the origin O and the contact P, the maximum value of the curvature ρ is larger than that of the “triangular wave”. Therefore, as shown in FIG. 20A, the trajectory is larger than the other two curves.

  On the other hand, “trigonometric function approximation” in FIG. 20B shows a curvature of a curve obtained by approximating a jerk minimizing trajectory having a curvature change rate of zero at the origin O and the contact point P with a trigonometric function. The change rate of the curvature ρ at the origin O and the contact point P is gentler than that of the “triangular wave”, and the maximum value of the curvature ρ is smaller than that of the “judgement minimizing trajectory”. Therefore, as shown in FIG. 20A, the trajectory is smaller than the “jumpiness minimizing trajectory” shown below. Specifically, with reference to the “triangular wave” curve, the bulge of the trajectory can be reduced to about half of the “minimum trajectory of trajectory”. In the target trajectory of parallel parking, which requires a large turning amount with a small turning radius, the influence of the freedom of the trajectory that can be generated due to this difference is large. Thus, it can be said that the “trigonometric function approximation” in FIG. 20B is a curve that combines the advantages of both the “triangular wave” and the “judgement minimization trajectory” that are the other two curves.

  The curvature ρ shown in the “judgement minimization trajectory” in FIG. 20B can be expressed as a function of the curve length (l) as shown in the equation (8). Here, “L” indicates the length of the entire curve from the origin O to the contact P.

  The “trigonometric function approximation” in FIG. 20B can also be expressed as a function of the curve length (l) as shown in the equation (9). Expression (9) is obtained by approximating a part of Expression (8) with a cosine function as an example of a trigonometric function.

The area surrounded by the three curves of “triangular wave”, “jumpiness minimizing trajectory” and “trigonometric function approximation” in FIG. 20B and the vertical axis of the graph is expressed by equation (10). All of the curves have the same value. That is, the yaw angle (attitude angle difference θ _P −θ _O ) when traveling from the origin O to the contact P is the same for the three curves.

  Next, as shown in FIG. 21B, consider a curve including a portion that maintains the maximum curvature between the origin O and the contact P, that is, a portion that maintains a constant steering angle (steering angle holding portion RC). . In this case, the trigonometric function approximating unit 43 sets the origin O of the jerk minimizing trajectory connecting from the origin O to the start point of the steering angle holding portion RC, that is, from “B4” to “B5” in FIG. Then, a jerk minimizing trajectory in which the rate of change in curvature at the starting point of the steering angle holding portion RC is zero is approximated by a trigonometric function to calculate a target segment path. Then, the trigonometric function approximating unit 43 holds the steering angle of the jerk minimizing trajectory connecting from the end point of the steering angle holding portion RC to the contact P, that is, from “B6” to “B7” in FIG. A target segment path is calculated by approximating a jerk minimizing trajectory in which the rate of change in curvature at the end point of the portion RC and the contact point P is zero with a trigonometric function. Then, by connecting these target segment paths via the steering angle holding portion RC, a target segment path from the origin O to the contact P is generated.

  The “trapezoidal wave” in FIG. 21B shows the curvature of a combination of a clothoid curve and a circular arc whose curvature ρ is proportional to the curve length L. Specifically, the “trapezoidal wave” in FIG. 21B is a clothoid curve between “B4” and “B5”, an arc between “B5” and “B6”, and “B6”. This is a combination of clothoid curves up to “B7”. In “B4” to “B7”, the rate of change of curvature is discontinuous.

  The “jeepness minimizing trajectory” in FIG. 21B represents the curvature of the jerk minimizing trajectory in which the rate of change in curvature is zero at the origin O, the start and end points of the steering angle holding portion RC, and the contact point P. Show. The curvature of the steering angle holding portion RC is the same as the “trapezoidal wave”. The rate of change of curvature is continuous between the origin O and the contact P. Further, the maximum value of the curvature ρ is the same as that of the “trapezoidal wave”.

  On the other hand, “trigonometric function approximation” in FIG. 21B is a jerk minimizing trajectory from the origin O to the start point of the steering angle holding portion RC, and from the end point of the steering angle holding portion RC to the contact P. The curvature of a curve that approximates each trajectory minimizing trajectory with a trigonometric function is shown. The change rate of the curvature ρ in “B4” to “B7” is more gradual than the “trapezoidal wave”.

The curvature ρ of the jerk minimizing trajectory from the origin O to the starting point of the steering angle holding portion RC in the “judge minimizing trajectory” in FIG. 21B can be expressed by equation (11). Here, “ρ _max ” indicates the maximum curvature of the steering angle holding portion RC, and “L ₁ ” indicates the curve length from the origin O to the starting point of the steering angle holding portion RC.

  Of the “trigonometric function approximation” in FIG. 21B, the curvature ρ from the origin O to the start point of the steering angle holding portion RC can be expressed by equation (12). Expression (12) is obtained by approximating a part of expression (11) with a sine function as another example of a trigonometric function.

As described above, the trigonometric function approximating unit 43 performs the following transit from the transit point based on the position of the transit point set by the transit point setting unit 41 and the circumferential approximation unit 42 and the attitude angle of the vehicle at the transit point. It is possible to calculate a target segmented path that approximates the jerk minimizing trajectory to a point with a trigonometric function. Then, the trigonometric function approximating unit 43 calculates target segment paths for each of the other via points excluding the last via point, and connects these target segment paths. Thus, it is possible to generate a target path that reaches from the initial position Ns shown in FIG. 10 to the target position Ne via each route point on the optimum route R _1.

  In the embodiment, one or more via points are set between the initial position Ns and the target position Ne, a target segment route is calculated for each via point, and these target segment routes are connected to form one target route. Generated. However, the present invention is not limited to this, and the trigonometric function approximating unit 43 directly generates a target path connecting the initial position Ns and the target position Ne without providing a via point between the initial position Ns and the target position Ne. It is also possible. In this case, in the route generation unit 33 in FIG.

Further, as shown in FIG. 22, in any path connecting the initial position S to the target position E, the (n-1) via points through which the path is the _{W i (i = 1~n-1} ), The initial position S is the first waypoint W0, and the target position E is the last waypoint Wn. When the posture angles θ _{i at} all the waypoints W _{0 to} W _n are given, the route generation unit 33 in FIG. 2 selects a target segment route that connects between the waypoints W _i as shown in FIG. It is possible to generate an arbitrary target route that is generated and connected.

  Furthermore, in an automatic operation vehicle that moves in a three-dimensional space such as an aircraft, the dynamics on the horizontal plane and the dynamics on the vertical plane are generally different. Therefore, when generating a target route in a three-dimensional space as shown in FIG. 23 (a), it is necessary to use different rates of curvature change in the horizontal plane (XY plane) HP and the vertical plane (YZ plane) VP. Here, when the generation of the target path in the two-dimensional plane described above is extended to the target path in the three-dimensional space, the target path in the three-dimensional space is projected onto the horizontal plane HP and the vertical plane VP, respectively, and the horizontal plane HP and What is necessary is just to perform the production | generation process of the target path | route in a two-dimensional plane in each of the vertical plane VP. Specifically, the trigonometric function approximation unit 43 first projects the initial position Ns and the target position Ne in the three-dimensional space onto each of the vertical plane and the horizontal plane. Then, for each of the projected vertical plane and horizontal plane, a target projection path is calculated by approximating a jerk minimizing trajectory with a curvature change rate at the initial position Ns and the target position Ne being zero with a trigonometric function. Then, the target paths in the three-dimensional space are calculated by synthesizing these target projection paths.

In the case of a three-dimensional space, the circumference approximation unit 42 approximates the path from the origin O to the coordinate Q using _two spheroids E ₁ and E ₂ that are in contact with each other instead of two circles that are in contact with each other. To do. The two spheroids E ₁ and E ₂ are ellipsoids having a major axis and a minor axis determined based on the vertical dynamics and the horizontal dynamics of the automatic operation vehicle. On contact P of two spheroid E _1, E ₂ are in contact, it comprises a contact point P, the and two spheroid E _1, the plane S where E ₂ is in contact can be set. Then, the attitude angle vector of the automatic operation vehicle at the contact point P can be set in the plane S.

  As described above, according to the embodiment, the following operational effects can be obtained.

  The trigonometric function approximating unit 43 calculates a jerk minimizing trajectory in which the rate of change in curvature ρ at the initial position Ns and the target position Ne is zero among the jerk minimizing trajectories connecting the initial position Ns to the target position Ne. The target route can be calculated by approximation with a function. Thereby, compared with the path | route which combined the clothoid curve, the circular arc, and the straight line, it can avoid the fall of the followability of a vehicle and generation | occurrence | production of the vibration of a vehicle. Further, the trajectory can be prevented from expanding compared to the jerk minimized trajectory in which the rate of change of the curvature ρ at the initial position Ns and the target position Ne is zero.

  The trigonometric function approximation unit 43 can calculate the target path by approximating the jerk minimization trajectory with a sine function. Thereby, in the case where the steering angle holding portion RC is included, it is possible to avoid a decrease in the followability of the vehicle and the occurrence of the vibration of the vehicle and to suppress the swelling of the track.

  The trigonometric function approximation unit 43 can calculate the target path by approximating the jerk minimization trajectory with a cosine function. Thereby, in the case where the steering angle holding portion RC is not included, it is possible to avoid a decrease in the followability of the vehicle and the occurrence of the vibration of the vehicle and to suppress the swelling of the track.

  When the target path passes through at least one or more waypoints from the initial position Ns to the target position Ne, the initial position Ns is the first waypoint, and the target position Ne is the last waypoint, the trigonometric function approximation unit 43, for each via point from the initial position to the target position, calculate the target segmented path by approximating the jerk minimizing trajectory from the transit point to the next via point with a trigonometric function, and connect the target segmented paths To generate a target route. As a result, it is possible to set a target route that passes through one or more route points set arbitrarily.

  The target path is a target path in the three-dimensional space, and the trigonometric function approximation unit 43 changes the curvature ρ at the initial position Ns and the target position Ne for each of the vertical plane and the horizontal plane on which the initial position Ns and the target position Ne are projected. The target projection path can be calculated by approximating the jerk minimizing trajectory where the rate is zero with a trigonometric function. Then, a target path in the three-dimensional space can be calculated by combining the target projection paths. Thereby, also in the production | generation of the target path | route in three-dimensional space, the fall of the followability of an automatic operation vehicle and generation | occurrence | production of the vibration of an automatic operation vehicle can be avoided, and the swelling of a track | orbit can be suppressed.

The target route generation device may further include at least one waypoint and a waypoint setting unit 41 that sets the attitude angle of the automatic operation vehicle at the waypoint. The waypoint setting unit 41 divides an obstacle existing between the initial position Ns and the target position Ne into a plurality of convex regions 102 to obtain the position of the center of gravity of each convex region 102, and determines the center of gravity as the base point 103. Using the Voronoi diagram and Delaunay diagram, it is possible to set the waypoint and the attitude angle of the automatic operation vehicle. As a result, it is possible to search for an optimal route R ₁ that reaches the target position Ne from the initial position Ns without colliding with the obstacle 101 with respect to the arbitrarily created obstacle map. Then, it is possible to set the attitude angle in the via-point and the via-point via the optimum route R ₁ is.

  Although the contents of the present invention have been described according to the embodiments, the present invention is not limited to these descriptions, and it is obvious to those skilled in the art that various modifications and improvements can be made.

DESCRIPTION OF SYMBOLS 33 ... Path | route production | generation part 41 ... Via-point setting part 42 ... Circumference approximation part 43 ... Trigonometric function approximation part 101 ... Obstacle 102 ... Convex area 103 ... Delaunay point (mother point)
104 ... Voronoi boundary 105 ... Delaunay side 106 ... Voronoi point θ ... Attitude angle HP ... Horizontal plane Ne ... Target position Ns ... Initial position VP ... Vertical plane

Claims (7)

A target route generation device that generates a target route from an initial position of an automatic operation vehicle to a target position,
Of the jerk minimizing trajectories connecting the initial position to the target position, the jerk minimizing trajectory in which the rate of change in curvature at the initial position and the target position is zero is approximated by a trigonometric function to calculate the target path. A target path generation device comprising a trigonometric function approximation unit. The target route generation device according to claim 1,
The trigonometric function approximating unit calculates the target path by approximating the jerk minimizing trajectory with a sine function. The target route generation device according to claim 1,
The trigonometric function approximator calculates the target path by approximating the jerk minimization trajectory with a cosine function. In the target route generation device according to any one of claims 1 to 3,
The target path passes through at least one or more waypoints from the initial position to the target position, the initial position is the first waypoint, and the target position is the last waypoint,
The trigonometric function approximating unit calculates the target segmented path by approximating the jerk minimizing trajectory from one waypoint to the next waypoint with a trigonometric function for each of the other waypoints except the last waypoint. And generating the target route by joining the target segmented routes. In the target route generation device according to any one of claims 1 to 4,
The target route is a target route in a three-dimensional space,
The trigonometric function approximating unit, for each of the vertical plane and the horizontal plane on which the initial position and the target position are projected, has a trigonometric function with a jerk minimizing trajectory in which the rate of change in curvature at the initial position and the target position is zero. A target path generation device characterized by calculating a target projection path by approximation and combining the target projection paths to calculate a target path in the three-dimensional space. The target route generation device according to claim 4,
A route point setting unit for setting the at least one route point and the attitude angle of the automatic operation vehicle at the route point;
The waypoint setting unit divides an obstacle existing between the initial position and the target position into a plurality of convex areas to obtain the position of the center of gravity of each convex area, and uses the center of gravity as a mother point. The at least one waypoint and the posture angle are set using a Voronoi diagram and a Delaunay diagram to perform the target route generation device. A target route generation method for generating a target route from an initial position of an automatic operation vehicle to a target position,
Of the jerk minimized trajectories connecting the initial position to the target position, calculating the target path by approximating with a trigonometric function the jerk minimized trajectory in which the rate of change in curvature at the initial position and the target position is zero. A target route generation method characterized by the above.