JP2019096269A - Trajectory planning device, trajectory planning method, and program - Google Patents

Abstract

To provide a method for planning a trajectory from a posture at a start edge to a posture at an end edge when the clothoid length is variable.SOLUTION: A program for trajectory planning is the one that optimizes a trajectory expressed by triple clothoid curves by a repeated optimization technique, the program performing this optimization by executing the steps below: a coarse search step for applying a sample parameter to an arrival point function and finding an arrival point and finding a parameter with which a nearest arrival point that is closest to the end edge among arrival points is obtained, the sample parameter being a plurality of combinations of the length of each of clothoid curves, with the total length of three clothoid curves assumed as being fixed; and an optimization step for varying the parameter of the arrival point function and finding an optimum parameter, with the parameter obtained in the coarse search step assumed as an initial value, so that a posture at the arrival point matches a posture at the end edge.SELECTED DRAWING: Figure 6

Description

  The present invention relates to trajectory planning of an automobile and an autonomous vehicle that can travel automatically without human operation.

  One of the trajectory plans of an autonomous vehicle is to plan the curvature 曲 with respect to the path length s. This is because there is a strong functional relationship between the curvature κ and the front wheel steering angle δ. FIG. 1 is a view showing a bicycle model in which the slip of front and rear wheels is ignored. In this figure, l is the wheel length of the vehicle.

  The bicycle model neglecting the slip of the front and rear wheels is expressed by the following differential equation.

なお、式（４）は車の最小回転半径に起因する条件である。

Equation (4) is a condition caused by the minimum turning radius of the vehicle.

  Here, a posture (posture) which is a state of a vehicle is defined as a state including four variables of x position, y position, vehicle direction θ, and curvature κ. That is, in the present specification, posture is a concept including position. As an operation form of the autonomous driving vehicle running on the road, there may be a form of traveling along a milestone determined in advance on the road. If a curved road is also assumed, not only the position but also the appropriate vehicle direction and curvature corresponding to the curved road are determined at this milestone.

  FIG. 2 is a diagram showing an example of a steering plan with a path length as an argument for placing a vehicle in an initial state on one milestone. When the vehicle arrives at a milestone, not only the position but also the curvature determined from the direction of the vehicle and the direction of the front wheel needs to be matched to the curved road. This steering plan should be considered as a trajectory plan starting from the initial position, orientation and front wheel orientation of the vehicle (Start) and ending with the posture at the milestone on the road (End) Can. The trajectory planning covered in this specification is intended for planning the curvature κ for this kind of path length s. In this type of trajectory planning, the curvature κ (s) is required to be a continuous function of the path length s in order for the vehicle to travel without stopping.

For the following description, it is assumed that the posture (posture) at the start point (Start) of the trajectory plan is vehicle position (x, y) = (x ₀ , y ₀ ), vehicle direction θ = θ ₀ , and curvature κ = κ ₀ . Similarly, the posture (posture) at the end (End) is vehicle position (x, y) = (x ₁ , y ₁ ), vehicle direction θ = θ ₁ , and curvature 曲 = κ ₁ . When the vehicle position is treated as a two-dimensional vector, it is expressed as P = [x, y] ^T.

The vehicle positions at the beginning and end are P ₀ = [x ₀ , y ₀ ] ^T and P ₁ = [x ₁ , y ₁ ] ^T , respectively. Hereinafter, the position of the start end is referred to as “start point”, and the position of the end end is referred to as “end point”. Trajectory planning from this type of starting point posture to the ending position posture is not limited to automatic driving, and many studies have been conducted (Patent Document 1, Non-Patent Document 1). 1, Non-Patent Document 2). However, keep in mind that the term posture may not be used.

JP, 2011-14579, A

