The work leading to the present invention has benefited from financial support from the coal and steel research funds of the European Community by virtue of grant agreement No. 607957.
The invention relates in a general manner to a device for real-time control of vehicle trajectory. The invention relates more precisely to a lateral control device for generating in real time a vehicle turning command, in particular for an autonomous automotive vehicle or a vehicle for which driving is partially delegated, that is to say a vehicle able to move totally or partially with no human driver.
Devices or systems for real-time control of trajectory, in particular on the basis of states relating to the vehicle, are already known in numerous technical fields, including in the automotive field.
For example, patent EP1074903B1 discloses a system for following a lane with detection of marking of the lane. A direct-chain action module produces a steering driving term based on a target line position with the help of a graphics processor. The elements disclosed allow an application of this system to a rectilinear lane, or indeed one with low-curvature bends with no noticeable influence on the vehicle's dynamics, to be envisioned. An application to a lane with high-curvature bends, as might be encountered for example on mountain roads, can pose a problem of dynamic reactivity of the vehicle in order to maintain itself flexibly in the lane.
Again for example, patent U.S. Pat. No. 8,751,089B2 discloses a trajectory following control system for mobile unit. The system controls a mobile unit so as to follow a target path described by free-form curves. Though this type of system may be appropriate for piloting a robot in a space with no constraint of such a nature as to prevent the freedom of form of the curves that are appropriate for a target path, it may pose problems for an autonomous vehicle whose piloting does not consist in making it follow a target path but in maintaining it in a traffic lane, irrespective of the form of the curves that are imposed by the geometry of the lane.
The objective of the invention is to address the problems posed by the prior art, in particular in terms of effectiveness, reactivity and ease of implementation.
To attain this objective, the subject of the invention is a device for real-time control of trajectory of an autonomous vehicle comprising a control module which produces in real time on the basis of a real or estimated vector of states at each instant of following of a lane by the vehicle moving at a current speed, a first turning command to stabilize the trajectory of the vehicle with respect to the lane. The device is noteworthy in that it comprises an anticipator module which generates a quantity representative of a meta-vector of disturbances for each predicted position of the vehicle at instants succeeding in given amount the current instant and of the vector of states at the current instant, and in that the control module produces the first turning command by quadratic optimization of a relation between said representative quantity generated and a meta-vector of successive turning commands for each predicted position of the vehicle at said instants succeeding in given amount the current instant.
Particularly, said disturbances are curvatures of the lane for each predicted position of the vehicle.
Particularly also, said representative quantity comprises, for each predicted position of the vehicle at said instants succeeding in given amount the current instant, at least one product of the vector of states at the current instant by a numerical matrix modeling a dynamic relation of the vehicle and raised to a power equal to a succession rank of said predicted position.
Advantageously, said representative quantity comprises, for each predicted position of the vehicle at said instants succeeding in given amount the current instant, at least one product of factors comprising on the one hand a disturbance on the lane at the predicted position, and on the other hand a numerical matrix modeling a dynamic relation of the vehicle and raised to a power equal to a succession rank of said predicted position.
Again particularly, the device comprises an apparatus combining the properties of an optical camera and of a radar so as to provide at least one director line geometry of the lane in the form of a polynomial.
More particularly, the anticipator module comprises a sub-module for calculating curvature on the basis of the median director line geometry of the lane by means of the formula:
Preferably, the relation between said representative quantity generated and the meta-vector of successive turning commands lo comprises, for each quadratic product of an anticipated state, an elementary states weighting matrix and an elementary command weighting matrix.
In particular, the elementary states weighting matrix for the last anticipated state comprises coefficients which are particular in that they are associated with a decreasing positive function of the Lyapunov type, so as to favor a convergence of the optimization.
Independently of the particular coefficients of the elementary states weighting matrix, the device comprises a module compelling all or some of the elementary commands of the meta-vector of successive turning commands to comply with constraints of type comprising at least one upper bound on the states, on the commands and/or on the measurements and/or comprising at least one lower bound on the states, on the commands and/or on the measurements.
Again advantageously, the control device comprises a module imposing constraints on all or some of the elementary commands of the meta-vector of successive turning commands, of type comprising an upper bound and a lower bound on the states, on the commands and/or on the measurements.
Particularly, the module moreover imposes constraints on the elementary commands of the meta-vector of successive turning commands, so that the last predicted state vector is in a polytope which ensures asymptotic convergence beyond the prediction horizon.
Other characteristics and advantages will become apparent on reading the description which follows, with reference to the appended drawings in which:
With reference to
The vehicle 1 comprises a steering column 44 whose top part is optionally equipped with a steering wheel, the latter possibly becoming unnecessary for a purely autonomous vehicle, and whose bottom part acts on a steering member known per se which makes it possible to direct the front wheels. The steering column 44 is equipped with an actuator piloted by a command signal u. The steering member is equipped with a sensor 46 at the base of the steering column or at any other location, for example with a rack acting on the front wheels, to measure an actual turning angle δ of the front wheels of the vehicle. The sensor 46 is for example a sensor of torque whose value is readily converted into turning angle.
The vehicle 1 also comprises a sensor 31 of the yaw rate {dot over (ψ)}=∂Ψ/∂t of the vehicle, that is to say of the rate of rotation of the vehicle about its center of gravity along an axis perpendicular to the plane of the carriageway on which the vehicle is traveling. The sensor 31 is for example a gyrometer situated very close to the center of gravity (CoG) of the vehicle. An apparatus 15 of combined radar and camera type, that is to say combining the properties of an optical camera and of a radar, makes it possible to measure coordinates of objects YCAM which are transverse to a median axis of the vehicle and XCAM along the median axis forward of the vehicle 1.
As known moreover to the supplier of the apparatus 15, fusing the optical measurements and radar measurements makes it possible to detect, for each marking point on the carriageway, the distance x of this point with respect to the front face of the apparatus 15 along its viewing axis and the distance y of this same point with respect to the viewing axis. An image processing, outside of the framework of the present invention, allows the apparatus 15 to provide a geometry y(x) [vehicle-line lateral distance] of at least one director line of the traffic lane of the vehicle in the form of a polynomial, for example of the demarcation line on the left and of the demarcation line on the right of the lane or of the median line of the lane at the center of the two demarcation lines. For a considered director line, the polynomial has for example the following form:
y=p3(t)·x3+p2(t)·x2+p1(t)·x+p0(t).
where for example the coefficient p0(t) corresponds to the vehicle-line lateral distance at the center of gravity.
The vehicle 1 is equipped in a manner known per se with an on-board computer (not represented as such) making it possible to control and to command various members of the vehicle. The computer can receive through connections, in particular via a CAN, Lin or automobile Ethernet bus or other on-board network, information originating from the sensors of longitudinal speed, from the sensor 31 of yaw rate, from the sensor 46 of the turning angle δ and from the apparatus 15. The on-board computer can also control the steering column 44 by communicating a command signal u to it. The on-board computer can host furthermore a controller device 17 to generate the command signal u so as to render a physical state vector of the vehicle compliant with a setpoint state vector ξ* which ensures that the vehicle 1 follows a desired trajectory. The physical states of the vehicle depend on numerous more or less well mastered physical data which act on its dynamic behavior. It is possible to determine in real time the vector ξk of physical states at each instant k when the vehicle is equipped with at least one sensor making it possible to measure each physical state. When the vehicle does not have a sensor for each controlled state, the on-board computer hosts an observer device 2 to generate in real time a vector of estimated states {circumflex over (ξ)} of trajectory following of the vehicle 1 moving at the current speed v, on the basis of the command u and of a current measurement vector η correlated with the vector of physical states ξ of trajectory following of the vehicle 1, corresponding to the lateral dynamics of the vehicle, which are measurable and non-measurable per se, as explained now with reference to
In the embodiment illustrated by
The state variables can have a meaning independent of the mode in which the vehicle is operating or a meaning specific to the mode in which the vehicle is operating. The yaw rate, which is the rate at which the vehicle pivots about an axis perpendicular to the plane of the to carriageway, is independent of the mode of operation. The same holds for the actual variation {dot over (δ)}ef=∂δ/∂t of angle of turning of the wheels of the vehicle over time, and for the actual turning angle δef of the wheels since these variables are related to the state of the wheels themselves with respect to the chassis of the servocontrolled vehicle. On the other hand the actual divergence Ycog,ef of the center of gravity of the servocontrolled vehicle with respect to its ideal trajectory, is the lateral deviation between the median line or director line of the lane and the center of gravity of the servocontrolled vehicle on the axis perpendicular to the axis of the vehicle in LCA (Lane Centering Assistance) mode. The actual lateral rate {dot over (Y)}CoG,ef=∂YCoG/∂t of divergence of the center of gravity is the temporal variation of the previous variable.
The actual state vector ξ of the vehicle 1 can also comprise scalar coordinates which differ in number and/or in nature according to the mode of operation of the vehicle. Returning to the illustrative example hereinabove, an optional state variable ∫−YCOGdt can represent a temporal integral of the deviations of the center of gravity of the servocontrolled vehicle with respect to the point on which it ought to be of the director line of the lane. In fact this state variable does not correspond to any real physical state, it is rather an artefact inherited from the closed loop servocontrols of PID type in which the integral component of the gain makes it possible to obtain a non-zero output of the amplifier for a zero error applied to its input, the technical effect of which is to impose a zero error, here on the actual divergence Ycog,ef of the center of gravity of the vehicle with respect to the median line of the traffic lane.
To this actual state vector ξ of the vehicle 1 there corresponds a reference state vector ξ* that one desires be attained at each instant of running of the servocontrolled vehicle. For example to the coordinates {dot over (ψ)}ef, Ψrel,ef, {dot over (Y)}CoG,ef, YCoG,ef of the state vector ξ there correspond coordinates {dot over (ψ)}ref, Ψref, {dot over (Y)}CoG,ref, YCoG,ref of the state vector ξ* of zero values since an absence of deviation of the actual trajectory of the servocontrolled vehicle from its ideal trajectory, for example the center of the lane, is of course desired.
The observer device 2 is particularly useful when the actual state vector ξ of the vehicle 1 is unknown because the internal state of the servocontrolled vehicle, governed by the natural laws of physics, comprises all or some of the inaccessible and non-measurable state variables.
It is recalled that the known laws of physics say that a vector {dot over (ξ)} of temporal evolution of the state vector ξ of the physical system consisting of the vehicle 1 is related to the state vector ξ by a dynamic relation Ard which, in the absence of exterior disturbance, tends generally to lead the state of the vehicle to a stable final state. The stable final state of the vehicle not necessarily being that which it is sought to attain, the aim of the invention is to produce a command u which permanently disturbs the system so as to maintain the actual state vector ξ in accordance with the reference state vector ξ*. The disturbance thus caused on the temporal evolution vector {dot over (ξ)} is related to the command u by an invasive relation Bri, likewise governed by the laws of physics. Other exterior disturbances, of globally unknown nature, also acting on the physical system consisting of the vehicle 1, a negative-feedback servocontrol mechanism is employed, in which the aim of the observer device 2 is to generate in real time, at each current instant indexed k, an estimated state vector {circumflex over (ξ)}k representing the actual state vector ξ as faithfully as possible.
To attain its aim, the observer device 2 comprises a module 4 which models the physical system consisting of the vehicle 1, in particular in terms of trajectory following. The module 4 comprises a calculated numerical matrix A(v), calculated in particular as a function of the speed v of the vehicle, and modeling the dynamic relation Ard and a numerical matrix Bs representative of the invasive relation Bri.
The numerical matrix A(v) can have various forms, each suitable for a mode of operation of the vehicle. The form of the numerical matrix A(v) is essentially related to the definition of the state vector ξ of the vehicle 1 and to the dynamic relation applied. The exemplary form set forth hereinbelow is based on the bicycle model well known in the technical field considered, as illustrated by numerous prior documents such as FR2925005 or WO2012084465.
The example considered here is that of the LCA mode, for which for example to the state vector
there corresponds the numerical matrix A(v) given by the following formula 101:
In which certain coefficients are variable and others are constant in the course of one and the same running sequence.
The variable coefficients are essentially those which depend on a speed, in particular on the longitudinal speed v of the vehicle. To adapt to the speed variations, the variable coefficients are recalculated in real time by reading on the one hand the speed v of the vehicle 1, in particular the longitudinal speed v generally accessible on the on-board network (CAN, LIN or automobile Ethernet Bus or other type) and on the other hand parameters generally accessible in memory, by means of the formulae 102 to 105 in the following order:
in which the parameters cr, cf, lr, lf, Iz, M each quantify in a manner known per se elsewhere (cf. filings FR1255068, FR1255188, or FR12563339) respectively the drift stiffness of the rear wheels 12 and the drift stiffness of the front wheels 11 obtained from the supplier of the wheel tires or by means of on-circuit running trials, the distance of the axis of the rear wheels and the distance of the axis of the front wheels with respect to the center of gravity CoG of the vehicle 1, the moment of inertia of the vehicle about the perpendicular to the plane passing through the axes of the front and rear wheels, and finally the mass of the vehicle.
The coefficients which are constant in the course of a running sequence can be precalculated during the design of the vehicle and then stored in on-board computer memory. They are determined by the formulae 106 to 111 in the following order:
In which on the one hand μ and ω designate respectively a damping coefficient and a natural angular frequency of the transfer function of the command for turning the wheels which models the dynamics of the steering column.
The coefficients which are constant in the course of a running sequence can also be calculated each time the vehicle is started up so as to take account of a mass M and of a moment of inertia Iz which may be different at each startup as a function of the number of passengers and of the loading. The variations of these parameters, caused for example by the consumption of fuel or a passenger alighting from the vehicle while en route, are generally quite insignificant.
The unitary coefficients exist on the row corresponding to the time derivative of a state variable which is itself a state variable of rank equal to that of the column of the matrix.
The other coefficients are zero, with the exception of the last column where they are all zero and of the last row where only the median coefficient is not zero but equal to −1 so as to artificially reproduce a PID type conventional linear servocontrol negative feedback.
The number of columns of the matrix is equal to the number of coordinates of the state vector and the number of rows is equal to the number of coordinates of the state temporal variation vector, that is to say equal to the number of rows. It will be understood that the numerical matrix A(v) can comprise different numbers of rows and columns from those of the examples hereinabove, in particular a greater number if the need makes itself felt to consider an additional state variable or lower if a state variable is considered unnecessary.
The numerical matrix Bs representative of the invasive relation Bri is suitable for the mode of command of the vehicle and of taking into account exterior factors. The form of the numerical matrix Bs is essentially related to the state vector ξ, the number of coordinates of which fixes the number of rows and to the interactions with the system, the amount of which fixes the number of columns. The main example of form set forth hereinbelow corresponds to the example set forth hereinabove.
For the LCA mode, the numerical matrix Bs is given by the following formula 112:
in which the single column is associated here with the first disturbance which is the turning command u. The coefficient b51 is here again equal to the square of the natural angular frequency ω of the transfer function of the wheels turning command.
To model the vehicle 1, the module 4 receives the command uk−1 at each discrete sampling instant k so as to generate the estimated state vector {circumflex over (ξ)}k by reproducing the equations of dynamics by means of the numerical matrices A(v) and Bs intervening in formulae 113 and 114 respectively:
{circumflex over (ξ)}=∫{circumflex over ({dot over (ξ)})}dt
{circumflex over ({dot over (ξ)})}=A(v){circumflex over (ξ)}+Bsuk−1
Several conditions ought to be satisfied in order that the estimated state vector {circumflex over (ξ)} faithfully reproduces the physical state vector ξ of the vehicle.
In formula 113 which is an integral with respect to time, it would be necessary that, at an initial instant, the estimated state vector {circumflex over (ξ)} be equal to the physical state vector ξ of the vehicle.
In formula 114, it would be necessary that the numerical matrices A(v) and Bs perfectly model the dynamic relation Ard and the invasive relation Bri which actually impact the vehicle.
It is clearly appreciated that these conditions cannot be satisfied for numerous reasons such as the accuracy of the parameters which qualify the vehicle although they are subject to manufacturing dispersions and to aging from usage, the unforeseen disturbances and many other known or unknown reasons except of course by equipping the vehicle with sensors in sufficient amount to measure each real state of the vehicle.
To reduce the deviation between the estimated state vector {circumflex over (ξ)} which is calculated, and the physical state vector ξ of the vehicle at the same instant k which is unknown, the observer 2 receives, on a second input, a vector η of current measurements which are representative of the state of the vehicle. The vector η of current measurements is correlated with the physical state vector ξ through an instrumental relation Cri which depends on the configuration of the state vector, that is to say on the mode of operation of the vehicle, and the measurement sensors with which the vehicle is equipped.
Returning to the example for the LCA mode of operation, the vector η of current measurement comprises five components which are the yaw rate {dot over (ψ)} such as is measured by the sensor 31, the angle Ψrel of relative deviation such as is measured by the apparatus 15 sighting a median line of the traffic lane, the radial divergence YCoG of the center of gravity with respect to the median line of the traffic lane such that this divergence is obtained on the basis of the apparatus 15 or of the on-board computer, the angle δ of turning of the wheels 11 such as is measured by the sensor 46 and the accumulation ∫YCoGdt over time of the opposite of the divergence YCoG of the center of gravity provided by the apparatus 15. By regarding the duration dt as the sampling period for each instant k, this pseudo measurement is obtained through the very simple formula 115:
└∫YCoGdt┘k=└∫YCoGdt┘k−1+[YCoG]k initialized to zero.
The instrumental relation Cri is approximated in the module 4 by a numerical matrix Cc so as to generate a measurement vector {circumflex over (η)}k estimated on the basis of the state vector {circumflex over (ξ)}k estimated for the mode of operation considered.
In the LCA mode of operation, for example to the vector of measurements
there corresponds the numerical matrix Cc given preferably by the following formula 116:
in which the non-zero coefficients c11, c22, c34, c46, c57, are constant and unitary since the measurements considered form part of the state variables in the particular case of the illustrative example.
The module 4 then calculates the estimated measurement vector {circumflex over (η)}k by multiplying in real time the estimated state vector {circumflex over (ξ)}k by the matrix Cc at each instant of index k.
Whatever the mode of operation of the vehicle, an estimation gain (sometimes dubbed Kalman gain) module 5 has the function of correcting the time derivative {circumflex over ({dot over (ξ)})} of the state vector so that the measurement vector estimated {circumflex over (η)} on the basis of the estimated state vector {circumflex over (ξ)} coincides under steady conditions with the real measurement vector η, so that the estimated state vector {circumflex over (ξ)} thus coincides with the trajectory following physical state vector ξ of the vehicle 1. Accordingly, the module 5 adjusts in real time the temporal variation {circumflex over ({dot over (ξ)})} of estimated state vector {circumflex over (ξ)} so as to reduce a stabilization deviation between the current measurement vector η and the estimated measurement vector {circumflex over (η)} by multiplying the stabilization deviation by a correction gain matrix Lc.
The stabilization deviation between the two measurement vectors η and {circumflex over (η)} being a vector of dimension equal to that of the measurement vectors and the correction to be added to the temporal variation {circumflex over ({dot over (ξ)})} of the estimated state vector being a vector of dimension equal to that of the state vectors, the matrix Lc comprises a number of rows which is equal to the number of coordinates of the state vector and a number of columns which is equal to the number of coordinates of the measurement vector.
Thus, the observer device 2 can behave as a Kalman observer defined by the equation of formula 117:
{circumflex over ({dot over (ξ)})}=Ac{circumflex over (ξ)}+Bsust+Lc(η−Cc{circumflex over (ξ)})
The matrix Lc corresponds to the matrix of the gains of the
Kalman observer, without it appearing to be necessary here to develop in greater detail the obtainment thereof which is known elsewhere.
The Kalman observer described hereinabove is described essentially by way of illustration for a better understanding of the implementation of the invention but other observers may be used such as for example high-gain observers, sliding mode observers, observers of extended Kalman type, Luenberger observers or other Bayesian type filters for which the person skilled in the art should effortlessly know how to carry across the teaching lavished here.
The estimated state vector {circumflex over (ξ)} thus generated by the module 2 is communicated to a module 14 in the didactic embodiment illustrated by
Of course, if one has a vector ξk of real states of the vehicle with respect to the lane at the instant k, it is the vector ξk of real states which is communicated to the module 14 in the didactic embodiment illustrated by
The description now explains how the anticipator module 14 or 19 generates a quantity representative of the curvatures of the lane for each predicted position of succession rank i of the vehicle 1 at instants succeeding in given amount N the current instant k and of the estimated state vector {circumflex over (ξ)}k at the current instant k.
The vector quantity generated by the module 14 is a meta state vector Ξk+1=(
It can be defined in the following manner:
For the instant k+1, no measurement vector ηk+1 is yet available at the instant k, but if the curvature of the lane γk which acts as a disturbance is known, it is possible to write formula 118:
where G(v) is a matrix with one column and n rows, each corresponding to a state coordinate of the state vector ξk.
In the column of the matrix G(v), only two coefficients g2 and g3 are non-zero considering that the curvature γk acts directly on the angle Ψrel,ef of actual relative deviation of the vehicle from its ideal trajectory and on the actual lateral rate {dot over (Y)}CoG,ef=∂YCoG/∂t of divergence of the center of gravity of the vehicle with respect to its ideal trajectory. The coefficient g2 is equal to the opposite of the longitudinal speed v of vehicle and the coefficient g3 is equal to the opposite of the square of the longitudinal speed v of vehicle.
By extrapolation if the following curvatures γk+i of the lane are known, the coordinates of the vector Ξk+1=(
For i=1:
For i=2:
For i=3:
We see the appearance of a formula 119 of the type:
or in compact matrix form by defining the following meta-matrices and meta-vectors:
=[A(v)i]i=1N is a meta-matrix with N times n rows and n columns. Stated otherwise, the first n rows of the meta-matrix retranscribe the matrix A(v) and the last n rows of the meta-matrix retranscribe the matrix A(v)N. The intermediate rows n(i−1)+1 to ni of the meta-matrix for their part retranscribe the matrix A(v)i. This comes down to considering the speed v on which the calculated matrix A(v) depends at each instant to be constant over the N anticipated instants of the prediction horizon. The value of the speed v will of course be refreshed at the following iteration of the calculation. If the calculation means so allow, it is also possible to predict a speed variation as a function of knowledge of acceleration or of deceleration.
is a meta-matrix with N times n rows and in the most general manner N times nc columns, where nc is the number of commands of the system. In the case of the LCA system with the turning angle command as sole command, nc=1. Stated otherwise, the first n rows of the first column of the meta-matrix retranscribe the matrix product A(v)0·Bs, that is to say the matrix Bs, and the first n rows of the following columns have zero coefficients, and likewise the last n rows of the last column of the meta-matrix retranscribe the matrix product A(v)0·Bs. The intermediate rows n(i−1)+1 to n(i−1)+n of a jth column of the meta-matrix , j lying between 1 and i, for their part retranscribe the matrix product A(v)i-j·Bs, the same rows in the following columns having zero coefficients.
Uk=(uk+i)i=1N is a meta-vector with N rows which lists the following N commands envisaged to be applied for a state evolution of the vehicle in accordance with that estimable on the basis of the current state.
is a meta-matrix with N times n rows and N columns. Stated otherwise, the first n rows of the first column of the meta-matrix retranscribe the matrix product A(v)0·G(v), that is to say the matrix G(v), and the first n rows of the following columns have zero coefficients, and likewise the last n rows of the last column of the meta-matrix retranscribe the matrix product A(v)0·G(v). The intermediate rows n(i−1)+1 to n(i−1)+n of a jth column of the meta-matrix , j lying between 1 and i, for their part retranscribe the matrix product A(v)i-j·G(v), the same rows in the following columns having zero coefficients.
Γk=(γk+i)i=1N is a meta-vector with N rows which lists the curvatures of the lane or stated otherwise of ideal trajectory of the vehicle over a prediction horizon of N future sampling instants. To obtain the meta-vector Γk, the module 14 comprises a calculation sub-module 9 which can generate the meta-vector Γk of anticipated curvatures of the lane in various ways.
In a first mode of prediction in which the anticipated curvatures of the lane are those imposed on the vehicle by the carriageway for the k future instants assuming a constant speed of the vehicle, the calculation sub-module 9 receives the polynomial yk(xk) given at the current instant k of processing by the apparatus 15 described hereinabove with reference to
where the distance of the future position of the vehicle xk+i ahead of the vehicle is itself calculable in real time on the basis of the speed v, for example as a first approximation by means of formula 121 hereinbelow:
xk+i=i·v·Ts
where Ts is the numerical processing and/or sampling period. Other more elaborate formulae can easily be envisioned by the person skilled in the art, for example taking account of a projection of the trajectory on the straight line ahead of the vehicle.
If the polynomial is of degree 3 as mentioned hereinabove, the first derivative is given in a simple manner by formula 122 hereinbelow:
y′(x)=3p3x2+2p2x+p1
Likewise, the second derivative is given by the simple formula:
y″(x)=6p3x+2p2
Each curvature γk+i can thus easily be calculated in real time by means of formula 123 hereinbelow:
In the first mode of prediction in which the anticipated curvatures of the lane are those imposed on the vehicle by the carriageway for the k future instants, an alternative implementation consists in arranging the calculation sub-module 9 so as to access a sufficiently accurate road system map, stored in on-board memory or downloaded in real time, combined with a high-precision satellite positioning system S, for example of GALILEO type, so as to deduce the traffic lane curvatures on the basis of said map and of the positioning of the vehicle.
In a second mode of prediction in which the predicted trajectory curvatures are those generated by a system for trajectory calculation in autonomous mode, for example so as to change lane or direction, to avoid an obstacle or for any other reason, the calculation sub-module 9 receives an equation descriptive of the ideal trajectory in autonomous mode originating from this calculation system. The curvatures γk+i to be applied to the trajectory may for example relate to those of a deviation to be performed by the vehicle to avoid an obstacle or to change lane.
Thus, the control device of the invention can anticipate the announcement of a bend in the manner of a driver who is watching the road in front of the vehicle so as to commence a bend, change lane or direction, avoid an obstacle or any other thing, rather than guiding the vehicle as if it were on a rail.
Advantageously the distance x varies as a function of the current speed v of the vehicle. Purely by way of nonlimiting illustration, it is possible to envision the distances x equal to the product of the speed v multiplied by given response times having fixed values or having values which are themselves dependent on speed. These response times may likewise be parametrizable to make it possible to adjust them in the course of the vehicle trials.
The meta-vector Γk=(γk|i)i=1N generated by the calculation sub-module 9 is transmitted to a multiplication sub-module 10 for multiplying the meta-vector Γk by the meta-matrix .
The anticipated curvatures of the lane constitute a succession of disturbances which are imposed by the environment of the vehicle or by any other event of an unintentional nature but which are predictable over the anticipation horizon considered. Other predictable disturbances can be taken into account by the device according to the invention. For example the sidewind communicated by a weather station or a dedicated sensor of the vehicle, in particular when traversing a viaduct crossing a valley. For example too an imbalance in tire pressure or the presence of black ice on the carriageway. Provision may thus be made for the device according to the invention to anticipate a succession of constant or variable disturbances over a given prediction horizon.
In the didactic embodiment illustrated by
Ξk+1=
In formula 124, the meta-vector Uk is ideally the vector of the N optimized commands uk, uk+i, i varying from 1 to N−1 such as they are calculated by the module 3 on the basis of the meta-state vector Ξk+1 transmitted by the module 14 to the module 3. In a sequential numerical processing, the meta-vector Uk can be initialized to the meta-vector Uk−1 of the previous processing instant k−1. Doing this enables the optimization algorithm executed by the module 3 to converge faster.
The meta-vector Uk of anticipation of commands has as optimal value that calculated by the module 3 on the basis of the states to anticipation meta-vector Ξk+1, in the manner now explained.
The module 3 hosts a first meta-cost function 1 given by formula 125 hereinbelow:
1=Ξk+1TΞk+1+UkTUk
in which the left-hand side of the meta-cost function 1 is a scalar value to be minimized, and the right-hand side is a sum of two quadratic-optimization terms. We recognize in the first term on the right-hand side the meta-vector Ξk+1 of anticipation of states to be minimized, so as to obtain anticipated states that are as close as possible to targeted states of zero value. We recognize in the second term on the right-hand side the meta-vector Uk of anticipation of commands to be minimized, so as to obtain anticipated commands that are as small as possible to attain the targeted states of zero value.
The anticipated states weighting meta-matrix comprises n·N rows and n·N columns with diagonal coinciding with the diagonals of N elementary states weighting matrices Qk+i−1, each comprising n rows and n columns, i varying from 1 to N. The diagonal of each elementary states weighting matrix Qk+i−1 comprises a weighting coefficient for each elementary state. The higher the weighting coefficient associated with an elementary state, the more the elementary state is minimized in the quadratic optimization associated with the first term. Thus the higher the weighting coefficient, the more importance there is in minimizing the associated elementary state. A priori all the elementary states weighting matrices Qk+i−1 are identical insofar as no reason exists to weight the elementary states of the successive states differently. However, as we shall now see in a preferred embodiment, it is beneficial for the last elementary states weighting matrix Qk+N to have different coefficients from the other matrices.
A first mechanism making it possible to ensure the stability of the dynamic system consists in parametrizing previously, in a study and trials phase, a matrix P of values different from those of the coefficients of the matrices Qk+1 to Qk+N−1, so that for the last state predicted at the horizon N:
Qk+N=P
In order to ensure that beyond the horizon N, the cost function J is decreasing, stated otherwise that it will end up converging, common practice is to define an access cost to be a decreasing Lyapounov function in a comparable manner to those described in documents US2002/0193920 or U.S. Pat. No. 6,560,493.
For an LTI (Linear Time Invariant) system, the terminal weighting matrix P is chosen in such a way that the cost function J is equivalent to a cost of infinite horizon as is the case in LQR (Linear Quadratic Regulator) controllers. The matrix P is obtained by solving a Ricatti equation known for example from the documents cited in the previous paragraph.
In the present case of the invention, the dynamic system which varies as a function of the speed of the vehicle is of LPV (Linear Parameter Varying) type. The terminal cost weighting is then calculated by solving Linear Matrix Inequalities (LMIs) which have already been described elsewhere, for example in documents EP2855238B1, U.S. Pat. No. 9,535,422B2, or EP2855240B1.
The meta-matrix of weighting of anticipated commands comprises N rows and N columns with diagonal coinciding with the diagonals of N elementary command weighting matrices Rk+i each comprising 1 row and 1 column, i varying from 1 to N. The diagonal of each elementary command weighting matrix Rk+i comprises a weighting coefficient for the anticipated command uk+i. The higher the weighting coefficient associated with an elementary command, the more the command is minimized in the quadratic optimization associated with the second term. Thus the higher the weighting coefficient, the more importance there is in minimizing the associated elementary command. A priori all the elementary command weighting matrices Rk+i are identical insofar as no reason exists to weight the elementary commands of the successive states differently.
Setting the values of the coefficients of the matrices Qk+i−1 and Rk|i in the phase of fine tuning by modeling or by on-track trials of the vehicle makes it possible to adjust the behavior of the controller module 3.
In formula 125, the optimization acts on the meta-vector of commands Uk, which constitutes the unknown of the equation or more exactly the quantity to be determined, the meta-vector of states, for its part, being given by formula 124 which makes it result from the commands, from the curvatures of the traffic lane and from the mechanical behavior of the vehicle. Replacing in formula 125 the meta-state vector Ξk+1 by its expression given in formula 124, we obtain formula 126:
1=(
Expanding formula 126, the first meta-cost function can be written in the form of a sum of four expressions:
1=xp1+xp2+xp3+xp4
in which the first expression xp1 depends only on the estimation vector
xp1=
The second expression xp2 depends only on the estimation vector
xp2=
The third expression xp3 depends only on the meta-vector Γk of curvatures of the lane, and the meta-vector of commands Uk to order 1:
xp3=UkTTΓk+ΓkTTUk=2ΓkTTUk
The fourth expression xp4 depends only on the meta-vector of commands Uk to order 2:
xp4=UkTTUk+UkTUk=UkT(T+)Uk
A dynamic quadratic optimization meta-matrix is defined which makes it possible to rewrite the fourth expression xp4 with the following formula 127:
xp4=UkT(T+)Uk=UkT()Uk
It is noted that the dynamic quadratic optimization meta-matrix =T+ involves through the meta-matrix a hierarchization of the commands which is correlated through the meta-matrix with a hierarchization of the states. The coefficients of the dynamic quadratic optimization meta-matrix are constants.
A dynamic cross-optimization meta-matrix is defined which makes it possible to rewrite the sum of the second xp2 and of the third xp3 expressions with the following formula 127:
xp2+xp3=2(
It is noted that the dynamic cross-optimization meta-matrix =2(
The first meta-cost function can then be rewritten in the following manner:
1=xp1+UkT()Uk+Uk
As the first expression xp1 does not depend on the meta-vector Uk which is the only variable on which it is possible to act to minimize the cost, it is possible to define a second meta-cost function 2 through the following formula 128:
2(Uk)=UkT()Uk+Uk
It will be observed that the meta-cost functions 1 and 2 have the same extrema in conjunction with the meta-vector Uk.
The embodiment illustrated by
In the preferred embodiment, illustrated by
The module 14 is replaced with a module 19 which comprises the same sub-module 8 and the same sub-module 10 to receive respectively the estimated state vector generated by the module 2 and the N anticipated curvatures of curves generated by the sub-module 9, but which no longer comprises the sub-module 18. A sub-module 16 receives the output of the sub-module 8 added to the output of the sub-module 10 to generate a value at the instant k of the dynamic cross-optimization meta-matrix or of its transpose by multiplying the sum (
=[2TT(
The advantage of the second embodiment is to clearly decouple, on the one hand the anticipation function carried out by the module 19 on the basis of knowledge of the curvatures of the lane at the locations where the vehicle is forecast to be situated at the N instants k+1, k+i, k+N following the current instant k, and on the basis of the current estimated state of the vehicle with respect to the lane, and on the other hand the pure optimization function carried out by the module 13.
The module 13 then uses the second meta-function to generate the optimal meta-vector of the commands by using one of the numerous known quadratic optimization algorithms, such as for example purely by way of nonlimiting illustration, the gradient method or Newton's method to solve the equation given by the following formula 130:
In each calculation cycle, only the command uk, first component(s) of the meta-vector Uk of rank k, is applied to the vehicle. The other components can be eliminated. They may, however, optionally be retained, for example to initialize the following calculation cycle so as to converge faster toward the optimal solution, or else to continue to operate over a limited number of calculation cycles posterior to the cycle in progress, so as to remedy a momentary loss of measurements or other information such as that providing the curvatures of the lane.
Constraints on the admissible states ξ of the system, on the feasible commands u and/or on the acceptable measurements η can usefully be added. In this way, it is ensured that the calculated values of the current command and of the anticipated commands are optimal in a universe of possibles, both in terms of physical limits of the vehicle and of safety for the road traffic and more particularly for the passengers of the vehicle, or indeed for their comfort.
In a first case of constraints pertaining to the states of the system, one may want each elementary state, a component of the estimated state vector
Ξk+1est=
for the upper bound:
for the lower bound:
which can be written in the form of the following inequality:
or else by putting for a limitation of the state values, the meta-matrices:
A sub-module 21 receives in the module 20 the meta vector Uk and employs the meta-matrix to generate the left-hand side of the previous inequality.
A sub-module 23 receives in the module 20 the vector of estimated or real states, as the case may be, at the instant k and employs in real time the meta-matrix to generate the right-hand side of the previous inequality in association with the sub-module 22.
The first case of constraints pertaining to the states of the system can be obtained by compelling the optimized value Uk to satisfy formula 132 hereinbelow:
ξUk≤ξ+ξ
The inequality of formula 132 hereinabove is hosted in a sub-module 24 receiving the left-hand side defined by the unknown meta-vector Uk, and the right-hand side equal to the sum of the terms obtained by the sub-module 22 and the sub-module 23, to compel the left-hand side to be less than the right-hand side. Stated otherwise, the sub-module 21 makes it possible to verify that a variation of the current state and of the predicted states, which is correlated with the sought-after current command and with the commands predicted by the meta-matrix ξ, is less than a distance which separates the current state and the predicted states from the admissible minimum and maximum states.
More generally, the meta-matrix ξ is included in a meta-matrix of the sub-module 21, the meta-matrix ξ is included in a meta-matrix of the sub-module 22, the meta-matrix ξ is included in a meta-matrix of the sub-module 23. Thus, the inequality managed by the sub-module 24 may again be written in the form:
Uk≤+
In a second case of constraints pertaining to the commands of the system, one may want each elementary command, a component of the meta-vector Uk, to be in a span of components of command vector [umin, umax].
One then puts for limitation of the command values, the meta-matrices:
to designate the zero matrix.
The second case of constraints pertaining to the commands of the system can be obtained by compelling the optimized value to satisfy formula 133 hereinbelow:
uUk≤u+u
It is possible to include the meta-matrices u, u, u respectively in the meta-matrices , , with or without the meta-matrices ξ, ξ, ξ according to whether one does or does not want to satisfy at one and the same time the constraints on the commands and those on the states.
In a third case of constraints pertaining to the measurements of the system, one may want each elementary measurement, a component of the vector ηk, to be in a span of components of command vector [ηmin, ηmax].
Then by putting for a limitation of the measurement values, the meta-matrices:
The third case of constraints pertaining to the measurements of the system can be obtained by compelling the optimized value to satisfy formula 134 hereinbelow:
ηUk+1≤η+η
It is possible to include the meta-matrices η, η, η respectively in the meta-matrices , , with or without the other meta-matrices previously mentioned according to whether one does or does not want to satisfy at one and the same time the constraints on the measurements and all or some of the other constraints.
In a fourth case of constraints pertaining to the stability and the feasibility of the predictive control of the model, one may want the last elementary state at the horizon, a component of the last vector of predicted states
Ω={ξϵn|FΩξ≤gΩ}
where n designates the Cartesian product of the set of real numbers to dimension n corresponding to the amount of elementary states of the state vector.
The aim here is to ensure that the last predicted state vector
Unlike the prior art disclosed in document WO201678715A1 which verifies in the design phase whether solutions attain the polytope, the device of the invention seeks to compel in real time the solution to attain the polytope.
The functions FΩ and gΩ are defined both in terms of command and also curvature and speed beyond the horizon N, by using a scheme known per se such as for example that of the document mentioned hereinabove.
One then puts for example for a limitation of the measurement values, the meta-matrices:
Number | Date | Country | Kind |
---|---|---|---|
1751593 | Feb 2017 | FR | national |
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/EP2018/051354 | 1/19/2018 | WO | 00 |
Publishing Document | Publishing Date | Country | Kind |
---|---|---|---|
WO2018/157999 | 9/7/2018 | WO | A |
Number | Name | Date | Kind |
---|---|---|---|
20110320163 | Markkula et al. | Dec 2011 | A1 |
20160357169 | Di Cairano | Dec 2016 | A1 |
20180043931 | Gupta | Feb 2018 | A1 |
Number | Date | Country |
---|---|---|
103879401 | Jun 2014 | CN |
106096525 | Nov 2016 | CN |
10 2007 008 624 | Aug 2008 | DE |
WO 2010053408 | May 2010 | WO |
WO 2011009009 | Jan 2011 | WO |
WO-2011009009 | Jan 2011 | WO |
WO 2014006327 | Jan 2014 | WO |
WO-2014006327 | Jan 2014 | WO |
Entry |
---|
Machine translation of CN-106096525-A (Year: 2016). |
Machine translation of CN-103879401-A (Year: 2014). |
Machine translation of WO-2014006327-A1 (Year: 2014). |
International Search Report dated Apr. 13, 2018 in PCT/EP2018/051354 filed on Jan. 19, 2018. |
French Preliminary Search Report dated Oct. 19, 2017 in French Application 1751593 filed on Feb. 28, 2017. |
Number | Date | Country | |
---|---|---|---|
20200001921 A1 | Jan 2020 | US |