A method for registering a number of landmarks, including a computer program and associated registration device.

Abstract

Method for recalibrating a plurality of landmarks, computer program and associated recalibration device. The present invention relates to a method for recalibrating a plurality of landmarks during the geolocation of a mobile device, comprising the following steps: - acquisition (110) of bearing and elevation measurements of at least four landmarks in a reference frame of the mobile device; - determination (120) of an angular deviation for each of at least three pairs of said landmarks; - determination (130) of an initial approximation of the position of the mobile device from said angular deviations; - determination (140) of an improved approximation of the position of the mobile device from the initial approximation and said angular deviations; - determination (150) of the distances between the mobile device and each of said landmarks; - determination (160) of a first approximation of the attitude of the mobile device from said distances.Figure for the abbreviation: Figure 2.

Description

Title of the invention: Method for registration on a plurality of landmarks, computer program product and associated registration device

[0001] The present invention relates to a method of registration on a plurality of landmarks.

[0002] The present invention also relates to a computer program product and a recalibration device associated with this recalibration method.

[0003] More particularly, the invention is applicable during the geolocation of a mobile device, in particular a mobile machine such as a vehicle or an aircraft, using landmarks.

[0004] By “landmark” is meant any natural or artificial object that can be located by the mobile device and whose position is known or determinable.

[0005] According to survey methods already known in the state of the art, it is possible to geolocate a mobile device or to recalibrate its position determined by another means, using a plurality of landmarks.

[0006] To do this, the mobile device is generally capable of measuring directions (sites or bearings) relative to the landmarks and / or distances to them. Thus, by analyzing these measurements and the known positions of the landmarks, it is generally possible to determine or specify the position of the mobile device.

[0007] However, these survey methods, when used without an additional device such as an inertial unit, generally only make it possible to solve the problem of geolocation of the mobile device in two-dimensional cases.

[0008] On the other hand, there is no known method of three-dimensional geolocation which allows, from measurements of angular surveys alone (bearing and site), to carry out geolocation in three-dimensional position and in three-dimensional attitude (heading, roll, pitch).

[0009] The aim of the present invention is to propose a solution for geolocating a mobile device in three-dimensional space using only angular survey measurements (bearing and site).

[0010] To this end, the invention relates to a method of resetting to a plurality of landmarks during the geolocation of a mobile device comprising the following steps:

[0011] - acquisition of deposit and site measurements of at least four landmarks in a mobile device reference;

[0012] - determination of an angular deviation for each of at least three pairs of said bitter;

[0013] - determining an initial approximation of the position of the mobile device at from said angular deviations;

[0014] - determining an improved approximation of the position of the mobile device at from the initial approximation and said angular deviations;

[0015] - determination of the distances between the mobile device and each of said landmarks;

[0016] - determination of a first approximation of the attitude of the mobile device at from the said distances.

[0017] According to other advantageous aspects of the invention, the method comprises the following characteristics, taken in isolation or in all technically possible combinations: - the angular distance of each pair of landmarks is determined according to the respective orientation of these landmarks relative to the mobile device; - the initial approximation of the position of the mobile device is determined by a simulated annealing type algorithm; - the improved approximation of the position of the mobile device is determined by a maximum likelihood estimator, preferably using a Gauss-Newton type algorithm; - the first approximation of the attitude of the mobile device is determined by a Kabsch type algorithm by determining an optimal rotation matrix between a tangent reference frame and a reference frame of the mobile device; - the method further comprises a step of determining a second approximation of the attitude of the mobile device from the first approximation of this attitude and an approximation of the position of the mobile device, advantageously from the improved approximation of the position of the mobile device; - the second approximation of the attitude of the mobile device is determined by a Levenberg-Marquardt type algorithm; and - the method further comprises a step of determining an accuracy of the first or second approximation of the attitude of the mobile device and / or of the improved approximation of the position thereof.

[0018] The present invention also relates to a computer program product comprising software instructions which, when implemented by computer equipment, implement the method as defined above.

[0019] The present invention also relates to a device for resetting to a plurality of landmarks during the geolocation of a mobile device, comprising technical means adapted to implement the method as defined previously.