D. H. Shin and S. Singh. “Path generation for robot vehicles using composite clothoid segments,” Carnegie-Mellon Univ. Pittsburgh PA Robotics Inst., No. CMU-TR-90-31, 1990. Toru Orchid, Hirofumi Tamai, and Hiroshi Makino. "Free point sequence interpolation with triple clothoid." Journal of Japan Society for Precision Engineering 76.10 (2010): 1194-1199.

  Although all the above-mentioned documents solve the trajectory planning from the initial posture (posture) to the posture at the end (posture) by three clothoids as a partial problem, the problem setting is restricted. That is, in the study by Shin et al., There is no restriction on the length of the clothoid, but there is a strong restriction that the absolute value of the rate of change of curvature according to the path length is constant over all three clothoids. In the study by Orchid et al., There is a strong constraint of pre-fixing the ratio of the three clothoids.

  In view of the above background, the present invention is a method of planning a trajectory from the posture at the beginning to the posture at the end when the clothoid length is variable as described above.

  The program according to the present invention is a program for planning a trajectory consisting of three clothoid curves, and the computer includes data of the position of the vehicle at the beginning, direction, and attitude data specified by the curvature and the position of the vehicle at the end. The input step of receiving the data of the posture specified by the direction and the curvature, and the function for obtaining the posture specified by the position, the direction and the curvature of the vehicle at the arrival point from the posture of the starting end The target function including the length of the curve as a parameter is read from the storage unit, and the total length of the three clothoid curves is fixed, and a plurality of combinations of the lengths of the clothoid curves are used as sample parameters. A parameter is applied to the arrival point function to obtain an arrival point, and among the arrival points, the nearest neighbor closest to the end The parameter of the arrival point function is varied with a rough search step for obtaining a parameter for obtaining the arrival point and the parameter obtained in the rough search step as initial values, and the attitude of the arrival point matches the attitude of the terminal And optimization steps to determine such optimal parameters.

  In the program according to the present invention, if there is no sample parameter that gives the nearest arrival point at which the distance to the end is equal to or less than a predetermined threshold in the rough search step, the solution satisfying the input condition It may be determined that there is no

  The program according to the present invention is a program for planning a trajectory consisting of three clothoid curves, and the computer includes data of the position of the vehicle at the beginning, direction, and attitude data specified by the curvature and the position of the vehicle at the end. The input step of receiving the data of the posture specified by the direction and the curvature, and the function for obtaining the posture specified by the position, the direction and the curvature of the vehicle at the arrival point from the posture of the starting end The target function including the length of the curve as a parameter is read from the storage unit, and the total length of the three clothoid curves is taken as a first fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters. The parameters of the sample are applied to the arrival point function to obtain an inner circumference arrival point before the end, and A first parameter calculating step of obtaining as a first parameter a parameter that obtains the nearest inner circumference arrival point closest to the terminal end among the arrival points; total lengths of the three clothoid curves longer than the first fixed length As a second fixed length, a plurality of combinations of lengths of the clothoid curve are used as parameters of the sample, and the parameters of the sample are applied to the arrival point function to obtain an outer peripheral arrival point behind the end; A second parameter calculating step of obtaining, as a second parameter, a parameter that obtains the nearest outer peripheral arrival point closest to the end among the outer peripheral arrival points, total lengths of the three clothoid curves and the first fixed length A plurality of combinations of the respective lengths of the clothoid curve as a medium fixed length in the middle of the second fixed length as a parameter of the sample, the sump The above parameters are applied to the arrival point function to obtain the nearest inner circumference arrival point and the nearest outer circumference arrival point, and the parameter for which the nearest inner circumference arrival point is obtained is obtained as the third parameter, and the nearest outer circumference arrival point The third and fourth parameter calculating steps for obtaining the fourth parameter as the fourth parameter, and the fourth nearest outer circumference arrival point given by the second parameter and the fourth nearest outer circumference arrival point given by the fourth parameter are compared If the nearest outer peripheral arrival point given by the parameter is closer to the end, the intermediate fixed length is taken as a new second fixed length, and the nearest outer peripheral arrival point given by the second parameter is the aforementioned second fixed length. When it is close to the end, the nearest inner circumference arrival point given by the first parameter is compared with the nearest inner circumference arrival point given by the third parameter, and given by the third parameter If the nearest inner circumferential arrival point is closer to the terminal end, the fixed length update step for changing the intermediate fixed length to a new first fixed length, and the first fixed length in the fixed length update step Alternatively, while updating the second fixed length, the nearest outer circumference arrival point and the nearest inner circumference arrival point are repeatedly determined, and the difference between the first fixed length and the second fixed length becomes equal to or less than a predetermined threshold. A parameter determination step of obtaining a parameter giving a nearest arrival point closer to the terminal among the nearest outer circumference arrival point and the nearest inner circumference arrival point, and the parameter obtained in the parameter determination step As an initial value, the parameter of the arrival point function is varied, and an optimization step is performed to obtain an optimum parameter such that the attitude of the arrival point matches the attitude of the end.

  The program according to the present invention compares the nearest inner circumference arrival point given by the first parameter with the nearest inner circumference arrival point given by the third parameter in the fixed length update step, using the first parameter. If the closest inner circumference arrival point to be given is closer to the end, a parameter giving the nearest arrival point closer to the end among the nearest outer circumference arrival point and the nearest inner circumference arrival point is determined May be

  The program of the present invention may determine that there is no clothoid curve connecting the start end and the end if the inner circumferential arrival point is not found by the calculation in the first parameter calculation step.

  In the program according to the present invention, when the outer periphery arrival point is not found by the calculation in the second parameter calculation step, the outer periphery arrival point is found or the second fixed length becomes equal to or more than a predetermined threshold. The second fixed length is increased to perform processing for obtaining an outer peripheral arrival point, and when the second fixed length is equal to or greater than a predetermined threshold value, it is assumed that there is no clothoid curve connecting the start end and the end. You may judge.

  In the program of the present invention, the arrival point function may include the turning curvature of the vehicle as a parameter, and may have the maximum value of the turning curvature as a constraint condition.

  The trajectory planning method according to the present invention is a method for planning a trajectory consisting of three clothoid curves by a trajectory planning device, wherein the trajectory planning device is an attitude specified by the position, orientation, and curvature of the vehicle at the beginning. And the trajectory planning apparatus receives the data of the position of the vehicle at the end, the orientation, and the data of the attitude specified by the curvature, and the trajectory planning device calculates the position, orientation, and curvature of the vehicle at the arrival point from the attitude of the starting end An arrival point function including at least a length of a clothoid curve as a parameter is read out from the storage unit, which is a function for obtaining a designated attitude, and the trajectory planning device takes the total length of the three clothoid curves as a fixed length. A plurality of combinations of respective lengths of the clothoid curve are used as parameters of the sample, and the parameters of the sample are applied to the arrival point function. A rough search step for obtaining an arrival point and obtaining a parameter for obtaining a nearest arrival point closest to the terminal among the arrival points, and the trajectory planning apparatus is configured to obtain an initial value of the parameter obtained in the rough search step And the optimization step of varying the parameter of the arrival point function to obtain an optimum parameter such that the attitude of the arrival point matches the attitude of the end.

  The trajectory planning method of the present invention is a method for planning a trajectory consisting of three clothoid curves by a trajectory planning device, wherein the trajectory planning device is specified by the position, orientation, and curvature of the vehicle at the beginning. The input step for receiving data on the attitude and the position, direction, and curvature of the vehicle specified at the end, and the trajectory planning device, from the start attitude to the position, direction, and curvature of the vehicle at the arrival point An arrival point function which is a function for obtaining a designated attitude and which includes at least the length of the clothoid curve as a parameter is read out from the storage unit, and the trajectory planning device determines the total length of the three clothoid curves as the first. A plurality of combinations of the respective lengths of the clothoid curve as fixed lengths are used as the parameters of the sample, and the parameters of the sample are the arrival point function. A first parameter calculation is performed to obtain an inner circumference arrival point before the end, and obtain a parameter that is the nearest inner circumference arrival point closest to the end among the inner circumference arrival points as a first parameter Step and the trajectory planning apparatus set a total length of three of the clothoid curves as a second fixed length longer than the first fixed length, and take a plurality of combinations of respective lengths of the clothoid curves as sample parameters; The parameter of the sample is applied to the arrival point function to obtain an outer peripheral arrival point that is deeper than the end, and among the outer peripheral arrival points, a parameter that is closest to the end is obtained as a second parameter. In the second parameter calculating step obtained as a parameter, the trajectory planning device calculates the total length of the three clothoid curves halfway between the first fixed length and the second fixed length. A plurality of combinations of the respective lengths of the clothoid curve as fixed lengths are used as parameters of the sample, and the parameters of the sample are applied to the arrival point function to determine the nearest inner circumference arrival point and the nearest circumference outer circumference arrival point The third and fourth parameter calculating steps of obtaining the parameter that has obtained the nearest inner circumferential arrival point as the third parameter and the parameter that has obtained the closest outer circumferential arrival point as the fourth parameter; If the nearest outer periphery arrival point given by the parameter is compared with the nearest outer periphery arrival point given by the fourth parameter and the nearest outer periphery arrival point given by the fourth parameter is closer to the end, the above If the intermediate fixed length is a new second fixed length, and the nearest outer peripheral arrival point given by the second parameter is closer to the end, the first parameter When the closest inner circumference arrival point given by the third parameter is compared with the nearest inner circumference arrival point given by the third parameter when the nearest inner circumference arrival point given by the third parameter is closer to the end A fixed length update step for setting the intermediate fixed length as a new first fixed length, and the track planning device updates the first fixed length or the second fixed length in the fixed length update step While determining the nearest outer peripheral arrival point and the nearest inner peripheral arrival point repeatedly, and when the difference between the first fixed length and the second fixed length becomes equal to or less than a predetermined threshold, the nearest outer peripheral arrival point And a parameter determination step of obtaining a parameter giving the nearest arrival point closer to the terminal among the closest inner circumference arrival points, and the trajectory planning apparatus uses the parameter obtained in the parameter determination step as an initial value , Said arrival point Of the parameter varying the, and a optimization step the posture of the goal seek the optimum parameters to match the orientation of the end. Hereinafter, this rough search method is referred to as "binary search method".

  The track planning apparatus according to the present invention is a track planning apparatus for planning a track consisting of three clothoid curves, and includes data of the position, orientation, and curvature of the vehicle at the beginning and data of the vehicle at the end. An input unit that receives data of an attitude specified by position, direction, and curvature, and a function for obtaining an attitude specified by the position, direction, and curvature of a vehicle at an arrival point from the attitude of the starting point, at least A storage unit storing an arrival point function including the length of the clothoid curve as a parameter, and a plurality of combinations of lengths of the clothoid curve with the total lengths of the three clothoid curves as fixed lengths are used as sample parameters The parameters of the sample are applied to the arrival point function read from the storage unit to obtain an arrival point, and the closest one of the arrival points closest to the end is obtained. A rough search unit for obtaining a parameter from which the side arrival point is obtained, and the parameter obtained by the rough search unit as an initial value, the parameter of the arrival point function is varied, and the attitude of the arrival point matches the attitude of the terminal And an optimization unit for determining an optimal parameter.

  A trajectory planning apparatus according to another aspect of the present invention is a trajectory planning apparatus for planning a trajectory consisting of three clothoid curves, comprising: data of a position, orientation, and a posture specified by a curvature at a starting end; It is a function to find the position specified by the position, direction, and curvature of the vehicle at the arrival point from the start position, and the input unit that receives data of the position specified by the position, direction, and curvature of the vehicle at the end A storage unit storing an arrival point function including at least the length of a clothoid curve as a parameter, and applying a plurality of combinations to sample arrival parameters to the arrival point function read from the storage unit; Among the arrival points, a rough search unit for obtaining a parameter for obtaining the nearest arrival point closest to the terminal, and the parameter obtained in the parameter determination step are set as initial values. An optimization unit for changing the parameter of the arrival point function to obtain an optimum parameter such that the attitude of the arrival point matches the attitude of the end, and the rough search unit includes the binary search method described above Run.

  According to the present invention, the trajectory can be planned from the attitude of the start and the attitude of the end when the clothoid length is variable.

