TRAJECTORY GENERATOR, TRAJECTORY GENERATING METHOD AND NON-TRANSITORY TANGIBLE COMPUTER-READABLE STORAGE MEDIUM

Abstract

A physical quantity of motion of a vehicle in future traveling is specified in time series in a traveling trajectory. A standard trajectory is calculated based on a standard response parameter as a traveling trajectory that does not consider a constraint condition in future traveling. A correction trajectory is calculated based on a correction response parameter different from the standard trajectory as a traveling trajectory having a correction amount for correcting the standard trajectory in time series according to the constraint condition. A target trajectory to which the vehicle follows is calculated as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory.

Description

CROSS REFERENCE TO RELATED APPLICATIONS

The present application claims the benefit of priority from Japanese Patent Application No. 2019-186954 filed on Oct. 10, 2019. The entire disclosure of the above application is incorporated herein by reference.

TECHNICAL FIELD

The present disclosure relates to a technique for generating a travelling trajectory in which a physical quantity of motion in future travelling of a vehicle is specified in time series.

BACKGROUND

Conventionally, a technique for generating a traveling trajectory which a vehicle follows in future driving has been proposed, for example. In the technology, the standard trajectory based on the traffic lane information is corrected by the constraint information to generate the traveling trajectory of the vehicle to follow.

SUMMARY

A physical quantity of motion of a vehicle in future traveling is specified in time series in a traveling trajectory. A standard trajectory is calculated based on a standard response parameter as a traveling trajectory that does not consider a constraint condition in future traveling. A correction trajectory is calculated based on a correction response parameter different from the standard trajectory as a traveling trajectory having a correction amount for correcting the standard trajectory in time series according to the constraint condition. A target trajectory to which the vehicle follows is calculated as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory.

BRIEF DESCRIPTION OF THE DRAWINGS

The above and other objects, features and advantages of the present disclosure will become more apparent from the following detailed description made with reference to the accompanying drawings. In the drawings:

FIG. 1 is a block diagram showing an overall configuration of a trajectory generator according to an embodiment;

FIG. 2 is a block diagram showing a detailed configuration of a trajectory generator according to the embodiment;

FIG. 3 is a schematic diagram for explaining a traveling trajectory according to the embodiment;

FIG. 4 is a schematic diagram for explaining a standard trajectory according to the embodiment;

FIG. 5 is a schematic diagram for explaining a constraint condition according to the embodiment;

FIG. 6 is a graph for comparing a standard trajectory and a correction trajectory according to the embodiment;

FIG. 7 is a schematic diagram for explaining a correction trajectory according to the embodiment;

FIG. 8 is a schematic diagram for explaining a target trajectory according to the embodiment;

FIG. 9 is a schematic diagram for explaining vehicle control that follows a target trajectory according to the embodiment;

FIG. 10 is a flowchart illustrating a trajectory generating method according to the embodiment; and

FIG. 11 is a graph for comparing the effect of the embodiment (a) with the comparative example (b).

DETAILED DESCRIPTION

In a conceivable technique, one of multiple types in the constraint information is reflected on the standard trajectory one by one. Therefore, when the number of predicted points on the standard trajectory is defined as N, the maximum number of calculations required to generate the final traveling trajectory may reach 2N or more.

According to the above point, a trajectory generator is provided such that reduces a calculation load for generating a traveling trajectory. Further, a trajectory generation method is provided such that reduces a calculation load for generating a traveling trajectory. Further, a trajectory generation program is provided such that reduces the calculation load for generating a traveling trajectory.

According to an example embodiment, a trajectory generator (1) for generating a traveling trajectory (X) in which a physical quantity of motion of a vehicle (3) in future traveling is specified in time series, the trajectory generator includes:

• • a standard calculator (100) that calculates a standard trajectory (X_b) based on a standard response parameter (Q_b, R_b) as a traveling trajectory that does not consider a constraint condition in the future traveling;
  • a correction calculator (140) that calculates a correction trajectory (Xm) based on a correction response parameter (Q_m, R_m) different from the standard trajectory as a traveling trajectory having a correction for correcting the standard trajectory in time series according to the constraint condition; and
  • a target calculator (160) that calculates a target trajectory (X_p) to be followed by the vehicle as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory.

According to an example embodiment, a trajectory generating method executed by a processor (12) for generating a traveling trajectory (X) in which a physical quantity of motion of a vehicle (3) in future traveling is specified in time series, the trajectory generating method includes:

• • calculating a standard trajectory (X_b) based on a standard response parameter (Q_b, R_b) as a traveling trajectory that does not consider a constraint condition in the future traveling (S101);
  • calculating a correction trajectory (X_m) based on a correction response parameter (Q_m, R_m) different from the standard trajectory as a traveling trajectory having a correction for correcting the standard trajectory in time series according to the constraint condition (S103); and
  • calculating a target trajectory (X_p) to be followed by the vehicle as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory (S104).

According to an example embodiment, a trajectory generating program that includes instructions executed by a processor (12) and is stored in a storage medium (10) to generate a traveling trajectory (X) in which a physical quantity of motion of a vehicle (3) in future traveling is specified in time series, the instructions includes:

• • calculating a standard trajectory (X_b) based on a standard response parameter (Q_b, R_b) as a traveling trajectory that does not meet a constraint condition in the future traveling (S101);
  • calculating a correction trajectory (X_m) based on a correction response parameter (Q_m, R_m) different from the standard trajectory as a traveling trajectory having a correction for correcting the standard trajectory in time series according to the constraint condition (S103); and
  • calculating a target trajectory (X_p) to be followed by the vehicle as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory (S104).

According to these embodiments, the standard trajectory without considering the constraint condition in the future traveling is corrected by the correction amount of the travelling trajectory in time series according to the constraint condition, and the correction trajectory is calculated based on the response parameter different from the standard trajectory. As a result, the constraint condition can be reflected at once in the calculation of the correction trajectory regardless of the type thereof. Therefore, it becomes possible to reduce the calculation load until the target trajectory is generated by superimposing the correction trajectory on the standard trajectory.