[0020] These characteristics and advantages of the invention will appear on reading the description which follows, given solely by way of non-limiting example, and made with reference to the appended drawings, in which:

[0021] - [Fig.l] [Fig.l] is a schematic view of a resetting device according to the invention; and

[0022] - [Fig.2] [Fig.2] is a flowchart of a recalibration method according to the invention, the process being implemented by the resetting device of [Fig.l].

[0023] [Fig. 1] shows a resetting device 10 according to the invention capable of determining or specifying the position of a mobile device 12 at each instant. The mobile device 12 is capable of moving, for example, according to two or three degrees of freedom, for example in a terrestrial reference frame.

[0024] According to an exemplary embodiment, the mobile device 12 is a mobile machine, such as for example a land vehicle or an aircraft, carrying the resetting device 10. In the case of an aircraft, it may be an airplane or a helicopter pilotable by an operator and / or by an avionics system from the latter, or a drone pilotable from a remote control center by an operator and / or by a suitable avionics system.

[0025] According to another exemplary embodiment, the mobile device 12 is carried by an operator or by any other user and has, for example, a mobile telephone. In this case, the resetting device 10 is, for example, integrated into the mobile device 12.

[0026] The resetting device 10 is capable of determining the position of the mobile device 12 using sighting measurements taken relative to the landmarks 14-1,..,,14-N by at least some of the sensors of a plurality of sensors 16.

[0027] Each landmark 14-1,.. ,,14-N presents a natural or artificial object arranged for example in a fixed manner on the Earth's surface according to a known position with a precision also known and identifiable from the mobile device 12. Thus, each landmark presents for example a pylon, a wind turbine, a lighthouse, a building, a mountain, a rock, etc.

[0028] Each landmark 14-1,..., 14-N is for example identifiable by an identifier specific to it. Furthermore, according to a particular embodiment of the invention, the identifier of each landmark 14-1,..,,14-N makes it possible to obtain its position using for example a position database provided for this purpose. Such a database may be publicly accessible or have restricted access.

[0029] At least some of the sensors 16 are capable of determining aiming measurements in relation to each of the landmarks 14-1,..., 14-N.

[0030] Advantageously according to the invention, each sighting measurement determined by the corresponding sensors 16 corresponds to a bearing measurement or to a site measurement, relative to a reference frame.

[0031] The reference frame is for example linked to the mobile device 12 and in the case of a mobile machine, has for example an axis coinciding with the heading of this machine. According to another example, the reference frame has a terrestrial reference frame.

[0032] As can be seen in [Fig.l], the recalibration device 10 comprises an input module 21, a calculation module 22 and an output module 23.

[0033] The input module 21 is connected to the sensors 16 and is capable of acquiring all of the measurements determined by these sensors 16. According to an exemplary embodiment, the input module 21 is further connected to at least one system, for example an on-board system, capable of providing a known position of the mobile device 12. This position may for example correspond to the last known position of the mobile device 12 or the position which needs to be specified.

[0034] The calculation module 22 is capable of analyzing the data acquired by the input module 21 in order to provide a result determining or specifying the position of the mobile device 12.

[0035] Finally, the output module 23 is capable of providing the result determined by the calculation module 22 to any interested system. For example, the output module 23 is capable of providing a display screen with the determined position and / or attitude of the mobile device 12 to display it. According to another example, the output module 23 is capable of providing this position, for example, to a precision device capable of comparing this position with that provided, for example, by a GNSS type geolocation system, in order to provide a precise position.

[0036] Each of the modules 21 to 23 is for example in the form of software implemented by a suitable processor. Alternatively, at least one of these modules 21 to 23 is at least partially in the form of a programmable logic circuit, such as an FPGA (Field Programmable Gate Array) type circuit.

[0037] The recalibration process will now be explained with reference to [Fig.2] showing a flowchart of its steps.

[0038] Initially, it is considered that the mobile device 12 moves near the landmarks 14-1, ..., 14-N and that in a fixed position, the sensors 16 carry out M bearing and elevation measurements with respect to at least M different landmarks 14-1, ..., 14-N. For example, the sensors 16 carry out a bearing and elevation measurement for each of the M landmarks visible by these sensors 16. Advantageously, according to the invention, the number M may be greater than or equal to 4 and, generally speaking, may correspond to any number less than or equal to N and greater than or equal to 4.

