METHOD FOR ESTIMATING NAVIGATION DATA OF A LAND VEHICLE USING ROAD GEOMETRY AND ORIENTATION PARAMETERS

Abstract

The invention concerns a method for estimating navigation date of a land vehicle, comprising steps of: recciving inertial data (100) acquired by an inertial sensor, receiving geometry and orientation parameters of a travelled road, integrating (106) the data on the basis of the parameters in order to produce navigation data comprising a movement of the vehicle relative to the road measured in a direction (Zr, Yr), the vehicle only being able to move in the direction within a bounded interval without leaving the road, estimating (108) an error affecting the navigation data by sol ing equations assuming that a difference between the calculated movement and a reference movement constitutes an error in the movement of the vehicle parallel to the direction, the reference movement having a value less than or equal to the length of the interval, correcting (110) the produced navigation data from the estimated error.

Claims

1. A method for estimating navigation data of a land vehicle, the method comprising: receiving inertial data acquired by an inertial sensor embedded in the land vehicle, receiving parameters relating to a geometry and an orientation of a road travelled by the land vehicle, integrating the inertial data on the basis of the parameters, such as to produce navigation data of the vehicle comprising at least one movement of the vehicle measured parallel to a direction perpendicular to a surface of the road, estimating at least one error that affects the produced navigation data, by solving a system of equations making the assumption that the at least one movement of the vehicle constitutes an error of movement of the vehicle parallel to the vertical direction, correcting the navigation data produced on the basis of the at least one error, such as to produce corrected navigation data.

2. A method for estimating navigation data of a land vehicle, the method comprising: receiving inertial data acquired by an inertial sensor embedded in the land vehicle, receiving parameters relating to a geometry and an orientation of a road travelled by the land vehicle, wherein the parameters comprise a width of the road measured along a transverse direction perpendicular to an average direction of circulation of a land vehicle on the road, integrating the inertial data on the basis of the parameters, such as to produce navigation data of the vehicle comprising at least one movement of the vehicle with respect to the road measured parallel to the transverse direction, computing a deviation between the at least one movement of the vehicle and an associated reference movement having a predetermined value less than or equal to the width of the road, estimating at least one error that affects the produced navigation data, by solving a system of equations making the assumption that the deviation constitutes an error of movement of the vehicle parallel to the transverse direction, correcting the navigation data produced on the basis of the at least one error, such as to produce corrected navigation data.

3. The method as claimed in claim 2, wherein the reference movement associated with the transverse movement is of zero value.

4. The method as claimed in claim 1 or claim 2, wherein estimating the at least one error is implemented by a Kalman filter, and the at least one movement of the vehicle is used by the Kalman filter as an item of observation data.

5. The method as claimed in claim 1 or claim 2, comprising steps of receiving satellite radio navigation data, image correlating implemented between the satellite radio navigation data and road data stored by a memory embedded in the vehicle, such as to produce the parameters relating to the geometry and orientation of the road.

6. The method as claimed in claim 5, wherein, when satellite radio navigation data cannot be received, the correlating step is implemented between corrected navigation data resulting from a previous implementation of the correcting step, and the road data stored by the memory.

7. A non-transitorv computer-readable medium comprising code instructions for causing a processor to perform the method as claimed in claim 1 or claim 2.

8. A device for estimating navigation data of a land vehicle, the device comprising: an interface for receiving inertial data acquired by at least one inertial sensor embedded in the land vehicle, an interface for receiving parameters relating to a geometry and an orientation of a road travelled by the land vehicle, at least one processor configured to: integrate the inertial data on the basis of the parameters, such as to produce navigation data of the vehicle comprising at least one movement of the vehicle with respect to the road measured parallel to a vertical direction perpendicular to a surface of the road, estimate at least one error that affects the produced navigation data, by solving a system of equations making the assumption that the at least one movement of the vehicle constitutes an error of movement of the vehicle parallel to the vertical direction, correct the navigation data produced on the basis of the at least one error, such as to produce corrected navigation data.

