METHOD FOR DETERMINING THE POSITION AND ORIENTATION OF A VEHICLE

Description

[0001] The invention relates to a method and system for determining the position and orientation of a vehicle. The invention also relates to a data recording medium for implementing this method.

[0002] Numerous methods for determining a vehicle's position and orientation are known. For example, a presentation of the state of the art in this field can be found in the following thesis: S. Godha, "Performance Evaluation of Low Cost MEMS-Based IMU Integrated With GPS for Land Vehicle Navigation Application", PhD report, 2006. This thesis is hereafter referred to as "Godha2006". The state of the art is also known from US2009 / 326740A1, US2018 / 017390A1, and the article by Syed Zainab et al.: "Economical and Robust Inertial Sensor Configuration for a Portable Navigation System", GNSS 2007 - Proceedings of the 20th International Technical Meeting of the Satellite Division of the Institute of Navigation, USA, September 28, 2007, pages 2129-2135.

[0003] Typically, an inertial measurement integration module constructs an estimated position Pe and an estimated orientation Oe of the vehicle from: measurements from an accelerometer and a gyroscope mounted in the vehicle, and the previous position and orientation determined for that vehicle.

[0004] Next, this estimated position Pe and estimated orientation Oe are corrected by a correction module to obtain a corrected position Pc and a corrected orientation Oc. The corrected position Pc and corrected orientation Oc are more accurate and are output to the localization system as the determined position and orientation for the vehicle. This corrected position Pc and corrected orientation Oc are also acquired by the integration module and then used by this module as, respectively, the previous position and previous orientation to construct the next estimated position and orientation of the vehicle.

[0005] The correction module corrects the position Pe and orientation Oe by taking into account measurements from sensors other than the onboard accelerometer and gyroscope. In particular, measurements from a satellite geolocation unit and, possibly, measurements from other sensors are used by the correction module for this purpose. Such a method for determining the position and orientation of a vehicle is disclosed, for example, in US application 2009 / 326740.

[0006] Furthermore, the measurements taken into account by the correction module very often include a measurement of the vehicle's yaw angle from another sensor, which differs from the estimate of this angle contained in the estimated orientation Oe. This measured yaw angle is subsequently denoted Ψm. The yaw angle Ψm is different from that which can be deduced from the orientation Oe.

[0007] The angle Ψm can be obtained in various ways. For example, it can be obtained from the speed measurements of the satellite geolocation unit. In this case, it is referred to as "GNSS heading". The angle Ψm can also be obtained from magnetometer measurements.

[0008] The invention aims to improve these known methods of determining the position and orientation of the vehicle so that they converge more quickly to a precise position and orientation of the vehicle.

[0009] Its object is therefore a method for determining the position and orientation of a vehicle according to claim 1.

[0010] The invention also relates to an information storage medium, readable by a microprocessor, comprising instructions for carrying out the above process, when these instructions are executed by a microprocessor.

[0011] Finally, the invention also relates to a localization system configured to implement the above method.

[0012] The invention will be better understood upon reading the following description, given solely by way of non-limiting example and made with reference to the drawings in which: there figure 1 is a schematic illustration of a vehicle location system; the figure 2 is a schematic illustration of different software modules implemented in the system of the figure 1 ; there figure 3 is a flowchart of a method for determining the position and orientation of a vehicle using the system of the figure 1 ; THE Figures 4 and 5 are graphs illustrating the evolution over time of the margin of error on orientation determined for the vehicle.

[0013] In these figures, the same references are used to designate the same elements.

[0014] In the remainder of this description, the well-known characteristics and functions of a person skilled in the art are not described in detail. In particular, for general knowledge of a person skilled in the art concerning vehicle location systems using a satellite geolocation unit and an inertial navigation unit, reference is made, for example, to the Godha2006 thesis.

[0015] In this description, detailed examples of embodiments are first described in Chapter I with reference to the figures. Then, in the following Chapter II, variations of these embodiments are presented. Finally, the advantages of the different embodiments are presented in Chapter III. Chapter I: Examples of Implementation Methods

[0016] There figure 1represents a motor vehicle 2 capable of moving on land. To this end, it typically has wheels or tracks. The vehicle 2 is also equipped with means 4 of propulsion such as an engine that drives the wheels or tracks.

[0017] Vehicle 2 is also equipped with a vehicle localization system 6. This system 6 is capable of determining the position, orientation, and velocity of vehicle 2 in a terrestrial frame of reference RT. Here, the terrestrial frame of reference RT is fixed to the Earth without any degrees of freedom. The frame of reference RT typically has three axes that are orthogonal to each other. A movable frame of reference Rb is also fixed to vehicle 2 without any degrees of freedom. This frame of reference Rb has three orthogonal axes, denoted xb, yb, and zb, respectively. Conventionally, when vehicle 2 moves horizontally, the xb and yb axes lie in a horizontal plane, and the zb axis is vertical. Here, the xb axis is oriented and points in the direction in which the vehicle is moving forward.

[0018] Here, the position of vehicle 2 in the RT frame is expressed by the latitude L, the longitude λ and the altitude h of the origin of the frame R b.

[0019] The orientation of vehicle 2 is expressed by the yaw angle ψ, the pitch angle θ, and the roll angle φ of the frame Rb relative to the frame RT. In practice, the vehicle's orientation is most often represented as an orientation matrix from which the yaw angle, pitch angle, and roll angle can be deduced. The vehicle's orientation can also be represented as a vector directly containing the yaw angle, pitch angle, and roll angle. Subsequently, we consider that these two cases are equivalent and therefore that the orientation of the vehicle includes the yaw angle, the pitch angle and the roll angle of the vehicle from the moment that these three angles can be deduced directly from a matrix or a vector.

[0020] The position, orientation and speed determined by system 6 are delivered on an output 7.

[0021] Typically, vehicle 2 includes a control station 8 to guide or assist in guiding vehicle 2 to a predefined destination. The control station 8 is connected to output 7. The control station 8 can be a manual and / or automatic control station. In the case of a manual control station, the determined position, orientation, and speed are transmitted to a human-machine interface to assist a human in piloting the propulsion means 4. In the case of an automatic control station, the determined position, orientation, and speed are automatically converted into piloting commands for the propulsion means 4, and then automatically transmitted to these propulsion means 4.

