JP6941039B2 - Orbit planning equipment, orbit planning methods, and programs - Google Patents

Description

The present invention relates to an automobile that can be automatically driven without a human driving operation, and a track plan for an autonomous driving vehicle.

One of the track plans for autonomous vehicles is to plan the curvature κ with respect to the route length s. This is because there is a strong functional relationship between the curvature κ and the front wheel steering angle δ. FIG. 1 is a diagram showing a bicycle model ignoring the slip of the front and rear wheels. In this figure, l is the wheel length of the vehicle.

The bicycle model that ignores 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, the posture (posture), which is the state of the vehicle, is defined as a state consisting of four variables: x position, y position, vehicle orientation θ, and curvature κ. That is, in the present specification, the posture is a concept including a position. As an operation mode of the self-driving vehicle running on the road, there may be a mode of traveling along a milestone determined in advance on the road. Assuming a curved road, this milestone defines not only the position but also the appropriate vehicle orientation and curvature corresponding to the curved road.

FIG. 2 is a diagram showing an example of a steering plan in which a route length for putting a vehicle in a certain initial state on one milestone is used as an argument. When the vehicle arrives at the milestone, it is necessary to match not only the position but also the curvature determined by the vehicle orientation and the front wheel orientation to the curved road. This steering plan should be considered as a track plan with the attitude (posture) consisting of the initial position, orientation, and front wheel orientation of the vehicle as the start (Start) and the attitude (posture) at the milestone on the road as the end (End). Can be done. The orbital design covered herein is intended to plan the curvature κ with respect to this type of path length s. In this type of track 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 explanation, the attitude (posture) at the start of the track plan is assumed to be vehicle position (x, y) = (x ₀ , y ₀ ), vehicle orientation θ = θ ₀ , and curvature κ = κ ₀ . Similarly, the posture at the end is the vehicle position (x, y) = (x ₁ , y ₁ ), the vehicle orientation θ = θ ₁ , and the 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 start 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". This kind of trajectory planning from the posture at the beginning (posture) to the posture at the end (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.

Japanese Unexamined Patent Publication No. 2011-145977

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. Horei Ran, Hirofumi Tamai, and Hiroshi Makino. "Free point sequence interpolation by triple clothoid." Journal of Precision Engineering 76.10 (2010): 1194-1199.

All of the above documents solve the trajectory plan from the initial posture (posture) to the posture at the end (posture) by the three clothoids as a partial problem, but 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 it gives a strong restriction that the absolute value of the rate of change due to the path length of curvature is constant across all three clothoids. Ran et al.'S study imposes a strong constraint that the ratio of the lengths of the three clothoids is fixed in advance.

In view of the above background, the present invention is a method of planning a trajectory from a posture at the beginning (posture) to a posture at the end (posture) when the clothoid length is variable without the above-mentioned restrictions.

The program of the present invention is a program for planning a trajectory consisting of three clothoid curves, and a computer is used to provide attitude data specified by the position, orientation, and curvature of the vehicle at the start and the position of the vehicle at the end. , An input step that accepts input of attitude data specified by orientation and curvature, and a function for obtaining the position, orientation, and curvature of the vehicle at the destination from the starting attitude, at least clothoid. The arrival point function including the length of the curve as a parameter is read from the storage unit, the total length of the three clothoid curves is set as a fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters. The rough search step of applying the parameters to the arrival point function to obtain the arrival point, and obtaining the parameter obtained by obtaining the nearest arrival point closest to the end among the arrival points, and the parameter obtained in the coarse search step. As an initial value, the parameter of the arrival point function is changed, and an optimization step of finding an optimum parameter such that the attitude of the arrival point matches the attitude of the end point is executed.

The program of the present invention satisfies the input condition when there is no sample parameter that gives the nearest neighbor arrival point at which the distance to the end is equal to or less than a predetermined threshold in the rough search step. May be determined that does not exist.

The program of the present invention is a program for planning a trajectory consisting of three clothoid curves, and a computer is used to provide attitude data specified by the position, orientation, and curvature of the vehicle at the start and the position of the vehicle at the end. , An input step that accepts input of posture data specified by orientation and curvature, and a function for obtaining the posture specified by the position, orientation, and curvature of the vehicle at the destination from the starting posture, and at least clothoid. The arrival point function including the length of the curve is read from the storage unit, the total length of the three clothoid curves is set as the first fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters. The parameter of the sample is applied to the arrival point function to obtain the inner circumference arrival point in front of the end, and the nearest inner circumference arrival point closest to the end among the inner circumference arrival points is obtained. As the first parameter calculation step, and the total length of the three clothoid curves is set as the second fixed length longer than the first fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are sampled. By applying the parameter of the sample to the arrival point function to obtain the outer peripheral arrival point deeper than the end, the nearest outer peripheral arrival point closest to the end among the outer outer arrival points was obtained. The length of each of the clothoid curves is set as the second parameter calculation step in which the parameter is obtained as the second parameter and the total length of the three clothoid curves as an intermediate fixed length between the first fixed length and the second fixed length. The plurality of combinations of the above are used as sample parameters, and the parameters of the sample 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 obtained from the nearest inner circumference arrival point. Is given as the third parameter, the third and fourth parameter calculation steps for obtaining the parameter obtained from the nearest outer peripheral reach point as the fourth parameter, the nearest outer peripheral reach point given by the second parameter, and the fourth parameter. When the nearest outer peripheral reach point given by the fourth parameter is closer to the end, the intermediate fixed length is set as a new second fixed length, and the second fixed length is used. When the nearest outer peripheral arrival point given by the parameter is closer 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. , Given in the third parameter When the nearest neighbor inner circumference arrival point is closer to the end, the fixed length update step in which the intermediate fixed length is set as 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 neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point are repeatedly obtained, and the difference between the first fixed length and the second fixed length becomes equal to or less than a predetermined threshold value. At that time, of the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point, the parameter determination step for obtaining the parameter giving the nearest neighbor arrival point closer to the end and the parameter obtained in the parameter determination step are obtained. As an initial value, the parameter of the arrival point function is changed, and an optimization step of finding an optimum parameter such that the attitude of the arrival point matches the attitude of the end point is executed.