9. A device for estimating navigation data of a land vehicle, the device comprising: an interface for receiving inertial data acquired by at least one inertial sensor embedded in the land vehicle, an interface for receiving parameters relating to a geometry and an orientation of a road travelled by the land vehicle, the parameters comprising a width of the road measured along a transverse direction perpendicular to an average direction of circulation of a land vehicle on the road, at least one processor configured to: integrate the inertial data on the basis of the parameters, such as to produce navigation data of the vehicle comprising at least one movement of the vehicle with respect to the road measured parallel to the transverse direction, compute a deviation between the at least one movement of the vehicle and an associated reference movement having a predetermined value less than or equal to the width of the road, estimate at least one error that affects the produced navigation data, by solving a system of equations making the assumption that the deviation constitutes an error of movement of the vehicle parallel to the transverse direction, correct the navigation data produced on the basis of the at least one error, such as to produce corrected navigation data.

10. A system intended to be embedded in a land vehicle, comprising: an inertial system comprising at least one inertial sensor, a device as claimed in claim 8 or claim 9 arranged to receive inertial data generated by the inertial system by means of the inertial sensor.

11. A system intended to be embedded in a land vehicle, comprising: a memory containing road data, a receiver configured for acquiring satellite radio navigation data, a correlating device configured for implementing a correlation between the satellite radio navigation data and the road data contained in the memory, such as to produce parameters relating to the geometry and orientation of a road, a device as claimed in claim 8 or claim 9 arranged to receive the parameters produced by the correlating device.

12. A land vehicle comprising a device as claimed in claim 9.

Description

DESCRIPTION OF THE FIGURES

[0050] Other features, aims and advantages of the invention will become apparent from the following description, which is purely illustrative and non-limiting, and which must be read with reference to the appended drawings wherein:

[0051] FIGS. 1a, 1b, 1c: are respectively side, front and top views of a land vehicle moving on a road.

[0052] FIG. 2 schematically represents a navigation system according to an embodiment of the invention.

[0053] FIG. 3 details the steps of a method implemented by the system represented in FIG. 2, according to an embodiment of the invention.

[0054] In all the figures, similar elements bear identical reference numbers.

DETAILED DESCRIPTION OF THE INVENTION

[0055] FIGS. 1a to 1c show a vehicle 1 on a road R as well as two reference frames: a local geographical reference frame and a road reference frame.

[0056] The origin of the local reference frame is a predetermined point of the vehicle. The geographical reference frame comprises three axes X, Y, Z: [0057] the axis X is a horizontal axis pointing to the geographical North of the Earth, [0058] the axis Y is a horizontal axis pointing to the geographical West, and [0059] the axis Z is orthogonal to the axes X and Y, and oriented toward the zenith.

[0060] Moreover, the road reference frame comprises three axes Xr, Yr, Zr: [0061] the axis Xr is a longitudinal axis parallel to a main direction of circulation of a vehicle along the road, [0062] the axis Yr is a transverse axis perpendicular to the axis Xr; the two axes Xr and Yr defining a plane wherein the road extends, [0063] the axis Zr is an axis vertical to the plane defined by the axes Xr, Yr, so perpendicular to the road surface.

[0064] The origin of the road reference frame is for example the same as that of the local geographical reference frame.

[0065] The road has geometrical and orientation parameters. These road parameters are generally well-known and are for example, in France, subject to regulations which must be taken into account when designing roads (see for example the document titled Comprendre les principaux paramtres de conception gomtrique des routes, Stra 2006).

[0066] The road parameters typically comprise: [0067] a longitudinal slope angle, [0068] an angle of cant or of transverse slope, [0069] an angle of heading of the road, [0070] a road width L.

[0071] The road parameters mentioned above make it possible to pass from the local geographical reference frame or vice versa.

[0072] The respective directions of the axes Yr and Zr are particular. Specifically, if one assumes that the vehicle is moving on the road without ever leaving it, then the movement of the vehicle parallel to one of the axes Yr and Zr is of necessity bounded.

[0073] In particular, as the land vehicle is not capable of flying, the movement of the vehicle parallel to the axis Zr is assumed to be zero, or on average zero if one takes into consideration oscillations of the vehicle along the axis Zr caused by any suspension of this vehicle.

[0074] Moreover, the movement of the vehicle along a transverse directioni.e. parallel to the axis Yris limited by two opposite lateral edges of the road. Hence, a transverse movement is intended to be contained within an interval bounded by these two opposite edges (this interval is therefore of a length equal to one road width).

[0075] On a straight portion of a journey, a vehicle can in particular effect body movements which in real time cause small movements along the axes Yr and Zr. However, over the time period needed to travel said journey portion, these movements are on average zero, since the vehicle always returns to an equilibrium position both along Yr and along Zr.

[0076] Of course, this is also valid for a road portion in a turn or on a slope.

