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

Abstract

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

Description

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

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

[0002] The technical field of the invention is that of localization by fusion of data 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 allow for the robust localization of a mobile carrier, such as a vehicle, even when GNSS signal reception conditions degrade. Indeed, low-power GNSS signals are susceptible to interference, whether intentional (jamming) or unintentional (areas not covered by GNSS signals).

[0004] The localization system using INS / GNSS fusion makes it possible to supplement the missing information from the satellite system by estimating the vehicle's movements from measurements of its accelerations (accelerometer) and angular velocities (gyroscope). 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] Figure 1 illustrates this effect based on field measurements. During this test, the mobile device carrier crosses an area where the GNSS signal is severely degraded, causing the fusion algorithm to calculate the position in this area solely from inertial measurements, resulting in a drift between the estimated trajectory and the actual trajectory. When the GNSS signal is recovered upon exiting the degraded area, the estimated trajectory undergoes a significant realignment, as seen in Figure 1.

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

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

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

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

[0010] - a real-time reconstruction for which the regularity of the trajectory (that is to say Describing the different positions of the object taking the picture is essential for reconstruction calculations. This reconstruction is preferably performed in real time to limit the amount of data to be stored;

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

[0012] Real-time GNSS / INS fusion algorithms and delayed-time GNSS / INS fusion algorithms are known in the prior 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 that 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), and Interacting Multiple-Models Kalman Filter (IMM-KF). Typically, these algorithms estimate the distribution of a state, that is, the mean value and 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 velocity, 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 mentioned above in order to then apply the necessary corrections. This is a particular architecture called a closed loop (see [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 consisting in particular of an estimate of attitude, position and velocity (so-called APV solution), which is then compared, with regard to position P and velocity V, to those from the absolute position information sensor CA, for example a GNSS receiver.

[0017] This difference is injected into the Kalman filter (P-EKF prediction and setting to day M-EKF) which calculates the necessary corrections to be made to the quantities of interest, but also to the sensors (bias, scale etc). All these corrections Ôip, ÔO, Ô(p, Ôpai vei, Ôhacc, ôbgvn SS f are applied to the inertial APV solution to produce a corrected APV solution 0, <p. pos, vel, bacc, bgyr.

[0018] Dans la présente demande, une trajectoire correspond à une solution APV.

[0019] Dans cet exemple, le vecteur d’état x| à l’instant 4, qui comprend toutes les grandeurs d’intérêt estimées, est formé par :

[0020] W6. 'Teb 4= 0¾ Ôba ôbg. ôsr,

[0021] Where $4^ corresponds to the three angular corrections (planes x, y and z), ôv^b corresponds to the three velocity corrections (planes x, y and z), ôr|b corresponds to the three position corrections (planes x, y and z), ôba corresponds to the three accelerometer bias corrections (planes x, y and z), and ôbg corresponds to the three gyrometer bias corrections (planes x, y and z).

[0022] In addition to calculating the corrections, the algorithm also estimates the covariance Pr associated with the solution which contains the accuracies of the set of values ​​which are tracked in the form of a matrix of dimensions mxm, where m represents the dimension of the state vector.

[0023] Another classic architecture, not shown here but based on identical principles, uses a closed loop but a tight-coupling, for which the raw measurements of the receiver (Pseudo-range and pseudo-range rate) are compared to a prediction of these same values ​​from the inertial solution.

[0024] These algorithms can use several different definitions for angles, positions and velocities, depending on the chosen frame of reference (ECI, ECEF, ENU etc.), so the estimated angles are not necessarily the roll, pitch and yaw angles.

[0025] 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, an odometer, a barometer (altitude), etc.

[0026] The common feature 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 co associated variances are extrapolated up to the current time;

[0028] - an update step (M-EKF) where the new measures 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 on [Fig.2]).

[0030] The relative position information sensor CR operates at the high acquisition frequency of the inertial measurement unit (fb, generally between 50 Hz and 2000 Hz, typically around 200 Hz for some inertial navigation systems). Processing is therefore initiated by the generation of a new measurement performed by the inertial measurement unit (accelerometer and gyroscope measurement) at time f(, which is then processed to update the angles / position / velocity of the APV solution.

[0031] At a lower frequency, generally between 0.1 Hz and 20 Hz and typically around 10 Hz, the Kalman filter processing is initiated by the provision of a new measurement / information, outside the inertial measurement unit. In its simplest configuration comprising only an inertial measurement 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 Wideband distance measurement, an odometer measurement, an altimetric measurement, the detection of a static phase (which is not a "measurement" per se, but falls into the category of "information"), the consideration of speed constraints, or even a radar measurement.

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

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

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

[0035] The approach proposed by [3] applies a very similar method for which the The data are reprocessed in both directions multiple times and then combined to obtain a smooth trajectory. However, this method requires having and reprocessing all the raw measurements, as previously mentioned.

[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 satisfies a regularity criterion. This approach therefore processes a reduced number of points but does not perform a true correction of the positions. Indeed, only outliers are processed (see the white and black points in [Fig. 3] of [4]). However, the drift of the positions resulting from the INS measurements relative to the actual trajectory is gradual; therefore, there are no outliers on the trajectory calculated from the INS measurements. Furthermore, 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 the trajectory of a moving carrier, which calculates a real-time and regular fused trajectory and a precise and regular delayed-time trajectory, while limiting the information flow and the storage capacity required for the delayed-time trajectory. Summary of the invention

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

[0039] SI) at each instant 4 determination of a fused trajectory of the moving carrier, the fused trajectory being obtained by fusing data provided by at least one sensor with relative position information with data provided by at least one sensor with absolute position information, and, at a plurality of instants determined 4 among the instants k, generation of a fused data packet comprising data fused at the instant determined 4 and at least one confidence indicator associated with the data packet of the position fused at the instant determined 4, the confidence indicator comprising information on the accuracy of the data provided by the sensor with absolute position information;

[0040] S2) at each determined instant 4, determination of a regularized trajectory in function of detecting a position jump in the fused data packet between the previous selected instant 4-1 and the selected instant 4-

[0041] Advantageously, the method further comprises a step S3) of segmenting the regularized trajectory into so-called high-confidence segments and so-called non-high-confidence segments, the segmentation being carried out according to the confidence indicator associated with the fused data packet for each determined instant 4, and of determining a post-processed trajectory, the post-processed trajectory cor-

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050]

