NAVIGATION ASSISTANCE METHOD FOR A MOBILE CARRIER

Abstract

The present invention relates to a navigation assistance method for a mobile carrier (1) comprising an inertial navigation unit (10) comprising at least one inertial sensor (12), in which, over a determined observation window, the following steps are implemented by an estimation unit (11) of the inertial navigation unit (10): (E10) parametrisation of a non-linear system configured to estimate a navigation state of the mobile carrier (1) over a given time interval at an iteration n as a function of a kinematic model and/or measurements acquired by at least one inertial sensor (12); (E20) linearisation of the system so that the system expresses a navigation state at iteration n as a function of the state at iteration n−1 and a correction to this navigation state, said system being initialised by a first a priori state; (E21) estimating, by a Kalman filter and stochastic cloning, a first correction of a navigation state at iteration n; (E22) estimating a second correction of the navigation state at iteration n by an information filter operating in reverse and stochastic cloning; (E30) determining a third correction by merging the first and second corrections; and (E40) correcting the navigation state at iteration n as a function of the third correction, the corrected state being used at iteration n+1.

Claims

1. A method of navigation assistance for a mobile carrier including an inertial navigation unit including at least one inertial sensor, wherein, the following steps are implemented by an estimation unit of the inertial navigation unit, over a determined observation window: parameterizing a non-linear system configured to estimate a navigation state of the mobile carrier over a given time interval at an iteration n as a function of a kinetic model and/or of measurements acquired by at least one inertial sensor (12); linearizing said system for expressing a navigation state at the iteration n as a function of the state at the iteration n−1 and of a correction to this navigation state, said system being initialized by a first a priori state; estimating by a Kalman filter and stochastic cloning of a first correction of a navigation state at the iteration n; estimating of a second correction of the navigation state at the iteration n by an information filter running backwards and stochastic cloning; determining a third correction by fusion of the first and second corrections; and correcting the navigation state at the iteration n as a function of the third correction, said corrected state being used at the iteration n+1.

2. The method of navigation assistance for a mobile carrier (1) as claimed in the claim 1, wherein the step of estimating by the Kalman filter and stochastic cloning of the first correction is done over successive timesteps, one timestep comprising steps of: propagation of a preceding navigation state of the carrier into a propagated state as a function of a kinetic model and/or of measurements acquired by at least one inertial sensor, updating of the propagated state as a function of direct or relative measurements acquired by at least one additional sensor, the step of estimation of the second correction by the iterative information filter and stochastic cloning is done over successive timesteps, and includes, for a timestep the steps of: back-propagating a correction of the posterior navigation state of the carrier into a correction of the back-propagated state as a function of a kinetic model and/or of measurements acquired by at least one inertial sensor, updating of the correction of the back-propagated state as a function of direct or relative measurements acquired by at least one additional sensor.

3. The method of navigation assistance for a mobile carrier as claimed in claim 2, wherein the step of estimating by the Kalman filter and stochastic cloning of the first correction, the correction of the navigation state propagated by the Kalman filter includes a clone of a correction of the navigation state earlier than the correction of the propagated navigation state, as long as said correction of the earlier navigation state is involved in a relative measurement of a state correction later than the correction of the propagated navigation state; and wherein in the step of estimating of the second correction by the information filter running backwards and stochastic cloning, the correction of the navigation state back-propagated by the information filter running backwards includes a clone of a navigation state correction later than the correction of the back-propagated navigation state, as long as said correction of the later navigation state is involved in a relative measurement of a state correction earlier than the correction of the propagated navigation state.

4. The method of navigation assistance for a mobile carrier as claimed in claim 1, wherein the non-linear system configured to estimate the navigation state is expressed as follows: $X^{\star }=\underset{X}{\arg \min }\underset{k}{\overset{}{.Math.}}{.Math.{\psi }_k\left(X\right).Math.}_{P_k}^2,$ where the ψ.sub.k are cost functions associated with the measurements of each sensor, P.sub.k is the covariance matrix associated with the k-th measurement, i.e. the uncertainty that is associated therewith, the notation
∥e∥.sub.P.sub.k.sup.2=e.sup.TP.sub.k.sup.−1e represents a Euclidian norm weighted by the inverse of the matrix P.sub.k−.

