METHOD OF RECONSTRUCTING A VEHICLE TRAJECTORY

Abstract

A method for generating a vehicle trajectory by optimizing a performance measure J. The trajectory may include a sequence of states x=(x.sub.k).sub.k=1.sup.N of the vehicle. The optimization is subject to predefined vehicle dynamics x.sub.k+1=f(x.sub.k, u.sub.k), where u.sub.k is a control input to the vehicle, and a condition that each position of the vehicle shall be close to a reference path X.sup.r. The vehicle's position is constrained inside a variable-width corridor around the reference path. A quantity r controlling the width of the corridor is included as an additional optimization variable and the performance measure includes a penalty on the corridor width. To define the corridor, each point of the reference path may be associated with a pair of laterally spaced ellipses and requiring each vehicle position to be outside the ellipses.

Claims

1. A method of generating a vehicle trajectory, which comprises a sequence of states x=(x.sub.k).sub.k=1.sup.N of the vehicle, by optimizing a performance measure J, wherein the optimization is subject to predefined vehicle dynamics x.sub.k+1=f(x.sub.k, u.sub.k), where u.sub.k is a control input to the vehicle, and a condition that each position of the vehicle, X.sub.k=h(x.sub.k, u.sub.k), shall be close to a reference path X.sup.r=(X.sub.l.sup.r).sub.l=1.sup.M, the method comprising: constraining the vehicle's position inside a corridor with variable width around the reference path X.sup.r; including, as an additional optimization variable, a quantity r=(r.sub.l).sub.l=1.sup.M controlling the width of the corridor; and including a penalty on the corridor width in the performance measure.

2. The method of claim 1, wherein said constraining comprises: delimiting the corridor laterally by associating each point X.sub.l.sup.r of the reference path with a pair of ellipses with variable radius (r.sub.l).sub.l=1.sup.M, wherein the radius is said quantity defining the width of the corridor; and requiring each vehicle position X.sub.k to be outside the ellipses.

3. The method of claim 2, further comprising enforcing a least radius r.sub.min of the ellipses.

4. The method of claim 1, wherein a gradient-based optimization solver is used, the method further comprising: initializing the vehicle trajectory such that the vehicle position X.sub.k for each state x.sub.k is inside the corridor.

5. The method of claim 1, further comprising: generating a sequence of control inputs u=(u.sub.k).sub.k=1.sup.N to the vehicle.

6. The method of claim 5, further comprising: controlling the vehicle by applying the sequence of control inputs.

7. The method of claim 1, further comprising: generating the sequence of states x=(x.sub.k).sub.k=1.sup.N by optimizing the performance measure J; and after completion of said generating, controlling the vehicle on the basis of the sequence of states.

8. The method of claim 7, wherein the penalty on the corridor width is enforced collectively for the full corridor.

9. The method of claim 1, which is performed offline.

10. The method of claim 1, wherein the performance measure J is a function of the sequence of states x and the sequence of control inputs u, if any.

11. The method of claim 10, wherein the performance measure J is dependent on one or more of: mission duration, a safety indicator, trajectory smoothness, position trajectory smoothness, deviation from the reference path X.sup.r, steering angle rate, lateral or longitudinal jerk, lateral or longitudinal acceleration.

12. The method of claim 1, wherein the vehicle dynamics f and/or the performance measure J depends on at least one environmental factor.

13. The method of claim 1, wherein the vehicle dynamics is non-holonomic.

14. The method of claim 1, wherein the reference path X.sup.r has a discrete-time representation and the optimization is configured to provide the sequence of states independently of said discrete-time representation.

15. The method of claim 1, further comprising one of: recording the reference path X.sup.r by sensing positions of an operating vehicle; defining the reference path X.sup.r using an operator interface.

16. The method of claim 1, wherein the vehicle is a vehicle combination.

17. A device for generating a vehicle trajectory, which comprises a sequence of states x=(x.sub.k).sub.k=1.sup.N of the vehicle, by optimizing a performance measure J, the device comprising processing circuitry configured to execute the method of claim 1.