[0022] System 6 includes a satellite geolocation unit 10 and an inertial measurement unit 12. Unit 10 is known by the acronym GNSS (“ Global Navigation Satellite System » . From the satellite signals it receives, unit 10 generates signals representing the vehicle's position and speed in the RT frame of reference. Unit 10 is a single-antenna geolocation unit, not a multi-antenna geolocation unit. Therefore, unit 10 is unable to measure the absolute orientation of vehicle 2 in the RT frame of reference from the signals emitted by the satellites.

[0023] Unit 12 is known by the acronym IMU ( Inertial Measurement Unit " .Unit 12 includes, in particular, a triaxial accelerometer 14 and a triaxial gyroscope 16. Thanks to these sensors, Unit 12 is able to measure the change in the orientation of Vehicle 2. However, Unit 12 is also unable to directly measure the orientation of Vehicle 2 in the RT frame. Here, the measurement axes of the accelerometer 14 and the gyroscope 16 coincide, respectively, with the xb, yb, and zb axes of the Rb frame. Furthermore, the accelerometer 14 is arranged so that a positive measurement of the acceleration of Vehicle 2 along the xb axis means that Vehicle 2 is accelerating while moving forward.

[0024] To determine the position, orientation, and speed of vehicle 2 from measurements of units 10 and 12, the system 6 includes a programmable electronic control unit 20. This control unit 20 is capable of acquiring measurements from units 10 and 12 and, from these measurements, determining the position, orientation, and speed of vehicle 2 in frame RT. The control unit 20 includes a microprocessor 22 and a memory 24 containing the instructions and data necessary for implementing the method described with reference to the figure 3 .

[0025] More specifically, memory 24 contains the instructions of a software module 26 capable of determining the position, orientation and speed of vehicle 2 from the measurements of units 10 and 12 when it is executed by the microprocessor 22. The module 26 implements in particular a fusion algorithm which establishes, from a previous estimate of the position, orientation and speed of vehicle 2 and new measurements of units 10 and 12 acquired since this previous estimate, a new estimate of the position, orientation and speed of vehicle 2. The fusion algorithm also establishes margins of error on each new estimate.

[0026] The general principles of fusion algorithms are well known to those skilled in the art. For example, interested readers can refer once again to the previously cited Godha2006 thesis. Typically, this fusion algorithm implements one or more Kalman filters. Here, module 26 implements an architecture known as a "closed-loop integration scheme" or "closed-loop approach."

[0027] There figure 2 The architecture of module 26 is described in more detail. Module 26 comprises an inertial measurement integration sub-module 30 and a correction sub-module 32. Such sub-modules 30 and 32 are known. For example, for a detailed description of various possible embodiments of these sub-modules, the reader may consult Chapter 4 of Godha's 2006 thesis. Therefore, only the details necessary for understanding the invention are described in detail hereafter.

[0028] Submodule 30 is known as "Mechanization". For each time k, submodule 30 constructs a rough estimate of the position Pe(k), orientation Oe(k), and velocity Ve(k) of vehicle 2. Each position Pe(k), orientation Oe(k), and velocity Ve(k) of vehicle 2 is a vector with three coordinates. The coordinates of the position Pe(k) in the RT frame are denoted xe(k), ye(k), and ze(k). The coordinates of the orientation Oe(k) are denoted ψe(k), θe(k), and φe(k). Successive times k are separated from each other by a period Te. The time immediately preceding time k is denoted k-1.

[0029] Submodule 30 constructs the position Pe(k), the orientation Oe(k) and the velocity Ve(k) from: of the previous position P d (k-1), the previous orientation O d (k-1) and the previous velocity V d (k-1) determined for vehicle 2 at time k-1 by system 6, and of the measurements of accelerometer 14 and gyroscope 16 acquired by sub-module 30 since time k-1.

[0030] The combination of submodule 30 and unit 12 forms what is known by the acronym INS ("Inertial Navigation System").

[0031] Submodule 32 corrects the position Pe(k), the orientation Oe(k), and the velocity Ve(k) constructed by submodule 30 to obtain a corrected position Pc(k), a corrected orientation Oc(k), and a corrected velocity Vc(k). The position, orientation, and velocity are corrected based on the unit measurements 10. To this end, submodule 32 includes a Kalman filter 34 and an adder 36. Here, filter 34 is known by the English term "Error State Kalman Filter" because it estimates corrections to be applied to the position, orientation, and velocity estimated by submodule 30. Thus, filter 34 establishes, for each instant k, a state vector Xk|k. The state vector X k|k contains in particular correction coefficients for position P e (k), orientation O e (k) and velocity V e (k).The adder 36 combines the correction coefficients established by the filter 34 with the position P e (k), the orientation O e (k) and the velocity V e (k) to obtain the corrected position P c (k), the corrected orientation O c (k) and the corrected velocity V c (k).

[0032] For example, here, the state vector X k|k contains correction coefficients δ x (k), δ y (k) and δ z (k) for the coordinates, respectively, xe (k), ye (k) and ze (k) of the position P e (k). The adder 36 adds these coefficients δ x (k), δ y (k) and δ z (k), respectively, to the coordinates xe (k), ye (k) and ze (k) to obtain the coordinates, respectively, xc (k), yc (k) and zc (k) of the corrected position P c (k).

[0033] The state vector X k|k also includes correction coefficients δ ψ (k), δ θ (k) and δ φ (k), respectively, of the coordinates ψ e (k), θ e (k) and φ e (k) of the orientation O e (k). The adder 36 adds these coefficients δ ψ (k), δ θ (k) and δ φ (k), respectively, to the coordinates ψ e (k), θ e (k) and φ e (k) to obtain the corrected coordinates, respectively, ψ c (k), θ c (k) and φ c (k) of the orientation O c (k).

[0034] Classically, the state vector X k|k also includes correction coefficients to correct other parameters, such as the velocity V e (k), measurement biases of the accelerometer 14 and the gyroscope 16 or others.

[0035] Filter 34 is a recursive algorithm which, at each instant k, provides the adder 36 with a new state vector X k|k calculated from: of the previous state vector X k-1|k-1, unit measurements 10 acquired since time k-1, and of position P e (k), orientation O e (k) and velocity V e (k) constructed by submodule 30.

[0036] However, filter 34 does not directly acquire or use a yaw angle measurement Ψm. In other words, filter 34 does not receive or use a yaw angle measurement obtained from measurements of a sensor other than unit 12.

[0037] Typically, filter 34 comprises a block 38 for predicting a state vector X k|k-1 and a block 40 for updating the predicted vector X k|k-1. These blocks are executed one after the other for each vector X k|k.

[0038] More precisely, block 38 constructs a prediction X k|k-1 of the state vector from the previous state vector X k-1 | k-1.

[0039] Here, an example of an embodiment of blocks 38 and 40 is described in the particular case where filter 34 is a linear Kalman filter.

[0040] The propagation or prediction equation of the state of filter 34 implemented by block 38 is defined by the following relation (1): X k|k-1 = A k-1 X k-1|k-1 + ν, where: X k-1|k-1 is the estimate of the state vector at time k-1 obtained by taking into account all the measurements up to time k-1, X k|k-1 is the prediction of the state vector at time k obtained by taking into account all the measurements up to time k-1 and without taking into account the measurements made at time k, A k-1 is the state transition matrix, v is the noise of the process.

[0041] The propagation or prediction equation of the error covariance matrix implemented by block 38 is defined by the following relation (2): P k|k-1 = A k-1 P k-1|k-1 A k-1 T< + Q k-1 , where: P k-1|k-1 is the estimate of the error covariance matrix at time k-1 obtained by taking into account all measurements up to time k-1, P k|k-1 is the estimate of the covariance matrix P k at time k obtained by taking into account only the measurements up to time k-1, Q k-1 is the covariance matrix of process noise v.

[0042] Block 40 corrects the prediction Xk|k-1 of the state vector to obtain the state vector Xk|k. The corrected vector Xk|k is constructed as a function of a difference Yk between: an estimate ẑ k of the measurements of physical quantities at time k, constructed from the prediction X k|k-1 provided by block 38, and the measurements zk of these physical quantities carried out at time k.

[0043] The Yk deviation is known as the "innovation". Here, the physical quantities measured are position and velocity deviations. No deviation between a measurement of the angle ψm(k) and its estimate is used.

[0044] The estimate ẑ k is obtained using the following relation (3): ẑ k = H k X k|k-1 , where H k is the measure matrix.

[0045] The zk measurements are obtained from the difference between the position P e (k), the velocity V e (k) and, respectively, a GPS position P (k) and a GPS velocity V (k) obtained only from the unit 10 measurements.

[0046] The innovation Y k is obtained using the following relation (4): Y k = zk - ẑ k .

[0047] Typically, block 40 corrects the prediction X k|k-1 by adding the innovation Y k multiplied by the Kalman gain K k. The gain K k is calculated using the following relation (5): K k = P k|k-1 H k T< (H k P k|k-1 H k T< + R k ) -1< , where the matrix R k is the covariance matrix of the noise on the measurements.

[0048] Next, the state vector X k|k is obtained using the following relation (6): X k| k = X k|k-1 + K k Y k .

[0049] The updated covariance matrix of the error at time k is calculated using the following relation (7): Pk|k = (I - Kk Hk)Pk|k-1, where I is the identity matrix. The matrix Pk|k contains the margins of error on the estimates of the correction coefficients. Thus, in particular, it contains the margins of error on the estimation of the coefficients δψ(k), δθ(k), and δφ(k).

[0050] In this particular embodiment, the adder 36 is a simple adder which adds to the position Pe(k), the orientation Oe(k) and the velocity Ve(k) the corresponding correction coefficients contained in the state vector Xk|k. Then, the adder 36 delivers to an output of the submodule 32 the corrected position Pe(k), the orientation Oc(k) and the velocity Vc(k) thus obtained.

[0051] Module 26 also includes a replacement sub-module 50. Sub-module 50 acquires, in particular, the position Pe(k), the orientation Oc(k), and the velocity Vc(k) provided by the correction sub-module 32. Then, it outputs, on output 7 of system 6, a position Pd(k), an orientation Od(k), and a velocity Vd(k) as, respectively, the position, orientation, and velocity of vehicle 2 determined by system 6 at time k. The operation of sub-module 50 is described in more detail with reference to the process of the figure 3 .

[0052] The position P d (k), the orientation O d (k) and the velocity V d (k) are also acquired by the integration submodule 30 which uses them as previous position, previous orientation and previous velocity to construct the estimated position P e (k+1), the orientation O e (k+1) and the velocity V e (k+1).

[0053] The use of system 6 is divided into successive periods of activity separated by periods of inactivity. During periods of inactivity, system 6 is not active; that is, it does not determine the position and orientation of vehicle 2. Typically, during periods of inactivity, units 10 and 12 do not take any measurements and do not transmit any measurements to control unit 20. Control unit 20 therefore does not perform any processing on these measurements. These periods of inactivity generally last several minutes, several hours, or several days. For example, during periods of inactivity, system 6 is switched off or in standby mode.

[0054] Conversely, during each active period, units 10 and 12 send new measurements to the computer 20, which processes them to determine the position, orientation, and speed of vehicle 2 based on these new measurements. These active periods follow one another sequentially and are each separated by a period of inactivity of varying length.

[0055] The operation of system 6 during one of these periods of activity will now be described with reference to the process of the figure 3 .

[0056] The operating period begins with a system 6 initialization phase 48. This phase 48 starts immediately after system 6 is activated, typically right after it is powered on. During this phase 48, various variables are initialized, such as initial estimates of the yaw, roll, and pitch angles. Algorithms exist that allow for quickly obtaining an initial estimate of the roll and pitch angles, as well as other desired parameters. For example, the initial estimate of the roll and pitch angles is obtained from measurements of the accelerometer 14. During phase 48, correction coefficients for the measurements of the accelerometer 14 and the gyroscope 16 can also be initialized.

[0057] The yaw angle is also initialized. However, at this stage, it is very difficult to obtain an initial estimate of the yaw angle without another sensor capable of directly and reliably measuring it. Therefore, the initial value of the yaw angle is chosen arbitrarily. For example, the yaw angle is systematically initialized to zero. To account for the large uncertainty in this initial yaw angle estimate, the margin of error on this initial estimate is initialized to a significant value. Here, the margin of error on the yaw angle estimate is represented by the standard deviation σψ. The initial value σψ(0) of the standard deviation σψ is thus initialized to a value greater than 60°, 90°, or 180°. Here, the value σψ(0) is initialized to 180°. The standard deviation σ ψ is equal to the standard deviation σ δψ on the estimation of the coefficient δ ψ.Therefore, the initial margin of error σ δψ (0) on the coefficient δ ψ (0) is taken to be 180°. The coefficient corresponding to the margin of error on the coefficient δ φ (0) in the matrix P 0|0 is thus initialized as a function of this initial margin of error σ δψ (0).

[0058] Once the initialization phase 48 is complete, a phase 70 for the execution of module 26 begins. This phase 70 then lasts until the end of the active period and therefore until the beginning of the next inactive period.

[0059] During phase 70, the fusion algorithm is executed repeatedly by module 26. For example, each time new measurements of unit 10 and / or unit 12 are acquired at time k by computer 20, the fusion algorithm is executed to update the estimate of the position, orientation, and speed of vehicle 2 at that time k.

[0060] At each execution of the merge algorithm, during step 72, module 26 establishes: the new estimates of the position P c (k), the orientation O c (k) and the velocity V c (k) of vehicle 2, and the new margins of error on these new estimates of position, orientation and velocity.

[0061] Step 72 includes an operation 74 in which the accelerometer 14 and the gyroscope 16 measure, respectively, the acceleration and angular velocity of the vehicle at time k. Then, during operation 74, submodule 30 constructs the estimates Pe(k), Oe(k) and Ve(k) from: of the previous position P d (k-1), of the previous orientation O d (k-1) and of the previous velocity V d (k-1), and of the measurements of the accelerometer 14 and of the gyroscope 16 made at time k.

[0062] During an operation 76, unit 10 measures the position and velocity of vehicle 2 at time k. Then, during operation 76, submodule 32 corrects the position P e (k), the orientation O e (k) and the velocity V e (k) to obtain the corrected position P c (k), the corrected orientation O c (k) and the corrected velocity V c (k).

[0063] More specifically, during operation 76, block 38 constructs the predictions Xk|k-1 and Pk|k-1. To do this, block 38 uses relations (1) and (2) described earlier. Consequently, block 38 does not use the measurements of unit 10 taken at time k. Then, block 40 obtains the state vector Xk|k and the matrix Pk|k by implementing relations (3) to (7) described earlier. Therefore, block 40 uses the measurements of the position and velocity of vehicle 2 taken by unit 10 at time k.

[0064] Finally, adder 36 adds the correction coefficients contained in the vector X k|k to the corresponding coordinates of the position P e (k), the orientation O e (k), and the velocity V e (k) to obtain the position P c (k), the orientation O c (k), and the velocity V c (k). The orientation O c (k) therefore contains, at this stage, a corrected yaw angle ψ c (k), a corrected pitch angle θ c (k), and a corrected roll angle φ c (k). The matrix P k|k contains a coefficient σ δψ (k) 2< which represents the margin of error on the coefficient δ ψ (k).

[0065] In parallel, after or before step 72, during step 80, submodule 50 obtains the yaw angle ψm(k) measured at time k. Here, the angle ψm(k) is obtained from the measurements veast(k) and vnorth(k) of the vehicle 2's velocity in the directions pointing, respectively, towards the East and North. Here, the velocities veast(k) and vnorth(k) are directly delivered by unit 10 to module 26. The angle ψm(k) thus obtained is different from the yaw angle contained in the orientations Oe and Oc.

[0066] Thus, step 80 begins with an operation 82 of acquisition by the computer 20 of the measurements v east (k) and v north (k) delivered by the unit 10 at time k.

[0067] Then, during an operation 84, submodule 50 calculates an angle α using the following relation: α = atan(v east (k) / v north (k)), where the symbol "atan" denotes the arctangent function.

[0068] The measurements v east(k) and v north(k) do not allow us to determine whether vehicle 2 is moving forward or backward. Therefore, during operation 86, submodule 50 detects whether the vehicle is moving backward. To do this, when vehicle 2 is stationary, the component v xb of vehicle 2's velocity along the xb axis is initialized to zero. The component a xb of vehicle 2's acceleration measured by accelerometer 14 is also initialized to zero. Then, each time computer 20 acquires a new measurement from accelerometer 14, submodule 50 calculates the velocity v xb(k) of vehicle 2 along the xb axis using the following relation: v xb(k) = v xb(k-1) + (a xb(k) - b ax)Te, where: v xb (k-1) is the previous speed of vehicle 2 along the xb axis calculated at time k-1, a xb (k) is the acceleration of vehicle 2 along the xb axis constructed from the acceleration measured, along the xb axis, at time k by the accelerometer 14, b ax is a correction coefficient initialized during phase 48 to correct a measurement bias of the accelerometer 14 along the xb axis, and T e is the duration of the time interval between times k and k-1.

[0069] Preferably, the value a xb (k) is constructed so as to minimize or eliminate the contribution of Earth's gravity to the acceleration measured by the accelerometer 14. For example, the value a xb (k) is constructed by filtering, using a high-pass filter, the measurements of the accelerometer 14 or by averaging, over a sliding window, the measurements of the accelerometer 14.

[0070] If the calculated speed vxb(k) exceeds a predetermined threshold S1, then sub-module 50 detects that vehicle 2 is moving forward. For this to occur, the threshold S1 must be greater than or equal to zero. For example, in this case, the threshold S1 is zero. Conversely, if the speed vxb(k) is less than -S1, then module 50 detects that vehicle 2 is moving backward.

[0071] If submodule 50 detects that vehicle 2 is moving forward, during step 88, the angle ψ m (k) is taken to be equal to the calculated angle α.

[0072] Conversely, during an operation 90, if sub-module 50 detects that vehicle 2 is moving in reverse, the angle ψ m (k) is taken to be equal to α+π rad.

[0073] Finally, during operation 92, submodule 50 calculates the margin of error σ ψm (k) on the measured yaw angle. For example, submodule 50 uses the following relationship for this purpose: σ ψ m k = ν north k σ north k 2 + ν east k σ east k 2 ν east k 2 + ν north k 2 Or : σ east (k) and σ north (k) are the standard deviations on the measurements, respectively, of the velocities v east (k) and v north (k).

[0074] Here, the standard deviations σ east (k) and σ north (k) are provided by unit 10, at the same time as it provides the velocities v east (k) and v north (k).

[0075] Once submodule 50 has obtained the position P e (k), the orientation O c (k), the velocity V c (k) and the angle ψ m (k), in a step 100 it checks the relevance of using the angle ψ m (k) instead of the angle ψ c (k).

[0076] Here, using the angle ψm(k) instead of the angle ψc(k) is considered appropriate if it improves the orientation determined for vehicle 2. To this end, in step 100, submodule 50 checks whether a first set of predetermined conditions is met. As long as this first set of conditions is not met, then using the angle ψm(k) instead of the angle ψc(k) is considered appropriate.

[0077] Here, this first set of conditions comprises one or more predetermined conditions. In this case, the set of conditions is considered satisfied as soon as at least one of the predetermined conditions of this first set of conditions is met. In this embodiment, by way of example, the conditions of the first set of conditions are chosen from the group consisting of the following conditions: condition (11): σ ψm k > 1 / β σ ψc k , condition (12): σ ψc k < S 12 , condition (13): ψ m k − ψ c k < S 13 , condition (14): 1 N + 1 ∑ i = k − N i = k Ψ m i − Ψ c i < S 14 where: N is a predetermined constant greater than two or four or ten and less than, generally, one hundred, fifty or twenty, i is an integer index that varies between kN and k σ ψm (i) is the standard deviation on the measure of the angle ψ m (i), σ ψc (k) is the standard deviation on the corrected angle ψ c (k), β is a constant greater than or equal to one and generally less than or equal to 100 or 50, and preferably between 1 and 10, the thresholds S 12 , S 13 and S 14 are predetermined constants, the symbol |...| denotes the absolute value of the term between the vertical bars.

[0078] The margin of error on the angle ψc(k) is equal to the margin of error on the coefficient δψ(k). Thus, the standard deviation σψc(k) is equal to the standard deviation σδψc(k) on the estimation of the coefficient δψc(k). The standard deviation σδψc(k) is obtained from the coefficient of the covariance matrix Pk|k established by filter 34 and corresponding to the margin of error on the coefficient δψ(k).

[0079] The thresholds S 12, S 13 and S 14 are typically greater than 1°. Preferably, they are between 1° and 15° or between 1° and 10°.

[0080] When the first set of conditions is not satisfied, the process continues with a step 102. Conversely, when this first set of conditions is satisfied, the process continues with a step 104.

[0081] In step 102, submodule 50 simply replaces the angle ψc(k) with the angle ψm(k) to obtain the orientation Od(k). In this step 102, submodule 50 leaves unchanged, in particular, the position Pc(k), the velocity Vc(k), and the corrected roll and pitch angles. Thus, the position Pd(k) and the velocity Vd(k) provided by submodule 50 are identical, respectively, to the position Pc(k) and the velocity Vc(k).

[0082] In step 104, conversely, submodule 50 does not replace the angle ψc(k) with the angle ψm(k). Thus, in this case, the position Pd(k), the orientation Od(k) and the velocity Vd(k) are equal, respectively, to the position Pc(k), the orientation Oc(k) and the velocity Vc(k).

[0083] At the end of steps 102 and 104, during a step 106, the sub-module 50 delivers on output 7 the position P d (k), the orientation O d (k) and the velocity V d (k).

[0084] In parallel, during step 108, submodule 30 acquires the position P d (k), the orientation O d (k) and the velocity V d (k). Then, submodule 30 uses this position P d (k), this orientation O d (k) and this velocity V d (k) as, respectively, the previous position, previous orientation and previous velocity to construct the next position P e (k+1), the next orientation O e (k+1) and the next velocity V e (k+1).

[0085] At the start of system 6, the margin of error σψc(k) on the angle ψc(k) is significant. Consequently, the initial set of conditions is generally not met during a transient phase that occurs immediately after system 6 starts. Thus, during this transient phase, system 6 provides the orientation Od(k) in which the angle ψc(k) has been replaced by the angle ψm(k). This allows for a more precise vehicle orientation during this transient phase.

[0086] Simultaneously, the measurement ψm(k) is not transmitted to filter 34 and is not used by filter 34 to construct the state vector Xk|k, which corrects the position, orientation, and velocity estimated by submodule 30. In particular, the angle ψm(k) is not used by block 40. Therefore, the difference between the angles ψc(k) and ψm(k) is not taken into account to correct the position Pe(k), orientation Oe(k), and velocity Ve(k) provided by submodule 30 during this transient phase. However, during the transient phase, the angle ψm(k) is taken into account in the subsequent estimation of the vehicle's position, orientation, and velocity through the feedback implemented in step 108.

[0087] As explained below, with reference to the graphs of Figures 4 and 5Compared to a conventional use of the angle ψ m (k), this particular use of the angle ψ m (k) allows the convergence of system 6 to be accelerated towards a precise and stable orientation of vehicle 2.

[0088] Then, as soon as the margin of error on the angle ψ c (k) is sufficiently small, in the case of the process of the figure 3The angle ψm(k) is no longer used to determine the vehicle's orientation. Discontinuing the use of angle ψm(k) once the first set of conditions is met allows for a more accurate estimate of the yaw angle than simply using the measured angle ψm(k). This is because, at this stage, the margin of error on the corrected yaw angle is small, and filter 34 then allows for a precise value of this angle. Furthermore, an erroneous measurement of the yaw angle ψm(k), for example, caused by a temporary poor reception of satellite signals by unit 10, degrades the margin of error on the position, orientation, and speed determined for vehicle 2 less rapidly.

[0089] There figure 4represents the evolution over time of the error margins on the angles ψ c (curve 120), θ c (curve 121) and roll φ c (curve 122) in the case where system 6 is implemented and where the initial difference between the angles ψ c (O) and ψ m (O) is equal to -90 degrees.

[0090] The graph of the figure 5 corresponds to a conventional location system. Here, this conventional system is identical to system 6, except that: The substitution submodule 50 is omitted, and the measured angle ψ m is passed to the Kalman filter and used by block 40 to update the state vector X k|k as a function of the difference between the predicted yaw angle and the measured yaw angle ψ m obtained from the unit 10 measurements.

[0091] On the Figures 4 and 5 The x-axis and y-axis represent, respectively, time, expressed in seconds, and the margin of error, expressed in degrees. figure 5represents the evolution over time of the error margins on the yaw angle ψ c (curve 130), on the pitch angle θ c (curve 131) and on the roll angle φ c (curve 132) in the case of the conventional system and in the case where the initial difference between the angles ψ c and ψ m is also equal to -90 degrees.

[0092] In these figures, vehicle 2 starts moving at time t=100 seconds.

[0093] It can be observed that the margin of error in the estimation of angles θc and φc converges much more rapidly to a small value in the case of system 6 than in the case of the conventional system. Furthermore, with the conventional system, the amplitude of the oscillations of the margins of error in angles θc and φc is much greater than in the case of system 6.

[0094] The fact that system 6 converges more quickly to a precise vehicle orientation is explained by the following phenomenon. In a conventional system, the significant difference between the angle ψm(k) and ψc(k) leads to a strong correction of the angle ψc(k), but also to corrections of other parameters of the state vector Xk|k, particularly the predictions of the pitch and roll angles. This also stems from the fact that the system of equations for filter 34 is obtained by assuming that the error in the yaw, pitch, and roll angles is always small, and therefore small from the outset. However, this is not the case in practice. Thus, during the transient phase, the significant difference between the angles ψm(k) and ψc(k) results in a degradation of the accuracy in estimating the angles θc(k) and φc(k).In system 6, since the difference between the angles ψ m (k) and ψ c (k) is not used by filter 34 to correct the different parameters of the state vector X k|k, the increase in the margin of error on the angles θ c (k) and φ c (k) is avoided. Chapter II: Variants Variants relating to obtaining the angle ψ m (k)

[0095] There are many other possible embodiments for step 86, which detects the reverse movement of vehicle 2. For example, in a particularly simple embodiment, the vehicle has a specific sensor that directly detects the direction in which vehicle 2 is moving. In this case, during step 86, submodule 50 acquires measurements from this specific sensor and detects the reverse movement based on these measurements. The specific sensor detects, for example, the engagement of reverse gear in the vehicle's transmission. The specific sensor could also be an odometer that measures the direction and distance traveled by vehicle 2.

[0096] Reversing can also be detected from the xc(k) coordinate of the velocity vc(k) established by submodule 32. If the velocity vxc(k) is negative, this means that the vehicle is moving in reverse. However, this implementation is highly dependent on the margin of error on the corrected velocity vc(k).

[0097] The method for carrying out step 86 described with reference to the figure 3 This works well, particularly when the pitch and roll angles of vehicle 2 are practically zero. Otherwise, it is possible to compensate for the effect of Earth's gravity on the vehicle's acceleration measurement by taking into account the pitch and roll angle values.

[0098] In another embodiment, the vehicle's reverse movement is detected by testing the following condition: |α-(ψ c (k)-π)| < |α-(ψ c (k)|. If this condition is met, it means the vehicle is moving in reverse. Conversely, if this condition is not met, it means the vehicle is moving forward. In another variant, the term ψ c (k) in the above condition is replaced by a yaw angle measured by a sensor independent of the unit. This additional sensor is, for example, a triaxial magnetometer.

[0099] In a simplified embodiment, unit 10 does not provide a measurement of the vehicle's speed, but only its position. In this case, the speeds v east (k) and v north (k) needed to calculate the angle α are deduced from the successive positions provided by unit 10. For example, the speeds v east (k) and v north (k) are obtained using the following relationships: v east k = x east k − x east k − 1 / T e , And v north k = y north k − y north k − 1 / T e , Or : x east (k) and y north (k) are the vehicle positions 2 in the East and North directions provided by unit 10 at sampling time k, and x east (k-1) and y north (k-1) are the vehicle positions along the East and North directions provided by unit 10 at sampling time k-1, and T e is the sampling period.

[0100] In the case above, the angle α can be directly calculated using the following relationship: α = atan x east k − x east k − 1 y north k − y north k − 1

[0101] The angle ψm(k) can be obtained using sensors other than the unit 10. For example, alternatively, the angle ψm(k) is obtained from the measurements of a magnetometer. In this case, the angle ψm(k) is, for example, obtained using the following relation: ψm(k) = -atan(by(k) / bx(k)), where: bx(k) and by(k) are the components of the magnetic field measured, respectively, along the xb and yb directions of the frame Rb.

[0102] It should be noted that when the angle ψm(k) is measured using a magnetometer, it is not necessary to determine the direction in which the vehicle is moving. In this case, the angle ψm(k) measurement provided is an absolute measurement. When the angle ψm(k) is measured using a sensor other than unit 10, unit 10 is not necessarily used to obtain the angle ψm(k). However, it is also possible to combine the angle ψm(k) measurement obtained from unit 10 with the angle ψm(k) measurement obtained from other sensors to obtain a more precise angle ψm(k) measurement.

[0103] Alternatively, step 82 also includes a filtering operation of the raw velocity measurements v east (k) and v north (k) before using these measurements to calculate the angle α. In a particularly simple embodiment, the filtering operation simply consists of calculating the average of the velocities v east (k) and v north (k) during a sliding window.

[0104] In another embodiment, the measurements of unit 10 and gyroscope 16 are combined to obtain a more precise measurement of the angle ψ m (k).

[0105] Alternatively, if unit 10 does not provide the standard deviations σ east (k) and σ north (k) on the velocity measurements, respectively v east (k) and v north (k), then these standard deviations are calculated by submodule 50.

[0106] In the particular case of a vehicle that cannot move in reverse, steps 86 and 90 are omitted and the angle ψ m (k) is systematically equal to the calculated angle α. Variations of step 100:

[0107] Alternatively, in step 100, the first set of conditions is considered to be satisfied only if all the conditions of that first set of conditions are met.

[0108] The first set of conditions may also include additional conditions. For example, in addition to or instead of conditions (11) to (14) described above, it may include the following conditions: condition (15): the speed of vehicle 2, in magnitude or in a given direction, is greater than a predetermined threshold S 15, condition (16): the angular speed of vehicle 2 is less than a predetermined threshold S 16, and condition (17): σ ψm (k) > S 17, where S 17 is a constant predetermined threshold.

[0109] In another variant, the first set of conditions includes a single condition chosen from the conditions (11) to (17) previously described.

[0110] It is also possible to enable the replacement of the angle ψc(k) by the angle ψm(k) only when a second set of predetermined conditions is met. For example, this second set of conditions includes one or more conditions chosen from the following group: condition (20): several unit 10 measurements have been acquired, condition (21): a static phase of a duration greater than 1 s or 5 s or 10 sa has been detected beforehand, condition (22): having received an instruction indicating that phase 48 has been successfully completed, condition (23): the time elapsed since the beginning of phase 48 and the current time is greater than a predetermined threshold S 23, this predetermined threshold S 23 being typically greater than 1 s or 5 s.

[0111] A static phase is a phase where vehicle 2 is stationary in the RT frame. If phase 48 is executed during a static phase, this allows for the precise initialization of the roll and pitch angles, as well as the correction coefficients of the gyroscope 16 measurements.

[0112] In a simplified variant, steps 100 and 104 are omitted. In this case, the angle ψ c (k) is systematically replaced by the angle ψ m (k) throughout the active phase of system 6. Variants of the Kalman filter :

[0113] Many other embodiments of the 34 filter are possible. For example, the 34 filter can be a linear Kalman filter, an extended Kalman filter or EKF (“Extended Kalman Filter”), a UKF (“Unscented Kalman Filter”) or even an adaptive Kalman filter.

[0114] Similarly, many variations of the state vector X k|k are possible. For example, when system 6 does not determine the vehicle's speed, the vehicle speed correction coefficients are omitted. The state vector X k|k may also omit correction coefficients for the biases of accelerometer 14 and gyroscope 16. The state vector X k|k may also include additional state variables.

[0115] What was previously taught in the specific case where the correction submodule 32 uses one or more Kalman filters also applies to correction submodules that construct the orientation O c (k) using estimators other than Kalman filters. More generally, what has been described here applies to any correction module that implements estimators whose systems of equations have been established under the assumption that the yaw angle error is small from the outset. Other variations :

[0116] The system 6 described here can be used in any vehicle that cannot move "crabbly", that is, in a direction parallel to the yb axis. Thus, the vehicle can also be a train, for example.

[0117] If the yaw angle is measured using a sensor that provides a yaw angle measurement unaffected by movement in the yb direction, then the vehicle can be any object capable of movement. For example, the vehicle could be an airplane, a boat, a submarine, a missile, a rocket, a smartphone, a laptop, or something similar.

[0118] In a simplified embodiment, system 2 does not determine the speed of vehicle 2. In this case, module 26 can be simplified.

[0119] Other implementations of submodule 32 are possible. For example, as an alternative, submodule 32 is arranged as described in the architecture known as "tight coupling." This architecture is described in more detail in Chapter 4.1.2 of the Godha2006 thesis.

[0120] Alternatively, system 6 is equipped with additional sensors, such as a magnetometer, odometer, or barometer. In this case, the correction sub-module 32 is modified to take into account the measurements from these additional sensors to correct the orientation O e (k) constructed by the integration sub-module 30.

[0121] In another embodiment, when the first set of conditions is satisfied—that is, when the replacement of the angle ψc(k) by the angle ψm(k) is inhibited—submodule 32 executes a different Kalman filter instead of filter 34 to construct the orientation Oc(k). This alternative Kalman filter, unlike filter 34, directly receives the angle ψm(k) and uses it to update the state vector Xk|k based on the difference between the yaw angles measured and predicted by block 38.

[0122] The principles described here also apply when the vehicle's position, orientation, and speed are expressed in other reference frames. In these cases, a simple change of reference frame allows us to return to the situation described here. As an example of other reference frames that can be used instead of the RT frame, we can cite the ECI (Earth-Centered Inertial) frame. The ECI frame is not fixed relative to the Earth's surface, since the Earth rotates within this frame. The RT frame can also be fixed relative to the stars. Chapter III: Advantages of the described implementation methods

[0123] Not using the measured angle ψm(k) to correct the orientation Oe(k) limits the instabilities that appear in the orientation determined by system 6. This therefore accelerates the convergence of the orientation determined by system 6 towards a precise orientation, that is, an orientation where the margin of error is less than 5 degrees or 1 degree. In parallel, providing an orientation Od(k) at output 7, in which the angle ψc(k) has been replaced by the angle ψm(k), allows for a more accurate estimation of the vehicle's orientation during a transient phase that occurs when system 6 starts up.

[0124] Stopping the substitution of angle ψc(k) with angle ψm(k) as soon as the margin of error on the corrected angle ψc(k) is sufficiently small allows us to obtain, after the first set of conditions is met, a determined orientation for vehicle 2 that is more precise than if angle ψc(k) continued to be replaced by angle ψm(k). This therefore improves the determination of the vehicle's orientation.

[0125] No longer using the angle ψm(k) to determine the vehicle's orientation once the first set of conditions is met simplifies system 6 and speeds up the determination of the vehicle's orientation. Furthermore, the vehicle's orientation determined in this way is considered to be more accurate.

[0126] Obtaining the angle ψm(k) from the ratio of the velocities veast(k) and vnorth(k) measured by unit 10 allows us to determine the angle ψm(k) without using an additional sensor, such as a magnetometer or a dual-antenna satellite positioning unit. A dual-antenna positioning unit can measure the vehicle's orientation in addition to its position and velocity. This also eliminates the need for methods of measuring the angle ψm(k) based on the Earth's rotation vector. This latter method is known as gyrocompassing. Gyrocompassing requires a highly sensitive gyroscope. Thus, measuring the angle ψm(k) from the velocities veast(k) and vnorth(k) simplifies the implementation of system 6.

[0127] Detecting a reverse movement allows this information to be taken into account to obtain a more accurate measured angle ψ m (k).

[0128] Detecting a reverse movement from the measurements of the accelerometer 14 makes it possible to detect this movement independently of the accuracy on the position, orientation and speed determined for the vehicle 2. This therefore makes it possible to detect more reliably the reverse movement just after the start of the system 2, i.e. during the transient phase where the margins of error on the position, orientation and speed determined can be significant.

[0129] Similarly, detecting a reverse movement from measurements from a sensor independent of the accelerometer 14, the gyroscope 16 and the unit 10 also allows for the reliable detection of this reverse movement, particularly during the transient phase.

Claims

1. Method for determining the position and the orientation of a vehicle by means of a location system mounted on the vehicle, this location system comprising a satellite geolocation unit, an inertial navigation unit, an inertial measurement integration sub-module and a correction sub-module, this method comprising the following steps: a) the measurement (76), by the satellite geolocation unit, of the position of the vehicle or of the position and of the speed of the vehicle, b) the measurement (74), by the inertial navigation unit, of the acceleration and of the angular speed of the vehicle, c) the construction (74), by the inertial measurement integration sub-module, of an estimated position and orientation of the vehicle on the basis of a previous position and of a previous orientation of the vehicle and using the measurements of the acceleration and of the angular speed which were carried out since this previous position and this previous orientation of the vehicle, then d) the correction (76), by the correction sub-module and using the measurements of the geolocation unit, of this estimated position and orientation in order to obtain a first corrected position and a first corrected orientation, the first corrected orientation notably containing a corrected yaw angle of the vehicle, the method further comprising: e) the obtention (80), by the location system, of a measurement of the yaw angle of the vehicle, which is independent of the measurements of the angular speed carried out by the inertial navigation unit, then f) the replacement (102), in the first corrected orientation, of the corrected yaw angle with the measured yaw angle, in order to obtain a second corrected orientation, then g) the outputting (106), on an output of the location system, of the first corrected position and of the second corrected orientation as the determined position and orientation, respectively, for the vehicle, and the use (108), during the next execution of step c), of the first corrected position and of the second corrected orientation as the previous position and previous orientation, respectively.