[0051]

[0052]

[0053]

[0054]

[0055]

[0056] responding 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 endpoints and the endpoint of the adjacent high-confidence segment. Advantageously, the relative position information sensor includes an inertial measurement unit. Advantageously, the absolute position information sensor includes 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. Advantageously, the first SI step includes the calculation of a correction indicator that 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 instants and 4. Advantageously, the correction indicator is determined by the following relationship: Icorr = ^.=kl || xt || xi corresponding to a trajectory correction at a time ti. Advantageously, the detection of a position jump includes comparing the correction indicator with a predetermined threshold. Advantageously, the regularized trajectory is determined by applying a compensation to the position on the merged trajectory (TrA) at the determined instant 4, the compensation being determined iteratively by the following relation: A = A + OK A k corresponding to the compensation at the determined instant 4, A corresponding to the compensation at the previous determined instant 4-1, &k corresponding to a correction at the determined instant 4 determined by the following relation: if no jump detected if a jump is detected pf™ corresponding to the position on the merged trajectory (TrA) at the determined instant tk; pf1^ corresponding to the position on the merged trajectory (TrA) at the previous determined instant 4-1- 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 rotation.

[0057]

[0058]

[0059]

[0060]

[0061]

[0062]

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069]

[0070]

[0071]

[0072] Advantageously, the geometric transformation further includes a homothety when the non-high confidence segment is preceded or followed by a high confidence segment. Advantageously, the geometric transformation (g J applied to a position P on The non-high confidence segment (g^FC) is defined by the relation: g„,(p) =H„r4p-p£;)+P£'t corresponding to the first point belonging to the non-high confidence segment, which coincides with the previous point pfus which belongs to the high confidence segment neighbor ; p'eg corresponding to the first point of the segment on the regularized trajectory TrB; Km Hm, corresponding to the homothety coefficient: H llpg-Ç.11 ■ IIpCÆJI pæs corresponds to the last point of the segment on the regularized trajectory TrB; pfus corresponds to the last point of the high-confidence segment preceding the non-high-confidence segment, pjys corresponds to the first point of the segment at high confidence following the segment to non-high confidence; With Rm defined by: 'costp sintp 0' -sincp costp 0 . 0 0 1. The angle of rotation $ is calculated using the following relationships: u-[0 0 1] T x= (pæ g -pf g ) v = ( pf us - p( ua ) z-x0y c = sign (z T u). || z || tp = atan(c, x T y) With (g) the cross product and x1 the transpose operator. Advantageously, step S3 further includes a weighted recombination of the merged trajectory and the post-processed trajectory. 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 the trajectory of a moving carrier, comprising:

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

[0075] - a fixed device, configured to determine, at each determined instant 4, a tra regularized jectory based on the detection of a position jump of the moving carrier in the fused data packet between the previous determined instant 4-1 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 carried out according to 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 endpoints and the endpoint 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 figures

[0078] Other features, details and advantages of the invention will become apparent from the description made with reference to the accompanying drawings given by way of example.

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

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

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

[0082] Fig. 4 schematically represents the different processing blocks for implementing the process according to the invention.

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

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

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

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

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

[0088] The main steps of the process according to the invention are described in relation to figures 3 and 4.

[0089] First step IF

[0090] The method includes a first step SI), which comprises determining a fused trajectory TrA of the moving carrier at each instant tj. The fused 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 considered, provided that data fusion between the sensors can be carried out, that the absolute position information sensor CA can provide navigation information with 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 unable to provide location information.