Hereinafter, an embodiment will be described with reference to the drawings.

As shown in FIG. 1, a trajectory generator 1 according to an embodiment is mounted on a vehicle 3 together with a driving control device 2. The trajectory generator 1 generates a traveling trajectory to be followed in the future traveling by the vehicle 3. The driving control device 2 executes a driving control on the vehicle 3 to follow the traveling trajectory generated by the trajectory generator 1. The vehicle 3 is, for example, an automatic driving vehicle or an advanced driving assistance vehicle that can autonomously travel on the driving path 4 steadily or temporarily by receiving the driving control from the driving control device 2.

A sensor system 5 is mounted on the vehicle 3 in addition to the trajectory generator 1 and the driving control device 2. The sensor system 5 acquires various kinds of information that can be utilized for the trajectory generation by the trajectory generator 1 and for the driving control by the driving control device 2. As shown in FIG. 2, the sensor system 5 includes an outside sensor 50 and an inside sensor 52.

The outside sensor 50 generates information about the outside of the vehicle 3, which is the surrounding environment of the vehicle 3. The outside sensor 50 may acquire the outside information by detecting an object existing in the outside of the vehicle 3. The outside sensor 50 of the detection type is at least one of a camera, a LIDAR (Light Detection and Ranging/Laser Imaging Detection and Ranging), a radar, a sonar, and the like, for example.

The outside sensor 50 may acquire the external information by receiving a signal from an artificial satellite of a GNSS (Global Navigation Satellite System) disposed in the outside of the vehicle 3 or a signal from a roadside device of ITS (Intelligent Transport Systems).

The outside sensor 50 of the reception type is at least one of, for example, a GNSS receiver, a telematics receiver, and the like.

The inside sensor 52 generates information about the inside of the vehicle 3, which is the internal environment of the vehicle 3. The inside sensor 52 may generate the internal information by detecting a specific motion physical quantity in the inside of the vehicle 3. The detection type inside sensor 52 is, for example, at least one of a gyroscope, a traveling speed sensor, an acceleration sensor, a steering angle sensor, and the like.

Based on the acquired information of the sensor system 5, the travel trajectory generated by the trajectory generator 1 and output to the driving control device 2 defines the physical quantity of motion of the vehicle 3 in future travel in time series. Specifically, the travelling trajectory is generated in a region from the current time-series point to the future time-series point, which is set ahead of the current time-series point by the predetermined number of steps, as the future prediction region. That is, the time-series points on the travelling trajectory indicated by white circles in FIG. 3 are predicted points for providing the travelling trajectory. When each time series point included in the future prediction region is identified by the index k, the current time series point at the present time is defined as k=0, and the future time series point ahead of the current time-series point by the predetermined number of steps is defined as k=N, the current time series point provides a starting point and the future time series point provides an end point of the travelling trajectory.

The travel trajectory defines a vector value or a scalar value at each time series point in the future prediction region so as to give a desired response characteristic with respect to a specific physical motion quantity of various physical motion quantities of the vehicle 3. The physical quantity of motion of the vehicle 3 defined by the travelling trajectory is at least one of a lateral position or a yaw angle, a running speed, an acceleration, a travelling distance, a steering angle, and the like relative to the travelling path 4. Here, in order to facilitate understanding, the present embodiment will be described below by describing an example of the case where the trajectory generator 1 generates a traveling trajectory in a lateral position relative to the traveling path 4. The lateral position relative to the traveling path 4 is defined as the relative position from the center position (see the broken line in FIG. 3) in the width direction of the traveling path 4, and is simply referred to as the lateral position in the following description. Further, the yaw angle relative to the traveling path 4 is defined as a relative angle between the center line of the traveling path 4 and the center line (see the dash-dotted line in FIG. 3) in the width direction of the vehicle 3, and will be simply described in the following description as the yaw angle.

When the lateral position and the yaw angle at time k are represented by e[k] and θ[k], respectively, the following equation 1 is approximately established between the lateral position and the yaw angle. In equation 1, k_p[k] is the curvature of the traveling path 4. In equation 1, k_v[k] is a curvature that the vehicle 3 should follow. In equation 1, Δ is a time interval between time series points. Under the condition that equation 1 is satisfied, the traveling trajectory X at that position is defined by the following equation 2 using the lateral position e[k] at each time series point where k is 1 to N.

$\begin{array}{cc}\left(\mathrm{Equations}1\mathrm{and}2\right)& \\ \left[\begin{array}{c}e\left[k+1\right]\\ \theta \left[k+1\right]\end{array}\right]=\left[\begin{array}{cc}1& \Delta \\ 0& 1\end{array}\right]·\left[\begin{array}{c}e\left[k\right]\\ \theta \left[k\right]\end{array}\right]+\left[\begin{array}{c}{\Delta }^2/2\\ \Delta \end{array}\right]·\left({\kappa }_v\left[k\right]-{\kappa }_p\left[k\right]\right)& 1\\ X={\left[e\left[1\right],e\left[2\right],\dots ,e\left[N\right]\right]}^T& 2\end{array}$

Here, “” in an equation means “Equation.”

Then the state quantity x[k] representing the motion state of the vehicle 3 and the operation quantity u[k] representing the driving operation of the vehicle 3 are defined by the following equations 3 and 4, respectively, the equation 1 is linearly approximated by the following equations 5 to 7. Under this approximation, the evaluation index J regarding the traveling trajectory X at the lateral position e[k] in the future prediction region is represented by the following equation 8. In equation 8, the parameter matrix Q is a response parameter that adjusts the action of causing the traveling trajectory X to approach the central position in the width direction of the traveling path 4. In equation 8, the parameter coefficient R is a response parameter that adjusts the degree of steepness in the change of the traveling trajectory X. The evaluation index J expressed by the equation 8 specifies the response characteristic of the traveling trajectory X that is determined according to the settings of the parameter matrix Q and the parameter coefficient R. The center position in the width direction of the traveling path 4 is simply referred to as the center position in the following description.