2. Method according to Claim 1, wherein the method comprises: - verifying (100) that a set of one or more predetermined conditions is met, this set of conditions comprising at least one condition chosen from the group consisting of the following conditions: 1) the margin of error on the measured yaw angle is greater than a predetermined threshold, 2) the margin of error on the corrected yaw angle is less than a predetermined threshold, 3) the deviation between the measured yaw angle and the corrected yaw angle is less than a predetermined threshold, and - as long as this set of conditions is not met, steps f) and g) are executed (102), then - as soon as this set of conditions is met, steps f) and g) are inhibited (104).

3. Method according to Claim 2, wherein, from the time when steps f) and g) are inhibited and as long as the predetermined set of conditions remains met, the use of the measured yaw angle for determining the orientation of the vehicle is inhibited.

4. Method according to Claim 2 or 3, wherein steps f) and g) are executed during a transition phase in which the determination method begins.

5. Method according to any one of the preceding claims, wherein the obtention of a measurement of the yaw angle, which is independent of the measurements of the angular speed carried out by the inertial navigation unit, comprises: - calculating (84) a first angle on the basis of a ratio between the measured speeds of the vehicle along a first and a second orthogonal direction, respectively, these first and second directions being fixed and parallel to the surface of the earth, then - generating the measured yaw angle on the basis of this first angle.