[0093] For example, the absolute position information sensor CA can be a GNSS signal receiver, a UWB, WiFi, or 4G / 5G radio wave transmitter / receiver terminal, 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, and is therefore likely to provide a position error that increases over time.

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

[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] Figure 5 illustrates in more detail the sub-steps of the first step of the process 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 the person skilled in the art.

[0099] The second substep S12 consists of receiving measurements from a relative position information sensor CR such as an inertial measurement unit (three acceleration values ​​provided by an accelerometer, three rotation speed values ​​provided by a gyroscope), at each measurement instant tj.

[0100] For each measurement instant tj the algorithm calculates in real time an inertial solution APV(i) according to techniques known to the person 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 (substep S14).

[0102] If so, a corrected inertial solution APV(i) is calculated according to techniques known to those skilled in the art (substep S15). The merged trajectory TrA of the moving carrier is thus determined at each measurement instant tj on the basis of the corrected inertial solution APV(i).

[0103] Substep S16 consists of determining whether the corrected inertial solution APV(i) calculated at the measurement time tj belongs to the set of times determined 4 among the times 4. This substep 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 measurement instants tj, using a decimation counter which can be loaded with 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 ti. When the counter reaches the value 0, it is loaded and substep S17 is executed.

[0105] Substep S17 consists of generating a fused data packet frf™ comprising fused data at the determined time 4 and at least one confidence indicator cp / s associated with the data packet of the fused position at time determined 4.

[0106] The data packet contains, for example: time 4, position p^us, velocity v(s), angles a^'s, one or more confidence indicators Iconf, and a correction indicator Icorr. Some variables are vectorial and can therefore include several values. Since the invention aims to compensate for positional discrepancies, it is essential that the data packet include the position p^us. Alternatively, all APV values ​​can be transmitted. The confidence indicator c's includes accuracy information for the data provided by the absolute position information sensor CA.