[0039] During an initial step 110, the input module 21 acquires all of the M bearing and site measurements, and transmits them to the calculation module 22. Subsequently, the bearing measurements are denoted by and the site measurements are denoted by s*', where * corresponds to the index associated with each of the M landmarks and therefore varies between 1 and M.

[0040] During the same step 110, the input module 21 also acquires the positions of the landmarks 14-1, ..., 14-N as well as the details associated with these positions. These data come, for example, from a database relating to the landmarks, as defined previously.

[0041] During the following step 120, the calculation module 22 determines an angular deviation for Ml pairs of landmarks 14-1, ..., 14-N.

[0042] These angular deviations are correlated but are not redundant.

[0043] Furthermore, these deviations are said to be “oriented” insofar as they are determined according to the respective orientations of the corresponding landmarks relative to the mobile device 12. To determine such an orientation for each pair of landmarks, the sign of the difference in the bearing measurements of these landmarks is used.

[0044] In particular, an angular difference &ij between a landmark having the index z and another landmark having the index j can be determined as follows:

[0045] A / . / LJ Aÿ = acosl sin^sins; + coss^coss ycosl g --g-

[0046] when Slgn( )<œ

[0047] and

[0048] = 2r - acosfsin^sins; + coss;cos5ycos(l. - l.))

[0049] when sig^.ÿj>0.

[0050] During the following step 130, the calculation module 22 determines an initial approximation of the position of the mobile device from said angular deviations Aÿ and advantageously from the known positions of the landmarks corresponding to these angular deviations A / y.

[0051] For this, the calculation module 22 applies for example a simulated annealing type algorithm.

[0052] In particular, the calculation module 22 initializes the algorithm on the barycenter of the landmarks. In other words, the vector Xo initializing the algorithm can be determined as follows: [°053] = (a1 + A2 + ... + A^lM

[0054] where Aj, ..., Am are the known positions of the M landmarks.

[0055] The calculation module 22 also makes an optimized choice of an energy factor, of a cooling factor and a random search space.

[0056] According to the simulated annealing type algorithm, the energy function f (X) is chosen as the value of the square of the residues:

[0057] a / v 4 a V2

[0058] where

[0059] a(X,A;,A;+1) is a theoretical angular difference between the landmark Aj and the landmark AM when the mobile device 12 is in position A.

[0060] The simulated annealing type algorithm is iterated with the following parameters:

[0061] - minimum energy threshold (to terminate an annealing) emin = 10 10 ;

[0062] - output energy threshold (to terminate super iterations) es = 106;

[0063] - initial temperature T0 = 10000°;

[0064] - number of steps (iterations) max = kmax = 10000;

[0065] - maximum number of attempts (restart of simulated annealing on the last solution in failure case) Kmax= 10;

[0066] - energy factor: factor a applied to the energy function;

[0067] - cooling factor: factor Æ applied to the temperature at each new landing.

[0068] The probability of keeping a new solution xn+i of energy f(xn+i) greater than f(xn) is expressed as follows:

[0069]

[0070] where Ae = f(xn+i) - f(xn).

[0071] The calculation module further defines the energy factor a and cooling Æ parameters such that:

[0072] - at the start (T=T0): Probability of increasing in energy P(T) = Pmax = 0.5;

[0073] - at the end (T=Tmin after kmax iterations of the level): P(T) = Pmin = 103;

[0074] p _ with ap î ^max — ^0 — 10¾

[0075] from where:

[0076] lOT^riP,^) ; has - -------

[0077] p . =

[0078] with A ef = emin and Ttnin = ;

[0079] from where:

[0080]

[0081] The calculation module 22 further defines the neighborhood of random drawing of a neighboring solution as a homothetic zone of the theater zone of radius R evolving between l / coef_Rmax = 1 / 10 of the initial radius Ro of the theater zone (T=T0) towards Rmin = 0.1 m (for T=Tmin)

[0082]