It is a figure which shows the bicycle model which disregarded the slip of front and rear wheels. It is a figure which shows the example of the steering plan which makes an argument the path length which puts the vehicle in a certain initial state on one milestone. It is a conceptual diagram which shows the trajectory model of 3 stations. It is a figure which shows the structure of the track planning apparatus of 1st Embodiment. It is a flowchart which shows operation | movement of the track planning apparatus of 1st Embodiment. It is a flow chart which shows processing of initial value determination in a 1st embodiment. It is a figure which shows operation | movement of the coarse search process in 2nd Embodiment. It is a figure which shows the process which _{calculates |} requires fixed full length lower limit _htotalLB . It is a figure which shows the process which _{calculates |} requires fixed full length upper limit _htotalUB . (A) It is a figure which shows the range of an inner periphery arrival point. (B) It is a figure which shows the range of an outer periphery arrival point. It is a figure which shows the data of the beginning and the end which perform the track planning of 1st Example. h _total = _| a diagram illustrating a and reaches point set when the _{S Parr (h total) | P} 1 -P 0. ^{_{h 1 ※, h 2 ※,}} h 3 ※, θ d, 1 4 given by minute change the following equation ^※ parameters ^{_{h 1 ※, h 2 ※,}} h 3 ※ + Δh 3, θ d, 1 ※ + Δθ d, by _{1 is} a diagram obtained by changing the arrival point at a position closer to the _{P arr} + _{ΔP arr.} It is a diagram showing the arrival point set _S ParrG _{(h total)} in the case where the _{| (a) h total = 1} · | P 1 -P 0. _(B) the _{h total} length 1 · _| P 1 -P _{0 |} increased _from, h total = 2 · _| a diagram showing a the reached point set when the _{S ParrG (h total) | P} 1 -P 0 is there. It is a figure which shows distribution of an arrival point when update processing of _htotalLB and _htotalUB is performed. It is a figure which shows the track | orbit obtained by performing an iterative optimization method (major dual interior point method). It is a figure which shows the calculated orbit in each other case.

  Hereinafter, a trajectory planning apparatus and a trajectory planning method according to an embodiment of the present invention will be described with reference to the drawings. First, the trajectory of the triple clothoid will be described as a premise to explain the trajectory planning apparatus of the present embodiment, but first, as a preparatory step of modeling the trajectory by the triple clothoid, one segment starting from the xy coordinate origin Describe the orbit of clothoid.

(Orbit of one segment clothoid)
The trajectory of this kind of clothoid is uniquely determined by the positions of the start and end and the direction and curvature of the vehicle at the start and end. For example, assuming that the direction and curvature at the beginning are (θ _α , _{α α} ) and the direction and curvature at the end are (θ _β , _{β β} ), the one segment total path length h of clothoid is expressed by the following equation.

１セグメント全経路長が定まるので、曲率κの軌道は次式で表される。ただし、Ｓは１セグメント全経路長hによる規格化経路長とする（Ｓ＝ｓ／ｈ）

Since the one-segment total path length is determined, the trajectory of curvature κ is expressed by the following equation. Where S is a normalized path length by one segment total path length h (S = s / h)

ここでクロソイドの１セグメントにおける向きの変化θ_ｄ＝θ_β−θ_αを導入すると，次式を得る。

Here, when the change in orientation θ _d = θ _β -θ _α in one segment of the clothoid is obtained, the following equation is obtained.

車両向きθの軌道、経路長ｓに対する曲線の変化率η、sharpnessは、次式で表される。

The trajectory of the vehicle direction θ, the rate of change 曲線 of the curve with respect to the path length s, and the sharpness are expressed by the following equations.

式（１２）において、Ｓ＝１とおき、１セグメントクロソイドのｘｙ移動量Ｐ_ｄ＝［ｘ_ｄ，ｙ_ｄ］^Ｔが次式で与えられる。

The rate of change of the curve 曲線, sharpness, is constant as apparent from the definition in one clothoid segment. If the trajectory of the vehicle direction θ is determined, the trajectory of P (S) = [x (S), y (S)] ^T representing the movement amount from the coordinate origin is determined by the following equation from Eqs. (1) and (2) Ru.

In equation (12), assuming that S = 1, the xy movement amount P _d = [x _d , y _d ] ^{T of} the one-segment clothoid is given by the following equation.

This P _d = (θ _α , _{α α} , h, θ _d ) is used as a function in modeling an orbit by a triple clothoid. The calculation of equations (12) and (15) will be described for the numerical solution of trajectory planning. However, numerical calculation of elements other than C _R (...) And S _R (...) Is omitted because it is easy. Calculation of C _R (...) and S _R (...) is divided into three cases.

[Case 1 (θ _d −hκ _α = 0 and _{α α} = 0)]
This case corresponds to linear motion. This case is also included in the clothoid by definition.

[Case 2 (θ _d −h _α = 0 and _{α α} ≠ 0)]
This case corresponds to a circular movement. This case is also included in the clothoid by definition.

ただし、 [Case 3 (θ _d −hκ _α ≠ 0)]
This case is difficult to calculate because it involves Fresnel integral. There is also a method that uses numerical integration, but in order to obtain sufficient accuracy, the amount of calculation increases. Calculate using the following formula.
The part corresponding to the Fresnel integral is a part of the equations (23) and (24), and two different approximate formulas of effective ranges of approximation are switched and used.

However,

In the above preparation, the xy movement amount P _d = (θ _α , _{α α} , h, θ _d ) of the one-segment clothoid is the direction θ _α and the curvature _{α α} at the beginning of the segment, one segment total path length h, 1 It was shown that it can be calculated and defined as a function with 4 variables of the direction change θ _d in the segment.

  Subsequently, the trajectory planning from the posture at the beginning to the posture at the end by a triple clothoid when the clothoid length is variable is formulated as a numerical optimization problem.

(The orbit of the triple crosoid)
The formulation of the numerical optimization problem for the triple clothoid orbit takes the following approach.
The posture of the start end of the first clothoid is set to (x ₀ , y ₀ , θ ₀ , _{0 0} ).
Connect three segments of clothoid so that the curvature θ and the orientation κ are continuous.
_Set the posture of the end of the third clothoid to (x _arr , y _arr , θ ₁ , _{1 1} ).
The parameters of the first to third clothoids are numerically optimized under the constraint of | κ | ≦ κ _limit until x _arr = x ₁ and y _arr = y ₁ .

  FIG. 3 is a conceptual diagram showing the trajectory model of the triple clothoid according to this approach. The variables for the kth segment (k = 1, 2, 3...) Are defined as follows.

