Method for determining a trajectory of a mobile carrier in real time and in delayed time

Abstract

The invention relates to a method for determining a trajectory of a mobile carrier, comprising the following steps: S1) at each instant, determining a merged trajectory (TrA) of the mobile carrier, the merged trajectory (TrA) being obtained by merging data provided by at least one relative position information sensor (CR) with data provided by at least one absolute position information sensor (CA), and, at a plurality of instants determined among the instants, generating a merged data packet comprising merged data at the determined instant and at least one confidence indicator associated with the merged position data packet at the determined instant, the confidence indicator comprising information on the accuracy of the data provided by the absolute position information sensor (CA);S2) at each determined instant, determination of a regularized trajectory (TrB) as a function of the detection of a position jump in the merged data packet between the previous selected instant and the selected instant. Figure for the abstract: Fig. 4;

Description

Title of the invention: Method for determining a trajectory of a mobile carrier in real time and in delayed time Technical field

[0001] The present invention relates to a method and a system for determining a trajectory of a mobile carrier.

[0002] The technical field of the invention is that of localization by data fusion between data from a sensor with relative position information, such as an inertial navigation system (or INS for Inertial Navigation System), and data from a sensor with absolute position information, such as a satellite navigation system (or GNSS for Global Navigation Satellites System).

[0003] Location systems using fusion make it possible to locate a mobile carrier such as a vehicle in a robust manner, even when the reception conditions of the GNSS signal deteriorate. Indeed, low-power GNSS signals are likely to be disturbed by intentional interference (jamming) or not (locations not covered by GNSS signals).

[0004] The localization system using INS / GNSS fusion makes it possible to complete the missing information from the satellite system, by estimating the movements of the vehicle from the measurements of its accelerations (accelerometer) and angular speeds (gyrometer). When the satellite signal is lost, an important characteristic of the trajectory reconstructed from the sensors is its drift over time, caused by the accumulation of measurement errors.

[0005] [Fig.l] illustrates this effect based on field measurements. During this test, the mobile carrier device crosses an area in which the GNSS signal is significantly degraded, which leads the fusion algorithm to calculate the position in this area from inertial measurements only, resulting in a drift of the estimated trajectory compared to the actual trajectory. When the GNSS signal is recovered, at the exit of the degraded area, the estimated trajectory undergoes a significant readjustment visible in [Fig.l].

[0006] Recalibrations are inherent to fusion techniques and, although unrealistic since the tracked object cannot make such position jumps, the estimated trajectory offers the best absolute precision in the context of real-time use.

[0007] On the other hand, when the trajectory is calculated in delayed time, the recalibration can be anticipated and corrected before the drift occurs. Indeed, for certain applications a regular real-time trajectory, i.e. one not including significant corrections, as well as a precise delayed-time trajectory, are required.

[0008] This is for example the case for topographic survey applications for which a mobile object takes a certain number of captures of a scene which are then processed in order to carry out a digital reconstruction of this same scene, in particular to carry out simultaneous localization and mapping, also called SLAM (Simultaneous Localization And Mapping).

[0009] The needs relating to this reconstruction have two aspects:

[0010] - a real-time reconstruction for which the regularity of the trajectory (that is to say (say the different positions of the object taking the shot) is essential for reconstruction calculations. This reconstruction is preferably carried out in real time in order to limit the volume of data to be stored;

[0011] - precise geo-referencing of the reconstructed scene which can be carried out in real time deferred, with necessary consistency between images when recombining images.

[0012] Real-time GNSS / INS fusion algorithms and delayed-time GNSS / INS fusion algorithms are known in the state of the art.

[0013] Real-time navigation systems combining the two inertial subsystems INS (Inertial Navigation System) and GNSS (Global Navigation Satellite System), rely on a fusion algorithm which classically belongs to the family of Kalman filters, as described in more detail in [1], or to its numerous derivatives such as Extended Kalman Filter (EKF), Error-State Kalman Filter (ES-KF), Unscented Kalman Filter (UKF), Interacting Multiple-Models Kalman Filter (IMM-KF). Typically, these algorithms estimate the distribution of a state, i.e. the mean value as well as the associated covariance of a set of variables of interest.

[0014] In the present case, this state vector can be formed for example by the position, the speed, the attitude angles, as well as the biases of the inertial sensors of the system considered.

[0015] Most often, in order to optimize the processing, the algorithm estimates the errors of the quantities cited previously in order to then apply the necessary corrections to them. This is a particular architecture called closed loop (cf. [1], §16), using loose coupling and illustrated in [Fig.2],

[0016] In this example, the relative position information sensor CR, for example an inertial system, continuously calculates a solution notably composed of an estimate of attitude, position and speed (solution called APV), which is then compared, with regard to the position P and the speed V, to those from the absolute position information sensor CA, for example a GNSS receiver.

[0017]

[0018]

[0019]

[0020]

[0021]

[0022]

[0023]

[0024]

[0025]

[0026] This difference is injected into the Kalman filter (P-EKF prediction and M-EKF update) which calculates the necessary corrections to be made to the quantities of interest, but also to the sensors (bias, scale etc.). All these corrections Ôt / A 50, 8(p, Ôp0S V€i, Ôbacc, 5bgyr, ôSf are applied to the inertial APV solution to produce a corrected APV solution VA A (p, pos, vel, bacc, bs 'gyr. In this application, a trajectory corresponds to an APV solution. In this example, the state vector xg at the instant which includes all the estimated quantities of interest, is formed by: 0¾ ôba ôbg In which ôt|^ corresponds to the three angular corrections (x, y and z planes), ôVgb corresponds to the three velocity corrections (x, y and z planes), 6¾ corresponds to the three position corrections (x, y and z planes), ôba corresponds to the three accelerometer bias corrections (x, y and z planes), and ôbg corresponds to the three gyrometer bias corrections (x, y and z planes). In addition to calculating the corrections, the algorithm also estimates the covariance 1\ associated with the solution which contains the precisions of the set of values that are tracked in the form of a matrix of dimensions mxm, where 111 represents the dimension of the state vector. Another classic architecture, not shown here but based on identical principles, uses a closed loop but a tight fusion called tight-coupling, for which it is the raw measurements of the receiver (Pseudo-range and pseudo-range rate) which are compared to a prediction of these same values from the inertial solution. These algorithms can use several different definitions for angles, positions and speeds, depending on the chosen reference frame (ECI, ECEF, ENU etc.), so the estimated angles are not necessarily the roll, pitch and yaw angles. In addition to inertial measurements and GNSS measurements, the algorithm can also take into account a wide variety of other sources of information such as measurements from a magnetometer, odometer, barometer (altitude), etc. The common point of all these architectures and variants is that they schematically comprise two stages, illustrated by [Fig.2]:

[0027] - a prediction step (P-EKF) where the previous state of the system as well as the associated covariance are extrapolated to the current time;

[0028] - an update step (M-EKF) where the new measurements are taken into account by the algorithm to adjust the current state of the system as well as its associated covariance.

[0029] The temporal organization of the processing is broken down into two sets operating at different rates, the relative position information sensor CR (typically the inertial unit), and the Kalman filter (P-EKF and M-EKF in [Fig.2]).

[0030] The relative position information sensor CR operates at the high acquisition frequency of the inertial unit (fb generally between 50 Hz and 2000 Hz, typically approximately equal to 200 Hz for certain inertial navigation systems). The processing is therefore initiated by the generation of a new measurement carried out by the inertial measurement unit (measurement by the accelerometer and by the gyrometer) at the instant which is then processed in order to update the angles / position / speed of the APV solution.

[0031] At a lower frequency, generally between 0.1 Hz and 20 Hz and typically equal to approximately 10 Hz, the Kalman filter processing is initiated by the provision of a new measurement / information, outside the inertial unit. In its simplest configuration comprising only an inertial unit and a GNSS receiver, this consists of a GNSS measurement, but for more complex systems it can also correspond to at least one of the following measurements: an Ultra Wide Band distance measurement, an odometric measurement, an altimetric measurement, the detection of a static phase (which is not a “measurement” strictly speaking, but falls into the category of “information”), the taking into account of speed constraints, or even a radar measurement.

[0032] With regard to deferred-time GNSS / INS fusion algorithms, the problem of calculating a deferred-time fused trajectory is known as optimal smoother.

[0033] One of the most widespread techniques to address this problem is called "Rauch-Tung-Streibel Smoother", or "RTS-Smoother", and described in [2]. This technique consists of reprocessing all the data (INS, GNSS measurements etc.) in the order of increasing times (i.e. as in real-time processing) to generate a first trajectory, then in the order of decreasing times to generate a second trajectory, and then calculating a solution that combines the two trajectories.

[0034] There are many variations to this method but all require having raw measurements, which involves saving and transferring a significant amount of data (for example 12 to 1000 bytes per measurement time, or 144 kB to 12 MB). for 1 minute of recording at 200 Hz) as well as a fairly complex processing of this data.

[0035] The approach proposed by [3] applies a very similar method where the data are reprocessed in the forward and reverse directions several times and are then combined to obtain a smooth trajectory. However, this method requires having and reprocessing all the raw measurements, as previously indicated.

[0036] Other approaches such as [4] reprocess the recorded positions of a trajectory by selecting only certain positions so that the trajectory formed by the selected points verifies a regularity criterion. This approach therefore processes a reduced number of points but does not carry out a real correction of the positions. Indeed, only the aberrant points are processed (see white and black points in [Fig.3] of [4]). However, the drift of the positions resulting from the INS measurements compared to the real trajectory is progressive, there is therefore no aberrant point on the trajectory calculated from the INS measurements. In addition, the approach described in [4] also has the disadvantage of reducing the number of points representing the trajectory.

[0037] The invention thus aims to provide a method for determining a trajectory of a mobile carrier, which calculates a merged trajectory in real time and regular and a precise and regular trajectory in deferred time, while limiting the flow of information as well as the backup capacity necessary for the trajectory in deferred time. Summary of the invention

[0038] An object of the invention is therefore a method for determining a trajectory of a mobile carrier, comprising the following steps:

[0039] SI) at each instant determination of a merged trajectory of the mobile carrier, the merged trajectory being obtained by merging data supplied by at least one sensor with relative position information with data supplied by at least one sensor with absolute position information, and, at a plurality of determined instants 4 among the instants generation of a merged data packet comprising data merged at the determined instant 4 and at least one confidence indicator associated with the data packet of the merged position at the determined instant the confidence indicator comprising information on the precision of the data supplied by the sensor with absolute position information;

[0040] S2) at each determined instant % determination of a regularized trajectory in function of detecting a position jump in the merged data packet between the previous selected time tk-i and the selected time