[0083] The core simulated annealing algorithm consists of iterating the steps by progressively lowering the temperature: Tn+^ = 0.T

[0084] Each new solution xn+i is randomly drawn around xn with a radius Rn = R(Tn) (differentiated horizontally and vertically).

[0085] It is retained (xe= xn+i) if:

[0086] - Ae <0;

[0087] - according to a probability P(Ae) = mjn (Pfnux, ) if Ae >0.

[0088] The stopping criterion of the core algorithm is f(xn) < emin and n = kmax.

[0089] The global stopping criterion is f(xe) < ee and K = Kmax.

[0090] At the end of this step, the calculation module 22 thus obtains an initial approximation of the position of the mobile device 12.

[0091] During the following step 140, the calculation module 22 determines an improved approximation of the position of the mobile device 12 from the initial approximation X and said angular deviations A / y. This improved approximation is also subsequently referred to as the first approximation of the position of the mobile device 12.

[0092] For this, the calculation module 22 uses a maximum likelihood estimator, preferably a Gauss-Newton type algorithm.

[0093] This estimator has the following expression: 100941 UM = Ui+ (H'H)'hY

[0095] where uî+i is the new solution obtained from the previous solution u' knowing

[0096] The improved approximation XCN of the position of the mobile device 12 is obtained by Gauss-Newton iteration with a stopping criterion on the difference of two consecutive solutions ||U / +j or on exceeding a maximum number of iterations.

[0097] In the expression of the estimator above, the matrix H is a Jacobian matrix (without taking into account the uncertainty of the measurements) with a relative angular deviation measure and the matrix Y is a matrix of deviations between the measured deviations and the corresponding theoretical deviations.

[0098] When M=4, the matrix Y can take the following form: [°0"] Aj.-A^aJ ' y — AQ $ - A (ïfp A?. A3) A 34 - A (u^ A3, A4)

[0100] where Am) means the theoretical angular difference between the landmark At and the landmark Am according to the ui solution at iteration 6

[0101] When M=4, the matrix H can take the following form:

[0102]

[0103]

[0104]

[0105]

[0106]

[0107]

[0108]

[0109]

[0110] [YES]

[0112]

[0113]

[0114]

[0115]

[0116] '3AI2 ' dX _ 3A _ ùA^ " - 9X - lx . SX . In other words, taking into account the directions of the deviations: = si gn (sin (g. -g.)) G^ where: ÙAi 3 ^34 SX . G 3A - SX - XÂ^XL / ' And A ( X, A^ Aj ) = acos In the following step 150, the calculation module 22 determines distances between the mobile device 12 and each M amers. These distances can be determined as follows: d(X 4) = 11¾ - Garlic- These distances make it possible to determine the spherical coordinates of the landmarks in a frame of reference of the mobile device 12, as explained below, and therefore the measured positions of the landmarks in this frame of reference. During the following step 160, the calculation module 22 determines a first approximation of the attitude of the mobile device 12 from said measured positions of the landmarks in the frame of reference of the mobile device 12 and advantageously, from the known positions Ab ..., AM of the M landmarks. To do this, the calculation module 22 determines by a Kabsch type algorithm by determining an optimal rotation matrix between a tangent reference frame and the reference frame of the mobile device 12. The tangent reference frame is for example a terrestrial reference frame of the NED type (meaning “North”, “East” and “Down” in English or “Nord”, “Est”, “Bas” in French). Subsequently, the values ​​relating to this tangent reference frame are referenced by the NED index and the values ​​relating to the reference frame of the mobile device 12 are referenced by the PTF index. In particular, the calculation module 22 first determines the barycenters GPTF and GNED of the positions of the landmarks in each of these reference points. In other words: Gptf - [XPTF + A2prF + ... +AMpT^!M

[0117] and

[0118] Gned = + A2NED + + Amne^M-

[0119] Then, the calculation module 22 determines the deviations Q and P of the position of each of the landmarks relative to the barycenter respectively in the reference frame of the mobile device 12 and the tangent reference frame:

[0120] Q- ' ^PTF~^PTF \ AMpte ' G PT F /

[0121] and

[0122] P =! A^ed~G^d \A^ned~GnedI

[0123] The optimal rotation matrix R is determined by the singular value decomposition of the matrix H -P'Q.

[0124] In particular, denoting by g, V] the singular value decomposition of the matrix H, the rotation matrix R can be determined as follows:

[0125] R = VTU.

[0126] The rotation matrix R can also be written in the following form:

[0127] cos^cosY cos^sin^ - sin# \ - sin^sin^cosW - cos^sinT cos^cosT + sin^sin^sin^ sin^cos# \ sin^sinT + cosÿsin^cosT cos^sinÆsinW - sin^cos1!7 cos^cosé* /

[0128] where 0, ÿ and are the Euler angles corresponding respectively to the pitch, roll and heading of the mobile device 12.

[0129] These angles can be determined as follows:

[0130] B = -asin(lî(l,3));

[0131] 0 = asin (M2,3) / cos (c7)); ct'E = atan2(M 1,2) / cos ( 0 ), R( 1,1) / cos«

[0132] These angles B, and therefore form a first approximation of the attitude of the mobile device 12.

[0133] Steps 170 and 180 described below are optionally implemented by the calculation module 22.

[0134] In particular, during step 170, the calculation module 22 determines second approximations of the position and attitude of the mobile device 12 by joint estimation. This estimation is unbiased and more precise than the first approximation. It also makes it possible to estimate the correlations between position coordinates and attitude coordinates. In other words, step 170 makes it possible to refine the

[0135]

[0136]

[0137]

[0138]

[0139]

[0140]

[0141]

[0142]

[0143]

[0144]

[0145]

[0146]

[0147]

[0148] first estimates of the attitude and position of the mobile device 12, obtained previously. To implement this step 170, the calculation module 22 uses, for example, a Levenberg-Marquardt type algorithm. In particular, during this step 170, the calculation module 22 implements the following iterations: ^n+1 A “b ^n ) A OR un is the state vector at iteration n which is composed of the vector X corresponding to the three-dimensional position (x, y, z) of the mobile device 12 in the tangent frame and the vector F corresponding to the three-dimensional attitude of the mobile device 12; un+i is the new solution obtained at each iteration from the previous solution un knowing that y^ with X? and Fe corresponding respectively to the position and attitude of the mobile device 12 obtained during the previous steps; Àn is an iteration parameter with T) = 0.999 and 40 = 0.1; Hn is a Jacobian matrix; Zn is a matrix of deviations of the actual sighting measurements and the corresponding theoretical measurements. The Jacobian matrix Hn in the case M=4 can take the following form: 1 dh^XY) dh^XY) 1 .....ax..... ay...... dh / XY) dh^XY) xx v * y - ^h^X?) dh^XY) -.....ax..... dY dhyXY) dh / XY) , dx ay i The matrix Z» is determined as follows: m - h2(xn, Y^ Z- / A ' m3 - h / Xft, Yn) - h^X^ with ni; = uPTF ( g.. s,- ) is the vector measurement of aiming of the landmark A' (spherical coordinates of this landmark in the reference frame of the mobile device 12).

[0149]

[0150] In the expressions of the two matrices Hn and A. yj corresponds to the theoretical aiming measurement of the landmark in the tangent frame for the state (x, y) of the mobile device 12. This expression h(x F) takes the following form:

[0151] h^X, Y) =R(Y)uXED(X,Ai)

[0152] where

[0153] RÇy) is the rotation matrix defined previously; and

[0154] uNEd(X' A) is the unit vector expressing the direction j in the reference frame NED tangent.

[0155] During the following step 180, the calculation module 22 determines an accuracy of the second approximations of the attitude and position of the mobile device 12 determined during the previous step.

[0156] This precision can for example be calculated as a Cramer-Rao bound (BCR) using the following expression:

[0157] BCR=(H ijfV1 \ * ga /

[0158] where

[0159] H g is a global Jacobian matrix (of dimensions 20x18 when M=4);

[0160] Bg is a covariance matrix of the measurements.

[0161] The global Jacobian matrix Hg is composed of a Jacobian matrix HP of the three-dimensional position measurements of landmarks and a Jacobian matrix Hr of bearings.

[0162] In other words:

[0163] [Hr\ [H Hrp\ z!2 r

[0164] where ùh^XY) ùh^XY) Ùh^XY) 1 dx ay àA. . . âh2(XY) 3h2(XY) 3h2(XY) dx ÙF • t)Aj Hr(X,Y) = dh / XY) 3h3(XY) 3h3(XY) dx dY - * • ^(XY) ùh4(XY) dh4(XX) ax 3F ■ ùriy 1