$\begin{array}{cc}\left(\mathrm{Equations}3\mathrm{to}8\right)& \\ x\left[k\right]={\left[e\left[k\right],\theta \left[k\right]\right]}^T& 3\\ u\left[k\right]={\kappa }_v\left[k\right]-{\kappa }_p\left[k\right]& 4\\ x\left[k+1\right]=A·x\left[k\right]+b·u\left[k\right]& 5\\ A=\left[\begin{array}{cc}1& \Delta \\ 0& 1\end{array}\right]& 6\\ b=\left[\begin{array}{c}{\Delta }^2/2\\ \Delta \end{array}\right]& 7\\ J=\frac{1}{2}·{x\left[N\right]}^T·Q·{x\left[N\right]}^T+\frac{1}{2}·\sum _{k=0}^{N-1}\left({x\left[k\right]}^T·Q·{x\left[k\right]}^T+R·{u\left[k\right]}^2\right)& 8\end{array}$

When the traveling trajectory U of the operation quantity u[k] is defined by the equation 9 using the operation quantity u[k] at each time series point with setting k from 0 to N−1, the equations 5 and 8 are rearranged, so that the following equations 10 to 12 are obtained. That is, the evaluation index J regarding the traveling trajectory X at the lateral position e[k] in the future prediction region may also be expressed by equation 10 by substituting equations 11 and 12 into equation 10.

$\begin{array}{cc}\left[\mathrm{Equations}9\mathrm{to}12\right]& \\ U={\left[u\left[0\right],u\left[1\right],\dots ,u\left[N-1\right]\right]}^T& 9\\ J=\frac{1}{2}·U^T·H\left(Q,R\right)·U+U^T·F\left(Q\right)·x\left[0\right]& 10\\ H\left(Q,R\right)={\left[\begin{array}{cccc}b& 0& \cdots & 0\\ A·b& b& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N-1}·b& A^{N-2}·b& \cdots & b\end{array}\right]}^T·[\begin{array}{ccc}Q& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & Q\end{array}]·\hspace{1em}[\begin{array}{cccc}b& 0& \cdots & 0\\ A·b& b& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N-1}·b& A^{N-2}·b& \cdots & b\end{array}]+\hspace{1em}\hspace{1em}[\begin{array}{ccc}R& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & R\end{array}]={\stackrel{\_}{B}}^T·\stackrel{\_}{Q}\left(Q\right)·\stackrel{\_}{B}+\hspace{1em}\stackrel{\_}{R}\left(R\right)& 11\\ F\left(Q\right)={\left[\begin{array}{cccc}b& 0& \cdots & 0\\ A·b& b& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N-1}·b& A^{N-2}·b& \cdots & b\end{array}\right]}^T·\left[\begin{array}{ccc}Q& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & Q\end{array}\right]·\left[\begin{array}{c}A\\ A^2\\ ⋮\\ A^N\end{array}\right]={\stackrel{\_}{B}}^T·\stackrel{\_}{Q}\left(Q\right)·\stackrel{\_}{A}& \end{array}$

In order to generate the travelling trajectory X having the highest evaluation index J (that is, the minimum in equations 8 and 10), the trajectory generator 1 shown in FIG. 1 includes at least one ECU (Electronic Control Unit). The ECU configuring the trajectory generator 1 may be an locator ECU that estimates the self-position of the vehicle 3 or an ECU that controls communication between the vehicle 3 and the external system.

In the case of these ECUs, the trajectory generator 1 is connected to the driving control device 2 and the sensor system 5 via at least one of a LAN (Local Area Network), a wire harness, an internal bus, and the like. The ECU that constituting the trajectory generator 1 may be an ECU that also functions as the driving control device 2. In the case of this ECU, the trajectory generator 1 is connected to the sensor system 5 via at least one of a LAN, a wire harness, an internal bus, and the like.

The trajectory generator 1 is a special purpose computer including at least one memory 10 and at least one processor 12. The memory 10 is at least one type of non-transitory tangible storage medium, such as a semiconductor memory, a magnetic storage medium, and an optical storage medium, for non-transitory storing or memorizing computer readable programs and data. The processor 12 includes, as a core, at least one type of, for example, a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), an RISC (Reduced Instruction Set Computer) CPU, and so on. The processor 12 executes a plurality of instructions included in the trajectory generation program stored in the memory 10.

Thereby, the trajectory generator 1 establishes a plurality of functional blocks for generating the traveling trajectory X. As described above, in the trajectory generator 1, the trajectory generation program stored in the memory 10 causes the processor 12 to execute a plurality of instructions in order to generate the traveling trajectory X, and thus a plurality of functional blocks are constructed.

As shown in FIG. 2, the plurality of functional blocks constructed by the trajectory generator 1 include a standard calculation block 100, a constraint calculation block 120, a correction calculation block 140, and a target calculation block 160.

The standard calculation block 100 calculates a standard trajectory X_b based on a standard response parameter as a traveling trajectory X that does not consider the constraint condition (detailed later) in the future traveling of the vehicle 3. Specifically, the standard calculation block 100 sets the parameter matrix Q_b and the parameter coefficient R_b for calculating the standard trajectory X_b as the parameter matrix Q and the parameter coefficient R which are the response parameters in the equation 10 representing the evaluation index J. The parameter matrix Q_b and the parameter coefficient R_b are set to be appropriate values that satisfy the equations 13 and 14, respectively, by, for example, an experimental result or a simulation result, or an operation for each time point based on these results. Equation 13 is a conditional equation for setting a condition that the parameter matrix Q_b is a positive semi-definite matrix.

[Equations 13 and 14]

Q _b≥0  13

R _b>0  14

