"Method For Simultaneously Locating And Mapping Via Resilient Non Linear Filtering"
Abstract:
The method according to the invention consists in carrying out, by means of an underwater vehicle moving above a given zone of the sea bed, observations on the basis of which the positions of elements of relief and of objects present on the bed are estimated. These observations are associated with fictitious objects, tied together by fictitious elastic links so as to constitute a fictitious elastic network. The network is in equilibrium as long as no action is exerted on one or the other of the fictitious objects constituting the network. The objects then occupy fixed positions, positions assigned to the corresponding observations. On the other hand, when two observations assumed to correspond to one and the same object are fused into a single observation, the two corresponding fictitious objects are brought closer together. This bringing closer together induces the appearance of stresses in the fictitious elastic network. These stresses, transmitted by the fictitious elastic links, are resolved by the modification of the positions of the fictitious objects of the network, the modified positions being assigned to the corresponding observations. In this way, when the estimated position of an object having formed the subject of two fused observations is corrected, the measurement of the estimated position of each of the objects that has given rise to an observation represented in the fictitious elastic network benefits advantageously from this correction.
Get Free WhatsApp Updates!
Notices, Deadlines & Correspondence
45 RUE DE VILLIERS, 92200 NEUILLY-SUR-SEINE, FRANCE
Inventors
1. JULIEN FERRAND
50 ALLEE DES PERVENCHES, 29280 PLOUZANE, FRANCE
Specification
Method for simultaneously locating and mapping via resilient non-linear filtering.
The invention is concerned with the detection and location of objects and the mapping of zones, by means of a detection device. It is more particularly concerned with the domain of detection sonars and the mapping of seabeds.
Carrying out the mapping of a zone of terrain consists mainly in pinpointing the geographical position of the prominent elements located over the zone considered, relief elements or fixed objects in particular. This mapping is generally carried out by means of detection systems mounted on an appropriate vehicle, the vehicle traversing the ground of the zone considered, or overflying this zone, as the case may be. In practice, the detection system determines the relative position of the prominent elements with respect to the vehicle and the absolute position of each element is determined by associating the measured relative position and the position, assumed known, of the vehicle at the instant of detection and which is for example determined by the navigation system of the vehicle.
Thus for example the mapping of an emerged zone of terrain may be carried out using a radar system having sufficient resolution, this radar being mounted on an aircraft, a remotely controlled aircraft or else an automatic system of Drone type. Hence the aircraft is deployed above the zone considered and performs the determination of the position of each prominent element by means of the measurements performed by the radar, which give the relative position of the element considered with respect to the aircraft and information about the geographical position of the aircraft, a position generally determined with the aid of a system of GPS receiver type located aboard the aircraft. Hence the position of the aircraft being known with precision, the position of the element considered may be determined with great precision, as long as the measurements provided by the radar have the desired precision.
However, there exist circumstances where the determination of the position of the prominent elements cannot be carried out in this way with sufficient precision.
2
Such is notably the case if the determination of the relative position of the prominent elements with respect to the vehicle tasked with carrying out the mapping is not carried out with the desired precision; because the measurements carried out by the detection system do not have the desired precision for example.
Such is also the case if the geographical position of the vehicle is known with insufficient precision. Such is for example the case if the vehicle is an aircraft which does not have any GPS system. Such is also the case if, for example, the zone to be mapped is a submerged, underwater zone. The mapping is then carried out with the aid of a sonar system carried by an autonomous or non-autonomous undenwater vehicle, which cannot possibly determine its position with the aid of the GPS system, GPS information being, in a known manner, inaccessible to a vehicle being deployed under water.
In these last two cases, the determination of the position of the vehicle at each instant of its displacement is carried out by implementing conventional means, inertial means for example, which, commencing from a starting position, assumed to be known with precision, determine the relative displacement of the vehicle with respect to this origin.
This so-called dead reckoning navigation technique makes it possible to estimate the position of the vehicle at any instant. The measurements carried out with the aid of such means are then generally less precise. Moreover a drift is noted in the course of time of the determination of the absolute position of the vehicle with respect to its real position. Ultimately the absolute position of the prominent elements is estimated with lesser precision, a precision which is sometimes even insufficient.
As regards the mapping of an undenA/ater zone, the latter is generally carried out by means of an undenwater vehicle, an undenwater drone for example, equipped with a lateral sonar and being deployed above the zone to be mapped, in proximity to the bed. The position measurements are generally carried out by grazing insonification of the seabed.
This type of insonification advantageously makes it possible to chart a prominent object not by the echo that it reflects but by the "acoustic" shadow that it casts on the bed. Detection based on acoustic shadow is particularly
3
advantageous for charting certain objects, certain stealthy undenwater mines in particular, which hardly reflect, if at all, the sound wave emitted by the sonar but which nonetheless act as a screen and therefore produce an acoustic shadow. It is recalled here that the acoustic shadow cast by an object can be defined here by the bed zone for which the object considered constitutes a screen preventing its insonification.
Hence by implementing any known appropriate processing, it is possible to determine the contours of the acoustic shadow cast by a prominent object, which contours make it possible to determine the position and the profile of the object itself and to carry out a classification of the located objects, in an easier manner than on the basis of the echoes reflected by the objects themselves, notably if this classification is carried out in an automatic manner.
However the determination of the exact position of an object, on the basis of the acoustic shadow that it produces, is sometimes difficult and in any event approximate. It depends in particular on the angle of insonification and the direction in which the object is insonified. Hence, even if the absolute position of the vehicle is known at any instant with precision, the relative position of the object with respect to the vehicle, and consequently its absolute position, can only be determined, estimated, with a bias due to the shift between the position of the acoustic shadow and that of the real object.
Furthermore, the zone to be mapped being insonified from various directions, on account of the displacements of the vehicle above this zone, it sometimes happens that one and the same prominent object is insonified several times in different directions. It then produces distinct acoustic shadows which give rise to the determination, for one and the same object, of several detections having different estimated positions, and which leads to several observations being identified for a single real object.
Hence it is then necessary to refine the mapping carried out by implementing appropriate means for associating the observations so as to determine whether two localized characteristic elements do or do not constitute one and the same element considered from different angles.
In the zones of low or mean density, the known solutions successfully carry out the pairing of the observations relating to one and the same object. Hence, the position of an object having formed the subject of multiple
4
locations, these locations having been recognized as relating to this same object, can then be re-estimated with greater precision by techniques for calculating weighted averages.
On the other hand, no particular benefit is derived from this readjustment operation as regards the other prominent objects which have formed the subject of only a single detection. The resulting mapping therefore makes it possible only to fuse the observations representing one and the same object, and thus to improve the precision of the estimation of the position of this object. It does not make it possible to improve the global precision of the location of the other objects detected in the mapped zone.
Moreover, these known solutions are not concerned with the problem posed by the positioning errors pursuant to the vehicle's absolute positioning error, which error is due mainly to the precision and to the drift of the navigation system which estimates the absolute position of the vehicle.
An aim of the invention is notably to propose a means making it possible to improve the precision of the mapping of an underwater zone, in particular when the latter is carried out on the basis of an undenwater vehicle moving over the zone considered and insonifying the zone by means of a lateral sonar and when the detection and the determination of the position of the prominent objects of the zone are carried out by detecting the acoustic shadows or the echoes produced by the latter.
Another aim of the invention is to limit the influence of the navigation system position errors, on the measurement of the position of the observed objects and on the determination of the absolute position of the vehicle.
For this purpose the subject of the invention is a method for estimation and dynamic correction of the absolute position of objects observed on the seabed by a sonar detection system carried by a vehicle being deployed above the bed, an object being detected by the observation of its acoustic shadow, this observation giving rise to the creation of a fictitious object, or contact, whose estimated position is initially that associated with the observation which gave rise to it and which represents the detected object. The method according to the invention comprises two phases.
5
A first phase consists in constructing a fictitious mesh on the basis of the observations performed in the course of time, the mesh being constructed by representing each observation performed by a node whose position is that of the observation and by connecting the nodes together by way of fictitious elastic links. Each new observation is associated with a node which is connected to a fictitious anchoring point, situated on the seabed, by a first fictitious elastic link. This node is integrated into the existing mesh by way of a set of fictitious elastic links, each of these links connecting the node to existing nodes of the mesh which correspond to observations preceding the observation considered in time. A node associated with a new observation is moreover tied by another fictitious elastic link to the node associated with the contact corresponding to this new observation, a node whose position is that of the observation site. The elastic links thus established have initial lengths such that having regard to the positions of the various nodes they do not exert any tension on the nodes to which they are connected.
A second phase consists, when two observations are considered to relate to one and the same object, in fusing these observations by associating them with a single contact, whose position is dependent on the two fused observations. The fusion is manifested at the level of the mesh by replacing the nodes associated with the contacts corresponding initially to the observations by a single node associated with the new contact formed. This node is tied to the nodes associated with the corresponding observations by way of the fictitious links initially connecting the node associated with each observation to the node associated with its contact. The introduction of this common node into the mesh induces on the nodes associated with the fused observations tensions which are transmitted to the other nodes of the mesh by the diverse fictitious elastic links and which are resolved by a modification of the positions of the nodes constituting the mesh. The modified position of each node is assigned to the observation or to the contact associated with the node considered.
According to the invention, the first phase is implemented throughout the duration of the seabed analysis operation, while the second phase is implemented each time that the fusion of two observations is decided.
6
According to a form of implementation of the method according to the invention, when particular observation is considered to correspond to an object whose real position is known, a variant of the second step is carried out consisting in modifying the position of the node associated with the contact corresponding to this observation in such a way that it definitively occupies the position of this object; the association of an observation and of a known object having the same effects on the mesh and therefore, on the positions of the other observations and contacts, as the fusing of two observations.
According to a particular form of implementation, the method according to the invention comprises a complementary phase, carried out after the second phase, consisting in recalculating the estimated position of the vehicle on the basis of the updated estimations of the positions of the observations.
According to a form of implementation of the method according to the invention, the fictitious elastic link tying a node i corresponding to a new observation to its ground inking point is a link of zero initial length, whose stiffness kA is defined by the relation:
KA(i) = A
Qi corresponding to the uncertainty in the measurement of the position of the observation i.
According to a form of implementation of the method according to the invention, the fictitious elastic link tying a node i corresponding to a new observation to the node of the mesh corresponding to an earlier observation j is a link whose length is determined by the positions of the nodes at the moment of the establishment of the link in such a way that the node corresponding to the new observation is positioned vertically in line with its anchoring point (42) and that the link does not exert any stress on the nodes considered, and whose stiffness kL is defined by the relations:
7
KL(U) = ^ and o-^ = ^a' + a/
o\ and Qj corresponding respectively to the uncertainties in tine measurements of tine positions of the observations i and j.
According to a form of implementation of the method according to the invention, the fictitious elastic link tying a node i corresponding to a new observation to the node corresponding to its contact is a link of zero initial length, whose stiffness kc is defined by the relation:
Kc(i,k) = 4-
o\ corresponding to the uncertainty in the measurement of the position of the observation i.
According to a form of implementation of the method according to the invention, the propagation of the stresses imposed by fusions of observations being manifested by a modification of the positions of the various nodes constituting the mesh, a vector PI of the initial positions of the various nodes is defined, whose size varies in the course of time as new observations are performed and whose components form a list of N components, the first Nobs components relating to the nodes associated with observations and the last N-Nobs components relating to the nodes associated with contacts, each component corresponding to the position occupied by a node upon its introduction into the mesh, and a vector PC of the corrected positions of the various nodes constituting the mesh at a given instant whose size varies in the course of time as new observations are performed and as fusions take place, the vector PC being defined on the basis of the vector PI by the following relation:
PC = PI + Qopt
8
In which Qopt represents the vector of the optimal nodal displacements each of whose components corresponds to the correction term to be applied to the corresponding component of the vector of the initial positions PI so as to determine the corresponding component of the vector of the corrected positions; each component of the vector Qopt being dependent on the configuration of the global mesh at the instant considered and the characteristics of the fictitious elastic links connecting the nodes together.
According to this form of implementation of the method according to the invention, the vector of the optimal nodal displacements Qopt may be defined by the following relation:
Qopt = -[M + Cr^CPI,
in which M and C represent two square matrices whose dimensions are equal to the number of nodes of the mesh at the instant considered, these matrices being previously initialized to zero;
the establishment of the matrix M being carried out gradually in two phases:
- a first phase during which the values of the elements (i, i) situated on
the diagonal of the matrix, an element of the diagonal characterizing the link
of each node associated with an observation i to its anchorage, are
calculated on the basis of the following relation:
M(i, i) = M(l, 0+ kA(i) with KACI) = Mo^
- a second phase during which the value of each element (i, j) of the
matrix, characterizing a link existing between the nodes of an observation i
and of an observation j, is calculated on the basis of the following relations:
M(i, i) = M(i, i) + kL(i, j),
MG, j) = MO, j) + kL(i, j) and
M(i, j) = M(i, j) - kL(i, j)
9
MG, i) = M(j, i) - Mi, j) with KL(i, j) = Mof and .] ' ^Observations
Pl= ■■■-' 01 ^Contacts
The vector PI thus takes the form of a string of N components, N being a number which varies in the course of time, whose first Nobs components relate to the nodes associated with observations carried out (the number of which is here Nobs), and whose last N-Nobs components relate to the nodes associated with contacts.
Q represents for its part the "nodal displacement" vector whose dimension and structure evolve like those of PI. Each of its components represents the displacement affecting the position of the node considered.
The matrix M is a square matrix whose dimension is equal to the sum of the number of observations and of the number of contacts. It describes the global mesh constructed gradually in a temporal manner, without the stresses.
From a practical point of view, the matrix M is constructed as follows:
Firstly, the matrix M is initialized as a matrix containing only zeros.
21
Secondly, for each observation Oi (each node), the value of the element M(l, I) corresponding to the anchorage of the observation is determined. Hence it is possible to write:
^i,i) = M(i,i) + KA(i)
where KACI) represents the stiffness of the link inking the node to the ground, given by the relation:
KA(i) = 4--
where o\ represents the position uncertainty for the observation Oj.
Thirdly, for each pair of observations (Oi, Oj) whose associated nodes are tied together, the stiffness KL(i, j) of the link between the nodes i and j corresponding to the two observations is calculated. K^i, j) is defined by the relation:
KL(i,J) = ^ where ay represents the relative uncertainty in position defined by:
/ 2 , 2 ay = ^O; + Oj .
0\ and Qj corresponding respectively to the uncertainties in the measurements of the positions of the nodes associated with the observations i and j.
Thereafter the elements of the matrix M are calculated with the aid of the following expressions:
M(i,i) = M(i,i) + KL(i,j) M(j,j) = M(j,j)+KL(i,j)
22
and
M(i,j) = M(U)-KL(i,j) M(j,i) = M(j,i)-KL(U)
The matrix M is thus constructed gradually: its size increases as new observations are carried out.
The matrix C is also a square matrix of the same dimension as the matrix M. It describes the constraints of the mesh, that is to say the relations between the observations and the contacts, knowing that the constraints are produced by the fusions of observations and that a fusion is manifested by the attachment of the nodes associated with the fused observations to a node associated with a common contact.
From a practical point of view, the matrix C is constructed as follows:
Firstly, all the elements of the matrix C are initialized to zero.
Secondly, for each pair formed by an observation Oi and the contact Tk, (Oi, Tk) with which it is associated, the stiffness Kc(i, k) of the link between the corresponding nodes is calculated. Kc(i, k) is defined by the relation:
K,(i,k) = -^
where cy, representing the position uncertainty for the observation Oi.
Thereafter the elements of the matrix C are calculated with the aid of the following expressions:
C(i,i) = C(i,i)+Kc(i,k)
C(i,k + Nb,t,s) = C(i,k + Nbobs)-Kc(i,k)
C(k + Nb„bs-i) = C(k + Nb,bs-i)-Kc(i,k)
23
C(k + Nbobs,k + Nbobs) = C(k + Nb,bs,k + Nb,bs) + Kc(i,k)
Like the matrix M, the matrix C is constructed gradually its size increasing as new observations are carried out.
The vector PI as well as the matrices M and C having been defined, the expression of relation [3] giving the energy E of the global mesh appears as a function of the "displacement" vector Q. Consequently, if it is sought to minimize the value of E it is appropriate to determine the optimal values of the components of Q, that is to say in particular the optimal value of the displacement undergone by each of the nodes, which allow this minimization. It may be shown in this regard that the optimal vector Q is given by the following expression:
Q„p,=-[M + C]-^C.PI
Hence the fictitious mesh is assigned a new position vector, the vector PC of the corrected estimated positions, which defines the new positions of the nodes and hence the new estimated positions of the observations and of contacts associated with these nodes. The vector PC of the corrected estimated positions is defined by the relation:
PC = PI + Qop,
The final step of the method according to the invention therefore consists in assigning to each observation a corrected position corresponding to that component of the vector PC which relates to the corresponding node.
It should be noted that like the vector PI, the vector PC contains the corrected positions TEc(k) of the contacts. Now, insofar as by definition a contact has no initial position, the value of this corrected position corresponds to the corresponding value Q(k) of the vector Q.
24
It should be noted that In the same way as the method according to the Invention makes It possible, as has been set forth, to benefit, for the set of observations performed, from the possibility of fusing certain tracks so as to improve the precision of the estimation of the positions of the set of observations, this method may be exploited to benefit from the fact that certain observations performed may relate to objects whose positions are known precisely. For this purpose the method according to the invention can comprise a, complementary, phase third phase, similar to the second phase described previously, Implemented when such an observation is signaled. In such a circumstance, the position of the node of the fictitious mesh, associated with the contact corresponding to this observation, may be modified, so that the appearance is observed of a tension between this node and the node associated with the corresponding observation which tension is resolved as in the case of a fusion of observations by a global modification of the positions of the set of nodes of the mesh. This modification is manifested in the real world by a modification of the estimated positions of the set of observations and contacts.
It should moreover be noted that, in parallel with its main object, the method according to the invention may be used to allow the vehicle to correct the error made by the onboard measurement means In the determination of the real position of the vehicle at a given Instant, an error In the estimated position of the vehicle giving rise to a systematic error in the estimation of the positions of the observations performed. For this purpose the method according to the invention can comprise a complementary phase, implemented after the second or the third phase, during which the estimation of the position of the vehicle is updated on the basis of the modified estimated positions of the observations.
CLAIMS
1. A method for estimation and dynamic correction of tlie absolute position of objects observed on tine seabed by a sonar detection system carried by a vehicle being deployed above the bed, an object being detected by the observation of its acoustic shadow, this observation giving rise to the creation of a fictitious object, or contact, whose estimated position is initially that associated with the observation which gave rise to it and which represents the detected object, characterized in that it carries out the following phases:
- a first phase (31) consisting in constructing a fictitious mesh on the basis of the observations performed in the course of time, the mesh being constructed by representing each observation performed by a node (41, 44) whose position is that of the observation and by connecting the nodes together by way of fictitious elastic links (46, 47); a new observation being associated with a node (41) which is connected to a fictitious anchoring point (42), situated on the seabed, by a first fictitious elastic link (43), and which is integrated into the existing mesh by way of a set of fictitious elastic links, each of these links (46, 47) connecting the node (41) to nodes of the mesh (44, 45) corresponding to observations preceding the observation considered in time; a node (41) associated with a new observation being moreover tied by another fictitious elastic link to the node associated with the contact (48) corresponding to this new observation, a node (not represented in the figure since it was merged initially with the node of the new observation) whose position is that of the observation site; the elastic links thus established having initial lengths such that having regard to the positions of the various nodes they do not exert any tension on the nodes to which they are connected.
- a second phase (32) consisting, when two observations are considered to relate to one and the same object, in fusing these observations by associating them with a single contact, whose position is dependent on the two fused observations, the fusion being manifested at the level of the mesh by replacing the nodes associated with the contacts corresponding initially to the observations by a node
26
(56) associated with the new contact formed, this node being tied to the nodes (51, 52) associated with the corresponding observations by way of the fictitious linl