This invention relates to the field of methods for navigating a vehicle. It more specifically relates to so-called hybrid navigation methods.
Hybrid navigation methods are methods in which measurements from several sensors (accelerometers, gyroscopes, GPS etc.) are fused to determine kinematic variables or information defining the state of a device implementing the method.
These kinematic variables are, for example, the position, the speed or the 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 rate or rotation speed of the device, speed measurements or position measurements of the device. The specific force is the sum of the external forces experienced by the device, other than gravitational, divided by the mass. This quantity thus has the dimension of an acceleration.
In particular, the use of the conventional extended Kalman filter to make a merge using altitude has limitations and requires a complex prior alignment procedure.
Conventional extended Kalman filtering works as long as the uncertainties on heading and position are small enough, which in particular involves an alignment procedure at the start of the navigation and more or less regular recourse to additional measurements (other than altitude) depending on the quality of the sensors used to obtain the different measurements.
In addition, a momentaneous loss of consistency, when the estimation error is greater than the uncertainty estimated by the Kalman filter, cannot always be corrected by the filter because of the non-linearity of the system. This loss of consistency is, for example, caused by an unforeseen increase in measurement noise.
The use of invariant filtering simplifies the alignment phase for hybridizations to which its theory applies, but also has limitations in the case of inertia-altimetry fusion. More precisely, the combination of invariant filtering with long phases of inertia-altimetry fusion intercut with rare position measurements does not provide any theoretical guarantees of convergence of the filtering. Generally the performance of invariant filtering is inferior to a conventional extended Kalman filter for inertia-altimetry fusion.
There is therefore a need for a new type of navigation method that makes it possible to use an altitude measurement in addition to the other available measurements.
The invention makes provision for remedying the aforementioned drawbacks.
In this regard, the invention makes provision, according to a first aspect, for 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 the kinematic variables of the navigation device and a current uncertainty matrix representative of an uncertainty on the respective current values of the kinematic variables, based on respective preceding values of the kinematic variables, on a preceding uncertainty matrix representative of an uncertainty on the respective preceding values of the kinematic variables and on a fictitious model of a terrestrial gravity experienced by the navigation device, the intensity of the modelled gravity increasing with the altitude of the navigation device, determining a correction based on respective current values of the kinematic variables and the measuring and updating of the respective current values of the kinematic variables based on the correction and on the current uncertainty matrix.
Thus this method makes it possible to determine the value of the kinematic variables of the navigation device. In particular it makes it possible to control at start-up the duration of the static initial phase also known as alignment.
In an embodiment, the kinematic variables comprise an orientation of the navigation device, a current value of which is a current orientation matrix and a preceding value of which is a preceding orientation matrix, a speed of the navigation device, a current value of which is a current speed vector and a preceding value of which is a preceding speed vector, and a position of the navigation device, 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 on the current orientation matrix, on the current speed vector and on the current position vector, and the preceding uncertainty matrix is representative of an uncertainty on the preceding orientation matrix, on the preceding speed vector and on the preceding position vector.
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 the determining of the current speed vector by adding to the preceding speed vector 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 the modeled gravity, the determining of the current position vector by adding to the preceding position vector an integration, over the time interval, of the preceding speed vector, the determining of the current orientation matrix by multiplying the preceding orientation matrix by a matrix representative of a rotation of the navigation device a determination of the current uncertainty matrix based on the preceding uncertainty matrix.
In an embodiment, the determining of the correction comprises a subtraction of the current speed vector and of the measurement, and a multiplication by a gain matrix.
In an embodiment, the determining of the correction comprises a subtraction of the current position vector and of the measurement, and a multiplication by a gain matrix.
In an embodiment, the correction is a correction vector, the updating comprises a sub-step 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 sub-step of updating the current speed vector by adding to the speed vector a multiplication of the current rotation matrix and of a second part of the correction vector and a sub-step of updating the current position vector by adding to the current position vector a multiplication of the current rotation matrix and of a third part of the correction vector.
In an embodiment, the determining of kinematic variables of the navigation device comprises a step of determining the fictitious model of the gravity experienced using the formula
where
In an embodiment, the determining of kinematic variables of the navigation device comprises a step of determining the fictitious model of the gravity experienced using the formula
where gn(Xn) is a modeled gravity vector, gréel is a terrestrial gravity vector coming from a physically consistent model, rT is a radius of the Earth, Xn is the current position vector, hn is a measured altitude of the navigation device, alt(Xn) is an altitude of the navigation device determined based on the current position vector and πh
Another aspect of the invention relates to a navigation device comprising a processing unit, three accelerometers and three gyroscopes. The navigation device also comprises a measuring device. The processing unit is configured for implementing the method for assisting with navigation previously described.
In an embodiment the navigation device further comprises a device for measuring an altitude of the navigation device.
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 navigation previously described, when the latter is executed by a processor.
Other features and advantages of the invention will become further apparent from the following description, which is purely illustrative and non-limiting and must be read with reference to the appended figures in which:
The processor or microcontroller can be an Application-Specific Integrated Circuit (or ASIC). It can also be a Field-Programmable Gate Array (or FPGA).
The memory can be fixed or removable and include different memory units which can include a combination of units allowing for volatile and non-volatile storage. The memory is configured to store software code usable by the processor or the microcontroller to embody a method for determining respective values of kinematic variables of the navigation device DISP.
The values of the kinematic variables are used to locate the navigation device DISP and therefore 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 of the navigation device DISP.
In addition, the navigation device DISP can also include a device 104 for measuring an altitude of the navigation device DISP. This measuring device 104 is for example an altimeter 104.
The three accelerometers 101-a to 101-c are able to deliver specific force data. The three accelerometers are respectively associated with three axes which can be mutually orthogonal.
The three gyroscopes 102-a to 102-c are able to deliver angular position data. The three gyroscopes are respectively associated 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 speed of the navigation device DISP. This angular speed 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 may either supply specific forces and angular speeds, or directly variations of speed and angle.
The device 103-a for measuring a position of the navigation device DISP:
The device 103-b for measuring a speed of the navigation device DISP is for example:
The data delivered by the three accelerometers 101-a to 101-c, by the three gyroscopes 102-a to 102-c, by the device 103-a for measuring a position or by the device 103-b for measuring a speed and where applicable by the altimeter 104 are received by the processing unit UNIT.
If the altimeter 104 is not present, a hypothesis can be made regarding the altitude of the navigation device DISP. If the bearer is a ship, the hypothesis that the altitude is zero can be made.
The processing unit UNIT is configured by the implementation of the method for determining respective values of kinematic variables of the navigation device DISP shown in
This method of
In this text, the fictitious model will be interchangeably known as “fictitious model of terrestrial attraction” and “fictitious model of terrestrial gravity”. This model does indeed have a fictitious nature, due to the fact that the modeled attraction is of an intensity increasing with the altitude of the device DISP, which is contrary to a realistic model of gravity/terrestrial attraction in which such an intensity would decrease with such an altitude.
The measurement is for example the measurement of a physical quantity, for example depending on one or more kinematic variables or depending on a bias of one of the accelerometers or of one of the gyroscopes. This measurement is for example the position or the speed of the navigation device DISP.
In addition the modeled attraction coincides with the real attraction at the measured altitude of the bearer.
The kinematic variables of the device comprise:
Other variables can be estimated simultaneously, in particular biases in accelerometers and gyroscope measurements.
In addition, an uncertainty matrix P is used representative of an uncertainty on the kinematic variables. This matrix is a covariance matrix.
In the remainder of the document, the variables (matrix or vector) bearing a circumflex accent represent estimated variables; the corresponding actual variables are written without a circumflex accent.
The method comprises the determining of the value of these variables which are respectively denoted {circumflex over (T)}n|n, {circumflex over (V)}n|n and {circumflex over (X)}n|n. The method also comprises the determining of the covariance matrix Pn|n representative of the uncertainty on the present estimate. It is also assumed that a covariance matrix P0|0, representing the initial uncertainty, is available at the start of the navigation.
The index n here represents the time increment and, conventionally in a Kalman filter, the index n|n represents the estimate of the value at the time n taking into account the observation made at the time n and the index n|n−1 represents the estimate of the value at the time n without taking into account the observation made at the time n.
The method of the invention uses a Kalman filter (advantageously the invention uses an invariant Kalman filter), which involves a succession of propagation phases (including the determining 202) using inertial measurements and the fictitious terrestrial attraction model, and updating phases (comprising the determining 203 of a correction and the correction 204) using the position data delivered by the device 103-a for measuring a position or the speed data delivered by the device 103-b for measuring a speed. In other embodiments other types of measurements are used to determine the correction 203 and to make the update 204. In addition, during the determining 202 of a state of the device, the method uses an altitude measurement to feed a non-physical gravity model.
This altitude measurement can be supplied by the altimeter 104 if one is present, for example if the navigation system is present in an aircraft, the altitude of which may vary. This altitude measurement can also be known a priori in the case of a boat.
The determining 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-1fn+gn({circumflex over (X)}n-1|n-1))
{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
With:
(l)xu=l×u
where x is a vector product.
Thus, in this determining step 202 a fictitious terrestrial attraction model
g
n(Xn)=αnXn
is used, with
This fictitious model is based on a realistic model but moves away from it. This fictitious model uses gréel which is an opposite of a spherical realistic model in which the intensity of the gravity is only a function of the distance from a point to the center of the Earth. The real gravity at a point
X
n
is then written
is a unit vector pointing in the direction of Xn.
In addition, a fictitious model gn(Xn)=αnXn of the effect of gravity is used, the intensity of which is increasing with altitude but coincident with the realistic model only at the altitude hn (indicated by the altimeter).
Thus, if the position Xn is located at an altitude h n this gives
∥Xn∥=rT+hn
and therefore
the two gravity models coincide at the altitude hn.
But the derivative of gn(Xn) with respect to Xn is different from the derivative of the gravity in a conventional model since gn(Xn) has become linear. This is no longer the case if a realistic, more sophisticated model of gravity is chosen. However, the behavior of the filters is the same.
It is also possible to choose a more sophisticated fictitious model of terrestrial attraction, for example using the following formula:
where the function g′réel is a vector of a terrestrial gravity resulting from a physically consistent model.
In an embodiment the step 203 of determining a correction ds comprises:
More accurately the step 203 of determining a correction ds may use the following equations:
z
n
={circumflex over (T)}
n
T(Yn−{circumflex over (X)}n|n-1)
K
n
=P
n|n-1
H
n
T(HnPn|n-1HnT+Rn)−1
ds=K
n
z
n
With:
H
n=(03 03 l3)
this matrix makes it possible to connect the measured position Yn to the other kinematic variables of the device DISP,
If the measuring device supplies a speed, the step 203 of determining a correction ds performs:
Thus, if the measuring device supplies a speed the computations of zn and Hn are replaced by:
z
n
={circumflex over (T)}
n
T(Yn−{circumflex over (V)}n|n-1)
H
n=(03 l3 03)
ds is a vector of size 9. The first three components (ds1:3) correspond to the rotation error. The next three components (ds4:6) correspond to the speed error. The last three components (ds7:9) correspond to the position error.
The matrix H comprises the concatenation of two null matrices of size 3 by 3 and an identity matrix of size 3 by 3.
K is known by the name of gain matrix.
This step 203 of determining a correction makes it possible to determine the deviation of all the kinematic variables of the navigation device based on the value of only one of the kinematic variables of the device.
This determining of the deviation is performed by the gain matrix K, which takes into account the uncertainties on the kinematic variables of the device DISP. If there is a low uncertainty, the position measurement Yn is given a low degree of consideration and, if there is a high uncertainty, the position measurement Yn is given a high degree of consideration. The term “the position measurement Yn is given a low degree of consideration” should be understood to mean that the value of the entries in the matrix K is low. The term “the position measurement Yn is given a high degree of consideration” should be understood to mean that the value of the entries in the matrix K is high.
The updating 204 uses the correction
ds
to perform 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=(l−KnHn)Pn|n-1
In another embodiment the updating 204 uses the correction ds to carry out the following equations:
in other words a matrix V(ds1:3) is inserted.
The steps 202 to 204 of the method are repeated throughout the navigation.
In particular the corrected speed vector {circumflex over (V)}n|n becomes the next preceding speed vector, the corrected position vector {circumflex over (X)}n|n becomes the next preceding position vector and the corrected orientation matrix {circumflex over (T)}n|n becomes the next preceding orientation matrix.
This method uses the matrix P which is the covariance matrix and the set of operations applied to P over time are called the “Riccati equation”. In the embodiment described above, the kinematic variables never appear in the matrix P (or only in the matrices Qn and Rn). Thus, this method shares an important property of linear systems. In more complex embodiments these kinematic variables may appear but the method of the invention makes it possible to reduce the negative effects of this dependence.
In the preceding embodiments, the fusion technique using the fictitious terrestrial attraction model is an invariant filter. In other embodiments other recalibration methods may be used, for example
In one of the implementations of the invention, the set of operations involving the matrix P do not cause the estimated error of the navigation device DISP to appear (or cause it to appear only in the matrices Qn and Rn). Thus one is in the same case as for a linear system and the feedback of
Number | Date | Country | Kind |
---|---|---|---|
FR2102417 | Mar 2021 | FR | national |
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/FR2022/050442 | 3/11/2022 | WO |