Under the setting of the parameter matrix Q_b and the parameter coefficient R_b, the standard calculation block 100 calculates the travelling trajectory U_b of the operation variable u_b[k], with which the evaluation index J with respect to the standard trajectory X_b is the highest, by the following equation 15. The standard calculation block 100 calculates the standard trajectory X_b of the lateral position e_b[k] where the standard evaluation index J is the highest, using the calculation results of the traveling trajectory U_b according to the following equations 16 and 17. In the calculation of these trajectories U_b and X_b, the state quantity x[0] of equations 16 and 17 is obtained based on at least the external information of the output information from the sensors 50 and 52. FIG. 4 shows an example of the standard trajectory X_b calculated without considering the constraint, together with the lateral position e_b[k] at each time series point when the constraint with respect to the vehicle 3 is to avoid the obstacle 6 (details will be described later). Note that, in FIG. 4, the area with dot hatching indicates an area where the vehicle 3 should be substantially restricted.

$\begin{array}{cc}\left[\mathrm{Equations}15\mathrm{to}17\right]& \\ U_b={\left[u_b\left[0\right],u_b\left[1\right],\dots ,u_b\left[N-1\right]\right]}^T=-{H\left(Q_b,R_b\right)}^{-1}·F\left(Q_b\right)·x\left[0\right]& 15\\ X_b={\left[e_b\left[1\right],e_b\left[2\right],\dots ,e_b\left[N\right]\right]}^T=E1·\stackrel{\_}{A}·x\left[0\right]+E_1·\stackrel{\_}{B}·U_b& 16\\ E_1=\left[\begin{array}{ccc}\left[\begin{array}{cc}1& 0\end{array}\right]& & 0\\ & \ddots & \\ 0& & \left[\begin{array}{cc}1& 0\end{array}\right]\end{array}\right]& 17\end{array}$

The constraint calculation block 120 illustrated in FIG. 2 determines a constraint predicted in the future traveling of the vehicle 3, that is, the traveling in the future prediction region. At this time, the constraint calculation block 120 sets at least one of constraints, such as avoidance of the obstacle 6, interaction with another vehicle, and legal driving restriction, as a restriction that affects the travel of the vehicle 3 in the future prediction region. Here, in order to facilitate understanding, the present embodiment will be described below by exemplifying a case where avoidance of the obstacle 6 is determined as a constraint.

As a condition that the constraint calculation block 120 limits the traveling trajectory X according to the determined constraint, the constraint condition regarding the lateral position e[k] at each time series point where k is from 1 to N is defined by the following equation 18. In equation 18, e_u[k] and e_l[k] are the upper limit position and the lower limit position of the lateral position e[k] shown in FIG. 5, respectively. Using the upper limit position e_u[k] and the lower limit position e_l[k] at each time series point where k is 1 to N, the upper limit trajectory X_u and the lower limit trajectory X_l of the traveling trajectory X are defined by the equations 19 and 20 under a definition of equation 18, so that the following equations 21 to 24 are obtained. Therefore, the constraint calculation block 120 calculates the upper limit trajectory X_u and the lower limit trajectory X_l so that the constraint condition is provided by the equation 21 to which the equation 17 and the equations 22 to 24 are substituted.

$\begin{array}{cc}\left(\mathrm{Equations}18\mathrm{to}24\right)& \\ e_i\left[k\right]\leqq e\left[k\right]\leqq e_u\left[k\right]& 18\\ X_u={\left[e_u\left[1\right],e_u\left[2\right],\dots ,e_u\left[N\right]\right]}^T& 19\\ X_1={\left[e_1\left[1\right],e_1\left[2\right],\dots ,e_1\left[N\right]\right]}^T& 20\\ \tilde{A}·x\left[0\right]+\tilde{B}·U\leqq Y_{u1}& 21\\ Y_{u1}=\left[\begin{array}{c}{X}_u\\ -X_1\end{array}\right]& 22\\ \tilde{A}=\left[\begin{array}{cc}{E}_1& \stackrel{\_}{A}\\ -E_1& \stackrel{\_}{A}\end{array}\right]& 23\\ \tilde{B}=\left[\begin{array}{cc}{E}_1& \stackrel{\_}{B}\\ -E_1& \stackrel{\_}{B}\end{array}\right]& 24\end{array}$

Specifically, the constraint calculation block 120 determines the lateral positions of the left and right side limits (that is, the left and right side edges) of the travelling path 4 in the future prediction area based on at least the external information of the output information from the sensors 50 and 52. At the same time, the constraint calculation block 120 determines the presence or absence of the obstacle 6 on the traveling path 4 based on the external information from the outside sensor 50.

When the obstacle 6 is disposed on the left side of the center position of the traveling path 4 (see FIG. 5), the constraint calculation block 120 calculates the lateral position of the obstacle 6 on the travelling path 4 in the future prediction based on the surface information of the left-side obstacle 6 among the external information from the outside sensor 50. Of the calculated lateral positions of the obstacle 6 and the left limit, the constraint calculation block 120 sets the lateral position closer to the vehicle 3 at each time series point to be the upper limit starting position. The constraint calculation block 120 subtracts the sum of the half width and the safety margin width of the vehicle 3 from the set upper limit starting position toward the center position side of the traveling path 4 to determine the upper limit trajectory X_u at each time series point.

On the other hand, when the obstacle 6 does not exist on the left side of the center position of the traveling path 4, the constraint calculation block 120 sets the calculated left side limit lateral position to be the upper limit starting position. The constraint calculation block 120 subtracts the sum of the half width and the safety margin width of the vehicle 3 from the set upper limit starting position toward the center position side of the traveling path 4 to determine the lateral position of the upper limit trajectory X_u at each time series point.

When the obstacle 6 exists on the right side of the center position of the travel path 4, the constraint calculation block 120 executes a process which is prepared by replacing the left side, the upper limit starting position, and the upper limit trajectory X_u with the right side, the lower limit starting position, and the lower limit trajectory X_l, respectively, in the above processing in which the obstacle 6 is disposed on the left side. On the other hand, when the obstacle 6 does not exist on the right side of the center position of the traveling path 4, the constraint calculation block 120 executes a process which is prepared by replacing the left side, the upper limit starting position, and the upper limit trajectory Xu with the right side, the lower limit starting position, and the lower limit trajectory Xl, respectively, in the above processing in which the obstacle 6 is not disposed on the left side.