[0107] According to an embodiment in which the absolute position information sensor includes a GNSS receiver supporting RTK mode, the confidence indicator 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 centimeter-level accuracy but requires receiving correction data. This data is generally calculated and transmitted from a nearby fixed base station (usually 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-point solution. In a fixed solution, the number of wavelengths is an integer, thus providing centimeter-level accuracy. In a floating-point solution ("RTK float"), 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 non-RTK ("no RTK," also called "stand-alone") operation, the accuracy is on the order of meters.

[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 instants 4-1 and 4-

[0112] In particular, the correction indicator is equal to the sum of the magnitudes of the trajectory corrections made at each instant between two consecutive selected instants 4-1 and 4, according to the formula: [°n3] Icorr = || ||

[0114] xi being here a component of the state vector defined previously, which can avan potentially be the position correction component along 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 cover certain components of the correction xî, such as the speed components, a distance between two successive positions, a statistic (mean deviation, variance etc.) relating to the distance between successive positions established for all the positions calculated between two transmitted packets, or the detection of particular phenomena (detection of odometer slippage, loss of GNSS signal, sensor saturation, data loss, etc.).

[0116] It may be noted that a variant of the decimation of the measurement instant count ti (substep S16) consists of calculating the time elapsed since the last execution of substep S17: if this time exceeds a predetermined duration, substep S17 is executed. According to another variant, substep S17 can be executed for each inertial position correction (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 a high frequency but with jumps in the trajectory due to recalibration.

[0118] Second step S2

[0119] The method includes a second step S2 of determining a regularized trajectory TrB as a function of the detection of a position jump in the fused data packet pfCentre the previous selected instant 4-1 and the selected instant 4.

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

[0121] In a first substep S21, a variable A k, which contains the accumulation of all regularization corrections, is initialized to 0 0 0]T'

[0122] The second substep S22 consists of receiving the fused data packet n / “çcomprising fused data at the determined time 4, at least one confidence indicator c£us associated with the data packet of the fused position at the selected time 4, and optionally a correction indicator.

[0123] The detection of a position jump advantageously includes comparing the correction indicator contained in the data packet with a predetermined threshold T jump (substep S23):

[0124] Icorr > Tsaut

[0125] For example, the predetermined threshold Tsaut can be between 0.01 and 10, this

[0126]

[0127]

[0128]

[0129]

[0130]

[0131]

[0132]

[0133]

[0134]

[0135]

[0136]

[0137]

[0138] value that can be determined according to the desired precision. In an advantageous embodiment, the predetermined threshold Tsaut is determined based on the quality of the relative position sensor. Indeed, with a very high-quality inertial measurement unit (IMU), it may be useful to set a relatively low threshold Tsaut, which implies a low tolerance for IMU drift. Conversely, with a lower-quality IMU, it may be useful to set a relatively high threshold Tsaut, which implies a high tolerance for IMU drift. Alternatively, the condition for detecting a position jump can relate to the successive violation of thresholds, a violation of several thresholds simultaneously (for example, a threshold linked to the sum of the position correction norms and a threshold linked to the sum of the velocity correction norms, both thresholds having to be exceeded) or undifferentiated (exceeding one of the two thresholds). The regularized trajectory TrB can be determined by applying the compensation A k to the position p^us on the merged trajectory TrA at the selected time tk, the compensation being determined iteratively by the following relation: A k = A k_I + ôk A k corresponding to the compensation at the determined instant tk, A corresponding to the compensation at the determined instant l]ti, ôk corresponding to a correction at the selected instant tk determined by the following relation: 0 if no jump detected ô = k pf us -pf u ? if a jump is detected 1 k ^ki p£us corresponding to the position on the merged trajectory TrA at the determined instant ; p^ corresponding to the position on the merged trajectory TrA at the determined instant W Thus, in Figure 6, substep S24 consists of updating the 8k compensation if a jump was detected in substep S23. In this case, ôk = Pkus_ Pkujs- Thus, Ak = A^ + p^-pf^ Conversely, if no position jump has been detected, the compensation does not change, and k = A kj Thus, in a fifth substep S25, the regularized position of the regularized trajectory TrB is calculated by applying a compensation A k to the position pt™, by k the following relationship: preg = pfUs + Finally, the position pPls of the merged trajectory TrA, the position p'^ of the tra regularized jectory TrB, the confidence indicator correction, as well as all other data (4, a{l's) are saved for post-processing (substep S26).

[0139] Applying the correction ôk has the effect of completely canceling the displacement between the selected times 4-1 and 4, with the consequence that the position remains identical between these two times. Alternatively, the complete cancellation can be followed by a substitution with a probable displacement, calculated, for example, based on the previous displacement between the selected times 4-2 and 4-1, or any other prior 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 jumps of position having been compensated).

[0142] The combination of the first two steps SI and S2 makes it possible to calculate a real-time trajectory which is, without jumps, i.e. without 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 absolute position information sensors and relative position information sensors (GNSS / INS fusion for example).

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

[0144] Third stage S3

[0145] The method according to the invention includes an optional third 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] To this end, at each determined instant 4, the regularized trajectory TrB is divided into high-confidence segments and non-high-confidence segments Par "Segment" refers to a portion of the trajectory. The segmentation is performed based on the confidence indicator c-^ associated with the merged data package. for each selected instant 4. The post-processed trajectory TrC corresponds to the merged trajectory TrA for each high-confidence segment sæ, i.e. the APV solution of the merged trajectory TrA. Furthermore, for each non-high confidence segment s^FC, the post-processed trajectory TrC is determined by a geometric transformation of the non-high confidence segment ^FC such that a continuity condition is satisfied between each of its endpoints and the endpoint of the adjacent high-confidence segment.

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

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

[0149] By way of example, using state-of-the-art techniques for calculating the trajectory in delayed time would require storing the raw sensor data and transmitting it from the mobile device where it is acquired to the computing device (a computer) where it is processed. In this example, a radio link offering a useful data rate of between 38.4 kb / s (three accelerometer data points, three gyroscope data points, encoded in four bytes, with an acquisition frequency of 200 Hz) or 200 kb / s (with more sensors) would be necessary. Furthermore, data loss during transmission would have a significant impact on trajectory reconstruction because it would lead to errors in the calculation of the inertial solution.