18. A computer program comprising instructions which, when executed, cause a processor to execute the method of claim 1.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Aspects and embodiments are now described, by way of example, with reference to the accompanying drawings, on which:

[0024] FIG. 1 is a flowchart of a method for generating a vehicle trajectory;

[0025] FIG. 2 is a device for generating a vehicle trajectory, coupled to an optional control interface for exchanging instructions and sensor data with a vehicle;

[0026] FIG. 3 illustrates a reference path, a position trajectory generated using an embodiment of the invention (which approximately coincides with the reference path), as well as sequences of circles which delimit the constraint corridor laterally;

[0027] FIG. 4A is a comparison of an articulation angle sensed during the recording of a reference path and the articulation angle according to a generated vehicle trajectory;

[0028] FIG. 4B shows a steering angle according to the same vehicle trajectory, which was generated on the basis of a reference path for which no steering-angle data was recorded; and

[0029] FIG. 5 illustrates, similar to FIG. 3 but for different settings, a reference path, a generated position trajectory (which deviates from the reference path, yet without departing from the corridor), as well as sequences of circles which delimit the constraint corridor laterally.

DETAILED DESCRIPTION

[0030] The aspects of the present disclosure will now be described more fully hereinafter with reference to the accompanying drawings, in which certain embodiments of the invention are shown. These aspects may, however, be embodied in many different forms and should not be construed as limiting; rather, these embodiments are provided by way of example so that this disclosure will be thorough and complete, and to fully convey the scope of all aspects of the invention to those skilled in the art. Like numbers refer to like elements throughout the description.

[0031] With reference to FIG. 1, there will now be described a method 100 for generating a vehicle trajectory x on the basis of a reference path X.sup.r=(X.sub.l.sup.r).sub.l=1.sup.M. The vehicle is modeled as a dynamical system which, in this example, is described in discrete time, wherein physical (continuous) time is segmented into epochs of equal duration which are represented by values of a discrete variable k. The model includes a definition of a state x.sub.k, a control input u.sub.k, vehicle dynamics x.sub.k+1=f(x.sub.k, u.sub.k) and a position operator h which maps each state x.sub.k to a position X.sub.k. The position operator h may optionally depend on the control input u.sub.k and it may optionally output a further quantity, such as the vehicle's heading. The vehicle dynamics f may be represented by a linear or nonlinear function; the vehicle dynamics f may in particular be non-holonomic. The state x.sub.k comprises relevant aspects of the vehicle's current condition. It may be composed of the vehicle's position, orientation, speed or another kinematic quantity, and may further comprise internal parameters such as steering angle, articulation angle (of a vehicle combination) and other conditions prevailing in a technical system in the vehicle, including the drivetrain. Similarly, a control input u.sub.k may represent a momentary actuation of a technical system in the vehicle, including accelerating, braking, increasing/decreasing a steering angle.

[0032] Accordingly, the trajectory may comprise a sequence x=(x.sub.k).sub.k=1.sup.N of states x.sub.k of the vehicle. From this, a position trajectory X=(X.sub.k).sub.k=1.sup.N, where X.sub.k=h(x.sub.k, u.sub.k), can be derived using the position operator h. As will be explained in detail below, the trajectory is generated by optimizing a performance measure J, which is a function of the sequence x of states and optionally of the control inputs u. The performance measure J may depend on one or more of: [0033] mission duration, which may be measured as the time needed to complete a representative utility task, [0034] a safety indicator, such as a collision probability, minimum vehicle separation, dwelling time in share sites such as intersections, etc., [0035] trajectory smoothness and position trajectory smoothness, as measured by peak rate of change, total variation, continuity of derivatives and similar metrics, [0036] deviation from the reference path X.sup.r, [0037] steering angle rate, lateral or longitudinal jerk, lateral or longitudinal acceleration, wherein the peak or average values may be indicative of the degree of mechanical wear on the vehicle.
The performance measure J, just like the vehicle dynamics f, may further depend on at least one environment factor. This may be used to express that a different driving behavior may be preferred in poor visibility, frost, wind, or when temporary regulations (e.g., roadworks) are in force. This also underlines the importance of being able to regenerate the trajectory conveniently.