The steering angle of the front wheels needs to be continuously changed with respect to the path of the traveling vehicle. For this reason, the direction θ of the vehicle and the curvature κ of the vehicle are continuous at the end points between the segments. First, considering the vehicle direction θ, it is necessary to satisfy the following equation from the continuity at both end points of each segment.

When the equations (25) to (40) are put together, the following equation is obtained.

If the eight variables h ₁ , h ₂ , h ₃ , θ _{d, 1} , θ _{d, 2} , θ _{d, 3} ,, _{α, 2} , _{α α, 3} satisfy the equation (41), the equation (25) to the equation The track defined by (40) satisfies the condition regarding the vehicle direction and curvature continuity of the track planning problem. Here, equation (41) is four equation constraints, and the essential parameters can be narrowed from eight variables to four variables. In the present invention, (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) is selected as four parameters of the triple clothoid segment (see FIG. 3). Θ _{d, 2} , θ _{d, 3} , κ _{α, 2} , κ _{α, 3} , θ _{α, 2} , θ _{α, 3} of each clothoid are parameters (h ₁ , h ₂ , h ₃ , It is determined by the following equation as a function of θ _{d, 1} ).

If h ₁ (h ₂ + h ₃ ) ≠ 0, the right sides of Formulas (42) to (47) will not be indefinite. In the present embodiment, h ₁ > 0, h ₂ > 0, and h ₃ > 0 are assumed, so this condition is satisfied. From equation (44), | κ _{α, 2} | <κ _limit is converted to the following inequality constraint for h ₁ , θ _{d, 1} .

From equation (45), | κ _{α, 3} | <κ _limit is converted to the following inequality constraint for h ₁ , h ₂ , h ₃ , θ _{d, 1} .

If the parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) are determined, θ _{α, k} , _{α α, k} , θ _{d, k} (k = 1, k = 1, k) according to equations (42) to (47) 2, 3) is decided. Further, the arrival point _Parr of the triple clothoid segment is calculated as follows from Expression (15).

In other words, if the parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) are used, the conditions on the vehicle direction and the curvature are automatically satisfied, and if the condition P _arr = P ₁ on position is satisfied, the desired The trajectory from the beginning posture to the ending posture is obtained. Hereinafter, the parameter expression by (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) is referred to as a four-parameter expression.

The problem of searching for (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) such that P _arr (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) = P ₁ is formulated as a numerical optimization problem Ru. The objective function f _obj (P _arr ) is chosen to be the Euclidean 2-norm squared value of P _arr −P ₁ . At this time, it can be described as the numerical optimization problem as follows.

上記の最適化問題を解いて，最適化の結果、

が達成できれば、解となる軌道が得られる。つまり、所望の始端の姿勢（posture）から終端の姿勢（posture）への軌道が得られる。

Solve the above optimization problem, and as a result of optimization,

If we can achieve, we can get a solution trajectory. That is, the trajectory from the desired beginning posture to the ending posture is obtained.

(First embodiment)
It is possible to apply an iterative optimization method such as principal dual interior point method (PDIPM) and successive quadratic programming (SQP) based on the pseudo-Newton method to the optimization problem as in equation (55). However, since the optimization problem of equation (55) is non-linear and non-convex, selecting an inappropriate initial value results in the problem that no solution is obtained. The present embodiment finds an appropriate initial value to be used for the iterative optimization method in the optimization problem of equation (55).

  FIG. 4 is a diagram showing the configuration of the trajectory planning apparatus 1 according to the first embodiment. As shown in FIG. 4, the trajectory planning apparatus 1 according to the first embodiment includes an input unit 10, a rough search unit 11, an optimization unit 12, an output unit 13, and a storage unit 14. .

  The input unit 10 has a function of receiving input of data on the start and end of the vehicle as a condition of trajectory planning. Specifically, the attitude (position, direction, curvature) of the start of the vehicle and the attitude (position, direction, curvature) of the end.

The storage unit 14 stores an arrival point function that calculates an arrival point from the beginning. The arrival point function is a function represented by the above-mentioned equation (53), and is a function having four parameters of (h ₁ , h ₂ , h ₃ , θ _{d, 1} ).

The coarse search unit 11 obtains a parameter from which the nearest neighbor arrival point closest to the end is obtained from among the arrival points obtained by applying an appropriate sample parameter to the arrival point function. The rough search unit 11 determines the arrival point set S defined by the following equation by a set of parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) when h _total = h ₁ + h ₂ + h ₃ is fixed. _{Calculate ParrG} (h _total ).

If this set is empty, it is judged that there is no appropriate initial value, and if it is not empty, it corresponds to the element (nearest neighbor arrival point) closest to the end position P ₁ in the arrival point set S _ParrG (h _total ) The four parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) are repeatedly used as initial values of the optimization method. Note that Δθ _fan in the equation defines an angular range in which the arrival point P _arr should exist, and is, for example, π / 4.

The optimization unit 12 performs parameter optimization processing that satisfies the above-mentioned equation (55) using the parameter obtained by the rough search unit 11 as an initial value, whereby the attitude of the reaching point becomes the attitude of the end. It has a function to obtain such parameters. This parameter plans the trajectory that the vehicle should follow. Subsequently, the optimization unit 12 has a function of also obtaining the turning curvature κ at each position from the obtained parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ).

The output unit 13 sends the parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) obtained by the optimization unit 12 and the turning curvature κ to an automatic driving control unit (not shown) that controls the vehicle. Output.

  FIG. 5 is a flowchart showing the operation of the trajectory planning apparatus 1 according to the first embodiment. The trajectory planning apparatus 1 receives input of data of the start and end of the vehicle as a condition of trajectory planning (S10). The trajectory planning apparatus 1 reads the arrival point function from the storage unit 14 (S11), and determines an initial value using the arrival point function (S12).

FIG. 6 is a flowchart showing the process of determining the initial value in the first embodiment. The rough search unit 11 sets an appropriate parameter for the arrival point function, and roughly searches for a parameter that can obtain the arrival point closest to the terminal (the nearest arrival point). Specifically, the rough search unit 11 fixes the total length (h ₁ + h ₂ + h ₃ ) of the triple clothoid to the linear distance h _total from the start point P ₀ which is the start end position to the end point P ₁ which is the end position S20) A combination of four parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) is selected, for example, 1000, and an arrival point when each parameter is applied to the arrival point function is determined (S 21).

If the coarse search unit 11 finds a near arrival points of arrival point for determining the parameters was obtained (YES in S22), arrival point nearest end point P ₁ as the initial value (S23). If no nearby arrival point is found (NO at S22), it is determined that there is no trajectory from the start to the end (S24).

Returning to FIG. 5, the description will be continued. When the trajectory planning apparatus 1 determines the initial value, it optimizes the parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) using the determined initial value, and optimizes the drawing of the trajectory from the start to the end Parameters are calculated (S13). The trajectory planning apparatus 1 also determines the turning curvature 旋回 at each position based on the determined optimum parameter. Subsequently, the trajectory planning device 1 outputs the parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) and the turning curvature κ to the automatic operation control unit (S 14).

  The configuration and operation of the trajectory planning apparatus 1 have been described above, but an example of the hardware of the above-described trajectory planning apparatus 1 is a computer provided with a CPU, RAM, ROM, hard disk, display, keyboard, mouse, communication interface, etc. is there. A program having a module for realizing each of the above-described functions is stored in a RAM or a ROM, and the above-described trajectory planning device 1 is realized by executing the program by the CPU. Such programs are also included in the scope of the present invention.

