Trajectory tracking method
The method enhances ADAS system precision and efficiency by iteratively piloting a digital vehicle to follow a trajectory, addressing accuracy and time issues in existing virtual driver models, especially at high speeds.
Patent Information
- Authority / Receiving Office
- FR · FR
- Patent Type
- Applications
- Current Assignee / Owner
- AMPERE SAS
- Filing Date
- 2024-12-20
- Publication Date
- 2026-06-26
Smart Images
Figure 00000000_0000_ABST
Abstract
Description
Title of the invention: Trajectory tracking method technical field
[0001] The present invention is in the field of advanced driver assistance systems (or ADAS, from the English "Advanced Driver Assistance Systems"), and more particularly in the field of trajectory tracking methods. Previous technique
[0002] AD AS systems include functions such as vehicle lane keeping Trajectory control, lane keeping assist, vehicle speed regulation, and emergency braking are all essential functions for improving the safety and driving comfort of modern vehicles.
[0003] The validation of these systems is crucial to guarantee their performance and their compliance with existing safety standards, for example EuroNCAP.
[0004] Validation of these systems generally requires a trajectory tracking method.
[0005] Existing solutions include virtual driver models allowing the following of a trajectory of a test scenario, in particular for the purpose of validating one of the aforementioned functions.
[0006] By "virtual driver" is meant an algorithmic model or a simulation system which imitates the decisions and actions of a human driver, in particular with a view to determining acceleration and steering wheel commands to pilot a vehicle.
[0007] The test scenario could, for example, involve the detection of a pedestrian crossing a road on which the vehicle is located, followed by emergency braking to avoid the pedestrian. As another example, the test scenario could involve keeping the vehicle in its lane along a curved section of road.
[0008] However, existing virtual driver models may lack precision, particularly for high vehicle speeds, or have excessively long calculation times due to vehicle parameterization or a numerical model representing vehicle dynamics.
[0009] The term "vehicle dynamics" refers to all the physical phenomena and mechanical behaviors that influence the vehicle's movement under the effect of external forces. Vehicle dynamics encompasses the interactions between the various components of the vehicle (chassis, suspension, tires, engine, steering, etc.). and the forces acting on it, such as gravity, inertia, air resistance, or adhesion to the ground.
[0010] We know from the applications CN112068445A, and WO202262452A1, and CN113460088A trajectory tracking methods based on an MPC (Model Predictive Control) method, with a linearized vehicle dynamics representative model, which may have limited accuracy in some cases.
[0011] Application WO202489595A1 presents a method for tracking the trajectory of a vehicle at low speeds, with a representative model of the simplified vehicle dynamics.
[0012] CN116974205A is also known as a trajectory tracking method using the IPOPT solver for an automatic parking function. However, the scope of the application may be relatively limited, particularly at speeds below 3.6 km / h.
[0013] The technical problem is the lack of accuracy, particularly at high vehicle speeds, and the significant parameterization time required for virtual driver models. Furthermore, existing virtual driver models may not be compatible with the cost and development time requirements of a new vehicle project.
[0014] There is also a need to improve the methods of trajectory tracking by a physical vehicle. Summary of the invention
[0015] The invention aims to meet this need and achieves this, according to one of its aspects, by means of a method for piloting a digital vehicle consisting of a digital model, to make it follow a reference trajectory, the method comprising successive main iterations, each of the successive main iterations comprising the following steps, in particular successive steps, implemented for each main iteration at a corresponding current instant: - (al) Acquisition of a current state of the digital vehicle, the current state including a current position of the vehicle, - then, (b) Determination of an effective trajectory from the reference trajectory and the current position, - then, (c) Determining a sequence of successive future times, subsequent to the current time, and a sequence of successive future positions belonging to the effective trajectory, each future position being associated with one of said future times,
[0016] - then, (d) Using a predictive model to determine a sequence of commands predictive vehicle commands to be applied respectively to future times in the sequence of future times, the sequence of predictive vehicle commands being determined at starting from the current state, the sequence of future times and the sequence of future positions, said predictive commands being determined so as to follow the effective trajectory, each of the predictive commands in the sequence of predictive commands comprising a predictive instruction for vehicle acceleration and a predictive instruction for steering angle of a steering wheel of the vehicle. - then, (f) Application of the first predictive command of the sequence of predictive commands to the digital vehicle, and determination of a resulting state of the latter, the process being characterized in that a series of successive time gaps between two successive future instants of the sequence of future instants is increasing.
[0017] In other words, the sequence of successive time differences between two successive future instants in the sequence of future instants is increasing as the successive future instants move away from the current instant.
[0018] By "longitudinal" or "longitudinal" we mean here a direction parallel to the axis of movement of the vehicle.
[0019] The front axle steering angle refers here to the angle that the vehicle's front wheels form with a longitudinal axis of the vehicle. The front axle steering angle can be directly controlled by the vehicle's steering wheel. In the predictive model, this angle can be calculated using a simplified static steering model, specifically a steering ratio model, when the stiffness of various mechanical components (column, torque sensor, etc.) is assumed to be infinitely rigid.
[0020] By "yaw angle" is meant an angle of rotation of the vehicle around a vertical axis, in particular perpendicular to the ground and passing through a center of gravity of the vehicle, measuring the direction in which the vehicle is oriented with respect to a reference, in particular the reference trajectory, or an axis of a road.
[0021] By "instants" we mean, for example, specific points in time, each instant being defined by a temporal measurement, in particular numerical, such as an hour, or as a duration measured from an original instant.
[0022] By "successive" moments, or similarly by "successive" iterations, it should be understood that these moments are organized linearly, in other words chronologically, one following the other. Similarly, "successive positions" refer to positions organized linearly, one following the other.
[0023] In the context of the invention, a “sequence,” particularly of instants, positions, or commands, should be understood as a finite sequence or list of terms in a specific order. The order of each term is defined by its rank. A sequence has, in particular, a first term and a last term. For example, for a sequence of four instants [T1, T2, T3, T4], the term of rank 1, or first term, is instant T1, the The second term, or second term, is time T2, the third term, or third term, is time T3, and the fourth term, or last term, is time T4.
[0024] By "sequence of successive time gaps," it is meant that the sequence of successive time gaps is constructed by successively traversing each pair of successive future instants in the sequence of future instants, and determining the corresponding gap. Thus, for a sequence of successive future instants [T1, T2, T3, T4], the sequence of successive time gaps is [T2-T1, T3-T2, T4-T3]. Time gaps
[0025] Preferably, the sequence of successive time gaps between two successive future instants of the sequence of future instants is strictly increasing from a predetermined rank Nm of the sequence of successive time gaps.
[0026] In one embodiment, the sequence of successive time differences between two successive future instants of the sequence of future instants is constant up to the predetermined rank Nm, then strictly increasing from the predetermined rank Nm.
[0027] In particular, the sequence of successive time intervals between two successive future instants in the sequence of future instants can grow linearly from the predetermined rank Nm. Alternatively, it can grow logarithmically, polynomially, or exponentially. States
[0028] Preferably, the current state includes a current vehicle position, a current vehicle longitudinal speed and a current vehicle lateral speed, a current vehicle yaw angle and a current vehicle yaw rate, and a current vehicle acceleration. The current acceleration can be used for longitudinal speed control.
[0029] Yaw rate denotes a variation of the yaw angle over time, in other words a derivative function of the yaw angle.
[0030] Preferably, the resulting state results from the application of the first command. By "resulting state" is thus designated a state of the vehicle, analogous to the current state, determined following the application of the first predictive command.
[0031] Preferably, the step (al) of acquiring the current state of the digital vehicle, implemented in a current main iteration, includes the acquisition of the resulting state determined in the previous main iteration. Initial iteration
[0032] The method may include a preliminary iteration, preceding all successive main iterations, comprising an acquisition of the reference trajectory, the acquisition of the reference trajectory comprising the determination of an initial trajectory, the initial trajectory being taken from a catalogue of trajectories.
[0033] By catalogue of trajectories, we mean here a set of predefined trajectories, in particular a set of test trajectories intended to evaluate the dynamics of the vehicle when following said trajectories.
[0034] Alternatively, the initial trajectory can be determined from maps of a road on which the vehicle is located, from virtual tracks, or from vehicle measurement data, including GPS data.
[0035] The preliminary iteration can be substantially analogous to the successive main iterations. State acquisition may involve determining an initial state. The initial state may include an initial position, an initial longitudinal velocity, an initial lateral velocity, an initial yaw angle, and an initial yaw velocity. Reference trajectory and effective trajectory
[0036] Step (b) of determining the effective trajectory may include determining a current reference point, the current reference point being the point on the reference trajectory closest to the current position of the vehicle.
[0037] The current reference point can be determined by restricting it to an interval around a predicted current point, the predicted current point being located at a distance of u*Ts from a previous current position of the vehicle, where u is the current speed of the vehicle, and Ts is a time step for the calculation of the predictive model. In particular, the interval can be centered on the predicted current point and be of length a*Ts, where a is an integer greater than or equal to 2, in particular equal to 2. Restricting the determination of the current reference point to an interval of length a*Ts can allow for consideration of trajectory deviation during successive iterations of the method according to the invention. In particular, the size of the interval can be increased if the current reference point is determined to be at the boundaries or outside the interval.
[0038] Step (b) of determining the effective trajectory may include determining a prediction time horizon, in particular between 0.5 and 6 seconds.
[0039] In particular, the prediction time horizon represents a maximum duration for anticipating the trajectory. The prediction time horizon can thus be used to model human behavior with a limited capacity for anticipation.
[0040] Step (b) of determining the effective trajectory may include determining a prediction distance from the prediction time horizon.
[0041] The effective trajectory may be determined by polynomial interpolation of several positions of the reference trajectory located upstream of the current reference position and at a distance from it that is less than the prediction distance measured along the reference trajectory. Alternatively, the effective trajectory can be directly interpolated from the complete reference trajectory and the prediction distance. This can be particularly suitable when the reference trajectory is relatively smooth, especially with a small radius of curvature.
[0042] By "measured along the reference trajectory", it is to be understood that the distance is measured continuously while moving along the reference trajectory, and not as the crow flies by measuring a segment directly connecting two points of the reference trajectory.
[0043] The effective trajectory can be determined by polynomial interpolation of Nin positions, comprising the current position of the vehicle and positions uniformly distributed along the reference trajectory, located upstream of the current reference position and at a distance from it less than the prediction distance measured along the reference trajectory.
[0044] In one embodiment, Nin is an integer greater than or equal to 5, or even greater than or equal to 10. The value of Nin may depend on the shape of the reference trajectory. In particular, Nin may depend on a radius of curvature of the reference trajectory. For example, the value of Nin may increase when a radius of curvature of the reference trajectory decreases.
[0045] Preferably, the Nin positions include the current reference point. Polynomial interpolation can thus take into account a difference between the current position and the current reference point.
[0046] Preferably, at least some of the Nin points are distributed uniformly along the reference trajectory.
[0047] Optionally, at least some of the Nin points are concentrated in a curve of the reference trajectory. This improves the accuracy of the polynomial interpolation. In one embodiment, the method includes a step (a2) for acquiring the reference trajectory. Preferably, the step (a2) for acquiring the reference trajectory at the current main iteration may include acquiring the actual trajectory determined at the previous main iteration. Thus, at each main iteration, the reference trajectory is updated.
[0048] Thus, the portion of the effective trajectory approximating the reference trajectory, in other words the shortest section of the effective trajectory including the interpolation points, corresponds to a predictive trajectory of future vehicle driving, with the current speed of the vehicle and the predictive time horizon of prediction.
[0049] Successive future positions and successive future times
[0050] In one embodiment, the successive future positions are expressed according to a polynomial expression whose coefficients depend on a normalized distance variable.
[0051] The successive future positions can be determined from the current speed of the vehicle.
[0052] Each future position can be determined to be reached at a respective future time in the sequence of future times, with the current speed of the vehicle.
[0053] Preferably, the prediction time horizon is an upper bound of the gap between the current instant and the last instant of the sequence of successive future instants.
[0054] Preferably, the prediction distance is an upper bound of the distance, measured along the reference trajectory between the current position or the current reference point, and the last future position of the sequence of successive future positions. Predictive model
[0055] Step (d) of using the predictive model involves applying a numerical integration method to a nonlinear system of equations representative of the vehicle dynamics.
[0056] As an example, the numerical integration method is chosen from an Adams-Bashforthet method and a Range-Kutta 4 method. Thus, it is possible to adjust the accuracy of the numerical integration, depending on the computation time and / or the desired accuracy.
[0057] The use of the predictive model may involve minimizing a predetermined cost function, the cost function comprising a sum of deviations, in particular quadratic and weighted with predetermined first weighting coefficients, between predictive states determined by the predictive model, and reference states determined from the current state and successive future positions, the predictive states and the reference states being determined at successive future positions. Advantageously, the cost function further comprises a quadratic sum of predictive efforts and a quadratic sum of predictive orders weighted respectively with second and third weighting coefficients.
[0058] Each of the reference states includes, for example, a reference longitudinal velocity, a reference position, a reference yaw angle.
[0059] Predictive efforts include, for example, efforts on the steering wheel, in particular efforts to turn the steering wheel, and / or a force exerted at the level of one of the wheels, in particular the driving force.
[0060] Preferably, the use of the predictive model includes the determination of at least one dynamic constraint of the vehicle during trajectory tracking, the dynamic constraint being able to be chosen from among a maximum deviation between a future position of the sequence of successive future positions and a predictive position of the vehicle, a maximum steering angle of the front axle, and a maximum acceleration in absolute value.
[0061] Preferably, the use of the predictive model involves the use of the IPOPT solver, the IPOPT solver being configured in particular to solve the nonlinear system of equations by minimizing the cost function. The optimal control solution for the predictive model is based on the IPOPT solver, which optimizes the cost function presented below with the predictive and nonlinear model.
[0062] IPOPT (from the English "Interior Point Optimizer") is a numerical solver well known to those skilled in the art, notably used to solve constrained nonlinear optimization problems. It is based on the interior point method, which allows inequality constraints to be reformulated to ensure that the desired solutions remain strictly within the feasible region defined by these constraints. This method proceeds by an iterative solution, progressively adjusting the candidate solution to minimize the cost function while respecting the imposed constraints.
[0063] The predictive model, otherwise known as the MPC method (from the English "Model Predictive Control"), thus involving the minimization of a predetermined cost function, under dynamic constraints of the vehicle, makes it possible to generate optimal predictive orders.
[0064] The use of the predictive model may include the initialization of the numerical integration method from the sequence of predictive commands obtained during the previous iteration. Orders
[0065] Preferably, the predictive acceleration setpoint is a predictive longitudinal acceleration setpoint.
[0066] Preferably, the application of the first predictive command in the sequence of predictive commands is performed from the last state determined during the preceding main iteration. When the frequency of determining the predictive commands is lower than the frequency of applying a command and determining a new state, the last two applied predictive commands may be interpolated. These interpolated commands are referred to below as intermediate commands. In particular, the last state determined during the preceding main iteration may be the last intermediate state determined after the application of the last intermediate command in a sequence of intermediate commands. In other words, the application state of the first predictive command, the application state being the vehicle state from which the first predictive command is applied, may be different from the current state.Thus, the vehicle may have moved along the reference trajectory or the actual trajectory, particularly during the use of the predictive model.
[0067] Alternatively, the application of the first predictive command of the predictive command sequence is carried out from the current state.
[0068] The resulting state may include a resultant position.
[0069] In particular, the determination of the resulting state can be obtained so that the value of the sequence of predictive commands minimizes the cost function, that is, in a way, so that the sum of the differences between the predicted future positions resulting from the application of the predictive commands and the future positions of the sequence of successive future positions is minimal.
[0070] The first predictive command may include a first predictive acceleration instruction, the application of the first predictive command including the conversion of the first predictive acceleration instruction into an accelerator pedal stroke or a brake pedal stroke.
[0071] The conversion of the first predictive acceleration command into an accelerator pedal travel or a brake pedal travel may include, via a PID controller, a longitudinal acceleration comparison between the vehicle's current acceleration and the first predictive acceleration command, as well as a longitudinal speed comparison between the vehicle's current longitudinal speed and a desired longitudinal speed command. These two comparisons allow, via the PID controller, the determination of a PID control command. When the PID control command is positive, it is converted into an accelerator pedal travel, and when the PID control command is negative, it is converted into a brake pedal travel.
[0072] By “PID”, we mean here a regulator, or controller of proportional, integral, derivative type. Synchronization and intermediate commands
[0073] The current main iteration may include, subsequent to step (d) of using the predictive model, in particular to generate the predictive commands, and prior to step (f) of applying the first predictive command, in particular to pilot the vehicle, a step (e) of comparing a frequency of application of the predictive model with a frequency of application of the command, the frequency of application of the predictive model being representative of a computation time to obtain the sequence of predictive commands from the sequence of future times and the sequence of future positions, the frequency of application of the command being representative of a computation time to apply the first predictive command and determine the resulting state, and, when the frequency of application of the predictive model is less than the frequency of application of the command, the comparison step (e) includes the determination of a sequence of intermediate commands,obtained by linear interpolation of the first predictive command of the main iteration, current with the first predictive command of the previous main iteration at least, for example by linear interpolation.
[0074] By "linear interpolation of a command Ci,1 and a command Ci-1,1", it is necessary to understand that a sequence of intermediate commands, comprising intermediate instructions, is generated by linearly interpolating the instructions of the commands Ci,1 and Ci-1,1.
[0075] The current main iteration may include a step (g) of applying intermediate commands, subsequent to the step (f) of applying the first command, comprising successively, for each of the intermediate commands in the sequence of intermediate commands, the application of the intermediate command and the determination of an intermediate state.
[0076] The number of intermediate commands in the sequence of intermediate commands C'i is less than or equal to, in particular equal to, the ratio between the command application frequency fv and the predictive model application frequency fmpc. Thus, in the case where fmpc < fv, it is still possible to determine intermediate commands and corresponding successive intermediate states, in particular between two successive future instants of the sequence of successive future instants.
[0077] For example, the application frequency of the predictive model fmpc can be equal to 100 Hz.
[0078] For example, the command application frequency fv can be equal to 1000 Hz.
[0079] Each of the predictive commands in the predictive command suite can The predictive model consists of Ne different instructions, where Ne is an integer greater than or equal to 2. The predictive commands in the sequence of predictive commands are constant from a certain constant rank greater than or equal to Ne. The number of instructions Ne can be small in order to reduce the complexity of calculating the predictive model.
[0080] In one embodiment, the linear interpolation of the first predictive command of the previous main iteration and of the first predictive command of the current main iteration is a linear interpolation in a [fy / fMPC] (or Ne) dimension space.
[0081] Alternatively, the determination of the sequence of intermediate commands can be obtained by constant interpolation of the first predictive command of the current main iteration.
[0082] Alternatively, the determination of the sequence of intermediate commands can be obtained by polynomial interpolation of degree greater than 2, if the previous effective commands are retained. The application of the first intermediate command of the sequence of intermediate commands can be carried out from the resulting state, the application of the following intermediate commands from the a series of intermediate commands being carried out from the successively determined intermediate states.
[0083] The characteristics described for predictive controls can also be applied to intermediate controls. In particular, each of the intermediate controls in the sequence of intermediate controls can include an intermediate acceleration command, the application of the intermediate controls involving the conversion of each of the intermediate acceleration commands into an accelerator pedal travel or a brake pedal travel. The conversion of the first intermediate acceleration command into an accelerator pedal travel or a brake pedal travel can include a comparison of the acceleration command with a vehicle acceleration determined from a corresponding intermediate state, as well as a comparison of the current speed and a vehicle speed command.The accelerator pedal travel can be determined by applying a PID control law. Specifically, speed control can be applied to the accelerator pedal if the PID control is positive, or to the brake pedal otherwise. The intermediate control can be implemented in the vehicle's digital model.
[0084] By "digital vehicle", it is necessary to understand for example a digital replica or a virtual representation of a physical vehicle, in particular used to simulate, analyze or optimize the behavior, performance or interaction of said physical vehicle with its environment, without requiring physical tests.
[0085] The application of the first predictive control can be carried out in the digital model of the vehicle.
[0086] By "digital vehicle model", we mean a computer model representing the vehicle.
[0087] The digital model of the vehicle can be a functional model of the vehicle's dynamics. In particular, the vehicle's aerodynamic characteristics can be simulated using digital software, also known as "professional software," notably CFD (Computational Fluid Dynamics) software, or a wind tunnel. The vehicle's suspension characteristics can be simulated beforehand using multibody software or measured directly on a K&C (Kinematic and Compliance) test bench. The vehicle's aerodynamic and suspension characteristics can be provided in the digital model in the form of tables, which also include the vehicle's engine's power characteristics. The modeling The vehicle's tire performance can be determined using a characterization bench and a suitable real-time system model, such as Pacejka. The numerical model may include a fixed-step discretization method and a real-time solution method, such as a first-order Euler method or a higher-order Range-Kutta method.
[0088] The digital vehicle model can take into account predictive or intermediate commands, particularly when the predictive vehicle commands are determined by the predictive model at a lower frequency than that required by the physical or digital vehicle. Furthermore, it can take into account quantities related to road traffic intensity, and / or meteorological data, and / or environmental data, and / or a traffic incident, etc. computer program product
[0089] The invention also relates, independently or in combination with the above, to a computer program comprising instructions, executable by a microprocessor or a microcontroller, when executed by the microprocessor or the microcontroller.
[0090] Such a computer program product can make it possible to implement the process with all or part of the process characteristics defined above.
[0091] The invention also relates, independently or in combination with the above, to an electronic device configured to implement the steps of the process defined above.
[0092] In one embodiment, the application of the first command is an implementation of the first command by the vehicle. The resulting and intermediate states can be determined from one or more sensors present on the vehicle. Brief description of the drawings
[0093] [Fig.1] is a schematic view of the sequences of various processes according to the invention.
[0094] [Fig.2] is a schematic view of a trajectory generation method effective.
[0095] [Fig.3] is a schematic view of a method for determining the sequence of predictive commands
[0096] [Fig.4] is a schematic view of a method for determining a sequence of intermediate orders
[0097] [Fig.5] is a schematic view of a predictive instruction process and intermediate acceleration in brake or acceleration pedal strokes according to a PID control law. Detailed description
[0098] Fig. 1 is a schematic representation of the sequence of steps of the process according to the invention, implemented by a supervisor.
[0099] A supervisor Sup can designate an initial trajectory Tinit from a catalog Ctg containing trajectories to be validated. The initial trajectory Tinit can be stored in memory. Optionally, the initial trajectory can be processed so as to be discretized and easily implemented by the method according to the invention.
[0100] The process 100 describes an implementation of steps (al1), (a2), (b) and (c).
[0101] The process 100, which receives the vehicle return status, recalculates at each step of The simulation time Ts determines the effective trajectory at a given prediction distance Lpred. These points of the effective trajectory are then synthesized using a polynomial formula as a function of a normalized distance. The effective trajectory is sent to process 200.
[0102] The process 200 describes an implementation of step (d).
[0103] The method 200 can optimize the steering wheel control and the acceleration signal longitudinal, and sends the predictive commands Ci,n to process 300 which deals with the synchronization problem occurring if a calculation time step of process 200 Ts and a calculation time step of process 500 Ts' are different.
[0104] The process 300 describes an implementation of steps (e) and (g).
[0105] The method 300 can send the predictive steering wheel instruction and instructions intermediate steering wheel to simulated vehicle in process 500, and the predictive acceleration command gMPC, and intermediate acceleration commands gMPC, in process 400.
[0106] The process 400 describes an implementation of step (f).
[0107] The process 400 which receives the predictive acceleration instruction, acceleration of the Process 300 can regulate the vehicle's speed by providing accelerator or brake pedal strokes, obtained from a PID controller. The accelerator or brake pedal strokes are sent to process 500.
[0108] The method 500 is a dynamic model of the vehicle, simulating the dynamic behavior of the vehicle. When the vehicle model is replaced by a physical vehicle, the present invention then relates to a method for tracking the trajectory of the physical vehicle.
[0109] Process 500 describes an implementation of step (f).
[0110] The process 500 can determine a resulting state or an intermediate state from predictive commands or intermediate commands, and send said resulting or intermediate state to processes 100, 200 and 400 as well as to the supervisor.
[0111] As can be seen in [Fig. 2], the main iteration It; is initiated by the acquisition of the current state Xcou;, comprising a current position. The current state Xcou; is sent by the process 500.
[0112] The method may include a step 101 of finding a current reference point, the current reference point being the point on the reference path closest to the current position of the vehicle. The current reference point can be determined by minimizing the difference between the current position of the vehicle and points on the reference path. When there is a certain difference between the current position of the vehicle and the current reference point, the current reference point can be readjusted to account for a corrective driving distance. In other words, the difference between the current position of the vehicle and the current reference point can be compensated for by adjusting the current reference point. For example, the minimization can be performed according to the formula:
[0113] , 7 / V ■ ( b dj = - X / refp + [yïreil - y^)2
[0114] , where (xi Veh, y; veh) is the current position of the vehicle, (x; ref, y; ref) is a position of the reference trajectory, the index i referring to the time step of the current instant, the index j representing points around i.
[0115] The reference current point can be determined by restricting it to an interval around a predicted current point, the predicted current point being located at a distance of Ui*Ts0 from a previous current position of the vehicle, with Ui the current speed of the vehicle, and Ts0 a computation time step, in particular equal to the computation time step of the predictive model Ts. In particular, the interval can be centered on the predicted current point, and be of length 10*Ts or another value greater than 2
[0116] In particular, the computation time step TsO can be the computation period of the predictive model Ts, in other words TsO = 10 / fmpc
[0117] A prediction distance Lpred can be determined, the prediction distance Lpred being a distance measured along the reference trajectory from the current reference point. The prediction distance Lpred can be expressed as:
[0118] Lpred = Tp.Ui + Lo,
[0119] Tp being a reference prediction duration and Lo being a minimum prediction distance.
[0120] The prediction time horizon Tpred can be determined by:
[0121] Tpred = Lpred / Ui
[0122] Tpred can be between 0.6 and 5 seconds. The prediction time horizon Tpred can be representative of a maximum duration of trajectory anticipation. The horizon Temporal prediction can thus make it possible to model human behavior, with a limited capacity for anticipation.
[0123] The points of the reference trajectory covered by the prediction distance Lpred, upstream of the current reference position, can be selected for polynomial interpolation of the reference trajectory.
[0124] At step 102, Nin data points (xi5 y;) are interpolated, the Nin points being uniformly distributed over a distance Lpred measured along the reference trajectory Tref.
[0125] Step 102 allows the effective trajectory to be obtained.
[0126] This interpolation reduces the regression time of step 103. The value Nin can, for example, be equal to 10.
[0127] In step 103, the points of the effective trajectory are expressed according to a normalized distance variable L;. For example, for a point (xi5 y;) of the effective trajectory, we can have [0i28] x . = a4^+a3^ 3 +a2^J+ a^+ao
[0129] + + + b0
[0130] with xi = ( Lj - Lj) ! (J^ where (xi5 y;) are the Nin interpolated points, Lj is the mean value of the Nin interpolated points, and is the standard deviation of the Nin interpolated points.
[0131] The parameters a;, b; as well as the mean value and standard deviation can be sent to step 104.
[0132] The method may include a step 104 of determining a sequence of successive future times, subsequent to the current time, and a sequence of successive future positions belonging to the effective trajectory, each future position being determined to be reached at a respective future time in the sequence of future times. The sequence of successive time intervals Ts(j) between two successive future times in the sequence of future times can be determined by
[0133] 7-0 = 1^ ' j~Nm SÜ) 1 TSotl+Mj-JVnJL for j>N m
[0134] with max( Ts(j)) < 5.Tsq' kp being a setting coefficient, in particular equal to 0.05.
[0135] In this example, the sequence of successive time gaps Ts(j) between two successive future instants of the sequence of future instants grows linearly from the predetermined rank Nm.
[0136] The use of a variable step of the discretized time makes it possible to reduce the computation time of the predictive model.
[0137] Thus, the successive future positions, Lref(j), from the identified current point, Lref(i), can be calculated by: 101381 Lre[U)=
[0139] uW) = Ui+Y Iongl .Tj with Tj = mm(ST s 0), T max )
[0140] Xjref = ai.îj + a3.x]+a-2^j+ a^Sj+ag 101411 yjref = b^j+b^3j+b2îj+ b^j+bg
[0142] Tmax being a parameter to limit the duration of acceleration phase.
[0143] These successive future positions Lref ( j) are then sent to process 200.
[0144] The method 200 may include the application of open-source software, for example CasADi, and the application of a predictive model, for example the IPOPT solver, in order to optimize the predictive commands for tracking the effective trajectory.
[0145] The application of the predictive model may include the application of a numerical integration method of a nonlinear system of equations representative of the vehicle dynamics.
[0146] The nonlinear system of equations representing the vehicle dynamics can be a simplified equation with three degrees of freedom (x, y), comprising two degrees of freedom in position (x,y) and one degree of freedom in yaw angle of the vehicle.
[0147] The nonlinear system of equations representing the vehicle dynamics can be ------+ vW
[0149] SCX being a frontal surface representative of an air resistance, Cr being a rolling coefficient, df being a front axle steering angle, Fl being a longitudinal force of the drive wheels.
[0150] The nonlinear system of equations can be rewritten as the following nonlinear equation:
[0151] ^=f(x, U)
[0152] X being a state variable, U being a system control variable defined by: '6fl. U= J L F 1J
[0153] The process may include a step 201 of initializing the predictive model, comprising determining the integration scheme of the integration method digital, a determination of at least one dynamic constraint of the vehicle when following the effective trajectory, the determination of a cost function.
[0154] The integration scheme of the numerical integration method can be chosen from a 2nd order integration scheme and a 4th order integration scheme.
[0155] For example, the second-order integration scheme can be the Adams-Bashforth method, having the following expression:
[0156] t / J Ai+1— 1 sj 2
[0157] The fourth-order integration scheme can be the Range-Kutta 4 method, having the following expression: [01581 k^ TSJ.f(Xt,Uj
[0159] k^T^X^O.Ôk^Uj
[0160] ^=^ / (^ + 0.5¾.^
[0161] k^= Tsj. f[Xj+k3 , Ujj
[0162] vv , Ai+1— 6
[0163] Depending on the required accuracy of the calculation, one of these two schemes can be used for the application of the predictive model.
[0164] Determining at least one dynamic constraint of the vehicle during tracking the effective trajectory may include determining a maximum deviation AXn2ax between a future position in the sequence of successive future positions and a predicted position of the vehicle, a maximum front axle steering angle Ôf max, and a maximum absolute value acceleration YjOngi max. The dynamic constraints can then be expressed as:
[0165] |AX|<ÆX maI
[0166] 16f| < 6r
[0167] iFjcmy, . 1 f longi max
[0168] m being a mass of the vehicle.
[0169] The predictive model may include the application of the IPOPT solver, performing an iterative method to find an exact solution to the optimization problem as defined by the nonlinear differential equation, the dynamic constraints, and the cost function.
[0170] The cost function can be determined as a sum of squared deviations of the predicted states determined by the predictive model, and of the reference states determined from the current state Xcou; and of successive future positions, the predicted states and the reference states being determined at successive future positions. For example, the cost function can be:
[0171]
[0172]
[0173]
[0174]
[0175]
[0176]
[0177]
[0178]
[0179]
[0180]
[0181]
[0182] J = 2 (^)2+gI(xrxrefi)2+(y,-yreff)2+L- v <refi)2++ t^y^ + ) where Vu, Qx, Qy, ^yl' ^52' ^y2 are weighting coefficients. The weighting coefficients can vary according to their index i. When the indices i are low, in other words when the future position considered is a short rolling distance from the current position, the predictive results are relatively more reliable, and one can thus apply a weighting coefficient 2 to 10 times greater than the weighting coefficients of the most distant future positions. The process may include a state definition step 202, comprising updating the current state from the last intermediate state value determined in the previous main iteration If i The yaw angle of a vehicle can be estimated from its position along the effective trajectory: y / lj = a tan The process may include a step 203 of specification of the non-linear model with the integration of cost function, constraints as well as reference values. The process may include a step 204 for initializing the numerical integration method from the sequence of predictive commands (Ci-l,n) obtained during the previous iteration (IL i). In other words, the predictive commands determined in the previous main iteration are used to make a first estimate of the solution in the current main iteration If, based on the last determined state of the vehicle. The process may include a step 205 of solving the nonlinear system of equations according to the determined dynamic constraints, by minimizing the cost function. The sequence of predictive commands Ci,n is then determined. In step 206, the first command Ci,l of the predictive command sequence for piloting the vehicle is acquired. As illustrated, the rest of the predictive command sequence is returned to step 204 to initialize the numerical integration method of the next main iteration. At step 207, the predictive steering wheel command and the predictive acceleration command, from the first predictive control Ci,l are sent to process 300. The process 300 includes a step 302 of comparing an application frequency of the predictive model fmpc, the application frequency of the predictive model fmpc being representative of the computation time TsO to obtain the sequence of commands predictive from the sequence of future times and the sequence of future positions, with a command application frequency fv, the command application frequency fv being representative of a computation time to apply the first predictive command Ci,l and determine the resulting state Xres;, and, when the predictive model application frequency fmpc is less than the command application frequency fv, the comparison step (e) involves the determination of a sequence of intermediate commands C'i, obtained by linear interpolation of the first predictive command Ci-1,1 of the previous main iteration If iet of the first predictive command Ci,l of the current main iteration.
[0183] For example, the time step of the predictive model Ts is equal to 10ms and the time step of the digital vehicle Ts' is equal to 1ms.
[0184] Predictive commands Ci,n can be recorded at step 301.
[0185] The predictive commands C'i, and the interpolated intermediate commands can be sent to processes 400 and 500. In particular, the predictive and intermediate steering angle commands are sent to process 500, and the predictive and intermediate acceleration commands are sent to process 400.
[0186] As illustrated, the method 400 comprises comparing the longitudinal acceleration, via a PID controller, between the current acceleration of the vehicle gveh and the first predictive acceleration setpoint gMpe, as well as comparing the longitudinal velocity between the current longitudinal velocity of the vehicle Vveh and a desired longitudinal velocity setpoint Vref. The two aforementioned comparisons allow, via the PID controller, the determination of a PID control setpoint.
[0187] If the PID control setpoint is positive, method 400 includes determining the accelerator pedal travel in step 401, or the brake pedal travel in step 402 if it is negative. The brake or accelerator pedal travels are determined by applying a PID control law.
[0188] At step 403, the accelerator or brake pedal travel command is sent to process 500.
[0189] The application of a PID control law for the determination of the accelerator pedal stroke or the accelerator pedal stroke can increase the accuracy and responsiveness of the process according to the invention.
[0190] The method 500 includes a simulation of the dynamic behavior of the vehicle, based on the steering angle command sent by the method 300, and the acceleration command sent by the method 400.
[0191] The method 500 comprises applying the first predictive command Ci,l of the predictive command sequence Ci,n, and determining the resulting state Xres. En Furthermore, process 500 includes, for each of the intermediate commands in the sequence of intermediate commands C'i, the application of the intermediate command and the determination of the corresponding intermediate state.
[0192] The resulting new or intermediate state of the vehicle can be referred to processes 100, 200, and 300.
Claims
Demands
1. A method for piloting a digital vehicle consisting of a digital model (500) to make it follow a reference trajectory, the method comprising successive principal iterations, each of the successive principal iterations comprising the following steps, implemented for each principal iteration at a corresponding current time: - (a1) Acquisition of a current state of the digital vehicle, the current state comprising a current position of the vehicle, - then, (b) Determination of an effective trajectory (102) from the reference trajectory and the current position, - then, (c) Determination of a sequence of successive future times (104), subsequent to the current time, and of a sequence of successive future positions (104) belonging to the effective trajectory, each future position being associated with one of said respective future times, - then,(d) Use of a predictive model (200) to determine a sequence of predictive vehicle commands to be applied respectively to future times in the sequence of future times, the sequence of predictive vehicle commands being determined from the current state, the sequence of future times, and the sequence of future positions, said predictive commands being determined so as to follow the actual trajectory, each of the predictive commands in the sequence of predictive commands comprising a predictive instruction for vehicle acceleration and a predictive instruction for the steering angle of a steering wheel of the vehicle. - then, (f) Application (500) of a first predictive command from the sequence of predictive commands to the digital vehicle, and determination of a resulting state of the latter, the method being characterized in that a sequence of successive time intervals between two successive future times in the sequence of future times is increasing.
2. A method according to the preceding claim, wherein the sequence of successive time gaps between two successive future instants of the sequence of future instants is strictly increasing from a predetermined rank Nm of the sequence of successive time gaps.
3. A method according to the preceding claim, wherein the sequence of successive time gaps between two successive future instants of the sequence of future instants is constant up to the predetermined rank Nm.
4. A method according to any one of the preceding claims, the current state comprising a current longitudinal velocity and a current lateral velocity, a current yaw angle and a current yaw rate, and a current acceleration of the vehicle.
5. A method according to any one of the preceding claims, wherein the step (al) of acquiring the current state of the digital vehicle, implemented in a current main iteration, includes the acquisition of the resulting state determined in the previous main iteration.
6. A method according to any one of the preceding claims, comprising, subsequent to step (d) of using the predictive model (200) and prior to step (f) of applying the first predictive command (500), a comparison step (e) (302) of a frequency of application of the predictive model with a frequency of application of the command, the frequency of application of the predictive model being representative of a computation time to obtain the sequence of predictive commands from the sequence of future times and the sequence of future positions, the frequency of application of the command being representative of a computation time to apply the first predictive command and determine the resulting state and, where the frequency of application of the predictive model is less than the frequency of application of the command: the comparison step (e) comprises the determination of a sequence of intermediate commands,obtained by interpolating the first predictive command of the current main iteration with the first predictive command of at least the preceding main iteration. The current main iteration includes a step (g) of applying intermediate commands, subsequent to the step (f) of applying (500) the first command, comprising successively, for each of the intermediate commands in the sequence of intermediate commands, the application (500) of the intermediate command and the determination (500) of an intermediate state.
7. A method according to the preceding claim, wherein the number of intermediate commands in the sequence of intermediate commands is equal to the ratio between the command application frequency and the predictive model application frequency.
8. A method according to any one of the preceding claims, wherein step (d) of using the predictive model (200) involves applying a numerical integration method of a nonlinear system of equations representative of the vehicle dynamics.
9. A method according to any one of the preceding claims, wherein the use of the predictive model (200) comprises: - minimizing a predetermined cost function, the cost function comprising a sum of deviations between predictive states determined by the predictive model, and reference states determined from the current state and successive future positions, the predictive states and reference states being determined at successive future positions, - and determining at least one dynamic constraint of the vehicle during trajectory tracking, the dynamic constraint being able to be chosen from a maximum deviation between a future position in the sequence of successive future positions and a predictive position of the vehicle, a maximum front axle steering angle, and a maximum absolute value acceleration.
10. A method according to the preceding claim, wherein the use of the predictive model (200) includes the initialization of the numerical integration method from the sequence of predictive commands obtained during the previous iteration.
11. A method according to any one of claims 1 to 10, wherein the application (500) of the first command is an implementation of the first command by the vehicle.
12. Computer program comprising instructions, executable by a microprocessor or microcontroller, for implementing the method according to any one of claims 1 to 11, when executed by the microprocessor or microcontroller.
13. Electronic device configured to carry out the steps of the process according to any one of claims 1 to 11.