5. The method of navigation assistance for a mobile carrier as claimed in claim 4, wherein the non-linear system estimating a navigation state X* of the mobile carrier is linearized to define a correction δXn* in the estimated state of the following form $\delta X_n^{\star }=\underset{\delta X}{\arg \min }\stackrel{}{{.Math.}_k}{.Math.A_k\delta X-b_k.Math.}_{P_k}^2.$

6. The method of navigation assistance for a mobile carrier as claimed in claim 2, wherein the propagation step implements an augmented transition matrix in the form $\left[\begin{array}{cc}\mathrm{Id}& \\ & F\end{array}\right]$ , with F corresponding to a transition matrix which relates a preceding state correction to a current state correction k, and Id an identity matrix of dimensions equal to the number of clones of the past states.

7. The method of navigation assistance for a mobile carrier as claimed in claim 2, wherein the updating step implements an augmented observation matrix in the form {tilde over (H)}=[. . . H.sub.i . . . H.sub.j . . . ], the blocks being of indices i and j, the rest being composed of zeros.

8. An estimation unit of a mobile carrier configured to implement the method as claimed in claim 1.

9. An inertial navigation unit of a mobile carrier comprising: an interface for receiving inertial measurements acquired by at least one inertial sensor, an interface for receiving additional measurements acquired by at least one additional sensor, an estimation unit as claimed in claim 8 for estimating a navigation state of the unit on the basis of measurements acquired by the interface for receiving inertial measurements and the interface for receiving additional measurements.

10. A computer program product comprising program code instructions for executing the steps of the method as claimed in claim 1, when this program is executed by an estimation unit of a trajectory of a mobile carrier.

Description

DESCRIPTION OF THE FIGURES

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

[0039] FIG. 1 shows a navigation unit for a mobile carrier according to an embodiment of the invention,

[0040] FIGS. 2 and 3 illustrate the steps of a method for tracking the location of a navigation unit of a mobile carrier, according to an embodiment of the invention,

[0041] FIGS. 4A, 4B, 4C show a time-domain diagram illustrating arrival times of acquired measurements and processing times taken by a navigation unit according to an embodiment of the invention, and

[0042] FIGS. 4D and 4E show a time-domain diagram illustrating arrival times of acquired measurements and processing times taken by a navigation unit according to different embodiments of the invention.

DETAILED DESCRIPTION OF THE INVENTION

[0043] The term “stochastic cloning” should be understood to mean an increase in the state of a system by duplication of the past states involved in a future observation.

[0044] The term “correction of a navigation state” should be understood to mean the estimate of the difference between the estimated state and the actual state of the system.

[0045] The term “fusion of two corrections” should be understood to mean the computation of a consolidated correction resulting from two previously-obtained corrections, usually by the application of a weighted mean. The weights are preferably obtained by the information matrix coming from the information filter and the inverse of the covariance matrix coming from the Kalman filter.

[0046] The term “information filter running backwards” should be understood to mean a recursive computation of the vector and of the information matrix of a Gaussian law representing the information from the future states, carried out from the last to the first timestep of the window.

[0047] The term “navigation state” should be understood to mean a set of variables representative of at least the orientation and the position or the orientation and the speed of a carrier, at a determined time or during a series of times. In the remainder of the text, the covariances of the measurements are written P with a lower index, and the covariances returned by the Kalman filter are written P with an upper index.

[0048] With reference to FIG. 1, an inertial unit 10 is integrated on a mobile carrier 1, such as a land vehicle, a helicopter, an airplane etc. The inertial unit 10 includes several parts: inertial sensors 12, additional sensors 13, and an estimation unit 11 for implementing estimation computations. These parts can be physically separate from one another.

[0049] The inertial sensors 12 are typically accelerometers and/or gyroscopes respectively measuring specific forces and rotation speeds that the carrier undergoes with respect to an inertial reference frame. The specific force is equivalent to the original non-gravitational acceleration. When these sensors are fixed with respect to the carrier, the unit is said to be of “strap down” type.