[0077] With reference to FIG. 2, the vehicle 1 comprises a navigation system comprising an inertial navigation system 2.

[0078] The inertial navigation system 2 comprises, in a manner known per se, a plurality of inertial sensors 3, typically gyroscopes and accelerometers (a single one of them being illustrated in FIG. 2).

[0079] The navigation system moreover comprises a system 4 for supplying geometrical and orientation road parameters, and a data fusion device 6.

[0080] The system 4 for supplying parameters comprises a receiver of radio navigation signals 8, a memory 10 and a correlating device 12.

[0081] The receiver 8 is known per se. It comprises an antenna configured for receiving signals emitted by one or more satellites (GPS/GNSS signals typically). The receiver 8 moreover comprises at least one processor configured for generating radio navigation data on the basis of signals received by the antenna (typically a vehicle position estimation) and delivering this data to the correlating device 12.

[0082] Moreover, the memory 10 contains a road database containing geometrical information about the roads of a geographical area wherein the vehicle is intended to be moving, such as the road represented in FIGS. 1 to 3.

[0083] The information stored in the database is typically geometrical equation parameters (line segment, clothoids), which offers the advantage of consuming little memory space.

[0084] The correlating device 12, also known per se, is configured for implementing a correlation processing known per se known as map matching, this correlation being made between the data supplied by the receiver 8 and data contained in the road database stored in the memory 8.

[0085] The fusion device 6 moreover comprises an interface 14 able to receive data from the inertial navigation system 2, and an interface 16 able to receive data computed by the correlating device 12 of the supplying system 4.

[0086] The fusion device 6 is configured for estimating vehicle navigation data (position, speed, attitude) on the basis of the data it receives via its interfaces.

[0087] The fusion device typically comprises a processor 18 configured for executing a program providing such a data estimate.

[0088] It should also be noted that the fusion device is also able to transmit data to the correlating device 12. It will be seen that such a transmission is advantageously implemented in the absence of reception by the receiver of any radio navigation signal.

[0089] With reference to FIG. 3, the following steps are implemented by the navigation system, when the vehicle 1 is moving on the road R.

[0090] The inertial system 2 acquires inertial data using its inertial sensors (step 100).

[0091] The inertial system 2 transmits the acquired inertial data to the fusion device 6.

[0092] Moreover, the receiver 8 receives radio navigation signals transmitted by satellites and generates radio navigation data on the basis of the received signals.

[0093] The correlating device 12 implements a correlation process between the radio navigation data supplied by the receiver and road data contained in the road database stored in the memory 10, such as to generate parameters relating to the geometry and the orientation of the road travelled by the land vehicle (step 104).

[0094] These parameters in particular comprise the width L of the road, or even the angles of cant, slope and heading mentioned above.

[0095] The system 4 for supplying parameters transmits the geometry and orientation parameters generated to the fusion device 6.

[0096] The fusion device 6 implements the following steps for estimating the navigation data of the land vehicle 1 (this navigation data can for example comprise a position, a speed, and an attitude of the vehicle in the local geographical reference frame).

[0097] The fusion device 6 integrates the inertial data on the basis of the parameters received from the supplying system 4, such as to produce navigation data of the vehicle. The integrating computation is carried out over a time interval of predetermined duration.

[0098] This navigation data of the vehicle comprises at least one movement of the vehicle with respect to the road R measured parallel to a direction, wherein the vehicle can only move, parallel to the direction, within a bounded interval without leaving the road.

[0099] The navigation data comprises a vertical movement of the vehicle with respect to the road R, i.e. measured parallel to the axis Zr. As has previously been said, this vertical movement is assumed to be zero or on average zero, since the vehicle cannot fly.

[0100] This navigation data further comprises a transverse movement of the vehicle with respect to the road R, i.e. measured parallel to the axis Yr. This transverse movement is, as indicated previously, limited by two opposite lateral edges of the road R. Hence, the transverse movement is contained within an interval of length equal to the width of the road as long as the vehicle is on the road R.

[0101] The fusion device 6 then estimates at least one error that affects the navigation data produced during the integrating step, by solving a system of equations making certain assumptions.

[0102] For at least one movement computed during the integrating step, the assumption is made by the fusion device 6 that a deviation between the computed movement and a reference movement associated with this computed movement constitutes an error of movement of the vehicle parallel to the direction of the movement under consideration. The reference movement associated with the computed movement has a value less than or equal to the length of the bounded interval under consideration.