[0041] Advantageously, the method further comprises a step S3) of segmenting the regularized trajectory into so-called high-confidence segments and so-called low-confidence segments. non-high confidence, the segmentation being performed as a function of the confidence indicator associated with the merged data packet for each determined instant 4, and determining a post-processed trajectory, the post-processed trajectory corresponding to the merged trajectory for each high confidence segment, and the post-processed trajectory being determined, for each non-high confidence segment, by a geometric transformation of the non-high confidence segment such that a continuity condition is satisfied between each of its ends and the end of the adjacent high confidence segment.

[0042] Advantageously, the relative position information sensor comprises an inertial unit.

[0043] Advantageously, the absolute position information sensor comprises a GNSS receiver supporting RTK mode, the confidence indicator being provided by the RTK value of the GNSS receiver, in particular the “RTK fix” value.

[0044] Advantageously, the first step SI comprises the calculation of a correction indicator which represents the difference between the data supplied by the relative position information sensor and the data supplied by the absolute position information sensor between two consecutive selected times 4-1 and 4 •

[0045]

[0046]

[0047]

[0048]

[0049]

[0050]

[0051]

[0052]

[0053]

[0054] Advantageously, the correction indicator is determined by the relationship: xi corresponding to a trajectory correction at an instant tj. Advantageously, the detection of a position jump includes the comparison of the correction indicator with a predetermined threshold. Advantageously, the regularized trajectory is determined by applying compensation to the position on the merged trajectory (TrA) at the determined time 4, the compensation being determined iteratively by the following relation: ^k~ Ak-i + Ôk A k corresponding to the compensation at the determined instant 4, A corresponding to the compensation at the previous determined instant 4-1, corresponding to a correction at the determined instant 4 determined by the following relation: 0 if no jump detected p(us _ pfus sj a jump is detected k * ki nfus lk-} corresponding to the position on the merged trajectory (TrA) at the instant determined 4; corresponding to the position on the merged trajectory (TrA) at the instant determined previous 4-1.

[0055]

[0056]

[0057]

[0058]

[0059]

[0060]

[0061]

[0062]

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069] Advantageously, the predetermined threshold is determined based on the quality of the relative position information sensor. Advantageously, the geometric transformation includes at least one translation or one rotation. Advantageously, the geometric transformation further comprises a homothety when the non-high confidence segment is preceded or followed by a high confidence segment. Advantageously, the geometric transformation 3 applied to a position P on the non-high confidence segment is defined by the relation: gm(p) = H,„Rm ( p - p"S ) + P^'Corresponding to the first point belonging to the non-high confidence segment, which coincides with the previous point p / 1® which belongs to the neighboring high confidence segment; pæg corresponding to the first point of the segment on the regularized trajectory TrB; ^m Hmcorresponding to the homothety coefficient: Hm p'eg corresponding to the last point of the segment on the regularized trajectory TrB; pfus corresponding to the last point of the high-confidence segment preceding the non-high-confidence segment, p[us corresponding to the first point of the segment at high confidence following the segment to non-high confidence; With Rm defined by: 'costp sintp 0' Rm = -sintp costp 0 . 0 0 1. The angle of rotation cp being calculated by the relations: u=[0 0 1]T z = xy c = sign (zTu).[|z|| tp - atan(c, xTy)

[0070] With the vector product and xT the transpose operator.

[0071] Advantageously, step S3 further comprises a weighted recombination of the merged trajectory and the post-processed trajectory.

[0072] Advantageously, steps S1 and S2 are executed in real time during the movement of the mobile carrier, and step S3 is executed in delayed time.

[0073] The invention also relates to a system for determining a trajectory of a mobile carrier, comprising:

[0074] - a mobile device, embedded on the mobile carrier, comprising at least one relative position information sensor and at least one absolute position information sensor, the mobile device being configured to, at each instant 4, determine a merged trajectory of the mobile carrier, the merged trajectory being obtained by merging data provided by the relative position information sensor with data provided by the absolute position information sensor, and, at a plurality of determined instants tk among the instants h, generate a merged data packet comprising merged data at the determined instant tk and at least one confidence indicator associated with the merged position data packet at the determined instant 4, the confidence indicator comprising information on the accuracy of the data provided by the absolute position information sensor;

[0075] - a fixed device, configured to determine, at each determined instant tk, a trajectory regularized according to the detection of a jump in the position of the mobile carrier in the merged data packet between the previous determined instant 4-î and the determined instant 4-

[0076] Advantageously, the fixed device is further configured to segment the corrected trajectory into so-called high-confidence segments and so-called non-high-confidence segments, the segmentation being performed as a function of the confidence indicator associated with the data packet for each determined instant 4, and to determine a post-processed trajectory, the post-processed trajectory corresponding to the merged trajectory for each high-confidence segment, and the post-processed trajectory being determined, for each non-high-confidence segment, by a geometric transformation of the non-high-confidence segment such that a continuity condition is satisfied between each of its ends and the end of the adjacent high-confidence segment.

[0077] Advantageously, the mobile device and the fixed device are connected to each other by a radio link. Description of the figures

[0078] Other characteristics, details and advantages of the invention will emerge on reading the description given with reference to the appended drawings given by way of example.

[0079] [Fig.l], already described, represents an example of trajectory recalibration after a position drift.

[0080] Fig. 2, already described, represents an example of data fusion architecture.

[0081] [Fig.3] is a block diagram which represents the main steps of the process according to the invention.

[0082] [Fig.4] schematically represents the different processing blocks for putting implement the method according to the invention.

[0083] [Fig.5] is a block diagram which represents the sub-steps of the first step of the method according to the invention.

[0084] [Fig.6] is a block diagram which represents the sub-steps of the second step of the method according to the invention.

[0085] [Fig.7] is a block diagram which represents the sub-steps of the third step of the method according to the invention.

[0086] [Fig.8] illustrates the principle of correction of the regular trajectory.

[0087] [Fig.9] illustrates an example of implementation of the system according to the invention.

[0088] The main steps of the method according to the invention are described in connection with Figures 3 and 4.

[0089] First step IF

[0090] The method comprises a first step SI), which comprises the determination of a merged trajectory TrA of the mobile carrier at each instant C. The merged trajectory TrA is obtained by merging data provided by at least one relative position information sensor CR with data provided by at least one absolute position information sensor CA.