[0050] The additional sensors 13 are variable according to the type of carrier, its dynamic range, and the intended application. Inertial units typically use a GNSS receiver (GPS for example). For a land vehicle, it can also be one or more odometers. For a boat, this can be a “loch”, giving the speed of the boat with respect to that of the water or seabed. Video cameras or radar, for example of LiDar type, form another example of additional sensors 13.

[0051] The output data of the estimation unit 11 are a state representative of the navigation of the carrier, which will be referred to in the remainder of the text as the navigation state x and where applicable internal states of the inertial unit 10.

[0052] Three different types of measurements can be discerned, according to the type of sensor from which they come. The following can thus be discerned: [0053] inertial measurements relating two consecutive states x.sup.i and x.sup.i+1; [0054] direct measurements involving only one of the x.sup.i at a time, such as GPS measurements; and [0055] relative measurements, i.e. they relate at least two states x.sup.i and x.sup.j which may not be consecutive. This is for example the case of the measurements obtained with video cameras or LiDar.

[0056] This navigation state may comprise at least one navigation variable of the carrier (position, speed, acceleration, orientation etc.) The navigation state can under any circumstances be represented in the form of a vector, each component of which is a navigation variable of the carrier.

[0057] The estimation unit 11 particularly comprises an algorithm configured to fuse the information given by the additional sensors 13 and the inertial sensors 12 such as to supply an optimal estimate of the navigation information. The fusion is done according to a continuous or discrete dynamic system serving as a model to predict the state at each time based on the state at the preceding time, using a non-linear propagation function f, and the way of observing it using an observation function h can also be non-linear. Such a system is non-linear.

[0058] The estimated quantities will subsequently be written with circumflex accents (5?) and the actual quantities without circumflex accents (x).

[0059] The estimation unit 11 includes a primary interface 21 for receiving measurements acquired by the inertial sensors 12, a secondary interface 22 for receiving measurements acquired by the additional sensors 13, and at least one processor 20 configured to implement the method described below.

[0060] The smoothing is an algorithm able to be encoded in the form of a computer program executable by the processor 20. The estimation unit 11 further comprises an output 23 for delivering output data computed by the processor 20.

[0061] With reference to FIGS. 2 and 3, certain steps are illustrated of a method of navigation assistance for a mobile carrier including an inertial navigation unit, implemented by the estimation unit 11 according to an embodiment of the invention,

[0062] As part of this method, the trajectory followed by the carrier is estimated, the navigation state of said carrier being the last state of said trajectory. Based on a trajectory {circumflex over (X)}.sub.n, estimated in any way whatsoever, a method is used to determine a correction to be applied to said trajectory to obtain a corrected trajectory that can take into account the information items given, at any time during the navigation, by the additional sensors 13 and the inertial sensors 12 such as to supply an optimal estimate of the navigation information.

[0063] Since the trajectory {circumflex over (X)}.sub.n is estimated by a non-linear problem, in a so-called parameterization step E10, it is implemented in the way in which a non-linear system is configured to describe a change of a navigation state of the mobile carrier 1. Consequently, the system after linearization forms a linear least squares problem to be solved to determine a correction δx.sub.n* of the estimated trajectory {circumflex over (X)}.sub.n.

[0064] The trajectory associated with the MAP is defined by a non-linear optimization problem of the form

[00004]$X^{\star }=\underset{X}{\arg \min }\underset{k}{\overset{}{.Math.}}{.Math.{\psi }_k\left(X\right).Math.}_{P_k}^2$

wherein: [0065] the ψ.sub.k are cost functions associated with the measurements of each sensor, [0066] P.sub.k is the covariance matrix associated with the kth measurement, i.e. the uncertainty that is associated therewith, [0067] the notation ∥e∥.sub.P.sub.k.sup.2=e.sup.TP.sub.k.sup.−1e epresents a Euclidian norm weighted by the inverse of the atrix P.sub.k.

[0068] The estimated trajectory {circumflex over (X)}.sub.n can then be corrected over an iteration of the method. The method relies on an iterative solving of the non-linear optimization problem by successive linearizations of the problem. A search is thus made for a series of solutions of the form {circumflex over (X)}.sub.n+1={circumflex over (X)}.sub.n+δX*.sub.n, while seeking to minimize a linear system approximating the problem of optimization of the MAP, which leads to solving a succession of linear least squares problems of the form:

[00005]$\delta X_n^{\star }=\underset{\delta X}{\arg \min }\stackrel{}{\underset{k}{.Math.}}{.Math.A_k\delta X-b_k.Math.}_{P_k}^2$

where the matrices A.sub.k and the vectors b.sub.k depend on the measurements, on {circumflex over (X)}.sub.n, and on the chosen parameterization. The method considering a trajectory, or part of it, composed of several successive states, δX.sub.n is in fact composed of several blocks each representing one of these states: δx.sub.n.sup.1, . . . , δX.sub.n.sup.p.

[0069] The problem encountered stems from the fact that the standard methods of solving these linear problems require the explicit computation of P.sub.k.sup.−1, which can lead to severe numerical problems in the case of high-accuracy inertial units, particularly on integrated architectures operating with reduced computation means, with 32-bit or even 16-bit cards.

[0070] An alternative that can also be applied in the proposed method consists in considering, not P.sub.k, but its square root, a matrix S.sub.k such that P.sub.k=S.sub.kSk.sup.T, which one also avoids inverting.

[0071] Thus the non-linear system is linearized about a navigation state (step E20). The navigation state at the iteration n is composed of the sequence of states {circumflex over (X)}.sub.n. It is considered that the system is initialized to the first iteration by a first a priori state. This state can be any state whatsoever,

[0072] Once the problem has been linearized (step E20), the steps E21, E22, E30 of solving of this system are performed. Provision is made for a solving algorithm, which finds the exact solution of the linear least squares problem, but makes it possible to avoid the numerical stability problems that could appear with a unit of too high accuracy.

[0073] To do this, use is made of the fact that the least squares problem can be solved without numerical instability by a method known as a Kalman smoother completed with a so-called “stochastic cloning” method, making it possible to vary the number of states of the system so that it is written in a form compatible with the Kalman smoother.

[0074] This smoother, which applies to linear systems, is constructed on the output of a Kalman filter, i.e. it is used to correct the estimates resulting from the Kalman filter by including the information contained in the observations coming from the future. Thus, to apply this smoother, one must first scan the data in the “direct” sense, i.e. from the time i=1 to the time i=T (where T is the duration of the observation window), by applying a Kalman filter to them. The information filter, which operates backwards, i.e. from the time k=T to the time k=1, can then be applied. Consequently, after the smoother, the estimate of the state at the time i does not only take into account the past observations Y1, . . . , Yi but all the observations contained in the observation window Y1, . . . , YT.

[0075] In a known manner, a Kalman filter KF is a recursive estimator described by a linear system. Here, these linearized states are the corrections to be applied to the navigation states of the trajectory.

[0076] The filter is initialized with an initial state, which will serve as input for a first timestep of the filter. Each following timestep of the filter takes as input a state estimated by a preceding timestep of the filter, and supplies a new estimate of the linearized state (or correction) of the carrier.

[0077] A timestep of the Kalman filter conventionally comprises two steps: a propagation, and an update.

[0078] The propagation step determines a propagated linearized state of the carrier on the basis of the preceding linearized state (or the initial linearized state for the first iteration), and this by means of the linearized propagation function.

[0079] The Kalman filter makes it possible to perform the approximation of the mean and variance of the conditional probability distribution of a linearized state knowing all the past observations, i.e. up to this time.

[0080] There exists a very specific set of circumstances in which δX*.sub.n can be obtained without having to compute the inverses of the matrices P.sub.k associated with measurements of an inertial unit: the one where the measures are separated into two categories: [0081] inertial measurements, coming from the inertial sensors 12, which relate δX.sub.n.sup.i and δX.sub.n.sup.i+1, the covariances of which are denoted Q, and [0082] direct measurements, coming from the additional sensors 13, involving only one of the x.sup.i at a time, such as GPS measurements.

[0083] Specifically, in this case, the Kalman smoother can be used to solve the problem, and one of its recent formulations makes it possible to avoid having to invert the problematic matrices.

[0084] This uses a second estimation by the Kalman filter, this time applied in the backward direction of the observations in a known form moreover known as “information filter”, supplying a second correction. This is then fused with the result of the first Kalman filter.