In the fixed length update step, the program of the present invention compares the nearest neighbor inner circumference arrival point given by the first parameter with the nearest neighbor inner circumference arrival point given by the third parameter, and uses the first parameter. When the given nearest neighbor inner circumference arrival point is closer to the end, the parameter giving the nearest neighbor arrival point of the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point, which is closer to the end, is obtained. You may.

The program of the present invention may determine that the clothoid curve connecting the start end and the end does not exist when the inner circumference arrival point is not found by the calculation in the first parameter calculation step.

In the program of the present invention, when the outer peripheral arrival point is not found by the calculation in the second parameter calculation step, until the outer peripheral arrival point is found or the second fixed length becomes equal to or more than a predetermined threshold value. When the second fixed length is lengthened to obtain the outer peripheral arrival point and the second fixed length exceeds a predetermined threshold value, the clothoid curve connecting the start end and the end end does not exist. You may judge.

In the program of the present invention, the reaching 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 track planning method of the present invention is a method for planning a track composed of three clothoid curves by a track planning device, and the track planning device is a posture specified by the position, orientation, and curvature of the vehicle at the start end. And the input step that accepts the input of the data of the posture specified by the position, orientation, and curvature of the vehicle at the end, and the track planning device is based on the position, orientation, and curvature of the vehicle from the posture of the start to the arrival point. A function for obtaining a specified posture, which is a function for obtaining at least the length of the clothoid curve as a parameter, is read out from the storage unit, and the trajectory planning device uses the total lengths of the three clothoid curves as fixed lengths. A plurality of combinations of the respective lengths of the clothoid curve are used as sample parameters, and the sample parameters are applied to the arrival point function to obtain the arrival point, and the nearest nearest arrival point among the arrival points is reached. The rough search step for obtaining the parameter obtained by the point and the orbit planning device change the parameter of the arrival point function with the parameter obtained in the rough search step as an initial value, and the attitude of the arrival point is the end. It is provided with an optimization step for finding an optimum parameter that matches the clothi cloth.

The track planning method of the present invention is a method for planning a track composed of three clothoid curves by a track planning device, and the track planning device is designated by the position, orientation, and curvature of the vehicle at the start end. The input step that accepts the input of the attitude data and the attitude data specified by the position, orientation, and curve of the vehicle at the end, and the track planning device are based on the position, orientation, and curve of the vehicle from the attitude at the start to the arrival point. A function for obtaining a designated posture, which is a function for obtaining at least the length of the clothoid curve as a parameter, is read out from the storage unit, and the trajectory planning device sets the total length of the three clothoid curves as the first. As a fixed length, a plurality of combinations of the respective lengths of the clothoid curve are used as sample parameters, and the sample parameters are applied to the arrival point function to obtain an inner circumference arrival point in front of the end point. The first parameter calculation step for obtaining the parameter obtained from the nearest inner circumference arrival point closest to the end among the circumference arrival points as the first parameter, and the trajectory planning device obtain 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 curve are used as sample parameters, and the sample parameters are applied to the reaching point function to be deeper than the end. The second parameter calculation step of obtaining a certain outer peripheral arrival point and obtaining the parameter obtained by obtaining the nearest nearest outer peripheral arrival point closest to the end among the outer peripheral arrival points as the second parameter, and the orbit planning device have three lines. The total length of the clothoid curve is set as an intermediate fixed length between the first fixed length and the second fixed length, and a plurality of combinations of the respective lengths of the clothoid curve are used as sample parameters, and the sample parameters are used as the sample parameters. The nearest inner circumference arrival point and the nearest outer circumference arrival point are obtained by applying to the arrival point function, the parameter obtained from the nearest inner circumference arrival point is the third parameter, and the parameter obtained from the nearest outer circumference arrival point is the third parameter. The third and fourth parameter calculation steps obtained as the four parameters are compared with the nearest outer peripheral arrival point given by the second parameter and the nearest outer peripheral arrival point given by the fourth parameter. When the nearest outer peripheral reach point given by the fourth parameter is closer to the end, the intermediate fixed length is set as a new second fixed length, and the nearest outer peripheral reach point given by the second parameter is the one. When is close to the end, the first parameter When the nearest neighbor inner circumference arrival point given by the function is compared with the nearest neighbor inner circumference arrival point given by the third parameter, and the nearest neighbor inner circumference arrival point given by the third parameter is closer to the end. Is a fixed length update step in which the intermediate fixed length is set 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 updating step. While doing so, the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point are repeatedly obtained, and when the difference between the first fixed length and the second fixed length becomes equal to or less than a predetermined threshold value, the nearest neighbor outer circumference arrival point Of the nearest neighbor inner circumference arrival points, the parameter determination step for obtaining the parameter giving the nearest neighbor arrival point closer to the end, and the trajectory planning device use the parameter obtained in the parameter determination step as an initial value. , The parameter of the reaching point function is varied, and an optimization step for finding an optimum parameter such that the posture of the reaching point matches the posture of the terminal is provided. Hereinafter, this rough search method is referred to as a "two-minute search method".