6. Method according to Claim 5, wherein the generation of the measured yaw angle on the basis of the first angle comprises: - the detection (86), by the location system, of a rearward movement of the vehicle, a forward movement being defined as being a movement of the vehicle in the direction towards which points a horizontal oriented axis xb of a movable reference marker attached to the vehicle without any degree of freedom, and the rearward movement being a movement in a direction opposite to the direction towards which the oriented axis xb points, and - when a rearward movement of the vehicle is detected, the measured yaw angle is taken to be equal (90) to the first angle plus 180°, and - in the absence of detection of a rearward movement of the vehicle, the measured yaw angle is taken to be equal (88) to the first angle.

7. Method according to Claim 6, wherein the detection of a rearward movement of the vehicle comprises: - obtaining (86), on the basis of measurements of an accelerometer of the inertial navigation unit, acceleration of the vehicle along the direction of forward movement of the vehicle, this accelerometer being arranged in the vehicle in such a way that a positive measurement of the acceleration of the vehicle along the oriented axis xb means that the vehicle accelerates by moving forward, then - calculating (86) the speed of the vehicle along the direction of forward movement on the basis of the acceleration obtained, then - comparing (86) the calculated speed with a predetermined threshold, and - when the calculated speed is greater than this threshold, the absence of detection of a rearward movement, and - when the calculated speed is less than this threshold, the detection of a rearward movement.