[0085] The Kalman smoother makes it possible to obtain the desired means x.sup.1, . . . , x.sup.p,

[0086] However, the Kalman smoother does not apply as such in other cases, particularly when the measurements other than inertial ones are relative measurements, i.e. they relate at least two states δX.sub.n.sup.i and δX.sub.n.sup.j. This is the case for example for measurements obtained video cameras or LiDars,

[0087] For all additional sensors 13, the covariances of the associated measurements are denoted R.

[0088] To be able to extend the Kalman smoother to problems combining inertial measurements and measurements of any kind, direct and/or relative, of the states, the method is based on the joint use of the smoother and the so-called “stochastic cloning” method, detailed hereinafter. This makes it possible to carry out, inter alia, inertia-vision and inertia-LiDar fusions based on numerically stable Maximum A Posteriori, even using high-accuracy navigation units, and in integrated architectures with reduced computational ability.

[0089] With reference to FIG. 4A, there is illustrated an example of a trajectory to be estimated by smoothing over a determined time period, The straight arrows between two successive states indicate inertial measurements and the curved arrows denote relative measurements between two states which may not be successive.

[0090] The use of stochastic cloning is illustrated in FIG. 4B. Thus, the state evolves by retrieving clones of the past states which are involved in later measurements, for example δX.sub.0 which will have a direct impact on δX.sub.3. The use of stochastic cloning is represented by alternative states V.sup.1, . . . , V.sup.p the size of which varies over time, since they will contain in memory clones of past states, as long as these are involved in a measurement with a later state. The alternative state V.sup.i is therefore formed of δX.sup.i and clones of earlier states δX.sup.j1, . . . , δX.sup.jm.

[0091] FIG. 4C illustrates the advantages of the joint use of the smoother and the so-called “stochastic cloning” method with a high linearization error. Thus, the propagation of the information is relatively stable, and the correction of the initial error brought about by the smoothing is therefore properly taken into account, to obtain an estimate of better quality.

[0092] By comparison, FIG. 4D illustrates the limits of the application of the Kalman filter with stochastic cloning. Thus, a high linearization error, as may exist in inertia-vision fusion, is propagated and never corrected afterwards, which can lead to an estimate of lower quality, or even one that is incoherent,

[0093] FIG. 4E illustrates the theoretical advantages and the practical limits of conventional smoothing with respect to the method illustrated in the example of FIGS. 3 and 4. Theoretically, the initial linearization error must be corrected owing to later measurements. However, in actual fact the numerical errors of the conventional implementations take over and severely degrade the estimate.

[0094] The joint use of the smoother and the so-called “stochastic cloning” method can be implemented by the following algorithm.

[0095] In particular, one is seeking to compute at each iteration a first correction (step E21) and a second correction (step E22) of the navigation state in the iteration under consideration.

[0096] In a step E21, to apply this smoother, the data are scanned in the “direct” direction, i.e. from the time i=0 to the time i=T (where T is the duration of the observation window), by applying a Kalman filter to them to determine a vector x.sup.i and a matrix P.sup.i, representative of the information up to the time i, i.e.

P(V.sup.i|Y.sub.0, . . . , Y.sub.i)˜(x.sup.i, P.sup.i)

[0097] In particular, the correction associated with the state i will be given by the last block of V.sup.i, therefore its mean by the last block of x.sup.i, and its covariance by the block on the bottom right of P.sup.i.

P(δX.sub.n.sup.i|Y.sub.0, . . . , Y.sub.i)˜(x.sub.i.sup.i, P.sub.ii.sup.i)

[1] Initialization

[0098]
x.sup.0=δX.sub.n.sup.0, P.sup.0=P.sub.0

[2] F each iteration i
[3] If δX.sub.n.sup.i−1 is involved in a later measurement, cloning of δX.sub.n.sup.i−1 in the mean x.sup.i−1 of the alternative state. The covariance P.sup.i−1 is extended by duplicating the last row, then the last column
[4] Propagation of the extended state:

[00006]$x^i\leftarrow \tilde{F}{x}^{i-1}\mathrm{with}\tilde{F}=\left[\begin{array}{cc}\mathrm{Id}& \\ & F\end{array}\right]$