Second Embodiment
Next, a trajectory planning apparatus according to a second embodiment will be described. The basic configuration of the trajectory planning apparatus of the second embodiment is the same as the configuration of the trajectory planning apparatus 1 of the first embodiment (FIG. 4), but the search processing in the coarse search unit 11 is different.

The outline of the process of the coarse search in the second embodiment is as follows. The total length (h ₁ + h ₂ + h ₃ ) of the triple clothoid is fixed so that the distribution of the arrival point occurs before the end point P _{1 when} viewed from the start point P ₀ . For example, if the linear distance from the start point P ₀ to the end point P ₁ of the triple clothoid is assumed, the distribution of the arrival point will occur in front of the end point P ₁ as viewed from the start point P ₀ . This fixed length is h _{total LB.}

Further, the distribution of the arrival point to secure the entire length _{(h 1 + h 2 + h} 3) of triplicate clothoid as viewed from the start point _{P 0} occurs in the back of the end point _{P 1.} For example, from the starting point P ₀ and 2 times the linear distance to the end point P _1, the normal distribution of the arrival point will occur at the back of the end point P ₁ when viewed from the starting point P _{0. Let} this fixed length be h _totalUB .

_Then, while maintaining the relationship of _{h totalLB} ≦ _{h totalUB,} by binary _search, h _TotalLB, updates the _{h TotalUB,} where the difference _{h totalUB} -h _totalLB becomes sufficiently small, reaching points set _S ParrG _{(h totalLB)} The four parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) corresponding to the element closest to the end position P ₁ among S _{Parr G} (h _totalUB ) are repeatedly used as initial values of the optimization method.

FIG. 7 is a diagram showing the operation of the coarse search process in the second embodiment. The coarse search unit 11 fixes the total length of the triple clothoid to the lower limit h _totalLB and _{obtains an} arrival point for a combination of a plurality of appropriately selected parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) From the points, the nearest inner circumferential arrival point is determined (S40). Here, the nearest inner goal, a goal that is included in the range shown in FIG. 10 (a), the closest goal to the end point P _1. The fan-shaped central angle θ _fan is a preset value, for example, π / 4. Subsequently, the rough search unit 11 fixes the total length of the triple clothoid to the upper limit h _totalUB, and _{obtains an} arrival point for a combination of a plurality of appropriately selected parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) It calculates | requires and calculates | requires the nearest outer periphery arrival point out of an arrival point (S41). Here, the nearest inner goal, a goal that is included in the range shown in FIG. 10 (b), the closest goal to the end point P _1.

FIG. 8 is a diagram showing processing for obtaining the nearest inner circumferential arrival point. The rough search unit 11 sets the linear distance between the start point P ₀ and the end point P ₁ as the lower limit h _{total LB} of the total length (h ₁ + h ₂ + h ₃ ) of the triple clothoid (S40), and appropriately selects a plurality of parameters satisfying this condition. Then, an arrival point corresponding to the parameter is calculated (S41). The rough search unit 11 determines whether or not there is an inner circumference arrival point among the obtained arrival points (S42), and if there is an inner circumference arrival point (YES in S42), the inner circumference arrival point among the closest to the end point P _1, it returns the corresponding parameters nearest inner goal (S43). When it is determined in the determination of the presence or absence of the inner circumferential arrival point that the inner circumferential arrival point is not present (NO in S42), it is determined that there is no solution of a triple clothoid connecting the beginning and the end (S44).

FIG. 9 is a diagram showing processing for obtaining the nearest outer peripheral arrival point. The rough search unit 11 sets a value obtained by multiplying the linear distance between the start point P ₀ and the end point P _{1 by} a constant C 2 (> 1) as the upper limit h _totalUB of the total length (h ₁ + h ₂ + h ₃ ) of the triple clothoid (S50) A plurality of parameters satisfying this condition are appropriately selected, and an arrival point corresponding to the parameters is calculated (S51). The rough search unit 11 determines whether or not there is an outer circumference arrival point among the obtained arrival points (S52), and if there is an outer circumference arrival point (YES in S52), among the outer circumference arrival points , closest to the end point _{P 1,} recently returns the corresponding parameters near the outer periphery goal (S53). When it is determined in the determination of the presence or absence of the outer circumferential arrival point that there is no outer circumferential arrival point (NO in S52), the upper limit h _totalUB is updated and the above-described process is repeated.

Specifically, a value obtained by multiplying the linear distance between the start point P ₀ and the end point P _{1 by} a constant C 3 (> 0) is added to the upper limit h _totalUB to obtain a new upper limit h _totalUB (S 54). If the new upper limit _htotalUB is not larger than the predetermined threshold (NO in S55), processing is performed to obtain an outer peripheral arrival point using the new upper limit _htotalUB (S51). If the new upper limit h _totalUB is larger than the predetermined threshold value (YES in S55), it is determined that there is no triple solution solution connecting the start end and the end (S56).

Referring back to FIG. 7, the operation performed by the rough search unit 11 will be described. The rough search unit 11 determines whether the distance between the upper limit h _totalUB and the lower limit h _{total LB} is smaller than a predetermined threshold (S32). If the difference between the upper limit h _totalUB and the lower limit h _totalLB is smaller than the threshold (YES in S32), the nearest outer peripheral arrival point obtained by fixing the total length of the triple clothoid to the upper limit h _totalUB and the lower limit h _totalLB of fixed and obtained nearest the periphery reaching point, the parameters given arrival point close to the end point P ₁ as the initial value (S39).

At the first stage, since the distance between the upper limit h _totalUB and the lower limit h _{total LB} is not smaller than the threshold (NO in S32), the process proceeds to step S33. The rough search unit 11 calculates an intermediate fixed length of the distance between the upper limit h _totalUB and the lower limit h _{total LB} (S33), and using this intermediate fixed length, finds the nearest outer peripheral arrival point and the nearest inner peripheral arrival point (S34).

Subsequently, the rough search unit 11 determines the end point of the nearest outer peripheral arrival point obtained by fixing to the intermediate fixed length, rather than the nearest outer peripheral arrival point obtained by fixing the total length of the triple clothoid to the upper limit h _totalUB. It is determined whether it is close to P ₁ (S35). This determination result, if those who asked the intermediate fixed length is close to the end point _{P 1} (YES at S35), the coarse search unit 11 sets the intermediate fixed length new upper limit _{h totalUB} (S36).

If the nearest outer peripheral arrival point determined with the upper limit h _totalUB is closer to the end point P ₁ (NO in S35), the rough search unit 11 determines the total length of the triple clothoid fixed to the lower limit h total _LB , and has recently determined it. than near the circumference arrival point, it determines whether towards the nearest inner circumference arrival point obtained by fixing the intermediate fixed length is close to the end point P ₁ (S37). The result of this determination, if those who asked the intermediate fixed length is close to the end point _{P 1} (YES at S37), the coarse search unit 11 sets the intermediate fixed length new lower limit _{h totalLB} (S38), the crude The search unit 11 returns to the process (S32) of determining whether the distance between the upper limit h _totalUB and the lower limit h _{total LB} is smaller than a predetermined threshold.