[0150] The method according to the invention would allow the use of 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] Figure [Fig. 7] illustrates some embodiments of the third step S3.

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

[0153] In a second substep S32, the regularized trajectory TrB is divided into high-confidence segments Sæ' and non-high-confidence segments S^C-

[0154] According to an embodiment in which the absolute position information sensor CA comprises a GNSS receiver supporting RTK mode, packets for which the confidence indicator c^' corresponds to the "RTK fix" value (with or without a tolerance margin) are classified as high-confidence segments sæ, and packets whose confidence indicator is too far from the "RTK fix" value (i.e., confidence indicator c-^ different from the "RTK fix" value with or without the tolerance margin) are classified as non-high-confidence segments S^iC-

[0155] Alternatively, segmentation can be performed without knowledge of the RTK value. In particular, the confidence indicator can be compared to a threshold. Additional test criteria can be added, such as comparing the confidence indicator c(us) with several thresholds, or requiring a sufficient number of consecutive points to meet a condition to be classified as a segment. high confidence sFC-

[0156] In a third substep S33, the corrected positions are calculated for all points of each segment with non-high confidence 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 tk) thus forming M trajectory segments S^f C represented by their indices: S^C = {km, (k+l)m, (k + l)J

[0158] The first point of the segment has the index km and the last point of the segment has the index ( k 4-1 )m.

[0159] For example, in Figure 8, the continuity condition between each of the ends of the non-high confidence segment and the end of the adjacent high confidence segment implies that the position p'cg must coincide with the position p£us, and the position p'eg must coincide with the position pfus. km+l+f

[0160] All points on the regular trajectory of the same segment are subject to the same geometric transformation, to calculate corrected positions pæir: pcorr _ g / pæg lk E NFC

[0161] The geometric transformation gm of the segment is calculated such that the segment S^C coincides at its two endpoints Épeg pieg j with the points of the \ km' ^m+l / High-confidence neighboring segments rnfus fus Precisely, and as illustrated by the U kra- Pkm+1 J Figure 8, therefore, the first point p'cs belonging to the SNTC segment must coincide with the previous point pFus of the preceding high-confidence segment, and the last point pfeg belonging to the SNFC segment must coincide with the following point p[us of the next high-confidence segment.

[0162] Thus, the continuity condition can be written

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

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

[0165] gmM =H^Rm(p~PZ)+PZr

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

[0167] p^arr = pl us kftîA

[0168] With Hm\c homothety coefficient

[0169] _ ll^^ll m WpA-- It

[0170] Rm is a rotation matrix calculated such that the equation p^''1', = pZ be verified.

[0171] costp simple 0' R m = -sintp costp 0 .0 0 1.

[0172] The angle of rotation 9 is calculated as follows: [° 173 ] u-[0 0 1] T y = (p(us - p[us]) z = x0y c = sign(zTu). || z || tp - atan(c, xTy)

[0174] With ® the cross product and xT 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 velocity and angles resulting from the fusion.

[0176] The fourth substep S34 then consists of reconstructing the post-processed trajectory TrC (consisting of the p£p positions) from the merged positions and the positions corrected.

[0177] Thus, for each high-confidence segment sFC: ppp = pfus mk * k

[0178] For each non-high confidence segment • PZ ~ P^n

[0179] According to one embodiment, substep S34 further comprises a recommendation weighted combination of the fused trajectory TrA and the post-processed trajectory TrC.

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

[0181] pPP _ pfus + A _ (JApcon- 1 k * k \ PK

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

[0183] The weighted combination makes it possible to limit the effect caused by close proximity between the starting and ending points of the post-processed trajectory. Indeed, when the trajectory forms a "loop," even a tiny error in the position of the starting / ending points is amplified by the geometric transformation, and the points in the middle of the loop can be significantly inaccurate. Weighted positioning helps avoid such errors.

[0184] The invention also relates to a system capable of implementing the predefined method. The system, illustrated in [Fig. 9], advantageously comprises a mobile device DM to be located and configured to calculate the merged trajectory TrA in real time. For this purpose, it includes an absolute position information sensor CA, a relative position information sensor CR, and a CPU computing unit 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 delayed time. A backup means MS, disposed in the fixed device DF, allows the data packets to be stored before post-processing.

[0185] Bibliographical 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 1 / , 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 tk among the instants 1 / , 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 f / t, the confidence indicator comprising an in- k / precision information of the data supplied 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;

2. Method according to claim 1, further comprising a step S3) of segmenting the regularized trajectory into so-called high-confidence segments and into so-called non-high-confidence segments, the transformation being carried out as a function of the confidence indicator (J™') ^k associated with the merged data packet for each determined instant tk, and of determining 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 P31, a geometric transformation of the non-high-confidence segment tc"c such that a continuity condition is satisfied between each of its ends lpreg p^gj and the end (pfus ptus ) of the adjacent high-confidence segment.

3. 3. Method according to one of the preceding claims, in which the relative position information sensor comprises an inertial unit.

4. 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 q / us) being provided by the RTK value of the GNSS receiver, in particular the “RTK fix” value.

5. 5. 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 instants 4-1 and 4

6. 6. Method according to claim 5, in which the correction indicator is determined by the relation: jcorr — || r. || , x' corresponding to a trajectory correction at an instant h .

7. 7. Method according to one of claims 5 or 6, in which the detection of a position jump comprises the comparison of the correction indicator with a predetermined threshold {Tjump).

8. 8. Method according to one of the preceding claims, in which the regularized trajectory (TrB) is determined by applying a compensation ( A to the position ^pfus^ on the merged trajectory (TrA) at the determined instant 4, the compensation being determined iteratively by the following relation: A k = A kA + ôk, in which A k corresponds to the compensation at the determined instant A corresponds to the compensation at the previous determined instant ôk corresponds to a correction at the determined instant tk determined by the following relation: 0 if no jump detected, p{us k Plm - PlT if a jump is detected corresponding to the position on the merged trajectory (TrA) at the determined instant 4; p^ corresponding to the position on the merged trajectory (TrA) at the previous determined instant 4-1-

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

10. 10. Method according to one of claims 2 or 3 to 9 in their dependence on claim 2, in which the geometric transformation comprises at least one translation or one rotation.

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

12. 12. The method of claim 11, wherein the geometric transformation (gj applied to a position P on the non-high confidence segment ($^c) is defined by the relation: g (p) = HmR ( p- p[e- ) + p“m' ^"corresponding to the first ÎT1 ïïl y point belonging to the non-high confidence segment, which coincides with the previous point nf ™ which belongs to the neighboring high confidence segment; p[eg corresponding to the first point of the segment on the regularized trajectory TrB; Hmcorresponding to the homothety coefficient: llPk^PvJI; p]eg 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, pfus corresponding to the first point of the high confidence segment following the non-high confidence segment; With Rm defined by: ' COStp sintp 0' ; The angle of rotation being calculated by the Rm= -sintp costp 0 .0 0 1. relations: u = [ 0 0 1 ]T ' ^vec ® the vector product and xT x=(pæg-p^ ) y=(pkus-pkis ) z = x0y c = sign (zTu). || z || tp = atan(c, xTy) the transposed operator.

13. 13. Method according to one of claims 2 or 3 to 12 in their dependence on claim 2, in which step S3 further comprises a weighted recombination of the merged trajectory (TrA) and of the post-processed trajectory (TrC).

14. 14. Method according to one of claims 2 or 3 to 13 in their dependence with claim 2, in which steps S1 and S2 are executed in real time during the movement of the mobile carrier, and step S3 is executed in deferred time.

15. 5System for determining a trajectory of a mobile carrier, comprising: - a mobile device (DM), on board the mobile carrier, comprising at least one relative position information sensor and at least one absolute position information sensor, the mobile device being configured for, at each instant 1;, determining 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 times h among the times tj, generating a merged data packet comprising merged data at the determined time and at least one confidence indicator associated with the merged position data packet at the determined time, the confidence indicator comprising precision information of the data provided by the absolute position information sensor; - a fixed device (DF), configured to determine, at each determined time 4, 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 time and the determined time;

16. The system of claim 15, wherein the fixed device (DF) is further configured to segment the corrected trajectory into so-called high-confidence segments and into so-called non-high-confidence segments, the segmentation being performed as a function of the confidence indicator (Jus\ 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 (S^0)' P31 a geometric transformation of the non-high confidence segment such that a continuity condition is satisfied between each of its endpoints Creg nreg j and the endpoint IPb Pi ) / fus nfus I of the adjacent high-confidence segment. kn>i, km+]+y

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

Images