[0038] The method 100 may be implemented in an onboard or offboard system. In particular, the method 100 may be carried out by the device 200 shown in FIG. 2, which comprises an input interface 210, processing circuitry 220 and an output interface 230. The input interface 210 is configured to receive parameters specifying the optimization problem to be solved, such as the reference path X.sup.r, performance measure J, as well as vehicle dynamics f and a position operator h relating to a vehicle 299 which is to execute the generated trajectory x. The vehicle 299 may be rigid or may constitute a vehicle combination with two or more units. The trajectory x and control inputs u are provided at the output interface 230. In FIG. 2, the device 200 provides its output to a control interface 240 configured to forward at least the sequence of control inputs u to the vehicle 299, e.g., using a wireless transceiver. If the vehicle 299 is autonomous, the control inputs u may be applied directly to technical systems therein. Alternatively, if the vehicle 299 is at least in part conventionally maneuvered, the control inputs u may be presented in human-intelligible form (e.g., on a driver display, by audio) or may be applied to a driver assistance system (e.g., haptic steering-wheel assistance). During execution, the control interface 240 or circuitry in the vehicle 299 may verify that the vehicle's 299 actual state evolves approximately according to the sequence x.

[0039] In a first step 110 of the method 100, the reference path X.sup.r is provided. This may be performed by recording 110.1 the reference path X.sup.r as a sequence of sensed positions of an operating vehicle. The vehicle which is used for the recording need not be the same the vehicle as that for which the method 100 is performed. Indeed, it is one of the objectives of the present disclosure to be able to generate a trajectory from a reference path using freely configurable vehicle dynamics. The reference path can be recorded directly in the form of positions X.sub.k.sup.r or in the form of states x.sub.k (and control inputs u.sub.k) of the recording vehicle, which can be converted into positions by X.sub.k.sup.r=h(x.sub.k, u.sub.k). Alternatively, the reference path X.sup.r is defined 110.2 using an operator interface, e.g., using a geographical information system or drawing tool.

[0040] In a second step 112 of the method 100, a constraint that the vehicle position shall be inside a corridor with variable width around the reference path X.sup.r is added to the optimization problem. The optimization problem may then be formulated, at least conceptually, as:

[00001]$\begin{array}{cc}\begin{array}{c}\underset{x,u}{\max }J\left(x,u\right)\\ \mathrm{subject}\mathrm{to}\end{array}& \left(1\right)\end{array}$$\begin{array}{cc}{x}_{k+1}=f\left(x_k,u_k\right)& \left(2\right)\end{array}$$\begin{array}{cc}{X}_k\mathrm{inside}\mathrm{corridor}\mathrm{around}{X}^r\mathrm{for}\mathrm{all}k,& \left(3\right)\end{array}$

where the position X.sub.k=h(x.sub.k, u.sub.k) as seen above. Accordingly, the performance measure J is used as the objective function in the optimization problem. The width of the corridor may be variable, and it may vary with the value of a width-controlling quantity. The width may also be different in different segments of the corridor, that is, it may have a variation along X.sup.r. It is emphasized that condition (3) is independent from the above-discussed option of letting the performance measure depend on the position trajectory's deviation from the reference path X.sup.r; that option and the enforcement of condition (3) rather constitute alternative ways to achieve the common goal of fitting the trajectory to the reference path X.sup.r.