8. Method according to any one of Claims 5 to 7, wherein the measured speeds of the vehicle along the first and second orthogonal directions, respectively, are obtained on the basis of measurements of the position or of the speed of the vehicle carried out by the satellite geolocation unit.

9. Method according to Claim 6, wherein: - the measured speeds of the vehicle along the first and second orthogonal directions, respectively, are obtained on the basis of measurements of the position or of the speed of the vehicle carried out by the satellite geolocation unit, and - the detection of a rearward movement of the vehicle comprises: - comparing a first term with a second term, the first term being equal to |α-(ψ(k)-π)| and the second term being equal to |α-(ψ(k)|, where: - α is the first calculated angle for an instant k and expressed in radians, - ψ(k) is equal to the corrected yaw angle at instant k expressed in radians or to the value, expressed in radians, of the yaw angle measured at instant k by an additional sensor independent of the satellite geolocation unit, the yaw angle being the yaw angle of said movable reference marker, and - when the first term is lesser than the second term, the detection of a rearward movement, and - when the first term is greater than the second term, the absence of detection of a rearward movement.

10. Method according to any one of the preceding claims, wherein, during step d), a Kalman filter is implemented to correct the position and the orientation which are estimated by the inertial measurement integration module.

11. Microprocessor-readable information recording medium (24), characterized in that this medium comprises instructions for carrying out a method according to any one of the preceding claims, when these instructions are executed by the microprocessor.