It is determined whether the nearest outer peripheral arrival point obtained by fixing to the intermediate fixed length is closer to the end point P ₁ than the nearest outer peripheral arrival point obtained by fixing the total length of the triple clothoid to the lower limit h _totalLB. in the case who determined lower limit _{h TotalLB} is close to the end point _{P 1} (at S37 NO), the overall length of the triple clothoid and nearest periphery arrival point obtained by fixing the upper limit _{h TotalUB,} the lower limit _{h TotalLB} of fixed and obtained nearest the periphery arrival point, the parameters given arrival point close to the end point P ₁ as the initial value (S39).

  In the following, an example is given in which trajectory planning is performed by specifically giving data on the attitude of the vehicle at the start and end points.

(First embodiment)
FIG. 11 is a diagram showing data of the start point and the end point for performing the trajectory planning of the first embodiment. The start position P ₀ (x ₀ , y ₀ ) is (0, 0) and the end position P ₁ (x ₁ , y ₁ ) is (25.61, 4.39). The direction θ ₀ of the vehicle at the beginning is 0 [rad], the turning curvature κ ₀ of the vehicle is 0 [1 / m], the direction θ ₁ of the vehicle at the end is 0.7854 [rad], the turning curvature _{1 1 of the} vehicle Is 0.0667 [1 / m].

As described above, the set of parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) when the _total length (h ₁ + h ₂ + h ₃ ) of the triple clothoid is fixed to h _total is Consider an arrival point set S _ParrG (h _total ) to be defined.

FIG. 12 is a diagram showing an arrival point set S _ParrG (h _total ) in the case of h _total = | P ₁ −P ₀ | in the case of FIG. 11. Here, Δθ _fan is π / 4. Further, since S _ParrG (h _total ) is an infinite set strictly, it is obtained by the approximate calculation as follows.

(1) A finite parameter set S _RS4Para (h _total ) sampled from the following set S _4Para (h _total ) of (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) is obtained. Note that θ _Range is a predetermined parameter, and here is pi / 2.

_{(2) S 4Para (h total} ) all the elements of _{(h 1, h 2, h} 3, θ d, 1) ∈S respect _{4Para (h total), P arr} (h 1, h 2, h 3, theta _{d, 1)} the calculated, of which the set of elements _{P arr} meeting the definition of the arrival point _S ParrG _{(h total)} of formula (70) and _S ParrG _{(h total).} As shown in FIG. 12, S _ParrG (h _total ) is a set of arrival points approximately in the direction of P ₁ -P ₀ .

From equation (15), P _d (θ _α , _{α α} , h, θ _d ) is continuous with respect to θ _α , _{α α} , h, θ _d , and from equations (42) to (47), θ _{d, 2} , θ _{d, 3} , _{α α, 2} , κ _{α, 3} , θ _{α, 2} , θ _{α, 3} are continuous with respect to (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) _Parr (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) indicated by (53) is continuous with respect to h ₁ , h ₂ , h ₃ , θ _{d, 1} .

Therefore, the near position P _arr + ΔP of the arrival point P _arr P _arr (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) with respect to certain four parameters h ₁ ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^* Considering _arr , the corresponding four parameters h ₁ , h ₂ , h ₃ , θ _{d, 1} are obtained by slightly changing h ₁ ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^* Be For example, specifically, as shown in FIG. 13, four parameters h ₁ ^* , h ₂ ^* , h given by the following equations slightly changed from h ₁ ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^* ₃ ^※ + _{Δh 3,} by ^{_{θ d, 1 ※ + Δθ d}} , 1, it is possible to change the arrival point at a position closer to the _{P arr} + _{ΔP arr.}

Conversely, if there is a solution (h ₁ ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^* ) in the optimization problem of equation (55), (h ₁ ^* , h ₂ ^* , H ₃ ^* , θ _{d, 1} ^* ) There is an arrival point set corresponding to the parameter set changed.

Since S _ParrG (h _total ) corresponds to this arrival point set, if S _{Parr G} (h _total ) is empty, it is indirectly inferred that the optimization problem of equation (55) has no solution.

Therefore, in the first embodiment, if S _ParrG (h _total ) is an empty set, it is determined that there is no appropriate initial value. If an element in S _{ParrG (h total),} parameters corresponding to the nearest neighbor goal is _{P arr} ∈S _ParrG endpoint _{P 1 (h total) (h} 1, h 2, h 3, θ d, 1) Use as initial value.

According to this method, since the optimization method can be repeatedly started from a position close to the end point P ₁ compared to when the initial value is randomly selected, it is less likely to be caught by the minimum value. When the iterative optimization method is suddenly applied, the calculation may not end without convergence due to the nonconvexity of the problem regardless of the presence or absence of a solution, but in this embodiment, the arrival point set S _ParrG ( in order to approximate calculation of the h _total) from a finite set of parameters _{S RS4para (h total),} it is estimated is always the presence or absence of a solution in a finite time.

Second Embodiment
Since the distance | P _arr −P ₀ | of the start point P ₀ to the arrival point P _arr is maximum when the triple clothoid segment is a straight line, the following equation always holds.
| _Parr (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) −P ₀ | ≦ h ₁ + h ₂ + h ₃ = h _total

FIG. 14 (a) is a diagram showing an arrival point set S _ParrG (h _total ) when h _total = 1 · | P ₁ −P ₀ |, which is the same as FIG. 12 described above. Viewed as those in in FIG. 14 _(a), are distributed in front of the end point _{P 1} Te always start _{P 0} pungency all elements of S _{ParrG (h total),} confirming the above equation. FIG. 14 _(b), the length of _{h total 1 · | P 1 -P} 0 | increased _{from, h total = 2 · | P} 1 -P 0 | and reach points set _S ParrG when the _{(h total)} FIG. In this case, the arrival point set S _ParrG (h _total ) is distributed behind the end point P ₁ as _viewed from the start point P ₀ .

FIGS. 14 (a) and 14 result shown in FIG. 14 (b), the formula (55) is the optimal solution _{^{(h 1 ※, h 2 ※}} , h 3 ※, θ d, 1 ※) of triplicate clothoid when having The total length h _total ^* is h _total ^* = h ₁ ^* + h ₂ ^* + h ₃ ^* . When h _total <h _total ^* and h _total are shortened, S _ParrG (h _total ) is distributed before P ₁ as viewed from P ₀ . Conversely, if h _total > h _total ^* and h _total are increased, S _ParrG (h _total ) will be distributed behind the end point P ₁ as viewed from the start point P ₀ .

S _{ParrG (h total)} lower limit _{h total} that are distributed in front of the end point _{P 1 h totalLB, S ParrG (} h total) is the upper limit _{h TotalUB} the _{h total} distributed at the back of the end point _{P 1.} In the second embodiment, the gap between the lower limit h _totalLB and the upper limit h _totalUB is narrowed by binary search to obtain a better initial value.

In order to explain the arrival points distributed before the end point P ₁ with respect to the start point P _0, a set of inner _{circumference} arrival points S _ParrGIn (h _total ) defined by the following equation and the corresponding four parameters (h ₁ , h A set S _RS4parrIn (h _total ) of ₂ , h ₃ , θ _{d, 1} ) is introduced.

Equation (200) theoretically becomes a set having infinite elements, but as shown in the first embodiment, it is assumed that it is an approximate finite set obtained from the sampled finite parameter set S _RS4parr (h _total ) .

Further, among S _ParrGIn (h _total ), the nearest inner circumference arrival point closest to the end point P ₁ is P _parGInNN (h _total ), and its four parameters are (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) _{Let InNN} (h _total ). It is as follows when it writes in a formula.

Similarly, in order to describe the arrival points distributed behind the end point P ₁ from the start point P ₀ , the arrival point set S _ParrGOut (h _total ) and the corresponding four parameters (h ₁ , h ₁ , defined by the following equation Introduce a set S _RS4paraOut (h _total ) of h ₂ , h ₃ , θ _{d, 1} ).