Here, as a constraint condition, when a motion physical quantity of a type different from the lateral position e[k] is additionally considered in addition to the lateral position e[k], equations 17 to 20 and 22 to 24 are modified according to the motion physical quantity. For example, in the equation 1, when the constraint condition regarding the yaw angle θ[k] at each time series point where k is 1 to N is added, the constraint condition is defined by the following equation 25. In equation 25, θ_u[k] and θ_l[k] are the upper limit angle and the lower limit angle of the yaw angle θ[k], respectively. Under the definition of Equation 25, using the upper limit angle θ_u[k] and the lower limit angle θ_l[k] at each time series point where k is 1 to N, the upper limit trajectory Θu and the lower limit trajectory Θ_l of the yaw angle θ[k] are defined by the following Equations 26 and 27, respectively. In this case, the following equations 28 to 30 instead of the equations 22 to 24 and the following equation 31 instead of the equation 17 are obtained. Therefore, the constraint calculation block 120 calculates the upper limit trajectories X_u, Θ_u and lower limit trajectories X_l, θ_l so that the constraint conditions regarding the lateral position e[k] and the yaw angle θ[k] are given by the equation 20 to which the equations 28 to 31 are substituted. The specific calculation processing of the upper limit trajectory θ_u and the lower limit trajectory θ_l is based on the specific calculation processing of the upper limit trajectory X_u and the lower limit trajectory X_l described above, and thus the explanation thereof will be omitted.

$\begin{array}{cc}\left[\mathrm{Equations}25\mathrm{to}31\right]& \\ {\theta }_1\left[k\right]\leqq \theta \left[k\right]\leqq {\theta }_u\left[k\right]& 25\\ {\Theta }_u={\left[{\theta }_u\left[1\right],{\theta }_u\left[2\right],\dots ,{\theta }_u\left[N\right]\right]}^T& 26\\ {\Theta }_l={\left[{\theta }_l\left[1\right],{\theta }_l\left[2\right],\dots ,{\theta }_l\left[N\right]\right]}^T& 27\\ Y_{u1}=\left[\begin{array}{c}{X}_u\\ -X_1\\ {\Theta }_u\\ -{\Theta }_1\end{array}\right]& 28\\ \tilde{A}=\left[\begin{array}{c}{E}_1\stackrel{\_}{A}\\ -E_1\stackrel{\_}{A}\\ E_2\stackrel{\_}{A}\\ -E_2\stackrel{\_}{A}\end{array}\right]& 29\\ \tilde{B}=\left[\begin{array}{c}{E}_1\stackrel{\_}{B}\\ -E_1\stackrel{\_}{B}\\ E_2\stackrel{\_}{B}\\ -E_2\stackrel{\_}{B}\end{array}\right]& 30\\ E_2=\left[\begin{array}{ccc}\left[\begin{array}{cc}0& 1\end{array}\right]& & 0\\ & \ddots & \\ 0& & \left[\begin{array}{cc}0& 1\end{array}\right]\end{array}\right]& 31\end{array}$

The correction calculation block 140 shown in FIG. 2 calculates the correction trajectory X_m in order to correct the standard trajectory X_b calculated by the standard calculation block 100 in time series according to the constraint condition calculated by the constraint calculation block 120. At this time, the correction calculation block 140 calculates a correction trajectory X_m based on a response parameter different from that of the standard trajectory X_b, as the traveling trajectory X having the lateral position correction amount with respect to the standard trajectory X_b.

Specifically, the correction calculation block 140 changes the parameter matrix Q and the parameter coefficient R which are the response parameters in the equation 10 representing the evaluation index J from the parameter matrix Q_b and the parameter coefficient R_b of the standard to the parameter matrix Q_m and the parameter coefficient R_m for calculating the correction trajectory X_m. At this time, the correction calculation block 140 sets the parameter matrix Q_m and the parameter coefficient R_m, so as to provide the correction trajectory X_m in which the operation quantity u[k] for the vehicle 3 becomes larger than the standard trajectory X_b, as a response characteristic that changes the correction trajectory X_m to be steeper than the standard trajectory X_b as shown in FIG. 6. Setting of the parameter matrix Q_m and the parameter coefficient R_m means the designing of the evaluation index J so that the response characteristic different from that of the standard trajectory X_b is more preferable.

In such a setting, the correction calculation block 140 sets the parameter matrix Q_m and the parameter coefficient R_m that satisfy the following equations 32 to 35 so as to provide the evaluation index J in which the N step vector p exists as a condition for satisfying the N step scheme (described in detail later). At this time, when the correction matrix M is defined by the following equation 36 using the parameter matrix Q_m and the parameter coefficient R_m, the correction matrix M for presenting the N step vector p is obtained by setting the parameter matrix Q_m and the parameter coefficient R_m that satisfy the following equation 37. In equation 37, M_αα is a submatrix of the correction matrix M corresponding to the index set α. Therefore, equation 37 is a conditional expression that establishes for all index sets α in which the submatrix M_αα is a positive definite matrix. In equation 37, p_α is a partial vector of the N step vector p corresponding to the index set α. Equation 33 is a conditional equation for setting a condition that the parameter matrix Q_m is a positive semi-definite matrix.

$\begin{array}{cc}\left(\mathrm{Equations}32\mathrm{to}37\right)& \\ Q_m\ne Q_b& 32\\ Q_m\geqq 0& 33\\ R_m\ne R_b& 34\\ R_m>0& 35\\ M=\tilde{B}·{H\left(Q_m,R_m\right)}^{-1}·{\tilde{B}}^T=\left[\begin{array}{cc}\stackrel{\_}{M}& -\stackrel{\_}{M}\\ -\stackrel{\_}{M}& \stackrel{\_}{M}\end{array}\right]& 36\\ {\left(M_{\alpha \alpha }\right)}^{-1}·p_{\alpha }>0& 37\end{array}$