[0091] According to one embodiment, the relative position information sensor CR is an inertial navigation unit, and the absolute position information sensor CA is a satellite navigation receiver.

[0092] Other types of sensor may be envisaged, provided that data fusion between the sensors can be performed, that the absolute position information sensor CA can provide navigation information at the desired accuracy, and that the relative position information sensor CR can continuously provide positioning information, i.e. even when the absolute position information sensor CA is not able to provide location information.

[0093] For example, the absolute position information sensor CA can be a GNSS signal receiver, a UWB type radio wave transmitter / receiver terminal, WiFi, or 4G / 5G or any other equivalent technology. The relative position information sensor CR can be an odometer, or an optical flow sensor.

[0094] More generally, an absolute position information sensor CA is a sensor that relies on an external element (for example a satellite or a telecommunications station). Conversely, a relative position information sensor CR is a sensor that does not rely on an external element, thus likely to provide a position error that increases as a function of time.

[0095] It is also possible to exploit constraints linked to the nature of the mobile carrier, for example a land vehicle whose position can only be characterized according to two dimensions, or even a train, whose trajectory includes constraints linked to the railway line taken.

[0096] The first step SI can be implemented by a BFD block which generates the merged trajectory TrA in real time at a frequency fb which corresponds to the generation frequency of the position information of the relative position information sensor CR (cf. [Fig.4]).

[0097] [Fig.5] illustrates in more detail the sub-steps of the first step of the method according to the invention.

[0098] The first sub-step SI 1 consists of obtaining an initialization of the APV solution, according to techniques known to those skilled in the art.

[0099] The second sub-step S12 consists of receiving measurements from a sensor with relative position information CR such as an inertial unit (three acceleration values provided by an accelerometer, three rotation speed values provided by a gyrometer), at each measurement instant h.

[0100] For each measurement instant h the algorithm calculates in real time an inertial solution APV(i) according to techniques known to those skilled in the art (third sub-step S13).

[0101] At each measurement instant tj, the BFD block determines whether measurements from the absolute position information sensor CA such as a GPS system (examples position, speed, raw pseudo-range measurements) have been received (sub-step S14).

[0102] If this is the case, a corrected inertial solution APV(i) is calculated according to techniques known to those skilled in the art (sub-step S15). The merged trajectory TrA of the mobile carrier is thus determined at each measurement time h on the basis of the corrected inertial solution APV(i).

[0103] Sub-step S16 consists of determining whether the corrected inertial solution APV(i) calculated at the measurement instant belongs to the set of instants determined among the instants 6. This sub-step is also implemented as long as no measurement from the absolute position information sensor CA has been received.

[0104] The determination of the instants 4 can be implemented by decimating the count of the measurement instants 1;, using a decimation counter which can be reloaded to a predetermined value Ndec, for example between 1 and 1000, preferably equal to 200, and which is decremented by one unit at each measurement instant tj. When the counter reaches the value 0, it is reloaded and the sub-step S17 is executed.

[0105] Sub-step S17 consists of generating a merged data packet comprising merged data at the determined instant and at least one confidence indicator associated with the data packet of the merged position at the determined instant k 4-