Similarly, of S _ParrGOut (h _total ), the nearest outer peripheral arrival point closest to the end point P ₁ is P _parGOutNN (h _total ), and its four parameters are (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) _{OutNN Let} (h _total ). It is as follows when it writes in a formula.

Here indicated nearest inner circumference arrival point, recently used near the outer circumference arrival point, it will narrow the search range by the processing of the binary search as shown in FIG. 7, from the plurality of sample parameters, recent endpoint P ₁ Find four parameters to reach the side.

FIG. 15 is a diagram showing the distribution of arrival points when h _totalLB and h _totalUB are updated according to the above-described algorithm for the case shown in FIG. FIG. 16 is a diagram showing trajectories obtained by performing the iterative optimization method (major dual interior point method) using the initial values obtained by the above-described processing. As shown in FIG. 16, it has been confirmed that the optimization problem shown in equation (55) can be solved appropriately by starting from an appropriate initial value.
Moreover, in three cases other than FIG. 11, the example which performed track planning using the initial value acquired by the method of this Embodiment is shown.

  FIG. 17 shows trajectories calculated in each of the above cases. As shown in FIG. 17, optimization was repeatedly performed properly and trajectories were obtained.

  The present invention is useful as an apparatus or the like for planning a track of a vehicle.

DESCRIPTION OF SYMBOLS 1 Trajectory planning apparatus 10 Input part 11 Coarse search part 12 Optimization part 13 Output part 14 Storage part

Claims (11)