Under the setting of the parameter matrix Q_m and the parameter coefficient R_m, the correction calculation block 140 calculates the correction trajectory X_m of the lateral position correction amount em[k] by the following equation 38 using the correction vector λ. At this time, when the target trajectory X_p (detailed later) generated by the target calculation block as the traveling trajectory X is defined by the following equation 39, the correction vector λ which satisfies the following equations 40 to 45 is necessary for calculating the correction trajectory X_m.

$\begin{array}{cc}\left(\mathrm{Equations}38\mathrm{to}45\right)& \\ X_m={\left[e_m\left[1\right],e_m\left[2\right],\dots ,e_m\left[N\right]\right]}^T=-\left[\begin{array}{cc}\stackrel{\_}{M}& \text{-}\stackrel{\_}{M}\end{array}\right]·\lambda & 38\\ X_p={\left[e_p\left[1\right],e_p\left[2\right],\dots ,e_p\left[N\right]\right]}^T& 39\\ z=\left[\begin{array}{c}{X}_u\\ -X_1\end{array}\right]-\left[\begin{array}{c}{X}_p\\ -X_p\end{array}\right]& 40\\ q=\left[\begin{array}{c}{X}_u\\ -X_1\end{array}\right]-\left[\begin{array}{c}{X}_b\\ -X_b\end{array}\right]& 41\\ z=M·\lambda +q& 42\\ {\lambda }^T·z=0& 43\\ \lambda \geqq 0& 44\\ z\geqq 0& 45\end{array}$

Therefore, the correction calculation block 140 calculates the correction vector λ by the N-step scheme in which the number N of time-series points on the correction trajectory X_m is the maximum calculation step number for each constraint type of future traveling. According to this N-step scheme, even when a constraint condition is considered for a plurality of motion physical quantities including the lateral position e[k], that is, even when a plurality of types of constraints are considered, the number of calculation steps of the correction vector A is limited by the number N of time series points for each constraint type. The correction calculation block 140 further calculates the correction trajectory X_m of equation 38 using the calculation result of the correction vector λ. FIG. 7 shows an example of the correction trajectory X_m calculated in consideration of the constraint when the constraint on the vehicle 3 is to avoid the obstacle 6, together with the lateral position correction amount e_m[k] at each time series point.

The target calculation block 160 shown in FIG. 2 superimposes the correction trajectory X_m calculated by the correction calculation block 140 on the standard trajectory X_b calculated by the standard calculation block 100 according to equation 46. With this superimposition, the target calculation block 160 calculates the target trajectory X_p of the lateral position e_p[k] defined by the equation 46 as the traveling trajectory X to be followed by the vehicle 3 by outputting to the driving control device 2. FIG. 8 shows an example of a target trajectory X_p obtained by modifying the standard trajectory X_b with the correction trajectory X_m in consideration of the constraint when the constraint on the vehicle 3 is to avoid the obstacle 6, together with the lateral position e_p[k] at each time series point.

(Equation 46)

X _p=[e_p[1],e_p[2], . . . ,e_p[N]]^T=X_b+X_m  46

The driving control device 2 shown in FIGS. 1 and 3 selects at least one of the time-series points included in the target trajectory X_p output from the target calculation block 160 as a control point. The driving control device 2 extracts the distance e_c from the lateral position e_p[k] of the selected control point to the center line of the vehicle 3 in the width direction as shown in FIG. 9. Therefore, the driving control device 2 calculates the target steering angle δ_t applying to the vehicle 3 by the following equation 47 so as to reduce the extracted distance e_c as a tracking error. The driving control device 2 calculates the steering torque T for controlling the vehicle 3 to follow the target trajectory X_p by the following equation 48 using the calculated target steering angle δ_t and the actual steering angle δ_w of the vehicle 3. g in equation 47 and g_p, g_d, and g_i in equation 48 are adaptive parameters for tracking control that are set to appropriate values.

$\begin{array}{cc}\left[\mathrm{Equations}47\mathrm{and}48\right]& \\ {\delta }_t=g·e_c& 47\\ T=-g_p·\left({\delta }_w-{\delta }_t\right)-g_d·\left(\frac{d{\delta }_w}{\mathrm{dt}}-\frac{d{\delta }_t}{dt}\right)-g_i·\int \left({\delta }_w-{\delta }_t\right)\mathrm{dt}& 48\end{array}$

From the above, the standard calculation block 100 corresponds to the “standard calculator”, the correction calculation block 140 corresponds to the “correction calculator”, and the target calculation block 160 corresponds to the “target calculator”.

The flow of the trajectory generation method in which the trajectory generator 1 generates the traveling trajectory X in cooperation with the functional blocks 100, 120, 140, 160 described above will be described below with reference to FIG. 10. It should be noted that this flow is started each time the vehicle 3 reaches the time series point where k is 0. Further, in this flow, “S” means a plurality of steps of the flow executed by a plurality of instructions included in the trajectory generation program.

In S101, the standard calculation block 100 calculates the standard trajectory X_b based on the standard response parameter as the traveling trajectory X that does not consider the constraint condition for future traveling. In S102, the constraint calculation block 120 calculates the constraint condition given to the vehicle 3 in the future traveling.

In S103, the correction calculation block 140 calculates the correction trajectory X_m based on the response parameter different from the standard trajectory X_b as the travel trajectory X of with correction amount for correcting the standard trajectory X_b calculated in S101 in time series according to the constraint condition calculated in S102. In S104, the target calculation block 160 calculates the target trajectory X_p which the vehicle 3 follows as the traveling trajectory X in which the correction trajectory X_m calculated in S103 is superimposed on the standard trajectory X_b calculated in S101. Although the present flow ends as described above, the target track X_p calculated in S104 is output to the driving control device 2, so that the vehicle 3 is controlled to follow the target track X.

(Functions and Effects)

The functions and effects in the present embodiment described above will be explained below.

According to the present embodiment, the correction trajectory X_m is calculated using the response parameter different from the standard trajectory X_b, as the correction amount of the travelling trajectory X for correcting the standard trajectory X_b that is obtained without considering the constraint condition in the future travelling, in the time series according to the constraint condition. As a result, the constraint condition can be reflected at once in the calculation of the correction trajectory X_m regardless of the type thereof. Therefore, it is possible to reduce the calculation load until the target trajectory X_p is generated by superimposing the correction trajectory X_m on the standard trajectory X_b.