[0041] In one embodiment, the corridor is delimited 112.1 laterally by associating each point X.sub.l.sup.r of the reference path with a pair of ellipses with variable radius (r.sub.l).sub.l=1.sup.M and requiring 112.2 each vehicle position X.sub.k to be outside the ellipses. The radius r.sub.l may be a scalar or a vector with two components representing the minor and major axes of each ellipse, e.g., r.sub.l=(r.sub.l,minor, r.sub.l,major). Accordingly, when a least radius r.sub.min of an ellipse is enforced, both the minor and major axes are required to be greater than or equal to r.sub.min. FIG. 5 illustrates the special case where the ellipses are circles, spaced apart by a distance 2R from the reference path point X.sub.l.sup.r in directions normal to the momentary heading θ.sub.l.sup.r of the reference path. This is to say, the circles are located in directions θ.sub.l.sup.r±π/2 as seen from the reference path point X.sub.l.sup.r. The heading θ.sub.l.sup.r may be recorded as a component of the reference path (e.g., using a heading sensor), a modified position function h which computes the heading from the recorded vehicle state may be used, or the heading may be computed in a per se known manner based on a vector difference of X.sub.l.sup.r and one or both of its adjacent reference path points X.sub.l±1.sup.r. It holds that

[00002]$\begin{array}{c}{X}_l^{1c}=X_l^r+R\left[\begin{array}{c}\cos \left({\theta }_l^r+\frac{\pi }{2}\right)\\ \sin \left({\theta }_l^r+\frac{\pi }{2}\right)\end{array}\right]\\ X_l^{2c}=X_l^r+R\left[\begin{array}{c}\cos \left({\theta }_l^r-\frac{\pi }{2}\right)\\ \sin \left({\theta }_l^r-\frac{\pi }{2}\right)\end{array}\right]\end{array}$

With this notation, condition (3) may be formulated as

∥X.sub.k−X.sub.l.sup.1c∥.sup.2≥r.sub.l.sup.2  (3a)

∥X.sub.k−X.sub.l.sup.2c∥.sup.2≥r.sub.l.sup.2  (3b)

for all k, l, where ∥Ψ∥ is any suitable norm, such as l.sup.1 or l.sup.2 (Euclidean) norm. For Y∈R.sup.2, the l.sup.2 norm is defined as ∥Y∥.sup.2=y.sub.1.sup.2+y.sub.2.sup.2. With this, inequalities (3a) and (3b) are quadratic if written on component form. It is optional to enforce 114.1 a least radius r.sub.min as per

r.sub.min≤r.sub.l≤R.  (3c)

This is equivalent to a condition that the corridor width must not exceed 2(R−r.sub.min) at the level of any of the reference path points X.sub.l.sup.r.

[0042] The radius r=(r.sub.l).sub.l=1.sup.M is included 114 as an optimization variable. This is to say, expression (1) includes a performance measure J(x, u, r) which is a function of the vehicle states x, control inputs u and the radius r. More precisely, the performance measure J includes 116 a penalty to the corridor width, in the sense that the performance measure increases if the corridor is narrowed for constant x, u. If the corridor is defined according to inequalities (3a) and (3b), the performance measure may be expected to increase with an increase in r.sub.l, since the prohibited circles will then occupy more lateral space and coerce the position trajectory X towards the reference path X.sup.r to a greater extent.

[0043] In a fifth step 118 of the method 100, the vehicle trajectory is initialized with an initial sequence of states x.sup.(0)=(x.sub.k.sup.(0)).sub.k=1.sup.N such that the vehicle position X.sub.k.sup.(0)=h(x.sub.k.sup.(0), u.sub.k.sup.(0)) is inside the corridor around the reference path X.sup.r. Here, if the position function h depends on the control input, the control inputs u.sup.(0)=(u.sub.k.sup.(0)).sub.k=1.sup.N may be initialized with neutral values, i.e., no active maneuvering is undertaken. This condition may be formulated using inequalities (3a) and (3b). It is emphasized that the discrete representations of the reference path X.sup.r=(X.sub.l.sup.r).sub.l=1.sup.M and the sequence of states x=(x.sub.k).sub.k=1.sup.N are independent; there is no necessity to synchronize the points (e.g., a pairwise correspondence between x.sub.k and X.sub.l.sup.r), nor to initialize x with the same number of states as there are reference path points (M=N).