A program for planning an orbit consisting of three clothoid curves, and a computer
An input step for receiving data of an attitude specified by the position, direction, and curvature of the vehicle at the start and a position, direction, and attitude of the vehicle at the end;
The reaching point function which is a function for obtaining the attitude specified by the position, the direction, and the curvature of the vehicle at the reaching point from the attitude of the starting point, and which includes at least the length of the clothoid curve as a parameter is read from the storage unit A plurality of combinations of the respective lengths of the clothoid curve are used as parameters of the sample, and the parameters of the sample are applied to the arrival point function to obtain an arrival point; A rough search step for obtaining a parameter for obtaining the nearest arrival point closest to the terminal in
An optimization step of varying the parameter of the arrival point function using the parameter obtained in the rough search step as an initial value, and finding an optimum parameter such that the attitude of the arrival point matches the attitude of the end;
A program that runs   In the coarse search step, when there is no parameter of the sample giving the nearest arrival point at which the distance to the end is equal to or less than a predetermined threshold, it is determined that there is no solution satisfying the input condition. The program according to claim 1. A program for planning an orbit consisting of three clothoid curves, and a computer
An input step for receiving data of an attitude specified by the position, direction, and curvature of the vehicle at the start and a position, direction, and attitude of the vehicle at the end;
The reaching point function which is a function for obtaining the attitude specified by the position, the direction, and the curvature of the vehicle at the reaching point from the attitude of the starting point, and which includes at least the length of the clothoid curve as a parameter is read from the storage unit Taking the total length of the clothoid curve as a first fixed length and using a plurality of combinations of the lengths of the clothoid curve as sample parameters, applying the sample parameters to the arrival point function and in front of the termination A first parameter calculating step of obtaining a certain inner circumferential arrival point, and obtaining, as a first parameter, a parameter obtained by obtaining a nearest inner circumferential arrival point closest to the terminal among the inner circumferential arrival points;
Taking the total length of the three clothoid curves as a second fixed length longer than the first fixed length, a plurality of combinations of the respective lengths of the clothoid curves as sample parameters, and the sample parameters as the arrival point function A second parameter calculating step of determining as a second parameter a parameter that is obtained by applying the above to the outer periphery arrival point that is deeper than the end, and obtaining the nearest outer periphery arrival point closest to the end among the outer periphery arrival points; ,
A plurality of combinations of respective lengths of the clothoid curve are used as sample parameters, with the total length of the three clothoid curves as an intermediate fixed length intermediate between the first fixed length and the second fixed length. The parameters are applied to the arrival point function to obtain the nearest inner circumference arrival point and the nearest outer circumference arrival point, and the parameter that obtained the nearest inner circumference arrival point is the third parameter, and the nearest outer circumference arrival point obtained Third and fourth parameter calculating steps of obtaining a parameter as a fourth parameter;
Comparing the nearest outer peripheral arrival point given by the second parameter with the nearest outer peripheral arrival point given by the fourth parameter, when the nearest outer peripheral arrival point given by the fourth parameter is closer to the end If the intermediate fixed length is a new second fixed length, and the nearest outer peripheral arrival point given by the second parameter is closer to the end, the nearest inner circumference given by the first parameter If the arrival point and the nearest inner circumference arrival point given by the third parameter are compared, and if the nearest inner circumference arrival point given by the third parameter is closer to the end, the intermediate fixed length is newly added. A fixed length update step to make the first fixed length
While updating the first fixed length or the second fixed length in the fixed length update step, the nearest outer peripheral arrival point and the nearest inner peripheral arrival point are repeatedly determined, and the first fixed length and the second fixed length are determined. When the difference between the fixed lengths of 2 becomes equal to or less than a predetermined threshold value, the parameter is determined which gives the nearest arrival point closer to the end among the nearest outer circumference arrival point and the nearest inner circumference arrival point Step and
An optimization step of varying the parameter of the arrival point function with the parameter obtained in the parameter determination step as an initial value, and finding an optimum parameter such that the attitude of the arrival point matches the attitude of the end;
A program that runs   In the fixed length update step, the closest inner circumference arrival point given by the first parameter is compared with the nearest inner circumference arrival point given by the third parameter, and the nearest inner circumference given by the first parameter 4. The method according to claim 3, wherein, when the arrival point is closer to the end, a parameter is determined which is given the nearest arrival point closer to the end among the nearest outer circumference arrival point and the nearest inner circumference arrival point. program.   5. The program according to claim 4, wherein if the inner circumference arrival point is not found by the calculation in the first parameter calculation step, it is determined that a clothoid curve connecting the start end and the end does not exist.   If the outer peripheral arrival point is not found by the calculation in the second parameter calculation step, the second fixed length is detected until the outer peripheral arrival point is found or the second fixed length becomes equal to or more than a predetermined threshold. Is performed to obtain the outer peripheral arrival point, and when the second fixed length becomes equal to or larger than a predetermined threshold value, it is determined that there is no clothoid curve connecting the start end and the end. Described program.   The program according to any one of claims 1 to 6, wherein the arrival point function includes a turning curvature of the vehicle as a parameter, and has a maximum value of the turning curvature as a constraint. A method for planning a trajectory consisting of three clothoid curves by a trajectory planning device, comprising:
The track planning device receives an input of data of a position, a direction, and a posture specified by a curvature at the start end, and a data of a position, a direction, and a posture specified by a curvature at the end;
The trajectory planning apparatus is a function for obtaining the position, orientation, and attitude specified by the curvature of the vehicle at the arrival point from the attitude of the start end, and stores the arrival point function including at least the length of the clothoid curve as a parameter And the trajectory planning device takes a plurality of combinations of respective lengths of the clothoid curve as parameters of the sample, and takes the parameters of the sample as the arrival point function, with the total length of the three clothoid curves as a fixed length. A rough search step of applying to obtain an arrival point, and obtaining a parameter for obtaining a nearest arrival point closest to the terminal among the arrival points;
The trajectory planning apparatus changes the parameter of the arrival point function using the parameter obtained in the rough search step as an initial value, and finds an optimum parameter such that the attitude of the arrival point matches the attitude of the end Step, and
Trajectory planning method comprising A method for planning a trajectory consisting of three clothoid curves by a trajectory planning device, comprising:
The track planning device receives an input of data of a position, a direction, and a posture specified by a curvature at the start end, and a data of a position, a direction, and a posture specified by a curvature at the end;
The trajectory planning apparatus is a function for obtaining the position, orientation, and attitude specified by the curvature of the vehicle at the arrival point from the attitude of the start end, and storing the arrival point function including at least the length of the clothoid curve as a parameter The trajectory planning apparatus uses the total length of the three clothoid curves as a first fixed length as a sample parameter with a plurality of combinations of the respective lengths of the clothoid curve as parameters of the sample and the arrival point of the sample A first function is applied to a function to obtain an inner circumference arrival point before the end, and among the inner circumference arrival points, a parameter that obtains the nearest inner circumference arrival point closest to the end is obtained as a first parameter Parameter calculation step,
The trajectory planning apparatus takes a total length of three of the clothoid curves as a second fixed length longer than the first fixed length, and takes a plurality of combinations of the lengths of the clothoid curves as sample parameters. A parameter is applied to the arrival point function to obtain an outer periphery arrival point located behind the end, and among the outer periphery arrival points, a parameter that obtains the nearest outer periphery arrival point closest to the end is determined as a second parameter A second parameter calculating step;
The trajectory planning apparatus samples a plurality of combinations of the respective lengths of the clothoid curve, with the total length of the three clothoid curves as an intermediate fixed length intermediate between the first fixed length and the second fixed length. The parameter of the sample is applied to the arrival point function to obtain the nearest inner circumference arrival point and the nearest outer circumference arrival point, and the parameter for which the nearest inner circumference arrival point is obtained is the third parameter, the nearest neighbor Third and fourth parameter calculation steps of determining the parameter from which the outer peripheral arrival point is obtained as the fourth parameter;
The trajectory planning apparatus compares the nearest outer peripheral arrival point given by the second parameter with the nearest outer peripheral arrival point given by the fourth parameter, and the nearest outer peripheral arrival point given by the fourth parameter is If it is close to the end, the intermediate fixed length is taken as a new second fixed length, and if the nearest outer peripheral arrival point given by the second parameter is close to the end, the first parameter is used. If the nearest inner circumference arrival point given is compared with the nearest inner circumference arrival point given by the third parameter, if the nearest inner circumference arrival point given by the third parameter is closer to the end, A fixed length updating step for setting the intermediate fixed length as a new first fixed length;
The trajectory planning apparatus repeatedly obtains the nearest outer peripheral arrival point and the nearest inner peripheral arrival point while updating the first fixed length or the second fixed length in the fixed length update step, and When the difference between the second fixed length and the second fixed length becomes equal to or less than a predetermined threshold value, the nearest arrival point closer to the terminal end is given among the nearest outer circumference arrival point and the nearest inner circumference arrival point Parameter determination step for determining the different parameters;
The trajectory planning apparatus changes the parameter of the arrival point function using the parameter obtained in the parameter determination step as an initial value, and the attitude specified by the position, direction, and curvature of the vehicle at the arrival point is the end Optimization steps to find the optimal parameters that match the pose of
Trajectory planning method comprising A trajectory planning device for planning a trajectory consisting of three clothoid curves,
An input unit that receives input of data of an attitude specified by the position, direction, and curvature of the vehicle at the start end, and position, direction, and attitude of the vehicle at the end;
A storage unit storing an arrival point function including at least a length of a clothoid curve as a parameter, which is a function for obtaining an attitude specified by the position, orientation, and curvature of a vehicle at an arrival point from the attitude of the start point;
Taking the total length of the three clothoid curves as a fixed length and taking a plurality of combinations of the lengths of the clothoid curves as sample parameters, applying the parameters of the sample to the reaching point function read from the storage unit A rough search unit for obtaining a point and for obtaining a parameter for obtaining a nearest arrival point closest to the terminal among the arrival points;
An optimization unit that changes the parameter of the arrival point function using the parameter obtained by the rough search unit as an initial value, and obtains an optimum parameter such that the attitude of the arrival point matches the attitude of the end;
Trajectory planning device comprising A trajectory planning device for planning a trajectory consisting of three clothoid curves,
An input unit that receives input of data of an attitude specified by the position, direction, and curvature of the vehicle at the start end, and position, direction, and attitude of the vehicle at the end;
A storage unit storing an arrival point function including at least a length of a clothoid curve as a parameter, which is a function for obtaining an attitude specified by the position, orientation, and curvature of the vehicle at the arrival point from the beginning attitude;
A rough search is performed to obtain an arrival point by applying plural combinations of sample parameters to the arrival point function read from the storage unit, and finding a parameter that has obtained a nearest arrival point closest to the end among the arrival points Department,
An optimization unit that changes the parameter of the arrival point function with the parameter obtained in the parameter determination step as an initial value, and obtains an optimum parameter such that the attitude of the arrival point matches the attitude of the end;
Equipped with
The coarse search unit
Taking the total length of the three clothoid curves as the first fixed length and taking a plurality of combinations of the respective lengths of the clothoid curves as sample parameters, applying the sample parameters to the arrival point function A first parameter calculating step of obtaining an inner circumference arrival point in front and obtaining a parameter which is a nearest inner circumference arrival point closest to the end among the inner circumference arrival points as a first parameter;
Taking the total length of the three clothoid curves as a second fixed length longer than the first fixed length, a plurality of combinations of the respective lengths of the clothoid curves as sample parameters, and the sample parameters as the arrival point function A second parameter calculating step of determining as a second parameter a parameter that is obtained by applying the above to the outer periphery arrival point that is deeper than the end, and obtaining the nearest outer periphery arrival point closest to the end among the outer periphery arrival points; ,
A plurality of combinations of respective lengths of the clothoid curve are used as sample parameters, with the total length of the three clothoid curves as an intermediate fixed length intermediate between the first fixed length and the second fixed length. The parameters are applied to the arrival point function to obtain the nearest inner circumference arrival point and the nearest outer circumference arrival point, and the parameter that obtained the nearest inner circumference arrival point is the third parameter, and the nearest outer circumference arrival point obtained Third and fourth parameter calculating steps of obtaining a parameter as a fourth parameter;
Comparing the nearest outer peripheral arrival point given by the second parameter with the nearest outer peripheral arrival point given by the fourth parameter, when the nearest outer peripheral arrival point given by the fourth parameter is closer to the end If the intermediate fixed length is a new second fixed length, and the nearest outer peripheral arrival point given by the second parameter is closer to the end, the nearest inner circumference given by the first parameter If the arrival point and the nearest inner circumference arrival point given by the third parameter are compared, and if the nearest inner circumference arrival point given by the third parameter is closer to the end, the intermediate fixed length is newly added. A fixed length update step to make the first fixed length
While updating the first fixed length or the second fixed length in the fixed length update step, the nearest outer peripheral arrival point and the nearest inner peripheral arrival point are repeatedly determined, and the first fixed length and the second fixed length are determined. When the difference between the fixed lengths of 2 becomes equal to or less than a predetermined threshold value, the parameter is determined which gives the nearest arrival point closer to the end among the nearest outer circumference arrival point and the nearest inner circumference arrival point Step and
Trajectory planning device to carry out.

Images