[0103] When the navigation data produced during the integrating step 106 comprises a vertical movement and a transverse movement, the system of equations makes two assumptions, one for each computed movement.

[0104] The transverse reference movement associated with the computed transverse movement has a value less than or equal to the width of the road. In other words, the system of equations makes the assumption that the vehicle does not cross one of the two lateral sides of the road it is travelling on. The value of the transverse reference movement is for example zero.

[0105] Moreover, the vertical reference movement associated with the computed vertical movement is zero. In other words, the system of equations makes the assumption that the vehicle does not fly.

[0106] The fusion device 6 then corrects the navigation data produced using the error or errors estimated by solving the system of equations (step 110).

[0107] The preceding steps are repeated over time, on the basis of new radio navigation signals received by the receiver and/or new inertial data produced by the inertial navigation system.

[0108] It may be that satellite radio navigation data cannot be received (for example when the vehicle is passing through a tunnel).

[0109] When an absence of radio navigation signals is detected, the image correlating step is implemented between corrected navigation data resulting from a previous implementation of the correcting step 12, and the road data stored by the memory.

[0110] In an embodiment, the fusion device 6 implements a Kalman filter for estimating the navigation data of the vehicle.

[0111] The operation of a Kalman filter is known per se (its principle is in particular described in the document Applied Optimal Estimation, The Analytic Sciences Corporation, Ed. Arthur Gelb, 1974). As a reminder, a Kalman filter recursively estimates a state X, taking the form of a vector. The Kalman filter has two separate phases: a prediction phase, and an updating phase.

[0112] The prediction phase takes as input an estimated state produced during a previous iteration of the filter, and uses a transition matrix to produce an estimate of the state, called the predicted state.

[0113] In the updating step, observations are used to correct the state predicted in the aim of obtaining a more precise estimate. For this purpose, an observation matrix linking the state with the observations is used. The updated estimate is passed as input to the step of prediction of a subsequent iteration of the filter, and so on.

[0114] In this case, the Kalman filter is configured with a state vector X governed by the following equations:

[00001]$\frac{d.Math.X}{d.Math.t}=f\left(X,t\right)$$\frac{d.Math..Math.X}{d.Math.t}=F.Math..Math..Math.X$

[0115] where f is a non-linear function, and t denotes time. F is moreover the dynamic matrix of the Kalman filter. It is obtained by linearization of the first equation mentioning the function f. Among the components of the vector X, it is possible to find: at least the 3 components of the associated position error with respect to the linearization point, the 3 components of the associated speed error with respect to the linearization point and the 3 components of the associated attitude error with respect to the linearization point.

[0116] The state vector X is thus for example as follows in an embodiment:

[00002]$.Math..Math.X=\left[\begin{array}{c}.Math..Math.\mathrm{Lat}\\ .Math..Math.G\\ .Math..Math.Z\\ .Math..Math.\mathrm{Vx}\\ .Math..Math.\mathrm{Vy}\\ .Math..Math.\mathrm{Vz}\\ {}_x\\ {}_y\\ {}_z\\ .Math..Math.b_{\mathrm{xm}}\\ .Math..Math.b_{\mathrm{ym}}\\ .Math..Math.b_{\mathrm{zm}}\\ .Math..Math.d_{\mathrm{xm}}\\ .Math..Math.d_{\mathrm{ym}}\\ .Math..Math.d_{\mathrm{zm}}\\ .Math..Math.{\mathrm{dep}}_{\mathrm{Yr}}\\ .Math..Math.{\mathrm{dep}}_{\mathrm{Zr}}\end{array}\right]$

where [0117] Lat is a latitude error (rad) [0118] G is a longitude error (rad) [0119] Z is an altitude error (m) [0120] Vx is an error of speed along the axis Xg (m/s) [0121] Vy is an error of speed along the axis Yg (m/s) [0122] Vz is an error of speed along the axis Zg (m/s) [0123] .sub.x is an error of attitude along the axis Xg (rad) [0124] .sub.y is an error of attitude along the axis Yg (rad) [0125] .sub.z is an error of attitude along the axis Zg (rad) [0126] b.sub.xm is an error of accelerometer bias along the axis Xm (m/s.sup.2) [0127] b.sub.ym is an error of accelerometer bias along the axis Ym (m/s.sup.2) [0128] b.sub.zm is an error of accelerometer bias along the axis Zm (m/s.sup.2) [0129] d.sub.xm is an error of gyroscope drift along the axis Xm (rad/s) [0130] d.sub.ym is an error of gyroscope drift along the axis Ym (rad/s) [0131] d.sub.zm is an error of gyroscope drift along the axis Zm (rad/s) [0132] dep.sub.Yr is an error of movement along the axis Yr of the road reference frame (m) [0133] dep.sub.Zr is an error of movement along the axis Zr of the road reference frame (m)