The track planning device of the present invention is a track planning device for planning a track composed of three clothoid curves, and includes attitude data specified by the position, orientation, and curvature of the vehicle at the start end and the vehicle at the end. An input unit that accepts input of attitude data specified by the position, orientation, and curve, and a function for obtaining the attitude specified by the position, orientation, and curvature of the vehicle at the arrival point from the starting attitude, at least. A storage unit that stores an arrival point function that includes the length of the clothoid curve as a parameter, and a plurality of combinations of the respective lengths of the clothoid curves as fixed lengths with the total lengths of the three clothoid curves as sample parameters. A rough search unit that applies the sample parameters to the arrival point function read from the storage unit to obtain the arrival point, and obtains the parameter obtained from the nearest arrival point closest to the end among the arrival points, and the above-mentioned rough search unit. It is provided with an optimization unit that uses the parameter obtained by the rough search unit as an initial value, fluctuates the parameter of the arrival point function, and obtains the optimum parameter so that the attitude of the arrival point matches the attitude of the end point.

The track planning device of another aspect of the present invention is a track planning device for planning a track composed of three clothoid curves, and includes data on a posture specified by the position, orientation, and curvature of the vehicle at the starting end. It is a function to obtain the posture specified by the position, orientation, and curvature of the vehicle at the reaching point from the input unit that accepts the input of the attitude data specified by the position, orientation, and curvature of the vehicle at the end. Then, a storage unit that stores at least the arrival point function including the length of the clothoid curve as a parameter and a plurality of combinations are applied to the arrival point function that reads the sample parameters from the storage unit to obtain the arrival point. The parameters of the arrival point function are varied by using the coarse search unit for obtaining the parameter obtained from the nearest arrival point closest to the end among the arrival points and the parameter obtained in the parameter determination step as initial values. The rough search unit is provided with an optimization unit that obtains an optimum parameter so that the attitude of the arrival point matches the attitude of the end point, and the rough search unit executes the above-mentioned two-minute search method.

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

It is a figure which shows the bicycle model which ignored the slip of the front and rear wheels. It is a figure which shows the example of the steering plan which takes the path length which puts a vehicle in a certain initial state on one milestone as an argument. It is a conceptual diagram which shows the orbital model of a triple clothoid. It is a figure which shows the structure of the track planning apparatus of 1st Embodiment. It is a flowchart which shows the operation of the trajectory planning apparatus of 1st Embodiment. It is a flowchart which shows the process of initial value determination in 1st Embodiment. It is a figure which shows the operation of the rough search process in 2nd Embodiment. It is a figure which shows the process which obtains the fixed total length lower limit h _totalLB. It is a figure which shows the process which obtains the fixed total length upper limit h _totalUB. (A) It is a figure which shows the range of the inner circumference arrival point. (B) It is a figure which shows the range of the outer peripheral arrival point. It is a figure which shows the data of the start end and the end which perform the trajectory plan of 1st Example. It is a figure which shows the reaching point set S _Parr (h _total ) when h _total = | P ₁ −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, It is a figure which changed the arrival point to the position close to _Parr + ΔP _{arr according to 1.} (A) It is a figure which shows the _destination set S ParrG (h _{total ) when h total} = 1 · | P ₁ −P _{0 |.} (B) In the figure showing the arrival point set S _ParrG (h _{total ) when the length of h total} is increased from 1 · | P ₁ −P ₀ | and h _total = 2 · | P ₁ −P _{0 |.} be. It is a figure which shows the distribution of the arrival point at the time of performing the update process of h _totalLB and h _totalUB. It is a figure which shows the trajectory obtained by performing the iterative optimization method (main dual interior point method). It is a figure which shows the trajectory calculated in each different case.

Hereinafter, the track planning device and the track planning method according to the 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 for explaining the trajectory planning device of the present embodiment. First, as a preparatory step for modeling the trajectory by the triple clothoid, one segment starting from the xy coordinate origin The orbit of clothoid is described.

(1 segment clothoid orbit)
The trajectory of this type of clothoid is uniquely determined by the position of the start and end and the orientation and curvature of the vehicle at the start and end. For example, if the orientation and curvature at the beginning are (θ _α , κ _α ) and the orientation and curvature at the end are (θ _β , κ _β ), the total path length h of one segment of the clothoid is expressed by the following equation.

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

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

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

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

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

The track of θ for the vehicle, the rate of change η of the curve with respect to the path length s, and sharpness are expressed by the following equations.

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

The rate of change η and sharpness of the curve are constant in one clothoid segment as is clear from the definition. Once the trajectory of the vehicle orientation θ is determined, the trajectory of P (S) = [x (S), y (S)] ^T representing the amount of movement from the coordinate origin is determined by the following equation from equations (1) and (2). NS.

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

This P _d = (θ _α , κ _α , h, θ _d ) is used as a function in the modeling of the orbit by the triple clothoid. The calculations of equations (12) and (15) will be described for the numerical solution of the trajectory plan. _However, C R _(···), since the numerical calculation of S R (· · ·) other than the elements is easy omitted. C _{R (···),} the calculation of S R (· · ·) are divided into three cases.

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

[Case 2 (θ _d -hκ _{α =} 0 cutlet κ α ≠ _0)]
This case corresponds to a circular motion. This case is also included in clothoid by definition.

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

However,

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

Subsequently, the trajectory plan from the posture at the start (posture) to the posture at the end (posture) by the triple crossoid when the crossoid length is variable is formulated as a numerical optimization problem.

(Orbit of triple clothoid)
The formulation for the numerical optimization problem of the orbit of the triple clothoid takes the following approach.
-The posture of the starting end of the first clothoid is (x ₀ , y ₀ , θ ₀ , κ ₀ ).
-Connect the clothoids of 3 segments so that the curvature θ and the direction κ are continuous.
_-Let the posture of the end of the third clothoid be (x arr, y _arr , θ ₁ , κ ₁ ).
_-Numerically optimize the parameters of the first to third clothoids under the constraint of _{| κ | ≤ κ limit} until x arr = x ₁ and y _arr = y _1.

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

As for the trajectory of the traveling vehicle, the steering angle of the front wheels needs to change continuously with respect to the path. Therefore, the direction θ of the vehicle and the curvature κ of the vehicle are continuous at the end points between the segments. First, considering the vehicle orientation θ, it is necessary to satisfy the following equation from the continuity at both end points of each segment.