[0165] The covariance matrix of the measurements Bg is written in the following form:

[0166] _ [# 0 1 Bg~ [0

[0167] where B is a covariance matrix of the bearing measurements (of dimension 8x8 when M=4) and BP is a covariance matrix of the position measurements of the landmarks (of dimension 12x12 when M=4).

[0168] The matrix B is calculated based on the aiming measurements sî and explained previously.

[0169] It is then understood that the present invention has a certain number of advantages. First of all, the invention makes it possible to determine or specify the three-dimensional position and attitude of a mobile device using only measurements of the elevation and bearing of the landmarks, without any other attitude determination device (such as an inertial unit for example).

[0170] In particular, the proposed solution consists of breaking down the determination problem into two sub-problems:

[0171] - estimation of the three-dimensional position by a bearing method, without use of a specific attitude determination device (such as an inertial unit for example);

[0172] - attitude estimation.

[0173] Finally, thanks to the invention, it is possible to estimate the precision of the solution obtained.

Claims

Claims

1. Method for resetting to a plurality of landmarks (14-1, 14-N) when geolocating a mobile device (12), comprising the following steps: - acquisition (110) of bearing and site measurements of at least four landmarks (14-1, ..., 14-N) in a reference frame of the mobile device (12); - determination (120) of an angular deviation for each of at least three pairs of said landmarks (14-1, ..., 14-N); - determination (130) of an initial approximation of the position of the mobile device (12) from said angular deviations; - determination (140) of an improved approximation of the position of the mobile device (12) from the initial approximation and said angular deviations; - determination (150) of the distances between the mobile device (12) and each of said landmarks (14-1, ..., 14-N); - determination (160) of a first approximation of the attitude of the mobile device (12) from measured positions of said landmarks (14-1, ..., 14-N) in the reference frame of the mobile device (12) and known positions of the landmarks (14-1, ..., 14-N) in a terrestrial reference frame, said measured positions being obtained from said distances.

2. A resetting method according to claim 1, in which the angular deviation of each pair of landmarks (14-1, ..., 14-N) is determined according to the respective orientation of these landmarks (14-1, ..., 14-N) relative to the mobile device (12).

3. A resetting method according to claim 1 or 2, wherein the initial approximation of the position of the mobile device (12) is determined by a simulated annealing type algorithm.

4. A recalibration method according to any one of the preceding claims, wherein the improved approximation of the position of the mobile device (12) is determined by a maximum likelihood estimator, preferably using a Gauss-Newton type algorithm.

5. A recalibration method according to any one of the preceding claims, wherein the first approximation of the attitude of the mobile device (12) is determined by an algorithm of the type Kabsch by determining an optimal rotation matrix between a terrestrial reference frame of type NED and the reference frame of the mobile device (12).

6. A recalibration method according to any one of the preceding claims, further comprising a step (170) of determining a second approximation of the attitude of the mobile device from the first approximation of this attitude and an approximation of the position of the mobile device (12), advantageously from the improved approximation of the position of the mobile device (12).

7. A recalibration method according to claim 6, wherein the second approximation of the attitude of the mobile device (12) is determined by a Levenberg-Marquardt type algorithm.

8. A recalibration method according to any one of the preceding claims, further comprising a step (180) of determining an accuracy of the first or second approximation of the attitude of the mobile device (12) and / or of the improved approximation of the position thereof.

9. A computer program product comprising software instructions which, when implemented by computer equipment, implement the method according to any one of the preceding claims.

10. Device (10) for resetting to a plurality of landmarks (14-1, ..., 14-N) when geolocating a mobile device (12), comprising technical means (21, 22, 23) adapted to implement the method according to any one of claims 1 to 8.

Images