According to this embodiment, the correction trajectory X_m is calculated so that the response characteristic is changed to be a steeper side than the standard trajectory X_b. Accordingly, the correction trajectory X_m having a different response parameter from the standard trajectory X_b can satisfy the constraint condition even with a small number of calculations. Therefore, it is possible to contribute to the reduction of the calculation load until the target trajectory X_p is generated.

According to the present embodiment, the corrected trajectory X_m is calculated so as to provide the response characteristic in which the operation quantity u[k] for the vehicle 3 is larger than the standard trajectory X_b. According to this, the correction trajectory X_m having a response characteristic steeper than that of the standard trajectory X_b can be accurately calculated even with a small number of calculations, and the constraint condition can be satisfied. Therefore, it is possible to reduce the calculation load until the target trajectory X_p is generated, while ensuring the generation accuracy thereof.

According to the present embodiment, the correction trajectory X_m is calculated so that the N step vector p exists as a condition for establishing the N step scheme in which the number N of time series points becomes the maximum number of calculation steps for each constraint type of future traveling. As a result, the number of calculations for the correction trajectory X_m can be suppressed. Here, in particular, even in the present embodiment in which the constraint conditions regarding the lateral position e[k] and the yaw angle θ[k] other than the lateral position are taken into consideration, the effect of suppressing the number of calculations can be obtained. From these, it becomes possible to contribute to the reduction of the calculation load until the target trajectory X_p is generated.

FIG. 11 shows, for each of the present embodiment (a) and the comparative example (b), among the time series points where k is 0 or more, the time series points at which the correction trajectory X_m can be calculated with the total number N or less by black circles. In FIG. 11, the larger the value of the time interval A on the vertical axis, the farther the time series point that can be calculated is disposed. Compared to the comparative example in which the parameter matrix Q_b and the parameter coefficient R_b are directly set to the parameter matrix Q_m and the parameter coefficient R_m according to the above-mentioned conceivable technique, in the present embodiment with the different settings, the time series points at which the correction trajectory X_m can be calculated increases far from zero point. This indicates that the correction trajectory X_m can be calculated in the present embodiment even if the maximum number of calculation steps in the N-step scheme is restricted to the number N of the time series points so that the number of calculations is small.

Other Embodiments

Although one embodiment has been described, the present disclosure should not be limited to the above embodiment and may be applied to various other embodiments within the scope of the present disclosure.

Specifically, the trajectory generator 1 of the modification may be a special purpose computer configured to include at least one of a digital circuit and an analog circuit as a processor. In particular, the digital circuit is at least one type of, for example, an ASIC (Application Specific Integrated Circuit), a FPGA (Field Programmable Gate Array), an SOC (System on a Chip), a PGA (Programmable Gate Array), a CPLD (Complex Programmable Logic Device), and the like.

Such a digital circuit may include a memory in which a program is stored.

In the modification under the condition that the equation 1 is satisfied, the equation 17 is replaced with the equation 31 so that the standard trajectory, the correction trajectory, and the target trajectory regarding the yaw angle θ[k] may be calculated according to the principle similar to the above embodiment. Alternatively, in a modification under the condition that the equation 1 is satisfied, equations 23 and 24 are replaced with equations 49 and 50 (in which I is an unit matrix), respectively, so that the standard trajectory, the correction trajectory and the target trajectory relating to the curvature correction amount k_v[k]−k_p[k] may be calculated according to the principle similar to the above embodiment. Further, in the latter modification, the curvature correction amount K_v[k]−K_p[k] and its constraint condition are converted into the steering angle, so that the standard trajectory, the correction trajectory, and the target trajectory related to the steering angle may be calculated.

$\begin{array}{cc}\left[\mathrm{Equations}49\mathrm{and}50\right]& \\ \tilde{A}=\left[\begin{array}{c}O\\ O\end{array}\right]& 49\\ \tilde{B}=\left[\begin{array}{c}I\\ -I\end{array}\right]& 50\end{array}$

The following equation 51 is established between the traveling distance s[k] and the traveling speed v[k] which are the state quantities of the vehicle 3 and the acceleration a[k] which is the operation quantity of the vehicle 3. In the modification under the condition that the equation 51 is satisfied, the equation 17 is replaced with the equation 31 so that the standard trajectory, the correction trajectory, and the target trajectory regarding the travelling speed v[k] may be calculated according to the principle similar to the above embodiment. Alternatively, in a modification under the condition that the equation 51, is satisfied, the equation 23 and the equation 24 are replaced with the equation 49 and the equation 50, respectively, so that the standard trajectory, the correction trajectory, and the target trajectory relating to the acceleration a[k] may be calculated according to the principle similar to the above-described embodiment.

$\begin{array}{cc}\left[\mathrm{Equation}51\right]& \\ \left[\begin{array}{c}s\left[k+1\right]\\ v\left[k+1\right]\end{array}\right]=\left[\begin{array}{cc}1& \Delta \\ 0& 1\end{array}\right]·\left[\begin{array}{c}s\left[k\right]\\ v\left[k\right]\end{array}\right]+\left[\begin{array}{c}{\Delta }^2/2\\ \Delta \end{array}\right]·a\left[k\right]& 51\end{array}$

In the modification, in order to make the response characteristic of the correction trajectory X_m different from that of the standard trajectory X_b, when the variation matrix δM that changes the correction matrix M is defined, the following equation 52 using the positive parameter coefficient ε may perturb the correction matrix M within the range where equation 37 is satisfied. Also in this modification, based on the correction matrix M that makes the response parameter different from the standard by the perturbation, the correction vector λ that satisfies equations 40 to 45 is calculated, and the correction trajectory X_m of equation 38 is can be calculated using the result of the correction vector calculation.

(Equation 52)

M←M+ε·δM  52