The following equations can be obtained by summarizing the equations (25) to (40).

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

If h ₁ (h ₂ + h ₃ ) ≠ 0, the right-hand side of equations (42) to (47) is not indefinite. In the present embodiment, h ₁ > 0, h ₂ > 0, h ₃ > 0 is assumed, and this condition is satisfied. From equation (44), | κ _{α, 2} | <κ _limit is converted into the following inequality constraints for _{h 1} , θ _{d, 1.}

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

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

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

The _{problem of searching for P arr} (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) = P ₁ (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) is formulated as a numerical optimization problem. NS. Select the objective function _obj ( _Parr ) as the square value of the Euclidean 2 norm of _Parr −P _1. At this time, the numerical optimization problem can be described as follows.

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

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

Solving the above optimization problem, the result of optimization,

If can be achieved, the orbit that becomes the solution is obtained. That is, a trajectory from the desired starting position (posture) to the ending position (posture) is obtained.

(First Embodiment)
Iterative optimization methods such as the principal dual interior point method (PDIPM) and the sequential quadratic programming (SQP) based on the pseudo-Newton method can be applied to the optimization problem such as the equation (55). However, since the optimization problem of equation (55) is non-linear and non-convex, there arises a problem that a solution cannot be obtained if an inappropriate initial value is selected. In this embodiment, in the optimization problem of the equation (55), an appropriate initial value to be used in the iterative optimization method is obtained.

FIG. 4 is a diagram showing a configuration of the track planning device 1 according to the first embodiment. As shown in FIG. 4, the trajectory planning device 1 of 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 accepting input of data on the start end and the end of the vehicle as a condition of track planning. Specifically, it is the posture (position, orientation, curvature) of the start end of the vehicle and the posture (position, orientation, curvature) of the end end.

The storage unit 14 stores a destination function that calculates the destination from the start. The reaching point function is a function shown by the above equation (53), and is a function having four parameters _{(h 1} , h ₂ , h ₃ , θ _{d, 1).}

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

If this set is empty, appropriate initial value, it is determined that no, not empty, corresponding to the closest element to the end position P ₁ in the goal set S _{ParrG (h total) (nearest} goal) The four parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) are used as the initial values of the iterative optimization method. Note that Δθ _fan in the equation defines the angle range in which the arrival point _Parr should exist, and is, for example, π / 4.

The optimization unit 12 uses the parameter obtained by the rough search unit 11 as an initial value, and performs the optimization processing of the parameter satisfying the above-mentioned equation (55), so that the posture of the arrival point becomes the posture of the end point. It has a function to obtain such parameters. This parameter plans the track that the vehicle should take. Subsequently, the optimization unit 12 has a function of also obtaining the turning curvature κ at each position from the _{obtained parameters (h 1} , h ₂ , h ₃ , θ _{d, 1).}

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

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

FIG. 6 is a flowchart showing a process of determining an initial value in the first embodiment. The coarse search unit 11 sets an appropriate parameter in the arrival point function, and roughly searches for a parameter capable of obtaining the arrival point closest to the end (nearest neighbor arrival point). Specifically, the coarse search unit 11 fixes the entire length (h ₁ + h ₂ + h ₃ ) of the triple clothoid to a linear distance h _{total from the start point P 0} which is the start position to the end point P ₁ which is the end position ( S20), for example, 1000 combinations of four parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) are selected, and the arrival point when each parameter is applied to the arrival point function is obtained (S21).

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 a nearby arrival point is not found (NO in S22), it is determined that there is no orbit from the start end to the end point (S24).

The explanation will be continued by returning to FIG. When the trajectory planning device 1 determines the initial value, it _{optimizes the parameters (h 1} , h ₂ , h ₃ , θ _{d, 1} ) using the determined initial value, and optimizes the trajectory from the start to the end. Calculate the parameters (S13). The trajectory planning device 1 also obtains the turning curvature κ at each position based on the obtained optimum parameters. Subsequently, the track planning device 1 _{outputs the parameters (h 1} , h ₂ , h ₃ , θ _{d, 1} ) and the turning curvature κ to the automatic operation control unit (S14).

The configuration and operation of the track planning device 1 have been described above, but the hardware example of the track planning device 1 described above is a computer equipped with a CPU, RAM, ROM, hard disk, display, keyboard, mouse, communication interface, and the like. be. The orbit planning device 1 described above is realized by storing a program having a module that realizes each of the above functions in a RAM or ROM and executing the program by a CPU. Such programs are also included in the scope of the present invention.

(Second Embodiment)
Next, the track planning device of the second embodiment will be described. The basic configuration of the track planning device of the second embodiment is the same as the configuration of the track planning device 1 of the first embodiment (FIG. 4), but the search process in the rough search unit 11 is different.

The outline of the rough search process 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 points occurs before the end point P _{1 when viewed from the start point P 0.} For example, if the linear distance from the start point P ₀ to the end point P ₁ of the triple clothoid is taken, the distribution of the arrival points will occur before the end point P _{1 when viewed from the start point P 0. Let} this fixed length be h totalLB.

_{Further, the total length (h 1} + h ₂ + h ₃ ) of the triple clothoid is fixed so that the distribution of the arrival points occurs at the back of the end point P _{1 when viewed from the start point P 0.} For example, if the linear distance from the start point P ₀ to the end point P _{1 is} doubled, the distribution of the arrival points usually occurs in the back of the end point P _{1 when viewed from the start point P 0. Let} this fixed length be h totalUB.

Then, _{while maintaining the relationship of h totalLB} ≤ h _{total UB} , h _{total LB and h total UB} are updated by a 2-minute search, and when the difference h _{total UB} −h _{total LB} becomes sufficiently small, the arrival point set S _ParrG (h _{total LB} ) , S _ParrG (h _{totalUB ), the four parameters (h 1} , h ₂ , h ₃ , θ _{d, 1} ) corresponding to the element closest to the terminal position P ₁ are used as the initial values of the iterative optimization method.