[0044] In a sixth step 120 of the method 100, the optimization problem (1), (2), (3) or the problem (1), (2), (3a), (3b), with the optional addition of (3c), is solved. The solution includes the sequences x, u of vehicle states and control inputs. For the solving, a gradient-based solver may be used. Just like the discrete representations of the states x and the reference path X.sup.r are independent, as explained, the optimization solver is preferably configured to provide the sequence of states x independently of the individual points in X.sup.r. This may mean that the position trajectory is coerced towards the reference path laterally but not longitudinally. In particular, inequalities (3a) and (3b) may be enforced collectively for all 1≤k≤N, 1≤l≤M rather than in a pointwise fashion.

[0045] It is not essential for the present invention whether the conditions (2) and (3) are expressed as constraints in the strict sense or are included in the performance measure J. Indeed, there are well known techniques for converting a constraint into a term in the objective function; such term may be a barrier function or indicator function assigning a penalty to values violating the constraint. There are also ways to translate a component of the objective function into one or more constraints, including linearization techniques and preconditioning techniques.

[0046] In an optional further step 122 of the method 100, a vehicle 299 is controlled by applying the generated sequence of control inputs u. Optionally, the controlling may include a concurrent monitoring of the vehicle's compliance with the generated trajectory x. Preferably, the vehicle 299 has identical or approximately corresponding vehicle dynamics as those expressed by equation (2) in the optimization problem. Such correspondence will ensure that the generated trajectory can be successfully executed by the vehicle 299.

[0047] FIG. 3 illustrates a reference path representing movement on a surface and a position trajectory which was generated, in a pilot study, by applying an embodiment of the method 100 to this reference path and applying the vehicle dynamics f of a vehicle combination. FIG. 3 further shows sequences of circles which delimit the constraining corridor laterally. Here, the performance measure J is defined in such manner as to penalize the position trajectory's deviation from the reference path X.sup.r, and it is seen in the figure that the resulting position trajectory approximately coincides with the reference path.

[0048] Still referring to the same pilot study, FIG. 4A is a comparison of the vehicle combination's articulation angle sensed during the recording of the reference path and the articulation angle according to the generated vehicle trajectory. It is noted that the angles are plotted as functions of curve length s, i.e., the vehicle's position along the path. FIG. 4B for its part shows the steering angle (again as a function of curve length) for the same vehicle trajectory. Because no steering-angle data was recorded with the reference path, no comparison like in FIG. 4A is possible. This illustrates that embodiments of the present invention may allow the reconstruction of unknown (unrecorded) vehicle states.

[0049] FIG. 5 illustrates, similar to FIG. 3 but for a different definition of the performance measure, a reference path, a generated position trajectory, as well as sequences of circles which delimit the constraint corridor laterally. The performance measure differs from that used in FIGS. 3-4 in that it does not award a penalty if the position trajectory deviates from the reference path X.sup.r. It is seen that the position trajectory deviates locally from the reference path, yet without departing from the corridor.

[0050] The foregoing sections of the present disclosure present a method for constructing a trajectory from a recorded reference path tailored for autonomous vehicles operating in closed environments using optimizing. The method expands on certain homotopy techniques previously proposed within numerical optimal control and utilize these to compute optimized trajectories that trade-off between tracking the recorded reference paths, removing unwanted disturbances embedded in the recorded reference paths, partly modifying the reference paths and (re)constructing data not included in the recordings. The method may be used to enhance safety, increase uptime and productivity and to enable adaptation based on, e.g., different types of vehicles as well as different weather conditions. Consequently, the method has the potential to improve the overall performance of an autonomous vehicle fleet.

[0051] The aspects of the present disclosure have mainly been described above with reference to a few embodiments. However, as is readily appreciated by a person skilled in the art, other embodiments than the ones disclosed above are equally possible within the scope of the invention, as defined by the appended patent claims. The invention can be generalized to three dimensions, which may be utilized to generate a trajectory for an aerial or submarine vessel. In three dimensions, the corridor around the reference path is preferably delimited by associating each point of the reference path with a torus having variable minor radius and/or variable major radius. Just like a pair of ellipses (which may be understood as the intersection between the torus and a ground vehicle's plane of movement), the condition that the vehicle position shall be outside the tori can be expressed as a quadratic constraint in the optimization problem, which therefore remains well posed.