[0106] The data packet contains for example: the time t^, the position p£us, the speed v(us, the angles a£us, one or more confidence indicators Iconf, a correction indicator Icorr. Some variables are vectorial and can therefore comprise several values. Given that the invention aims to compensate for position deviations, it is essential that the data packet comprises the position p^L. Alternatively, all of the values APV can be transmitted. The confidence indicator comprises information on the precision of the data supplied by the absolute position information sensor CA.

[0107] According to an embodiment in which the absolute position information sensor comprises a GNSS receiver supporting the RTK mode, the confidence indicator gf118 k is provided by the RTK value of the GNSS receiver, in particular the “RTK fix” value.

[0108] RTK (“Real Time Kinematic”) is a GNSS technique that allows centimetric accuracies to be achieved but which requires receiving correction data. These are generally calculated and transmitted from a fixed base located nearby (generally at a distance of less than 10 km).

[0109] RTK uses a mathematical ambiguity resolution algorithm to calculate the exact number of radio wavelengths between the satellites and the ground station antenna and produce a fixed or floating solution. In a fixed solution, the number of wavelengths is an integer, which provides centimeter accuracy. In a float solution, the algorithm does not provide an acceptable fixed solution, so the ambiguity is allowed to be a decimal or floating point number. The accuracy is typically sub-meter. In a non-RTK mode of operation, the order of magnitude of the accuracy is metric.

[0110] Many other confidence indicators can be calculated, such as, for example, indicators established from the coefficients of the covariance matrix of the solution calculated by the Kalman filter.

[0111] According to one embodiment, the correction indicator represents the difference between the data provided by the relative position information sensor and the data provided by the absolute position information sensor between two consecutive selected times ^4 and

[0112] In particular, the correction indicator is equal to the sum of the norms of the trajectory corrections carried out at each instant h between two consecutive selected instants and 4, according to the formula: [°113] Icorr=^^j|xj|

[0114] xî being here a component of the state vector defined previously, which can advantageously be the component of correction of the position according to at least one of the three dimensions.

[0115] Many other correction indicators can be calculated, such as for example a sum of the norms which would only relate to certain components of the correction xi, such as the speed components, a distance between two successive positions, a statistic (average deviation, variance etc.) relating to the distance between successive positions established for all the positions calculated between two transmitted packets, or even the detection of particular phenomena (detection of odometer slippage, loss of the GNSS signal, saturation of the sensors, loss of data, etc.).

[0116] It may be noted that a variant of the decimation of the counting of the measurement instants li (sub-step S16) consists of calculating the time elapsed since the last execution of sub-step S17: if this time exceeds a predetermined duration, sub-step S17 is executed. According to another variant, sub-step S17 may be executed for each correction of the inertial position (triggered by the reception of a GNSS measurement for example).

[0117] The merged trajectory TrA, determined by the BFD block ([Fig.4]), is said to be precise and not regular because the navigation information is generated at high frequency but with jumps in the trajectory due to the resetting.

[0118] Second step S2

[0119] The method comprises a second step S2 of determining a regularized trajectory TrB as a function of the detection of a position jump in the merged data packet / j / "scntrc the previous selected instant 4-i and the selected instant

[0120] Some embodiments of the second step are described in more detail in [Fig.6].

[0121] In a first sub-step S21, a variable A k, which contains the accumulation of all the regularization corrections, is initialized to ( Q () () jT.

[0131] ôk =

[0122] The second sub-step S22 consists of receiving the merged data packet p / ^comprising merged data at the determined instant at least one confidence indicator (Jus associated with the data packet of the merged position at K the selected time 4, and possibly a correction indicator.

[0123] The detection of a position jump advantageously comprises the comparison of the correction indicator contained in the data packet with a predetermined threshold Tsaut (sub-step S23):

[0124] Icorr> Tsaut

[0125] For example, the predetermined threshold Tsaut may be between 0.01 and 10, this value being able to be determined according to the desired precision.

[0126] According to an advantageous embodiment, the predetermined threshold Tsaut is determined as a function of the quality of the relative position information sensor. Indeed, with a very good quality inertial unit, it may be useful to set a relatively low Tsaut threshold, which implies a low tolerance to drifts of the inertial unit. Conversely, with a lower quality inertial unit, it may be useful to set a relatively high Tsaut threshold, which implies a high tolerance to drifts of the inertial unit.

[0127] Alternatively, the condition for detecting a position jump may relate to the successive violation of the thresholds, a violation of several thresholds simultaneously (for example a threshold linked to the sum of the position correction standards and a threshold linked to the sum of the speed correction standards, the two thresholds both having to be exceeded) or else undifferentiated (the exceeding of one of the two thresholds).

[0128] The regularized trajectory TrB can be determined by applying the compensation A k to the position p^'s on the merged trajectory TrA at the selected time tk, the compensation being determined iteratively by the following relation:

[0129] Ak=Ak.1 + ôk

[0130] A k corresponding to the compensation at the determined instant tk, A k4 corresponding to the compensation at the determined instant tk_i, $k corresponding to a correction at the selected instant tk determined by the following relation: 0 if no jump detected pfus _ pfus if an eSf jump detected A k 1 k-1

[0132] plus corresponding to the position on the merged trajectory TrA at the determined instant tk;

[0133] p^u® corresponding to the position on the merged trajectory TrA at the determined instant tk-1-

[0134] Thus, in Figure 6, sub-step S24 consists of updating the compensation ôk if a jump has been detected in sub-step S23. In this case, ôj. = Thus, Ak = Ak4 + p^-p^

[0135] Conversely, if no position jump has been detected, the compensation does not change, and Ak = Ak4.

[0136] Thus, in a fifth sub-step S25, the regularized position of the regularized trajectory TrB is calculated by applying a compensation A k to the position nfliS, by the following relation:

[0137] = ri fi

[0138] Finally, the position pf™ of the merged trajectory TrA, the position p™8 of the regularized trajectory TrB, the correction ôk, the confidence indicator , as well as K all other data (4, vlus, a('ls) are saved for KK post-processing (sub-step S26).

[0139] Applying the 5k correction has the effect of completely canceling the displacement between the selected times 4-1 and 4, which means that the position remains identical between these two times. Alternatively, the complete cancellation may be followed by a substitution by a probable displacement, calculated for example as a function of the previous displacement between the selected times 4-2 and 4-1, or any other previous displacement.

[0140] The second step S2 can be implemented by a trajectory correction block BCT, which generates the regularized trajectory TrB in real time (fig. 4).

[0141] The regularized trajectory TrB is said to be regular and not precise because there is consistency between the different positions of the trajectory, but it does not correspond to the precisely estimated trajectory (the position jumps having been compensated).

[0142] The combination of the first two steps S1 and S2 makes it possible to calculate a trajectory in real time which is free of jumps, i.e. strong discontinuities according to the value of the jump detection threshold set, but also as precise as possible even in a degraded GNSS environment, thanks to the fusion of data from sensors with absolute position information and sensors with relative position information (GNSS / INS fusion for example).

[0143] The regularized trajectory TrB can be very useful for the assembly of different shots (photogrammetry, radar imagery), which requires very precise relative positions between the different shots, which is not the case in the presence of a strong correction.

[0144] Third step S3

[0145] The method according to the invention comprises a third optional step. It consists of calculating a post-processed trajectory TrC from the merged trajectory TrA, the regularized trajectory TrB, and the confidence indicators (see [Fig.4]).

[0146] For this, at each determined instant the regularized trajectory TrB is divided into high confidence segments and non-high confidence segments * segment", we mean a portion of trajectory. The splitting is performed according to the confidence indicator cf™ associated with the merged data packet [)fus for each selected instant. The post-processed trajectory TrC corresponds to the merged trajectory TrA for each high-confidence segment SFC - i.e. the APV solution of the merged trajectory TrA. In addition, for each non-high-confidence segment, the post-processed trajectory TrC is determined by a geometric transformation of the non-high-confidence segment S^FC such that a continuity condition is satisfied between each of its ends and the end of the adjacent high-confidence segment.

[0147] Thus, the post-processed trajectory TrC is regular in real time and regular and precise in delayed time in post-processing.

[0148] The method according to the invention makes it possible to considerably limit the flow rate required between the mobile carrier and the fixed system responsible for calculating the post-processed trajectory, which also offers better robustness in the event of data loss during transmission.

[0149] As an example, the use of state-of-the-art techniques for calculating the trajectory in deferred time would require storing the raw data from the sensors and transmitting them from the mobile device where they would be acquired, to the calculation means (a computer) where they would be processed. According to this example, a radio link offering a useful throughput of between 38.4 kb / s (three accelerometer data, three gyrometer data, coded on four bytes, with an acquisition frequency equal to 200 Hz) or 200 kb / s (with more sensors) would be necessary. In addition, the loss of data during transmission would have a significant effect on the reconstruction of the trajectory because this would lead to errors during the calculation of the inertial solution.

[0150] The method according to the invention would make it possible to use a radio link at 0.9 kb / s or 3.84 kb / s depending on whether the position alone is transmitted, or the entire APV solution.

[0151] [Fig.7] illustrates certain embodiments of the third step S3.

[0152] In a first sub-step S31, the packets, or points of the trajectory, stored in the MS backup means, are supplied to the BPT trajectory processing block ([Fig.4]).

[0153] In a second sub-step S32, the regularized trajectory TrB is cut into high confidence segments sæ, and into non-high confidence segments Snæ-

[0154] According to an embodiment in which the absolute position information sensor CA comprises a GNSS receiver supporting RTK mode, the packets for which the confidence indicator corresponds to the value “RTK fix” (with plus or minus K a tolerance margin) are classified as high confidence segments, and packets whose confidence indicator is too far from the “RTK” value fix" (ie confidence indicator ^ / fts different from the "RTK fix" value with plus or minus minus the tolerance margin) are classified as non-high confidence NFC segments Year

[0155] Alternatively, the segmentation may be performed without knowledge of the RTK value. In particular, the confidence indicator may be compared to a threshold. Criteria additional tests can be added, such as comparing the confidence indicator with several thresholds, or even ensuring that a sufficient number of consecutive points must meet a condition to be classified as a high confidence segment çFC.

[0156] In a third sub-step S33, the corrected positions are calculated for all the points of each non-high confidence segment §^FC, as illustrated in [Fig.8].

[0157] The points belonging to a non-high confidence segment s^FC are classified into subgroups of consecutive points (in time, according to the determined instants ^) thus forming M trajectory segments S^! C represented by their indices: S^C = (k„ ( k + 1 ) .... (k + l)J

[0158] The first point of the segment S„7C has the index Kn and the last point of the segment has the index (k + l). v 7m

[0159]

[0160]

[0161] For example, in Figure 8, the continuity condition between each endpoint of the non-high-confidence segment and the endpoint of the adjacent high-confidence segment implies that position p^8 must coincide with position pfus, and position pæs must coincide with position p[us. km+hl All points of the regular trajectory of the same segment S^FC have the same geometric transformation Sm applied to them, to calculate corrected positions p£°": keNFC The geometric transformation of the segment S^! C is calculated such that the segment coincides at its two ends / reg ea: \ with the points of the 'tin-! / high-confidence neighboring segments Y Precisely, and as illustrated by the US | ' cnn-î+l / figure 8, it is therefore necessary that the first point p'es belonging to the segment çNFC coincides Km with the previous point plus the previous high confidence segment, and that the last ■Km- b point pæ8 belonging to the segment coincides with the next point plus of the next high confidence segment.

[0162] Thus, the continuity condition can be written .corr _ km nfilS km-l fCorr — nfus km+l

[0163] Advantageously, the geometric transformation comprises at least one translation or one rotation, as well as one homothety when the non-high confidence segment is preceded or followed by a high confidence segment. Thus, at the start or end of the trajectory, only the translation or the rotation can be applied.

[0164] These geometric transformations applied to a position p are defined as follows:

[0165] gm(p) =

[0166] P^ corresponds to the last point of the high confidence segment.

[0167] piorr = pfus K fit Km-}

[0168] With Hmle homothety coefficient

[0169] g-ig _ \pk"s~pt's

[0170] Rm is a rotation matrix calculated such that the equation pL?,Tl = pf™ is Kin™ *tn+l+l verified.

[0171]

[0172]

[0173] costp sintp 0' Rm = -sintp costp 0 The angle of rotation is calculated as follows: .. rn a iiT X= (CO z — xy c = sign (zTu).|| zj| tp = atan(c, xTy)

[0174] With the vector product and xr the transpose operator.

[0175] The continuity condition was defined with geometric transformations on the positions of the APV solution of the estimated trajectory. Other geometric transformations can be considered, for example by also taking into account the speed and angles resulting from the merger.

[0176] The fourth sub-step S34 then consists of reconstructing the post-processed trajectory TrC (made up of the ppp positions) from the merged positions and the positions corrected.

[0177]

[0178] So, for each high trust segment Sæ For each non-high confidence segment S^!C: PPP = Pk™

[0179] According to one embodiment, sub-step S34 further comprises a weighted recombination of the merged trajectory TrA and the post-processed trajectory TrC.

[0180] The weighted combination of positions can be expressed by the following relation:

[0181] ppp = œp{us + * kk \ rk

[0182] 0} is a scalar between 0 and 1.

[0183] . The weighted combination makes it possible to limit the effect caused by strong proximity between the start point and the end point of the post-processed trajectory. Indeed, when the trajectory forms a "loop", a very small error in the position of the start / end points is multiplied by the geometric transformation and the points in the middle of the loop can be highly erroneous. The weighted combination of positions helps to avoid such errors.

[0184] The invention also relates to a system capable of implementing the predefined method. The system, illustrated by [Fig.9], advantageously comprises a mobile device DM to be located and configured to calculate the merged trajectory TrA in real time. For this, it comprises an absolute position information sensor CA, a relative position information sensor CR, and a calculation unit CPU comprising a data fusion block BFD. The system further comprises a fixed device DF, comprising a trajectory correction block BCT for calculating the regularized trajectory TrB in real time, and a post-processing block BPT for calculating the post-processed trajectory TrC in deferred time. A backup means MS, arranged in the fixed device DF, makes it possible to store the data packets before post-processing.

[0185] Bibliographic references

[0186] [1] P- Groves « Principles of GNSS, Inertial and multisensor integrated navigation Systems 2nd édition » Artech House, 2013

[0187] [2] H. E. Rauch, F. Tung, and C. T. Striebel, “Maximum likelihood estimâtes of linear dynamic Systems,” AIAA journal, vol. 3, no. 8, pp. 1445-1450

[0188] [3] US2016 / 0252354 Al

[0189] [4] US2019 / 0187296 Al

Claims

Claims

1. Method for determining a trajectory of a mobile carrier, comprising the following steps: SI) at each instant 4 determining a merged trajectory (TrA) of the mobile carrier, the merged trajectory (TrA) being obtained by merging data supplied by at least one relative position information sensor (CR) with data supplied by at least one absolute position information sensor (CA), and, at a plurality of determined instants 4 among the instants h, generating a merged data packet (rJus\ comprising merged data at the determined instant 4 and at least one confidence indicator associated with the merged position data packet at the determined instant t / (, the confidence indicator / comprising 'A / precision information of the data supplied by the absolute position information sensor (CA);S2) at each determined instant 4, determination of a regularized trajectory (TrB) as a function of the detection of a position jump in the merged data packet / ^“4 between the previous selected instant ^k / 4-1 and the selected instant 4;S3) segmentation of the regularized trajectory into so-called high-confidence segments (s^) and into so-called non-high-confidence segments, the segmentation being carried out according to the confidence indicator ^Ms) associated with the merged data packet. For each determined instant 4, and determination of a post-processed trajectory (TrC), the post-processed trajectory (TrC) corresponding to the merged trajectory (TrA) for each high-confidence segment (S^), and the post-processed trajectory (TrC) being determined, for each non-high-confidence segment (S^C)' P31 a geometric transformation of the non-high-confidence segment such that a continuity condition is satisfied between each of its ends / and T end / fus \ of the high-confidence segment PÎ (Pb plus adjacent confidence..;

2. 2. Method according to claim 1, in which the relative position information sensor comprises an inertial unit.

3. Method according to one of the preceding claims, in which the absolute position information sensor comprises a GNSS receiver supporting RTK mode, the confidence indicator / / “•') being provided by the RTK value of the GNSS receiver, in particular the “RTK fix” value.

4. 4. Method according to one of the preceding claims, in which the first step SI comprises the calculation of a correction indicator which represents the difference between the data provided by the relative position information sensor and the data provided by the absolute position information sensor between two consecutive selected times 4-1 and 4.

5. 5. Method according to claim 4, in which the correction indicator is determined by the relation: jcorr _ 11X-11 ' X' corresponding to a trajectory correction at an instant.

6. 6. Method according to one of claims 4 or 5, in which the detection of a position jump comprises the comparison of the correction indicator with a predetermined threshold (Tsaut)-

7. 7. Method according to one of the preceding claims, in which the regularized trajectory (TrB) is determined by applying a compensation ( AJ to the position / on the trajectory k} \Pk ' merged (TrA) at the determined instant 4, the compensation being determined iteratively by the following relation: A k = A + Ôk, in which A k corresponds to the compensation at the determined instant 4, A corresponds to the compensation at the previous determined instant 4-1, ^k corresponds to a correction at the determined instant 4 determined by the following relation: 0 if no jump detected, nf^ _ kk ~ p{us _ if a jump is detected corresponding to the position on the merged trajectory (TrA) at the determined instant 4; pfus corresponding to the position on the * kl merged trajectory (TrA) at the previous determined instant 4-1-

8. Method according to one of claims 6 or 7, in which the predetermined threshold (Tsaut) is determined as a function of the quality of the relative position information sensor.

9. 9. Method according to one of the preceding claims, in which the geometric transformation comprises at least one translation or one rotation.

10. 10. The method of claim 9, wherein the geometric transformation further comprises a homothety when the non-high confidence segment is preceded or followed by a high confidence segment.

11. 11. The method of claim 10, wherein the geometric transformation 1 applied to a position P on the non-high confidence segment is defined by the relation: g ( p ) = HmR ( p - p^8 ) + p?°rr ; P^corresponding to the in 111 y hj first point belonging to the non-high confidence segment, which coincides with the previous point pfus.which belongs to the neighboring high-confidence segment; pæ8 corresponding to the first point of the segment on the regularized trajectory TrB; Hm corresponding to the homothety coefficient: ; Hm ~ 11« J pæ8 corresponding to the last point of the segment on the regularized trajectory TrB; p[us corresponding to the last point of the high-confidence segment preceding the non-high-confidence segment, p[us corresponding to the first point of the high-confidence segment following the non-high-confidence segment; With Rm defined by: ' COStp sintp 0' ; The rotation angle Rm = -sintp costp 0 . 0 0 1. ip being calculated by the relations: u = [0 0 1]T ' With v — / «reg «res \ x “ Pk ' Pt , \ NïH-l / . y=(Pk,s-Pkus ) \ Km+l+l / z = xyc — sign (zTu) J| z]| <p = atan(c, xTy) the vector product and xT the transpose operator.

12. 12. Method according to one of the preceding claims, in which step S3 further comprises a weighted recombination of the merged trajectory (TrA) and the post-processed trajectory (TrC).

13. 13. Method according to one of the preceding claims, in which steps S1 and S2 are executed in real time during the movement of the mobile carrier, and step S3 is executed in delayed time.

14. 4System for determining a trajectory of a mobile carrier, comprising: - a mobile device (DM), embedded on the mobile carrier, comprising at least one relative position information sensor and at least one absolute position information sensor, the mobile device being configured to, at each instant k, determine a merged trajectory (TrA) of the mobile carrier, the merged trajectory (TrA) being obtained by merging data provided by the relative position information sensor with data provided by the absolute position information sensor, and, at a plurality of determined instants tk among the instants h', generate a merged data packet comprising merged data at the determined instant and at least one confidence indicator.fusx associated with the data packet of the merged position at / the determined instant the confidence indicator comprising information on the precision of the data provided by the absolute position information sensor; - a fixed device (DF), configured to determine, at each determined instant a regularized trajectory (TrB) as a function of the detection of a jump in position of the mobile carrier in the merged data packet between the previous determined instant and the determined instant; in which the fixed device (DF) is, in addition, configured to segment the corrected trajectory into so-called high confidence segments and so-called non-high confidence segments, the segmentation being carried out as a function of the confidence indicator. / fux\ associated with the data packet for each determined instant and to determine a post-processed trajectory (TrC), the post-processed trajectory (TrC) corresponding to the merged trajectory (TrA) for each high-confidence segment (^), and the post-processed trajectory (TrC) being determined, for each non-high-confidence segment by a geometric transformation of the non-high-confidence segment such that a continuity condition is satisfied between each of its ends / rPCT rP„ \ and the end Pk ' Pc \ Mi Mi+l / \ of the adjacent high-confidence segment.

15. 15. System according to claim 14, wherein the mobile device (DM) and the fixed device (DF) are connected to each other by a radio link.

Images