FIG. 7 is a diagram showing the operation of the rough 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, finds the} arrival point for a combination of a plurality of appropriately selected parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1), and reaches.} The nearest inner circumference arrival point is obtained from the points (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 central angle θ _{fan of the} sector is a preset value, and is, for example, π / 4. Subsequently, the coarse search unit 11 fixes the total length of the triple clothoid to the upper limit h _totalUB , and determines the arrival point for a combination of a plurality of appropriately selected parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1).} It is obtained, and the nearest neighbor outer peripheral arrival point is obtained from the arrival points (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 a process of finding the nearest inner peripheral arrival point. The coarse search unit 11 sets _{the linear distance between the start point P 0} and the end point P ₁ as the lower limit h _totalLB of the total length (h ₁ + h ₂ + h ₃ ) of the triple clothoid (S40), and appropriately selects a plurality of parameters satisfying this condition. Then, the arrival point corresponding to the parameter is calculated (S41). The coarse 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). In the determination of the presence or absence of the inner circumference arrival point, if it is determined that there is no inner circumference arrival point (NO in S42), it is determined that there is no solution of the triple clothoid connecting the start end and the end (S44).

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

Specifically, the value obtained by multiplying the linear distance between the start point P ₀ and the end point P _{1 by the constant C3 (> 0) is added to the upper limit h totalUB} to obtain a new upper limit h _totalUB (S54). When the new upper limit h _totalUB is not larger than a predetermined threshold value (NO in S55), a process of finding the outer peripheral arrival point is performed using the _{new upper limit h totalUB (S51).} When the new upper limit h _totalUB is larger than a predetermined threshold value (YES in S55), it is determined that there is no solution of the triple clothoid connecting the start end and the end point (S56).

Returning to FIG. 7, the operation performed by the coarse search unit 11 will be described. The coarse search unit 11 determines whether or not the distance of the difference between the _{upper limit h total UB} and the lower limit h _{total LB is smaller than a predetermined threshold value (S32).} When the distance between the difference between the upper limit h _totalUB and the lower limit h _totalLB is smaller than the threshold value (YES in S32), the nearest neighbor outer peripheral reach 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).

In the first stage, since _{the distance of the difference between the upper limit h totalUB} and the lower limit h _totalLB is not smaller than the threshold value (NO in S32), the process proceeds to step S33. The coarse search unit 11 _{calculates an intermediate fixed length of the distance between the upper limit h totalUB} and the lower limit h _totalLB (S33), and uses this intermediate fixed length to obtain the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point. (S34).

