The present invention relates to the field of navigation methods for a vehicle. More particularly, it relates to so-called hybrid navigation methods.
Hybrid navigation methods are methods in which measurements originating from a plurality of sensors (accelerometers, gyroscopes, GPS, etc.) are combined in order to determine variables or kinematic information defining the state of a device implementing the method.
These kinematic variables are, for example, a position, velocity or orientation of the device.
The measurements are, for example, inertial measurements, for example obtained from accelerometers and gyroscopes, such as the specific force, the angular velocity or velocity of rotation of the device, velocity measurements or position measurements of the device. The specific force is the sum of the external forces other than gravity, divided by the mass. This quantity therefore has the dimensions of an acceleration.
Linear Kalman filters are known, an example of which is shown in
However, these linear Kalman filters cannot operate in the case where the model describing the state to be estimated is not linear. In this case, it is necessary to use a non-linear (or “extended”) Kalman filter, shown in
In
This feedback is not intrinsic to the system considered and depends on the coordinate system used, which has great importance for extended Kalman filtering. More precisely, the feedback depends on the coordinates used for the representation of errors, referred to as “error variables”. A common system of error variables for extended Kalman filtering is simply (with T, V, X the orientation, velocity and position variables):
e
T
={circumflex over (T)}
−T
T
e
V
=V−{circumflex over (V)}
e
X
=X−{circumflex over (X)}
A system of error variables used to construct an invariant inertia-GPS combination filter and enabling the feedback to be almost entirely suppressed, is:
e
T
={circumflex over (T)}
−T
T
e
V
={circumflex over (T)}
−T(V−{circumflex over (V)})
e
X
={circumflex over (T)}
−T(X−{circumflex over (X)})
A system of error variables used to construct an invariant inertia-odometer combination filter and enabling the feedback to be almost entirely suppressed, is:
e
T
=T{circumflex over (T)}
−T
e
V
=V−T{circumflex over (T)}
−T
{circumflex over (V)}
e
X
=X−T{circumflex over (T)}
−T
{circumflex over (X)}
However, none of the two preceding systems of error variables enables the feedback to be strongly suppressed both for the inertia-GPS combination and the inertia-odometer combination. In particular, nothing enables, in a given filter, combining of inertial measurements, position measurements and odometer-based velocity measurements. The invention makes this combination possible using the last error variable defined above.
It is also known to use two filters, independently using the measurements from position sensors and odometry sensors. However, this does not make it possible to combine these measurements and therefore the result is sub-optimal and corresponds approximately to the performance of the better of two filters.
There is therefore a need for a novel type of navigation method which will enable combining of position measurements and inertia measurements.
The invention proposes to overcome the above-mentioned disadvantages.
For this purpose, the invention proposes, according to a first aspect, a method for assisting with the navigation of a vehicle equipped with a navigation device comprising the following steps: acquiring a priori values of kinematic variables of the navigation device; determining respective current values of kinematic variables of the navigation device and a current uncertainty matrix representative of an uncertainty of the respective current values of the kinematic variables, on the basis of respective preceding values of the kinematic variables; determining a correction from the respective current values of the kinematic variables, of a current uncertainty matrix representative of an uncertainty of the respective current values of the kinematic variables, of a measurement of one of the kinematic variables, and of a gain matrix dependent on the measurement; updating (204) the respective current values of the kinematic variables on the basis of the correction and of the current uncertainty matrix.
Hence, this method makes it possible to determine the value of the kinematic variables of the navigation device. It offers more precise navigation by combining all the available measurements in the same filter.
In an embodiment the kinematic variables comprise an orientation of the navigation device (DISP), a current value of which is a current orientation matrix and a preceding value of which is a preceding orientation matrix, a velocity of the navigation device (DISP), a current value of which is a current velocity vector and a preceding value of which is a preceding velocity vector and a position of the navigation device (DISP), a current value of which is a current position vector and a preceding value of which is a preceding position vector. The current uncertainty matrix is representative of an uncertainty of the current orientation matrix, the current velocity vector and the current position vector. The preceding uncertainty matrix is representative of an uncertainty of the preceding orientation matrix, the preceding velocity vector and the preceding position vector.
In an embodiment, a virtual observation function is determined on the basis of the measurement, an observation matrix obtained by applying a limited first order expansion to the virtual observation function and the gain matrix obtained on the basis of the observation matrix.
In an embodiment, the current values are associated with a current time and the preceding values are associated with a preceding time. The determining of the current values of the kinematic variables comprises: determining the current velocity vector by addition, to the preceding velocity vector, of an integration over a time interval between the preceding time and the current time, of a sum of a specific force of the navigation device and of a model of Earth gravity experienced by the navigation device; determining the current position vector by addition, to be preceding position vector, of an integration, over the time interval, of the preceding velocity vector; determining the current orientation matrix by multiplication of the preceding orientation matrix with a matrix representative of a rotation of the navigation device; and/or determining the current uncertainty matrix on the basis of the preceding uncertainty matrix.
In an embodiment, the measurement is a measurement of the position of the navigation device, the determining of the correction comprises a subtraction of the current position vector and of the measurement of the position, and a multiplication of the subtraction by the gain matrix. In an embodiment, the correction is a correction vector, the updating comprises a substep of updating the current orientation matrix by multiplication of a rotation matrix of a first part of the correction vector and of the current orientation matrix, a substep of updating the current velocity vector by addition, to a second part of the correction vector, of a multiplication of the rotation matrix of the first part of the correction vector and of the current velocity vector, and/or a substep of updating the current position vector by addition, to a third part of the correction vector, of a multiplication of the rotation matrix of the first part of the correction vector and of the current position vector.
In an embodiment, the respective current values of the kinematic variables are first respective current values of the kinematic variables, the respective preceding values of the kinematic variables are first respective preceding values of the kinematic variables, the correction is a first correction, the measurement of one of the kinematic variables is a first measurement of one of the kinematic variables, the gain matrix is a first gain matrix. The method further comprises determining second respective current values of the kinematic variables of a navigation device, on the basis of second respective preceding values of the kinematic variables; and, determining a second correction from second respective current values of the kinematic variables, of a second measurement of one of the kinematic variables and of a second gain matrix. The method also comprises updating of the second respective current values of the kinematic variables on the basis of the second correction.
In an embodiment, the method further comprises determining respective consolidated values of the kinematic variables from the first respective current values of the kinematic variables and second respective current values of the kinematic variables.
In an embodiment, the determining of respective consolidated values of the kinematic variables comprises determining a similarity between the first current values and the second current values and, when the similarity exceeds a threshold, the determination comprises averaging of the first current values and second current values or weighted averaging of the first current values and second current values or selecting the first current values or second current values.
Another aspect of the invention relates to a navigation device comprising a processing unit, three accelerometers, three gyroscopes and a measurement device of a position of the navigation device. The processing unit is configured to implement the method for assisting with the navigation.
Another aspect of the invention relates to a computer program product comprising program code instructions for executing the steps of the method for assisting with the navigation.
Other features and advantages of the invention will emerge from the following description, which is given purely by way of illustration and not being limiting and which should be read with reference to the attached figures, in which:
The processor or microcontroller can be an application-specific integrated circuit (ASIC), it can also be a programmable logic circuit or field-programmable gate array (FPGA).
The memory can be fixed or removable and include different memory units that can include a combination of units enabling volatile and non-volatile storage. The memory is configured to store a software code that can be used by the processor or the microcontroller in order to carry out a method for determining respective values of kinematic variables of the navigation device DISP.
The values of the kinematic variables enable the location of the navigation device DISP and thus the navigation of the bearer of this device.
The navigation device DISP also comprises
The navigation device DISP can also include other devices for measuring a kinematic variable, a combination of several kinematic variables, or additional variables also estimated by the Kalman filter of the navigation device DISP.
Moreover, the navigation device DISP can also include a measurement device 104 of a distance travelled by the bearer of the navigation device DISP. This measurement device 104 is, for example, an odometer 104.
The three accelerometers 101-a to 101-c are able to deliver specific force data. The three accelerometers are associated respectively with three axes which can be mutually orthogonal.
The three gyroscopes 102-a to 102-c are able to deliver angular velocity data. The three gyroscopes are associated respectively with three axes which can be mutually orthogonal.
More precisely, the accelerometers measure a specific force fn of the navigation device DISP and the gyroscopes measure an angular velocity of the navigation device DISP. This angular velocity is then transformed into a rotation matrix Ωn representative of the rotation of the device. The time interval between two measurements is denoted dt.
Accelerometers and gyroscopes can either provide specific forces and angular velocities, or directly supply variations in velocity and angle.
The measurement device 103 of a position of the navigation device DISP is for example:
In the case where the measurement device 103 of a position is not co-located with the navigation device DISP, the distance between the measurement device 103 and the navigation device DISP is considered as a constant bias on the virtual measurement of the position of the beacon in the reference frame of the bearer. This virtual measurement is defined below. This bias is, for example, known or estimated at the time of implementing the method of the invention.
The data delivered by the three accelerometers 101-a to 101-c, by the three gyroscopes 102-a to 102-c, by the measurement device 103 of a position and optionally by the odometer 104 are received by the processing unit UNIT.
The processing unit UNIT is configured by implementing the navigation method by means of determining respective values of kinematic variables of the navigation device DISP. This method is shown in
The method of the
The correction is calculated by a transformation matrix or gain matrix (denoted K in the following). This gain matrix K is calculated from an observation matrix (denoted H in the following). The observation matrix H in the prior art links, to first order, an error estimation of the state of the system to an error prediction of the GPS measurement. The gain matrix K will use the observation matrix H in order to carry out the reverse operation: determining a correction of the state based on a prediction error of the measurement observed.
In the invention, the observation matrix H and the innovation z (the difference between the current value of one of the kinematic variables and of the measurement of the kinematic variable) are no longer associated with the actual measurement of position provided by the GPS but to the virtual measurement of position of the point indicated by GPS in the reference frame of the bearer. As in the virtual observation techniques of velocity mentioned below, the value given to this virtual observation will be zero.
In a (linear, extended or invariant) Kalman filter, an observation is defined as a function of the state predicting the measurement of a sensor, for example a GPS receiver supplying an observation of position or an odometer supplying an observation of velocity in the reference frame of the bearer.
The Kalman filtering uses a matrix H linking, to first order, the state estimation error to a measurement prediction error.
A virtual observation is defined as a function of the state on which information is available which does not necessarily come from a sensor.
A piece of information modelled by a virtual observation can be assimilated using a Kalman filter, which uses a matrix H linking, to first order, the estimation error of the state to a prediction error of the virtual measurement. In particular, the fact of having or not having a sensor supplying this measurement does not make a difference. On the contrary, the same piece of information assimilated through different virtual observations leads to different Kalman filters (as is the case, for example, for the detecting of stopping above). The filters obtained will, in general, have a behaviour that is similar for small initial estimation errors, but different for larger initial estimation errors.
The invention is a method for combining inertial measurements with GPS measurements, but it relies on the notion of resetting on navigation marks, as shown in
Moreover, as shown in
The kinematic variables of the device comprise:
Moreover, an uncertainty matrix P representative of an uncertainty of the kinematic variables is used. This matrix is a covariance matrix.
In the rest of this document, the (matrix or vector) parameters bearing a circumflex accent represent estimated parameters, while the corresponding real parameters are denoted without circumflex accent.
The method comprises determining the value of these parameters which are respectively denoted {circumflex over (T)}n|n, {circumflex over (V)}n|n and {circumflex over (X)}n|n. The method also comprises determining the covariance matrix Pn|n representative of the uncertainty of the current estimation. It is also assumed that a covariance matrix P0|0, representing the initial uncertainty, is available at the start of the navigation.
Here, the index n represents the time step and, in conventional manner in Kalman filtering, the index n|n represents the estimation of the value at time n by taking account of the observation carried out at time n and the index n|n−1 represents the estimation of the value at time n without taking account of the observation carried out at time n.
The method of the invention uses a Kalman filter (more precisely an invariant Kalman filter), when the propagation phases (comprising the determination 202) using the inertial measurements succeed one another, and the updating phases (comprising the determination 203 of a correction and the updating 204) using the position data delivered by the position measurement device 103.
The determination 202 uses the following equations:
{circumflex over (V)}
n|n−1
={circumflex over (V)}
n−1|n−1
+dt.({circumflex over (T)}n−1|n−1fngn)
{circumflex over (X)}
n|n−1
={circumflex over (X)}
n−1|n−1
dt.{circumflex over (V)}
n−1|n−1
{circumflex over (T)}
n|n−1
={circumflex over (T)}
n−1|n−1Ωn
P
n|n−1
=F
n
P
n−1|n−1
F
n
T
+Q
n
Where:
In an embodiment, step 203 of determining a correction ds uses a measurement of the position of the navigation device DISP, by means of the following equations:
z
n
={circumflex over (X)}
n|n−1
−Y
n
K
n
=P
n|n−1
H
n
T(HnPn|n−1HnT+Rn)−1
ds=K
n
z
n
Where:
Where log( ) designates here a function which can obtain a rotational vector from a rotation matrix. This observation matrix differs from the matrix used in the prior art because it contains the value Yn of the measurement itself; hence Hn represents an observation matrix or virtual observation matrix in the reference frame of the bearer of the navigation device DISP of the position of the point indicated by the GPS. The virtual observation being defined as a measurement of the position in the reference frame of the bearer of a beacon located at the coordinates measured by the position determining device. Then, a person skilled in the art knows to process this position by using a step of updating the extended or invariant Kalman filtering. In the case where the measurement is a velocity measurement in a fixed reference frame, coming from a GPS receiver for example, the virtual observation is defined as a measurement of the velocity in the reference frame of the bearer having, in a fixed reference frame, the velocity measured by the device for determining a velocity and having, in a fixed reference frame, the estimated position of the bearer. Then, a person skilled in the art knows to process this relative velocity measurement in the reference frame of the bearer by using a step of updating the extended or invariant Kalman filtering.
The determination 203 of a correction makes it possible to determine the deviation of all the kinematic variables of the navigation device on the basis of the measurement of only one of the kinematic variables of the device.
This determining of the deviation is carried out by the gain matrix K, which takes into account the uncertainties in the kinematic variables of the navigation device DISP. If there is a low uncertainty, the position measurement Yn is weakly taken into account and, if there is a large uncertainty, the position measurement Yn is strongly taken into account.
The updating 204 uses the correction ds to produce the following equations:
{circumflex over (T)}
n|n
=R(ds1:3){circumflex over (T)}n|n−1
{circumflex over (V)}
n|n
=R(ds1:3){circumflex over (V)}n|n−1+ds4:6
{circumflex over (X)}
n|n
=R(ds1:3){circumflex over (X)}n|n−1+ds7:9
P
n|n=(I−KnHn)Pn|n−1
In another embodiment, the updating 204 uses the correction ds in order to produce the following equations:
{circumflex over (T)}
n|n
=R(ds1:3){circumflex over (T)}n|n−1
{circumflex over (V)}
n|n
=R(ds1:3){circumflex over (V)}n|n−1+V(ds1:3ds4:6
{circumflex over (X)}
n|n
=R(ds1:3){circumflex over (X)}n|n−1+V(ds1:3)ds7:9
P
n|n=(I−KnHn)Pn|n−1
with
in other words a matrix V(ds1:3) is inserted.
Steps 202 to 204 of the method are repeated throughout the navigation.
In particular, the corrected vector velocity {circumflex over (V)}n|n becomes the next preceding velocity vector, the corrected vector position {circumflex over (X)}n|n becomes the next preceding velocity vector and the corrected orientation matrix {circumflex over (T)}n|n becomes the next preceding orientation matrix.
This method thus enables the location of the navigation device DISP and thus the navigation of the bearer of this device. The method offers a more precise navigation by combining all the available measurements in the same filter.
In the case of an autonomous vehicle subject to a possible loss of availability of the sensors, the subject matter also offers greater safety.
In other embodiments, other resetting methods can be used, for example
As shown in
The first measurement is generally the position of the navigation device DISP.
The method of
In an embodiment the first measurement and the second measurement are identical. In this case, the method makes it possible to have two different inertia-GPS combination methods in order to limit the risk of undetected failure.
In an embodiment the branches use different measurements. These measurements can come from the same sensor and be used alternately by one branch or by the other, or come from two different sensors.
Steps 202-a to 204-a are the steps of the first branch.
Steps 202-b to 204-b are the steps of the second branch.
Finally the method of
Step 202-a uses the following equations:
n|n−n=n−1|n−1+dt.(n−1|n−1fn+gn)
n|n−1
={circumflex over (X)}′
n−1|n−1
+dt.
n−1|n−1
n|n−1=n−1|n−1Ωn
P′
n|n−1
=F′
n
P′
n−1|n−1
F′
n
T
+Q′
n
Where:
Step 202-b uses the following equations:
n|n−1=n−1|n−1+dt.(n−1|n−1fn+gn)
n|n−1
={circumflex over (X)}″
n−1|n−1
+dt.
n−1|n−1
n|n−1=n−1|n−1Ωn
P″
n|n−1
=F″
n
P″
n−1|n−1
F″
n
T
+Q″
n
Where:
Step 203-a of determining the first correction ds′ uses a measurement of the position of the navigation device DISP, via the following equations:
z
n
={circumflex over (X)}
n|n−1
′−Y
n′
K′
n
=P′
n|n−1
H′
n
T(H′nP′n|n−1H′nT+Rn′)−1
ds′=K′
n
z
n
H′
n=((Yn)x 03 −I3)
Where:
Where log( ) designates here a function which can obtain a rotational vector from a rotation matrix. This observation matrix differs from the matrix used in the prior art because it contains the value Yn of the measurement itself; hence Hn represents an observation matrix or virtual observation matrix in the reference frame of the bearer of the navigation device DISP of the position of the point indicated by the GPS. The virtual observation being defined as a measurement of the position in the reference frame of the bearer of a beacon located at the coordinates measured by the position determining device. Then, a person skilled in the art knows to process this position by using a step of updating the extended or invariant Kalman filtering. In the case where the measurement is a velocity measurement in a fixed reference frame, coming from a GPS receiver for example, the virtual observation is defined as a measurement of the velocity in the reference frame of the bearer having, in a fixed reference frame, the velocity measured by the device for determining a velocity and having, in a fixed reference frame, the estimated position of the bearer. Then, a person skilled in the art knows to process this relative velocity measurement in the reference frame of the bearer by using a step of updating the extended or invariant Kalman filtering.
Step 203-b of determining the second correction ds″ uses a measurement of the position of the navigation device DISP, via the following equations:
z
n
={circumflex over (T)}
n|n−1
T″(Yn″−{circumflex over (X)}″n|n−1)
K″
n
=P″
n|n−1
H″
n
T(H″nP″n|n−1H″nY+Rn″)−1
ds″=K″
n
z
n
Where:
ds′ and ds″ are vectors of size 9. The three first components (ds′1:3 and ds″1:3) correspond to the error in rotation. The following three components (ds′4:6 and ds″4:6) correspond to the error in velocity. The last three components (ds′7:9 and ds″7:9) correspond to the error in position.
The matrices K′n and K″n are known under the name of gain matrices.
Step 204-a uses the following equations:
n|n
=R(ds′1:3)n|n−1
n|n
=R(ds′1:3)n|n−1+ds′4:6
n|n
=R(ds″1:3)n|n−1+ds ′7:9
P′
n|n=(−K′nH′n)P′n|n−1
Step 204-b uses the following equations:
{circumflex over (T)}
n|n
′={circumflex over (T)}
n|n−1
′R(ds1:3′)
{circumflex over (V)}
n|n
′={circumflex over (V)}
n|n−1
′+{circumflex over (T)}
n|n−1
′ds
4:6′
{circumflex over (X)}
n|n
′={circumflex over (X)}
n|n−1
′+{circumflex over (T)}
n|n−1
′ds
7:9′
P
n|n′=(I−Kn′Hn′)Pn|n−1′
The combination 401 enables the determining of respective consolidated values of the kinematic variables. This determination can, for example, be carried out by a weighted average of the corrected first vector position with the corrected second vector position, of the corrected first vector velocity with the corrected second vector velocity and of the corrected first orientation matrix with the second corrected orientation matrix.
This step 401 can also implement a statistical test checking the similarity or consistency of two estimators in order to obtain an estimator with better integrity than a conventional Kalman filter. The plausibility test would check the following relations p(Δs″|0, Pn|n″)>α and p(Δs′|0,Pn|n′)>α where Δs′ (resp. Δs″) is a vector representing the deviation between the two navigation states, coming from branches a and b expressed in the same coordinate system as P′(resp. P″), p(s|0,P) is the density of the centred multivariate normal distribution of covariance matrix P, evaluated at point s, and α is a threshold fixed beforehand. The deviation Δs can be, for example, the characteristic logarithmic error of the invariant filtering. This test can be summarised by saying that it checks that each estimation agrees, to a high probability, with the other estimation.
Another possible test consists in calculating a deviation between the two normal distributions returned by the two filters, this deviation being, for example, the Kullback-Leibler divergences defined by the formula:
Where tr( )is the trace function, k is the dimension of the state of the system (k=9 if only the orientations, velocities and positions are estimated) In( ) designates the logarithmic function, and det( ) the determinant function, {tilde over (P)}″ is the matrix P″ written in the same coordinate system as P′ (the two Kalman filters of the branches a and b are different and can therefore represent their errors in different coordinate systems). Alternatively, it is possible to swap the rules of P′ and P″:
The test will be positive if the deviation thus defined remains less than a threshold fixed beforehand.
If the test is positive, the estimation of one of the two branches is repeated, for example that which does not use the invention. It can also return the mean of the kinematic variables estimated by the two branches. Another possibility is to leave the state s′ and to define a correction δs′=P′[P′+{tilde over (P)}″]−1Δs′ which will be applied to s′ by the same formulas as the correction ds′ of the updating step.
Symmetrically, it is possible to leave the state ds″ and to define a correction δs″=P″[P″+{tilde over (P)}′]−1Δs″ which will be applied to s″ by the same formulas as the correction ds″ of the updating step.
If the first values and the second values are coherent and can return, for example:
By comparing the Riccati equation of the first branch (steps 202-a to 204-a) and of the second branch (steps 202-b to 204-b) it is observed that:
The first branch will therefore be especially robust to inertial errors and the second to GPS errors. This makes the use of a statistical test checking the similarity or consistency particularly advantageous.
Number | Date | Country | Kind |
---|---|---|---|
FR2102423 | Mar 2021 | FR | national |
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/FR2022/050443 | 3/11/2022 | WO |