12. Location system, capable of being mounted on a vehicle, for determining the position and the orientation of this vehicle, this location system comprising: - a satellite geolocation unit (10) capable of measuring the position of the vehicle or the position and the speed of the vehicle, - an inertial navigation unit (12) containing an accelerometer (14) and a gyrometer (16) for measuring the acceleration and the angular speed of the vehicle, - an inertial measurement integration sub-module (30) configured to construct an estimated position and orientation of the vehicle on the basis of a previous position and of a previous orientation of the vehicle and using the measurements of the acceleration and of the angular speed which were carried out since this previous position and this previous orientation of the vehicle, then - a correction sub-module (32) configured to correct, using the measurements of the geolocation unit, the estimated position and orientation in order to obtain a first corrected position and a first corrected orientation, the first corrected orientation notably containing a corrected yaw angle of the vehicle, - the system comprising a substitute sub-module (50) configured to: - obtain a measurement of the yaw angle of the vehicle, which is independent of the measurements of the angular speed carried out by the gyrometer of the inertial navigation unit, then - replace, in the first corrected orientation, the corrected yaw angle with the measured yaw angle, in order to obtain a second corrected orientation, then - deliver, on an output of the location system, the first corrected position and the second corrected orientation as the determined position and orientation, respectively, for the vehicle, and - the integration sub-module (32) also being configured to use, during the next construction of an estimated position and orientation of the vehicle, the first corrected position and the second corrected orientation as the previous position and the previous orientation, respectively.