[0134] In this case, the transition matrix F is written:

[00003]$F=\left[\begin{array}{ccccccccccccccccc}0& 0& 0& \frac{1}{R}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& -\frac{1}{R.Math..Math.\cos .Math..Math.L.Math..Math.\mathrm{at}}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 2.Math.{}_t.Math.\sin .Math..Math.\mathrm{Lat}& -2.Math.{}_t.Math.\sin .Math..Math.\mathrm{Lat}& 0& -G_0& \mathrm{sc}.Math.{\mathrm{fa}}_{m.Math..Math.x}& -\mathrm{cc}& \mathrm{sc}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -2.Math.{}_t.Math.\sin .Math..Math.\mathrm{Lat}& 0& -2.Math.{}_t.Math.\sin .Math..Math.\mathrm{Lat}& G_0& 0& \mathrm{cc}.Math.{\mathrm{fa}}_{\mathrm{mx}}& \mathrm{sc}& \mathrm{cc}& 0& 0& 0& 0& 0& 0\\ 0& 0& \frac{2.Math.G_0}{R}& 0& -2.Math.{}_t.Math.\cos .Math..Math.\mathrm{Lat}& 0& -\mathrm{sc}.Math.{\mathrm{fa}}_{\mathrm{mx}}& -\mathrm{cc}.Math.{\mathrm{fa}}_{m.Math..Math.x}& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -\mathrm{sr}& -\mathrm{cr}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

where: [0135] Lat is a latitude of the vehicle (rad) [0136] f.sub.x.sub.m is a force measured by an accelerometer along the axis Xg (m/s.sup.2) [0137] f.sub.y.sub.m is a force measured by an accelerometer along the axis Yg (m/s.sup.2) [0138] f.sub.z.sub.m is a force measured by an accelerometer along the axis Zg (m/s.sup.2) [0139] .sub.t is an angular rotation speed of the Earth (radis) [0140] G.sub.0 is a value of the average gravity (m/s.sup.2) [0141] cc is the cosine of the heading of the vehicle [0142] sc is the sine of the heading of the vehicle [0143] cr is the cosine of the heading of the road [0144] sr is the sine of the heading of the road [0145] R is the Earth's radius

[0146] During the prediction step, the Kalman filter computes the predicted state using the dynamic matrix F.

[0147] The Kalman filter moreover uses as observation the movement along the axis Yr and the movement along the axis Zr. In this case, the observation matrix H of the Kalman filter is written:

[00004]$H=\left[\begin{array}{ccccccccccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

[0148] The innovation of the Kalman filter is then computed.

[0149] The innovation is, in a manner known per se, a deviation between reference data and data observed by the filter.

[0150] The innovation Inno is for example as follows:

[00005]$\mathrm{Inno}=\left[\begin{array}{c}0\\ 0\end{array}\right]-\left[\begin{array}{c}{\mathrm{dep}}_{y.Math..Math.r}\\ {\mathrm{dep}}_{\mathrm{zr}.Math.}\end{array}\right]$

[0151] With:

[0152] dep.sub.yr: Movement of the vehicle along the y axis of the road reference frame

[0153] dep.sub.zr: Movement of the vehicle along the z axis of the road reference frame

[00006]$\left[\begin{array}{c}{\mathrm{dep}}_{y.Math..Math.r}\\ {\mathrm{dep}}_{\mathrm{zr}.Math.}\end{array}\right]=\left[\begin{array}{c}{}_0^t.Math.\left(-\mathrm{sr}.Math.\mathrm{Vx}-\mathrm{cr}.Math.\mathrm{Vy}\right).Math.\mathrm{dt}\\ {}_0^t.Math.\mathrm{Vz}.Math.\mathrm{dt}\end{array}\right]$

[0154] In this computation, the two-component zero vector is the reference movement vector discussed above. The innovation of the Kalman filter therefore corresponds to a vector of deviations between a vector of computed movements and a vector of zero reference movements. The use of this innovation by the Kalman filter during its implementation is illustrative of the assumptions made, namely that the vehicle is not leaving the road by moving along the axes Yr and Zr.