Subsequently, the coarse search unit 11 ends at the nearest neighbor outer peripheral arrival point obtained by fixing the intermediate fixed length to the nearest neighbor 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 1 (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 towards the nearest outer periphery arrival point obtained by the upper limit _{h TotalUB} is close to the end point _{P 1} (NO in S35), the coarse search unit 11, recently determined by fixing the entire length of triplicate clothoid the lower _{h TotalLB} 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 or not the distance of the difference between the _{upper limit h total UB} and the lower limit h _{total LB is smaller than a predetermined threshold value.}

Than nearest periphery goal that the total length was determined by fixing the lower limit h _TotalLB triplicate clothoid, determination towards nearest periphery destination point was determined by fixing the intermediate fixed length whether closer to the end point P ₁ 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 in which track planning is performed by specifically giving data on the posture of the vehicle at the start and end is shown.

(First Example)
FIG. 11 is a diagram showing data at the start and end of 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 vehicle orientation θ _{0 at the} beginning is 0 [rad], the vehicle turning curvature κ ₀ is 0 [1 / m], the vehicle orientation θ _{1 at the} end is 0.7854 [rad], the vehicle turning curvature κ _1. Is 0.0667 [1 / m].

As explained above, the set of parameters (h ₁ , h ₂ , h ₃ , θ _{d, 1 ) when the total length (h 1} + h ₂ + h ₃ ) of the triple clothoid is fixed to h total is calculated by the following equation. Consider the defined goal set S _ParrG (h _total).

FIG. 12 is a diagram showing a set of arrival points S _ParrG (h _{total ) when h total} = | P ₁ −P _{0 | in the case of FIG.} However, here, Δθ _fan was set to π / 4. Further, since S _ParrG (h _total ) is strictly an infinite set, it is obtained by approximate calculation as follows.

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

_{(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)} becomes approximate arrival point set in the direction of the _P 1 -P _0.

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

Therefore, the vicinity position _Parr + ΔP of the arrival point _{Parr Parr} (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 1} ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^*. Be done. For example, specifically, as shown in FIG. 13, _{four parameters h 1} ^* , h ₂ ^* , h given by the following equation, which are slightly changed from _{h 1} ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^*. ₃ ^* + Δh ₃ , θ _{d, 1} ^* + Δθ _{d, 1} allows the arrival point to be changed to a position close to _Parr + ΔP _arr.

Conversely, the solution to the optimization problem of Equation ^{_{(55) (h 1 ※,}} h 2 ※, h 3 ※, θ d, 1 ※) if any, is in the vicinity of _{(h 1} ^※, _{h 2} ^※ , H ₃ ^* , θ _{d, 1} ^* ) There is a set of arrival points corresponding to the set of parameters changed.

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

Therefore, in the first embodiment, if _SParrG (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) Is used as the initial value.

According to this method, as compared with the case where randomly chosen initial values, in order to optimize techniques repeatedly from a position near the end point P ₁ can be started, it is less bound by the minimum value. Further, in the case of applying a sudden iterative optimization technique, or without solution, but the calculation without converged by non-convex issues may not ended, in this embodiment, the reaching points set S _ParrG ( Since h _total ) is approximately calculated from the finite parameter set S _RS4para (h _total ), the presence or absence of a solution is always estimated in a finite time.

(Second Example)
Since the distance from the start point P ₀ to the arrival point _{Parr | Parr} −P ₀ | 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. 14A is a diagram showing a set of arrival points S _ParrG (h _{total ) when h total} = 1 · | P ₁ −P ₀ |, and is the same as FIG. 12 shown above. As can be seen in FIG. 14 (a), all the elements of _{S ParrG} (h _total ) are always distributed before the end point P _{1 when viewed from the start point P 0} , supporting the above equation. FIG. 14 (b) shows the _destination set S ParrG (h _total ) _{when the length of h total} is increased from 1 · | P ₁ −P ₀ | and h _total = 2 · | P ₁ −P _{0 |.} It is a figure which shows. In this case, the goal set _S ParrG _{(h total)} distributed starting _{P 0} viewed from Te in the back of the end point _{P 1.}

The results shown in FIGS. 14 (a) and 14 (b) show the triple clothoid when _{equation (55) has the optimum solution (h 1} ^* , h ₂ ^* , h ₃ ^* , θ _{d, 1} ^*). _{Let the total} length h ^{total *} be h _total ^* = h ₁ ^* + h ₂ ^* + h ₃ ^* . When h _total <h _total ^* and h _total are shortened, S _ParrG (h _total ) is distributed before _{P 1 when} viewed from _{P 0.} On the other _hand, the longer the _{h total>} h _total ^※ and _{h total, S ParrG (h total} ) is intended to be distributed to the back of the end point _{P 1} as seen from the starting point _{P 0} is generated.

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, a better initial value is obtained by narrowing the gap between _{the lower limit h total LB} and the upper limit h _{total UB by a two-minute search.}

To illustrate the arrival point of the start point _{P 0} viewed from Te distributed in front of the end point _{P 1,} 4 parameter and its corresponding set _{S ParrGIn} of deliberate who point defined by the following equation _{(h total) (h 1,} h ₂ , h ₃ , θ _{d, 1} ) set S _RS4parrIn (h _total ) is introduced.

Equation (200) is theoretically a set with infinite elements, but as shown in the first embodiment, 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 neighbor inner peripheral arrival point closest to the end point P _{1 is P parGInNN} (h _total ), and the four parameters are (h ₁ , h ₂ , h ₃ , θ _{d, 1} ). _{Let it be InNN} (h _total ). The formula is as follows.

Similarly, to explain with respect to reach points starting _{P 0} viewed from Te distributed to the back of the end point _{P 1,} defined by the following equation, reaching points set _{S ParrGOut (h total)} and 4 parameters corresponding thereto _(h 1, Introduce the set S _RS4paraOut (h _total ) of h ₂ , h ₃ , θ _{d, 1).}

Similarly, _{among S ParrGOut} (h _total ), the nearest neighbor outer peripheral arrival point closest to the end point P _{1 is P parGOutNN} (h _total ), and the four parameters are (h ₁ , h ₂ , h ₃ , θ _{d, 1} ) _OutNN. (H _total ). The formula is as follows.

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 the 4 parameters that reach the side.

FIG. 15 is a diagram showing the distribution of arrival points when the update processing of _{h total LB} and h _{total UB} is performed by the above algorithm for the case shown in FIG. FIG. 16 is a diagram showing a trajectory obtained by performing an iterative optimization method (main dual interior point method) using the initial values obtained by the above processing. As shown in FIG. 16, it was confirmed that the optimization problem shown in Eq. (55) can be appropriately solved by starting from an appropriate initial value.
Further, in the three cases other than FIG. 11, an example in which the trajectory is planned using the initial values acquired by the method of the present embodiment is shown.

FIG. 17 shows the orbits calculated in each of the above cases. As shown in FIG. 17, all of them were appropriately repeatedly optimized to obtain an orbit.

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

1 Orbit planning device 10 Input unit 11 Rough search unit 12 Optimization unit 13 Output unit 14 Storage unit

Claims (11)

A program for planning an orbit consisting of three clothoid curves with continuous curvatures at a connection point, which can be applied to a computer.
An input step that accepts input of attitude data specified by the position, orientation, and curvature of the vehicle at the beginning and attitude data specified by the position, orientation, and curvature of the vehicle at the end.
It is a function for obtaining the posture specified by the position, orientation, and curvature of the vehicle at the reaching point from the starting posture, and reads out the reaching point function including at least the length of the crossoid curve as a parameter from the storage unit, and three of them. With the total length of the crossoid curve as the fixed length from the start end to the end, a plurality of combinations of the respective lengths of the crossoid curve are used as sample parameters, and the sample parameters are applied to the arrival point function to reach the arrival point. And a rough search step to find the parameter that obtained the nearest reaching point closest to the end among the reaching points.
An optimization step in which the parameter obtained in the rough search step is used as an initial value, the parameter of the arrival point function is changed, and an optimum parameter for obtaining an optimum parameter such that the posture of the arrival point matches the posture of the end point is obtained.
A program that executes. In the rough search step, if there is no sample parameter that gives the nearest neighbor reach point at which the distance to the end is equal to or less than a predetermined threshold value, 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 with continuous curvatures at a connection point, which can be applied to a computer.
An input step that accepts input of attitude data specified by the position, orientation, and curvature of the vehicle at the beginning and attitude data specified by the position, orientation, and curvature of the vehicle at the end.
It is a function for obtaining the posture specified by the position, orientation, and curvature of the vehicle at the arrival point from the attitude of the start end, and reads out the arrival point function including at least the length of the clothoid curve as a parameter from the storage unit, and three of them. With the total length of the clothoid curve as the first fixed length from the start end to the end, a plurality of combinations of the respective lengths of the clothoid curve are used as sample parameters, and the sample parameters are applied to the destination function. In the first parameter calculation step, the inner circumference arrival point in front of the end is obtained, and the parameter obtained from the inner circumference arrival point closest to the end is obtained as the first parameter. ,
The total length of the three clothoid curves is set as a second fixed length longer than the first fixed length, a plurality of combinations of the respective lengths of the clothoid curves are set as sample parameters, and the sample parameters are set as the reaching point function. In the second parameter calculation step, the outer peripheral arrival point deeper than the end is obtained, and the parameter obtained from the outer peripheral arrival point closest to the end is obtained as the second parameter. ,
The total length of the three clothoid curves is set as an intermediate fixed length between the first fixed length and the second fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters. The parameters were applied to the arrival point function to obtain the nearest inner circumference arrival point and the nearest outer circumference arrival point, and the parameter obtained from the nearest inner circumference arrival point was the third parameter, and the nearest outer circumference arrival point was obtained. The third and fourth parameter calculation steps to obtain the parameters as the fourth parameter, and
Comparing the nearest neighbor outer peripheral arrival point given by the second parameter with the nearest neighbor outer peripheral arrival point given by the fourth parameter, when the nearest neighbor outer peripheral arrival point given by the fourth parameter is closer to the end. Uses the intermediate fixed length as a new second fixed length, and when the nearest neighbor outer circumference reaching point given by the second parameter is closer to the end, the nearest neighbor inner circumference given by the first parameter Comparing the arrival point with the nearest neighbor inner circumference arrival point given by the third parameter, if the nearest neighbor inner circumference arrival point given by the third parameter is closer to the end, the intermediate fixed length is newly added. The fixed length update step, which is the first fixed length,
While updating the first fixed length or the second fixed length in the fixed length updating step, the nearest neighbor outer peripheral arrival point and the nearest neighbor inner circumference arrival point are repeatedly obtained, and the first fixed length and the first fixed length are obtained. When the difference between the fixed lengths of 2 is equal to or less than a predetermined threshold value, the parameter determination for finding the parameter giving the nearest neighbor arrival point closer to the end of the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point. Steps and
An optimization step in which the parameter obtained in the parameter determination step is used as an initial value, the parameter of the arrival point function is changed, and an optimum parameter is obtained so that the posture of the arrival point matches the posture of the end point.
A program that executes. In the fixed length update step, the nearest neighbor inner circumference arrival point given by the first parameter is compared with the nearest neighbor inner circumference arrival point given by the third parameter, and the nearest neighbor inner circumference given by the first parameter is compared. The third aspect of claim 3, wherein when the arrival point is closer to the end, a parameter giving the nearest neighbor arrival point closer to the end among the nearest outer peripheral arrival point and the nearest inner circumference arrival point is obtained. program. The program according to claim 4, wherein when the inner peripheral arrival point is not found by the calculation in the first parameter calculation step, it is determined that the clothoid curve connecting the start end and the end 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 until the outer peripheral arrival point is found or the second fixed length becomes equal to or more than a predetermined threshold value. 4. When the second fixed length exceeds a predetermined threshold value, it is determined that the clothoid curve connecting the start end and the end does not exist. Described program. The program according to any one of claims 1 to 6, wherein the reaching point function includes the turning curvature of the vehicle as a parameter and has the maximum value of the turning curvature as a constraint condition. It is a method for planning a trajectory consisting of three clothoid curves having continuous curvatures at a connection point by a trajectory planning device.
The track planning device includes an input step that accepts input of attitude data specified by the position, orientation, and curvature of the vehicle at the start end and attitude data specified by the position, orientation, and curvature of the vehicle at the end.
The track planning device is a function for obtaining a posture specified by the position, orientation, and curvature of the vehicle at the reaching point from the posture of the starting point, and stores a reaching point function including at least the length of the clothoid curve as a parameter. Read from the unit, the trajectory planning device uses the total length of the three clothoid curves as a fixed length from the start end to the end, and uses a plurality of combinations of the respective lengths of the clothoid curves as sample parameters. A rough search step in which the parameters are applied to the arrival point function to obtain the arrival point, and the parameter for which the nearest arrival point closest to the end of the arrival points is obtained is obtained.
The trajectory planning device uses the parameter obtained in the rough search step as an initial value, fluctuates the parameter of the arrival point function, and obtains the optimum parameter so that the posture of the arrival point matches the posture of the end point. With the conversion step
Orbit planning method. It is a method for planning a trajectory consisting of three clothoid curves having continuous curvatures at a connection point by a trajectory planning device.
The track planning device includes an input step that accepts input of attitude data specified by the position, orientation, and curvature of the vehicle at the start end and attitude data specified by the position, orientation, and curvature of the vehicle at the end.
The track planning device is a function for obtaining a posture specified by the position, orientation, and curvature of the vehicle at the reaching point from the starting posture, and stores a reaching point function including at least the length of the clothoid curve as a parameter. Read from, the orbit planning device uses the total length of the three clothoid curves as the first fixed length from the start end to the end, and uses a plurality of combinations of the respective lengths of the clothoid curves as sample parameters. The parameter of the sample is applied to the arrival point function to obtain the inner circumference arrival point in front of the end, and the parameter obtained from the inner circumference arrival point closest to the end is obtained. The first parameter calculation step to be obtained as the first parameter, and
In the trajectory planning device, the total length of the three clothoid curves is set as a second fixed length longer than the first fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters. The parameter is applied to the arrival point function to obtain the outer peripheral arrival point deeper than the end, and the parameter obtained from the outer peripheral arrival point closest to the end is obtained as the second parameter. The second parameter calculation step and
The trajectory planning device sets a total length of three clothoid curves as an intermediate fixed length between the first fixed length and the second fixed length, and samples a plurality of combinations of the respective lengths of the clothoid curves. As parameters, the parameters of the sample 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 obtained from the nearest inner circumference arrival point is the third parameter, the nearest neighbor. The third and fourth parameter calculation steps for obtaining the parameter obtained from the outer peripheral arrival point as the fourth parameter, and
The orbit planning device compares the nearest neighbor outer circumference arrival point given by the second parameter with the nearest neighbor outer circumference arrival point given by the fourth parameter, and the nearest neighbor outer circumference arrival point given by the fourth parameter is better. When it is close to the end, the intermediate fixed length is set as a new second fixed length, and when the nearest neighbor outer peripheral arrival point given by the second parameter is closer to the end, the first parameter is used. Comparing the given nearest neighbor inner circumference arrival point with the nearest neighbor inner circumference arrival point given by the third parameter, if the nearest neighbor inner circumference arrival point given by the third parameter is closer to the end, A fixed length update step in which the intermediate fixed length is set as a new first fixed length, and
The orbit planning device repeatedly obtains the nearest neighbor outer peripheral arrival point and the nearest neighbor inner circumference arrival point while updating the first fixed length or the second fixed length in the fixed length updating step, and the first When the difference between the fixed length of No. 1 and the second fixed length becomes equal to or less than a predetermined threshold value, the nearest neighbor arrival point of the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point, whichever is closer to the end, is given. The parameter determination step to find the specified parameters and
The track planning device fluctuates the parameter of the arrival point function with the parameter obtained in the parameter determination step as an initial value, and the posture specified by the position, orientation, and curvature of the vehicle at the arrival point is the end. The optimization step to find the optimum parameters that match the posture of
Orbit planning method. An orbit planning device for planning an orbit consisting of three clothoid curves having continuous curvatures at a connection point.
An input unit that accepts input of attitude data specified by the position, orientation, and curvature of the vehicle at the beginning and attitude data specified by the position, orientation, and curvature of the vehicle at the end.
A storage unit that stores the arrival point function, which is a function for obtaining the attitude specified by the position, orientation, and curvature of the vehicle at the arrival point from the attitude of the start end, and includes at least the length of the clothoid curve as a parameter.
The total length of the three clothoid curves is set as a fixed length from the start end to the end, and a plurality of combinations of the respective lengths of the clothoid curves are set as sample parameters, and the sample parameters are read from the storage unit. A rough search unit that obtains the arrival point by applying it to the point function and obtains the parameter that obtains the nearest arrival point closest to the end among the arrival points.
An optimization unit that uses the parameter obtained by the rough search unit as an initial value, fluctuates the parameter of the arrival point function, and obtains an optimum parameter so that the posture of the arrival point matches the posture of the end point.
Orbit planning device equipped with. An orbit planning device for planning an orbit consisting of three clothoid curves having continuous curvatures at a connection point.
An input unit that accepts input of attitude data specified by the position, orientation, and curvature of the vehicle at the beginning and attitude data specified by the position, orientation, and curvature of the vehicle at the end.
A storage unit that stores the arrival point function, which is a function for obtaining the attitude specified by the position, orientation, and curvature of the vehicle at the arrival point from the attitude of the start end, and includes at least the length of the clothoid curve as a parameter.
A rough search is performed to obtain the arrival point by applying a plurality of combinations to the arrival point function read from the storage unit and obtaining the parameter obtained from the nearest arrival point closest to the end among the arrival points. Department and
An optimization unit that uses the parameter obtained by the rough search unit as an initial value, fluctuates the parameter of the arrival point function, and obtains an optimum parameter so that the posture of the arrival point matches the posture of the end point.
With
The rough search unit
The total length of the three clothoid curves is used as the first fixed length from the start end to the end, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters, and the sample parameters are used as the destination function. First parameter calculation to obtain the inner circumference arrival point in front of the end by applying, and to obtain the parameter obtained from the inner circumference arrival point closest to the end as the first parameter. Steps and
The total length of the three clothoid curves is set as a second fixed length longer than the first fixed length, a plurality of combinations of the respective lengths of the clothoid curves are set as sample parameters, and the sample parameters are set as the reaching point function. In the second parameter calculation step, the outer peripheral arrival point deeper than the end is obtained, and the parameter obtained from the outer peripheral arrival point closest to the end is obtained as the second parameter. ,
The total length of the three clothoid curves is set as an intermediate fixed length between the first fixed length and the second fixed length, and a plurality of combinations of the respective lengths of the clothoid curves are used as sample parameters. The parameters were applied to the arrival point function to obtain the nearest inner circumference arrival point and the nearest outer circumference arrival point, and the parameter obtained from the nearest inner circumference arrival point was the third parameter, and the nearest outer circumference arrival point was obtained. The third and fourth parameter calculation steps to obtain the parameters as the fourth parameter, and
Comparing the nearest neighbor outer peripheral arrival point given by the second parameter with the nearest neighbor outer peripheral arrival point given by the fourth parameter, when the nearest neighbor outer peripheral arrival point given by the fourth parameter is closer to the end. Uses the intermediate fixed length as a new second fixed length, and when the nearest neighbor outer circumference reaching point given by the second parameter is closer to the end, the nearest neighbor inner circumference given by the first parameter Comparing the arrival point with the nearest neighbor inner circumference arrival point given by the third parameter, if the nearest neighbor inner circumference arrival point given by the third parameter is closer to the end, the intermediate fixed length is newly added. The fixed length update step, which is the first fixed length,
While updating the first fixed length or the second fixed length in the fixed length updating step, the nearest neighbor outer peripheral arrival point and the nearest neighbor inner circumference arrival point are repeatedly obtained, and the first fixed length and the first fixed length are obtained. When the difference between the fixed lengths of 2 is equal to or less than a predetermined threshold value, the parameter determination for finding the parameter giving the nearest neighbor arrival point closer to the end of the nearest neighbor outer circumference arrival point and the nearest neighbor inner circumference arrival point. Steps and
Orbit planning device to perform.

Images