The controllers and methods described in the present disclosure may be implemented by a special purpose computer created by configuring a memory and a processor programmed to execute one or more particular functions embodied in computer programs. Alternatively, the controllers and methods described in the present disclosure may be implemented by a special purpose computer created by configuring a processor provided by one or more special purpose hardware logic circuits. Alternatively, the controllers and methods described in the present disclosure may be implemented by one or more special purpose computers created by configuring a combination of a memory and a processor programmed to execute one or more particular functions and a processor provided by one or more hardware logic circuits. The computer programs may be stored, as instructions being executed by a computer, in a tangible non-transitory computer-readable medium.

It is noted that a flowchart or the processing of the flowchart in the present application includes sections (also referred to as steps), each of which is represented, for instance, as S101. Further, each section can be divided into several sub-sections while several sections can be combined into a single section. Furthermore, each of thus configured sections can be also referred to as a device, module, or means.

While the present disclosure has been described with reference to embodiments thereof, it is to be understood that the disclosure is not limited to the embodiments and constructions. The present disclosure is intended to cover various modification and equivalent arrangements. In addition, while the various combinations and configurations, other combinations and configurations, including more, less or only a single element, are also within the spirit and scope of the present disclosure.

Claims

1. A trajectory generator for generating a traveling trajectory in which a physical quantity of motion of a vehicle in future traveling is specified in time series, the trajectory generator comprising: a standard calculator that calculates a standard trajectory based on a standard response parameter as a traveling trajectory that does not consider a constraint condition in the future traveling;a correction calculator that calculates a correction trajectory based on a correction response parameter different from the standard trajectory as a traveling trajectory having a correction amount for correcting the standard trajectory in time series according to the constraint condition; anda target calculator that calculates a target trajectory to which the vehicle follows, as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory.

2. The trajectory generator according to claim 1, further comprising: one or more processors; anda memory coupled to the one or more processors and storing program instructions that when executed by the one or more processors cause the one or more processors to provide at least: a standard calculator; a correction calculator;and a target calculator.

3. The trajectory generator according to claim 1, wherein: the correction calculator calculates the correction trajectory so as to change a response characteristic to be steeper than the standard trajectory.

4. The trajectory generator according to claim 3, wherein: the correction calculator calculates the correction trajectory so as to provide a response characteristic for providing an operation amount with respect to the vehicle to be larger than the standard trajectory.

5. The trajectory generator according to claim 1, wherein: the correction calculator calculates the correction trajectory so as to present an N step vector as a condition for establishing an N step scheme in which a numerical number of time-series points on the correction trajectory becomes a maximum number of calculation steps for each constraint type of the future traveling.

6. The trajectory generator according to claim 1, wherein: the constraint condition is a condition for restricting a lateral position and a yaw angle of the physical quantity of motion of the vehicle relative to a traveling path.

7. The trajectory generator according to claim 1, wherein: the constraint condition includes a plurality of types of constraint; andthe correction trajectory is calculated based on the plurality of types of constraint.

8. A trajectory generating method executed by a processor for generating a traveling trajectory in which a physical quantity of motion of a vehicle in future traveling is specified in time series, the trajectory generating method comprising: calculating a standard trajectory based on a standard response parameter as a traveling trajectory that does not consider a constraint condition in future traveling;calculating a correction trajectory based on a correction response parameter different from the standard trajectory as a traveling trajectory having a correction amount for correcting the standard trajectory in time series according to the constraint condition; andcalculating a target trajectory to which the vehicle follows, as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory.

9. The trajectory generating method according to claim 8, wherein: the correction trajectory is calculated so as to change a response characteristic to be steeper than the standard trajectory.

10. The trajectory generating method according to claim 9, wherein: the correction trajectory is calculated so as to provide a response characteristic for providing an operation amount with respect to the vehicle to be larger than the standard trajectory.

11. The trajectory generating method according to claim 8, wherein: the correction trajectory is calculated so as to present an N step vector as a condition for establishing an N step scheme in which a numerical number of time-series points on the correction trajectory becomes a maximum number of calculation steps for each constraint type of the future traveling.

12. The trajectory generating method according to claim 8, wherein: the constraint condition is a condition for restricting a lateral position and a yaw angle of the physical quantity of motion of the vehicle relative to a traveling path.

13. The trajectory generating method according to claim 8, wherein: the constraint condition includes a plurality of types of constraint; andthe correction trajectory is calculated based on the plurality of types of constraint.

14. A non-transitory tangible computer readable storage medium comprising instructions being executed by a computer, the instructions including a computer-implemented method for generating a traveling trajectory in which a physical quantity of motion of a vehicle in future traveling is specified in time series, the instructions include: calculating a standard trajectory based on a standard response parameter as a traveling trajectory that does not consider a constraint condition in future traveling;calculating a correction trajectory based on a correction response parameter different from the standard trajectory as a traveling trajectory having a correction amount for correcting the standard trajectory in time series according to the constraint condition; andcalculating a target trajectory to which the vehicle follows, as a traveling trajectory in which the correction trajectory is superimposed on the standard trajectory.

15. The non-transitory tangible computer readable storage medium according to claim 14, wherein: the correction trajectory is calculated so as to change a response characteristic to be steeper than the standard trajectory.

16. The non-transitory tangible computer readable storage medium according to claim 15, wherein: the correction trajectory is calculated so as to provide a response characteristic for providing an operation amount with respect to the vehicle to be larger than the standard trajectory.

17. The non-transitory tangible computer readable storage medium according to claim 14, wherein: the correction trajectory is calculated so as to present an N step vector as a condition for establishing an N step scheme in which a numerical number of time-series points on the correction trajectory becomes a maximum number of calculation steps for each constraint type of the future traveling.

18. The non-transitory tangible computer readable storage medium according to claim 14 wherein: the constraint condition is a condition for restricting a lateral position and a yaw angle of the physical quantity of motion of the vehicle relative to a traveling path.

19. The non-transitory tangible computer readable storage medium according to claim 14, wherein: the constraint condition includes a plurality of types of constraint; andthe correction trajectory is calculated based on the plurality of types of constraint.