the identity matrix being of dimensions equal to the number of clones stored in the alternative state. The covariance is propagated conventionally.
[5] If there is a measurement between i and j with j<i
[6] Update according to the known Kalman gain equations with

{tilde over (H)}=[. . . H.sub.i . . . H.sub.j . . . ]

the blocks being of indices i and j
[7] If δX.sub.n.sup.j is not involved in a later measurement, deletion of δX.sub.n.sup.j in the alternative state: deletion of the block, rows and columns associated in x.sup.i and P.sup.i.

[0099] In a step E22, the smoother is applied to recursively compute the distribution a posteriori

P(V.sup.i|Y.sub.i+1, . . . , Y.sub.T)

, in the form representative of the information starting from the time i, i.e. via a vector y.sup.i and an information matrix J.sup.i encoding a normal distribution

(J.sub.i.sup.−1y.sup.i, J.sub.i.sup.−1)

The matrix J.sup.i therefore corresponds to the inverse of the covariance matrix associated with this distribution.
[1] initialization y.sup.0=0, j.sup.0=0
[2] for each iteration i ranging from n to 0
[3] If δX.sub.n.sup.i is involved in a preceding measurement, extend y.sup.i with as many zeros as the dimension of δX.sub.n.sup.i, and add to j as many rows of zeros, then as many columns of zeros.
[4] If there is a measurement Y.sub.ij between δX.sub.n.sup.i and δX.sub.n.sup.j with j<i
[5] Update in the form of information with

{tilde over (H)}=[. . . H.sub.i . . . H.sub.j . . . ]

the blocks being of indices i and j:

y.sup.iζy.sup.i+{tilde over (H)}.sup.TR.sup.−1Y.sub.ij

and J.sup.iζJ.sup.i+{tilde over (H)}.sup.TR.sup.−1{tilde over (H)}

[6] If δX.sub.n.sup.j is not involved in an earlier measurement, fusion of δX.sub.n.sup.j in the extended information item:

y.sub.i.sup.iζy.sub.i.sup.i+y.sub.j.sup.iand J.sub.ii.sup.iζJ.sub.ij.sup.i+J.sub.ji.sup.i+J.sub.jj.sup.i

, where y.sub.k.sup.i denotes the part of y corresponding to the clone δX.sub.n.sup.k and J.sub.kl.sup.i corresponds to the information block associated with δX.sub.n.sup.k and δX.sub.n.sup.l, then deletion of the blocks associated with δX.sub.n.sup.j,

[0100] [7] Back-propagation of the extended information item:

y.sup.i−1ζ{tilde over (F)}.sup.T(Id+J.sup.iQ).sup.−1y.sup.i with F≤+

the identity matrix being of dimensions equal to the number of clones stored in he alternative state, and

J.sup.i−1ζ{tilde over (F)}.sup.T(Id+J.sup.iQ).sup.−1J.sup.i{tilde over (F)}.sup.T

[0101] In a step E30, the “direct” and “backward” estimates are then fused. It is thus a question of fusing the first and second corrections obtained in steps E21 and E22 respectively.

[0102] Thus for each iteration i, one may now proceed to the following computation of the final correction of

{circumflex over (V)}.sub.n.sup.i=(Id+P.sup.iJ.sup.i).sup.−1(x.sup.i+P.sup.iy.sup.i)

Then, for each the correction of the navigation state δX.sub.n.sup.i* is obtained as the last block of the extended state {circumflex over (V)}.sub.n.sup.i.
Next, once the correction is determined, the objection (step E40) of the navigation state is then performed in the following form:

{circumflex over (X)}.sub.n+1={circumflex over (X)}.sub.n+δX*.sub.n.

[0103] The proposed method therefore makes it possible to extend the Kalman smoother to problems combining inertial measurements and measurements of any kind, direct and/or relative, of the states. The method is based on the joint use of the smoother and of the so-called “stochastic cloning” method detailed hereinafter. This makes it possible to carry out, inter alia, inertia-vision and inertia-LiDar fusions based on numerically stable Maxima A Posteriori, even while using high-accuracy navigation units, and in integrated architectures with reduced computational ability.