LOCATION OF A TARGET BY A TRACKING VEHICLE

Abstract

Some embodiments relate to a method for locating a mobile target for a tracking vehicle, including a first communication module in a first location on the tracking vehicle in order to determine a first distance measurement between the first location and the mobile target at a first time, and a second communication module at a second location on the tracking vehicle in order to determine a second distance measurement between the second location and the mobile target at a second time, and a processing module for determining a forecast of the movement of the tracking vehicle between the first and second times, and for determining a location of the mobile target relative to the tracking vehicle from the first and second distance measurements, taking into account the forecast, so as to compensate for the movement, between the first and second times.

Claims

1. A method for locating a mobile target by a tracking vehicle, comprising: determining a first measurement of distance between the mobile target and a first location on the tracking vehicle, taken at a first time; determining a second measurement of distance between the mobile target and a second location on the tracking vehicle, taken at a second time; predicting movement of the tracking vehicle between the first and second times; and determining a location of the mobile target with respect to the tracking vehicle based on the first and second measurements of distance, taking into account the prediction, in such a manner as to compensate for the movement between the first and second times.

2. The method according to claim 1, wherein the prediction and the location are determined by a Kalman filter.

3. The method according to claim 2, wherein the prediction of the movement is determined based on a linear speed measurement and on the orientation of a steering carriage of the tracking vehicle.

4. The method according to claim 3, wherein prediction equations of the Kalman filter, between a time k and a time k?1 are $X_{k/k-1}={R?\left({?}_{?}\right)}^T.Math.\left(X_{k-1/k-1}-\left(\begin{array}{c}-{?}_D.Math..Math.\sin ?\left({?}_{?}/2\right)\\ {?}_D.Math..Math.\cos ?\left({?}_{?}/2\right)\end{array}\right)\right)$
P.sub.k/k?1=R(?.sub.?).sup.T(P.sub.k?1/k?1+G.sub.kQ.sub.uG.sub.k.sup.T)R(?.sub.?)+Q.sub.xy wherein Q.sub.u is a covariance matrix associated with uncertainties in the proprioceptive information coming from the tracking vehicle, such that: $Q_u=\left(\begin{array}{cc}{?}_{v_r}^2& 0\\ 0& {?}_{{?}_r}^2\end{array}\right).Math..Math.\mathrm{and}.Math.$ $G_k=\left(\begin{array}{cc}-{?}_T.Math..Math.\sin ?\left({?}_{?}/2\right)& -0.5.Math..Math.{?}_D.Math..Math.\cos ?\left({?}_{?}/2\right)\\ {?}_T.Math..Math.\cos ?\left({?}_{?}/2\right)& -0.5.Math..Math.{?}_D.Math..Math.\sin ?\left({?}_{?}/2\right)\end{array}\right)$ with ${?}_D=v_{r,k}.Math.{?}_T,.Math.\mathrm{and}.Math..Math.{?}_{?}=v_{r,k}.Math.\frac{{?}_T.Math..Math.\tan .Math..Math.{?}_{r,k}}{L}.$ ?.sub.T is the period of time between the times k and k?1; v.sub.r,k is the linear speed of the tracking vehicle at the time k; ?.sub.r,k is the orientation of the steering carriage of the tracking vehicle at the time k, and, L is the track-width of the tracking vehicle.

5. The method according to claim 1, further comprising determining a control command aimed at adapting the speed and the steering of the tracking vehicle in order to direct it as a function of the location of the target while keeping at a setpoint distance.

6. A device for locating a mobile target for a tracking vehicle, comprising: a first communications module at a first location on the tracking vehicle for determining a first measurement of distance between the first location and the mobile target at a first time; a second communications module at a second location on the tracking vehicle for determining a second measurement of distance between the second location and the mobile target at a second time; and a processing module for determining a prediction of movement of the tracking vehicle between the first and second times, and determining a location of the mobile target with respect to the tracking vehicle based on the first and second measurements of distance, taking into account the prediction in such a manner as to compensate for the movement between the first and second times.

7. The locating device according to claim 6, wherein the processing module determines the prediction of the movement of the tracking robot and the location of the target by a Kalman filter.

8. The locating device according to claim 7, wherein the processing module determines the prediction of the movement based on proprioceptive measurements of the tracking vehicle.

9. A tracking vehicle, comprising: the device according to claim 6.

10. A system, comprising: the tracking vehicle according to claim 9; and a communications module equipping the mobile target.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0033] FIG. 1 schematically shows one example of a context in which some embodiments may be applied.

[0034] FIG. 2 shows a schematic diagram of the principle of localization by trilateration.

[0035] FIGS. 3a, 3b, 3c schematically show various mechanisms for distance measurements.

DETAILED DESCRIPTION OF EXEMPLARY EMBODIMENTS

[0036] FIG. 1 illustrates schematically the context in which some embodiments may be applied.

[0037] Some embodiments allow a mobile target A to be located by a tracking vehicle B. The mobile target A is typically a human being, who may be a pedestrian, or a vehicle, including an automated vehicle. The tracking vehicle B may be an automated vehicle, in other words a vehicle driven by an automatic mechanism.

[0038] The mobile target A is equipped with a communications module S3. It will be seen hereinbelow that it may include more than one, but a single module is, generally speaking, sufficient for the implementation of some embodiments.

[0039] The tracking vehicle B is equipped with at least two communications modules S1, S2, situated at two separate locations on the vehicle. These two modules should be at a sufficient distance from each other in order to obtain the best possible or better localization results. They may for example be close to the two lateral sides of the tracking vehicle. They may be mutually rigidly installed, in other words their relative distance does not vary. They may be rigidly attached to the chassis of the tracking vehicle.

[0040] The communications modules S1, S2, S3 are intended to determine measurements of distance d1, d2, respectively between the module S1 and the module S3 and between the module S2 and the module S3.

[0041] These communications modules may notably be based on the standardized protocol IEEE 802.15.4a. This protocol is a standard specifying that the communications modules S1, S2, S3 incorporate a physical layer capable of performing measurements of distance. This protocol has two communication formats: the IRUWB (for Impulse Radio Ultra Wide-Band) and the CSSS (for Chirp Spread Spectrum Signals) formats.

[0042] A related art mechanism for measuring a distance can be used, but some embodiments are directed to the novel exploitation of these distance measurements. The measurement of distance may be carried out in various ways, including those used in the related art.

[0043] In the framework of the IEEE 802.15.4a protocol, the methods for measuring distance are based on the principle of measurement of the time of flight (or Time of Arrival) needed for a message to go from one communications module to another.

[0044] Thus, the Time Of Arrival (TOA) method, also referred to as Time of Flight, is a very simple method which allows the distance between a mobile station and a base station to be calculated.

[0045] For the generality of the description, in the following paragraphs, the expressions mobile station and base station are used, which may, in the framework of some embodiments, represent the tracking vehicle B and the target to be tracked A, respectively. It should be noted that the target may itself be mobile and that these terms mobile station and base station are to be understood in a relative sense: the mobile station is mobile in the frame of reference of the base station (whose frame of reference may itself be mobile with respect to the Earth's frame of reference for example).

[0046] The mechanisms described are illustrated in FIGS. 3a, 3b, 3c, which schematically show the exchange protocols between two stations A, B, with a view to the measurement of distance between them.

[0047] The Time Of Arrival (TOA) method is illustrated in FIG. 3a.

[0048] The mobile station A sends a message RFRAME to the base station B at the date of transmission t.sub.e, this date being transmitted within the message. The base station receives the message and notes the date of acquisition t.sub.a.

[0049] The distance d separating the base station from the mobile station is then given by the expression

d=c.Math.(t.sub.e?t.sub.a)

in which c is the speed of light. The main drawback is that the clocks of the two stations must or should be perfectly synchronized in order to obtain a very precise measurement. This may require an infrastructure that is very difficult to implement.

[0050] In order to overcome problems of synchronization of the clocks between the base stations and the mobile stations, the Two Way Time Of Arrival method has been developed. The latter is illustrated in FIG. 3b. It includes the following dialog between the base station and the mobile station: the base station A sends a request RFRrame_req to a mobile station B and records the date of transmission to of this message, the mobile station responds to the request by sending the message RFRrame_rep to the base station, the base station receives the response message and records the date of receipt t.sub.a. the base station A calculates the period of time during this exchange by the expression T.sub.r=t.sub.e?t.sub.a, and determines the distanced separating the base station from the mobile station by d=c.Math.T.sub.r/2.

[0051] The base and mobile stations are therefore both transmitters and receivers. Since the time T.sub.r is measured by the base station A, there is no need to synchronize their clocks. Nevertheless, the response of the mobile station cannot be immediate. This is because it needs to decode the request message sent by the base station, and to create a response message and finally to send the latter. This process introduces a delay T.sub.ta.sup.B and biases the distance measurement. For example, an error of a few nanoseconds introduces errors of the order of a decimeter. This delay should be estimated very precisely.

[0052] It is possible to use a more elaborate technique in order to obtain the estimation of T.sub.ta.sup.B and hence to obtain a more precise measurement of the distance.

[0053] This time, the dialog between the base station and the mobile station is as follows: the base station A sends a request RFRrame_req to the mobile station B and records the date of transmission of the last byte of the SFD (Start Frame Delimeter) of the message RFRrame_req, the mobile station B responds to the request by sending the message RFRrame_rep to the base station and, in parallel, it launches a counter as soon as the last byte of the SFD of the message RFRrame_rep is read and stops it when the last byte of the SFD of the message RFRrame_rep is sent, the base station receives the reply message and records the date of receipt of the last byte of the SFD of the message RFRrame_rep, the mobile station sends a second message to the base station containing the value of the T.sub.ta.sup.B estimated by the counter, the base station receives the message containing the estimation of the delay T.sub.ta.sup.B, the base station sends an acknowledgement to the mobile station.

[0054] This time, it is possible to obtain a more precise measurement of distance by calculating:

[00003]$d_{\mathrm{TW}}=c.Math.\frac{T_r-T_{\mathrm{ta}}^B}{2}$

[0055] A major source of error in the TW-TOA method is the shift in clock frequency between the base station and the mobile station. The onboard clocks in these modules use quartz crystals which do not operate at exactly the same frequency. Thus, delays or advances appear in the measurement of the time of arrival which, multiplied by the speed of light, can introduce significant errors into the measurement of the distance.

[0056] The Symmetric Double Sides Two Way Time Of Arrival (SDS-TW-ToA) method allows this problem to be solved. Such a technique is illustrated in FIG. 3c.

[0057] The idea is, this time, to also estimate the time T.sub.ta.sup.B needed for the base station to decode the message RFRAMErep coming from the mobile station and to re-transmit a second message RFRAMEreq to the mobile station. Thus, the measurement of distance d.sub.SDS is as follows:

[00004]$d_{\mathrm{SDS}}=c.Math.\frac{\left(T_r^A-T_{\mathrm{ta}}^A\right)+\left(T_r^B-T_{\mathrm{ta}}^B\right)}{4}$

[0058] In order to demonstrate the benefit of this method, the errors in frequencies e.sub.A and e.sub.B of the clocks on board the base station and the mobile station are defined, such that:

[00005]$e_A=\frac{{\mathrm{Rf}}_A-{\mathrm{Nf}}_A}{{\mathrm{Nf}}_A}$$e_B=\frac{{\mathrm{Rf}}_B-{\mathrm{Nf}}_B}{{\mathrm{Nf}}_B}$

where Rf.sub.A and Nf.sub.B respectively represent the real and nominal frequencies of each of the clocks. By introducing them into the previous equations, the following is obtained:

[00006]$\{\begin{array}{cc}{\hat{d}}_{\mathrm{TW}}=& c.Math.\frac{T_r^A?\left(1+e_A\right)-T_{\mathrm{ta}}^B?\left(1+e_B\right)}{2}\\ {\hat{d}}_{\mathrm{SDS}}=& c.Math.\frac{\left(T_r^A-T_{\mathrm{ta}}^A\right).Math.\left(1+e_A\right)+\left(T_r^B-T_{\mathrm{ta}}^B\right).Math.\left(1+e_B\right)}{4}\end{array}$

[0059] In the framework of the tracking of a pedestrian, the distance measurement is a few tens of meters at the most, so T.sub.r will not exceed 100 nanoseconds. On the other hand, the time T.sub.ta needed to process the request message and to respond to it is of the order of a millisecond. This means that the transmission time is much shorter than the time for processing the data which represents the major part of the error on the distance measurement:

[00007]$\{\begin{array}{cc}{\hat{d}}_{\mathrm{TW}}=& c.Math.\frac{T_r^A?\left(1+e_A\right)-T_{\mathrm{ta}}^B?\left(1+e_B\right)}{2}\\ {\hat{d}}_{\mathrm{SDS}}=& c.Math.\frac{\left(T_r^A-T_{\mathrm{ta}}^A\right).Math.\left(1+e_A\right)+\left(T_r^B-T_{\mathrm{ta}}^B\right).Math.\left(1+e_B\right)}{4}\end{array}$

[0060] It can accordingly be seen that the error due to the shift in frequency of the clocks is compensated with the SDS-TW-TOA method if

T.sub.ta.sup.B>>(T.sub.ta.sup.A?T.sub.ta.sup.B)

[0061] According to some embodiments, the communications modules S1, S2, S3 may implement such mechanisms for measurement of distance. In practice, these mechanisms can be implemented by communications modules available in the market and that the method and the device according to some embodiments can use.

[0062] Such modules available in the market may notably be those from the company Decawave, and more particularly, the sensor DW1000. This sensor conforms to the aforementioned communications protocol IEEE 802.15.4a and operates within a frequency range from 3.5 GHz to 6.5 GHz with a bandwidth of 1 GHz. It enables the measurement of distance with a precision of 10 cm. This sensor offers, amongst others, the advantages of having a relatively low cost and of having a small size, for example 23 mm?13 mm for the model DWM1000.

[0063] FIG. 2 is a schematic diagram showing the principle of localization by trilateration.

[0064] The problem that needs to be solved is to locate the communications module S3 with coordinates (x, y) knowing the locations of the communications modules S1 and S2, with respective coordinates (x1, y1) and (x2, y2) and the distances d1, d2 measured between the communications module S3 and, respectively, the communications modules S1 and S2.

[0065] The position of the module S3 being the point of intersection between two circles centered on each of the modules S1, S2 and with radii d.sub.1 and d.sub.2, this problem may be very easily solved analytically.

[0066] By posing:

(d.sub.1).sup.2=(x?x.sub.1).sup.2+(y?y1).sup.2=x.sup.2?y.sup.2

and

(d.sub.2).sup.2=(x?x.sub.2).sup.2+y.sup.2

the position of the module S3 with respect to that of the module S1 is then:

[00008]$x=\frac{{\left(d_1\right)}^2-{\left(d_2\right)}^2-{\left(x_2\right)}^2}{2.Math.x_2}$$\mathrm{and}$$y=?\sqrt{{\left(d_1\right)}^2-{\left(x\right)}^2}$

[0067] The position of the module S3 in the frame of reference of the world being:

x=x+x.sub.1

y=y+y.sub.1

[0068] There are of course two solutions to this problem; the sign of the ordinate y must or should be determined either by a constraint of the user or by adding a third communications module onto the tracking vehicle.

[0069] Thus, theoretically, it is possible to estimate the location of the communications module S3 under any circumstances.

[0070] However, in practice, because of measurement noise, it turns out that, under certain circumstances, there is no intersection between the two circles and hence no solution to the localization problem.

[0071] Furthermore, the measurement noise introduces very significant discontinuities over time. Indeed, the measurement noise introduces significant variations in the localization results, especially on the measurement of the heading. These variations are abrupt, hence the discontinuities, but the notion of discontinuity may be omitted. To give one example, in the framework of experiments carried out by the applicant, when the vehicles are static, the estimation of the heading can vary by plus or minus 5? from one measurement to the other because of the measurement noise on the distances. The Kalman filter implemented by one embodiment allows this problem to be attenuated while providing an original solution to the problems of synchronization.

[0072] Furthermore, and above all, the measurements of distance d1, d2 are not supplied synchronously, whereas the mechanisms described hereinabove assume a synchronization of the measurements. As will be seen hereinbelow, this phenomenon amplifies even more the errors on the estimation of the heading.

[0073] Generally speaking, it is difficult to provide a synchronization of the stream of messages transmitting the distance measurements, but furthermore the communications module S3 generally disposes of a communications interface allowing it to dialog alternately with the communications modules S1, S2. In other words, the measurements of distance d1, d2 are non-synchronized.

[0074] For example, the module S3 of the mobile target dialogs with a first module S1 of the tracking vehicle for a certain period of time, for example 5 ms. A measurement frequency may furthermore be fixed at 50 Hz. In such a situation, the measurements are spaced out by at least 20 ms.

[0075] However, the tracking vehicle is mobile and, as a result, between the time t1 at which the measurement of distance d1 is determined and the time t2 at which the measurement of distance d2 is determined, it will move by a distance determined by the speed of the vehicle, by its direction and by the inter-measurement period.

[0076] Consequently, the measurements of distance d1 and d2 do not correspond to the same position of the tracking vehicle and the trilateration methods cannot operate in a satisfactory manner.

[0077] The method according to some embodiments includes steps for determination of at least a first measurement of distance d1 between the mobile target A and a first location in (or on) the tracking vehicle B, typically corresponding to a first communications module S1, taken at a first time t1, determination of a second measurement of distance d2 between the mobile target A and a second location S2 in (or on) the tracking vehicle B, typically corresponding to a second communications module S2, taken at a second time t2, determination of a prediction of the movement of the tracking vehicle between the first and second times, t1, t2, and determination of a location of the mobile target B with respect to the tracking vehicle A based on these first and second measurements of distance, d1, d2, taking into account the movement prediction, in such a manner as to compensate for the movement of the tracking vehicle between the first and second times.

[0078] The determination of the movement may typically be made based on the measurement of linear speed of the tracking vehicle and on the orientation of its steering carriage. These measurements are supplied by the mechanisms for controlling and monitoring the tracking vehicle, notably by the proprioceptive sensors carried on board the latter.

[0079] The sensors for measurements on the state of the vehicle itself are referred to as proprioceptive sensors or sensors of proprioception. This is in contrast to the sensors on the external information. One example of a proprioceptive sensor is a speed sensor. This term is commonly understood by those of ordinary skill in the art as referred to in the Wikipedia page dedicated to robotics: https://en.wikipedia.org/wiki/Autonomous_robot

[0080] Thus, by predicting the location of the tracking vehicle, the method according to some embodiments can take into account its estimated movement in order to compensate for it. The measurement data d1, d2 may then be used in a valid and precise manner despite their asynchronism. The same is of course true if more than two measurement data values are supplied.

[0081] Various methods may be used in order to take into account the asynchronism of the measurements of distances together with the precision of the data d1 and d2. These methods make use of filtering techniques whose characteristics allow the following to be integrated: the asynchronism of the data coming from marker points, the movement of the vehicle between the time t1 and t2, the uncertainty in movement of the vehicle between the times t1 and t2, the uncertainty associated with the measurements d1 and d2.

[0082] Amongst the most widespread filtering techniques, Kalman filters and particulate filters are often the most effective, but others would be perfectly applicable as long as they incorporate the aforementioned characteristics.

[0083] According to one embodiment, a Kalman filter is used to determine the movement prediction and the location of the mobile target with respect to the tracking vehicle. This Kalman filter allows the location to be inferred and filtered at any moment.

[0084] The state vector of the Kalman filter represents the parameters that it is desired to estimate. In the framework of some embodiments, the state vector X.sub.k of the Kalman filter reflects the position (x,y) of the mobile target A at the time k such that

[00009]$X_k=\left(\begin{array}{c}{x}_k\\ y_k\end{array}\right)$

[0085] By construction, the Kalman filter also estimates the precision of the estimation at any moment. The latter is represented by the covariance matrix Q.sub.k.

[0086] Given that the measurements of distance between the mobile target A and the tracking vehicle B are asynchronous, one after the other, each measurement d.sub.n,k is compared with its prior measurement d.sub.n,k/k?1 and the location of the mobile target is updated as a function of their difference. At each new measurement of distance coming from the communications module S.sub.n (with n=1 or n=2, in the case of two modules), the equation for updating the Kalman filter at the time k is the following:

d.sub.n,k=d.sub.n,k/k?1=?{square root over ((x.sub.k/k?1?x.sub.n).sup.2?(y.sub.k/k?1?y.sup.n).sup.2)}

in which x.sub.n and y.sub.n are the coordinates of the location of the positioning of the communications module S.sub.n on the tracking vehicle.

[0087] The next update of the state is carried out by the related art equations of the Kalman filter:

K.sub.n,k=P.sub.k/k?1H.sub.n,k.sup.T(H.sub.n,kP.sub.k/k?1H.sub.n,k.sup.T+?.sub.d.sup.2).sup.?1

X.sub.k/k=X.sub.k/k+K.sub.n,k(d.sub.n,k?d.sub.n,k/k?1)

P.sub.k/k=(I?K.sub.n,kH.sub.n,k)P.sub.k/k?1

in which ?.sub.d is the standard deviation of the distance measurements. The Jacobian of the observation function is described by:

[00010]$H_{n,k}=\left(\frac{x_{k/k-1}}{d_{n,k/k-1}}.Math.\frac{y_{k/k-1}}{d_{n,k/k-1}}\right)$

[0088] Experimentally, it can be shown that, under certain circumstances, the Kalman filter may consider locations with a negative ordinate to be more probable than those with a positive ordinate. In order to overcome this problem, an estimation with a constraint may be implemented, in order to impose that the ordinate of the solutions found is usually positive: y.sub.k/k?1>y.sub.n, where y.sub.n is the ordinate of the modules S.sub.1, S.sub.2 of the tracking vehicle. This hypothesis implies that the frame of reference is chosen in such a manner that the two modules S.sub.1, S.sub.2 are situated on a straight line parallel to the x axis.

[0089] Numerous methods exist for applying such a constraint within a Kalman filter. By way of examples may be mentioned those presented in the articles Kalman filtering with state constraints: a survey of linear and nonlinear algorithms by D. Simon, in IET Control Theory and Applications, 1303-1318, 2010, and Constrained Kalman filtering via density function truncation for turbofan engine health estimation by Dan Simon and Donald L. Simon, in Int. J. Systems Science, 41(2), 159-171, 2010.

[0090] In order to use the technology of Kalman filters to locate the target with respect to the tracking vehicle, the following new variables must or should be defined:

[0091] (x.sub.r.sup.w, y.sub.r.sup.w) is the location of the tracking vehicle in the frame of reference of the world;

[0092] ?.sub.r.sup.w is the orientation of the tracking vehicle in the frame of reference of the world;

[0093] (x.sub.p.sup.w, y.sub.p.sup.w) is the location of the target in the frame of reference of the world;

[0094] (x.sub.p.sup.r, y.sub.p.sup.r) is the location of the target in the frame of reference of the tracking vehicle. This frame of reference may be defined to be centered on the middle of the rear axle of the vehicle, for example.

[0095] The kinetic model of the tracking vehicle then needs to be defined. One example of such a kinetic model may be:

[00011]$\{\begin{array}{c}=v_r.Math.\cos .Math..Math.?\\ =v_r.Math.\sin .Math..Math.?\\ =v_r.Math.\frac{\tan .Math..Math.{?}_r}{L}\end{array}$

in which

[0096] y.sub.r is the linear speed of the tracking vehicle;

[0097] ?.sub.r is the orientation of the steering carriage, and

[0098] L is the track-width of the tracking vehicle.

[0099] The term track-width refers to the distance between the right and left wheels on the same axle.

[0100] It is then possible to define the simple dynamic model, known as the Ackermann model:

[00012]$\{\begin{array}{c}{x}_{r,k+1}^w=x_{r,k}^w-{?}_D.Math..Math.\sin ?\left({?}_{r,k}^w+{?}_{?}/2\right)\\ y_{r,k+1}^w=y_{r,k+1}^w-{?}_D.Math..Math.\cos ?\left({?}_{r,k}^w+{?}_{?}/2\right)\\ {?}_{r,k+1}^w={?}_{r,k}^w+{?}_D.Math.\end{array}$

[0101] in which

[00013]${?}_D=v_{r,k}.Math.{?}_T,.Math.\mathrm{and}.Math..Math.{?}_{?}=v_{r,k}.Math.\frac{{?}_T.Math..Math.\tan .Math..Math.{?}_{r,k}}{L}$

[0102] ?.sub.T is the period of time between the times k and k?1, in other words the sampling period.

[0103] The equations hereinabove describe the movement of the tracking vehicle within the world. However, the problem here is to know the movement of the target in the frame of reference of the tracking vehicle.

[0104] The location of the target in the frame of reference of the tracking vehicle at the time k is defined as (x.sub.p.sup.r, y.sub.p.sup.r).sub.k. It is assumed for the moment that the target is immobile. At the time k+1, the location of the target in the frame of reference of the tracking vehicle is given by the following equations:

[00014]$\left(\begin{array}{c}{x}_{p,k+1}^r\\ y_{p,k+1}^r\end{array}\right)={R?\left({?}_{?}\right)}^T.Math.\left(\begin{array}{c}{x}_{p,k}^r-\left(x_{r,k+1}^w-x_{r,k}^w\right)\\ y_{p,k}^r-\left(y_{r,k+1}^w-y_{r,k}^w\right)\end{array}\right)$

[0105] Using the preceding system of equations, the following is obtained:

[00015]$\left(\begin{array}{c}{x}_{p,k+1}^r\\ y_{p,k+1}^r\end{array}\right)={R?\left({?}_{?}\right)}^T.Math.\left(\begin{array}{c}{x}_{p,k}^r+{?}_D.Math..Math.\sin ?\left({?}_{?}/2\right)\\ y_{p,k}^r-{?}_D.Math..Math.\cos ?\left({?}_{?}/2\right)\end{array}\right)$

where R(?.sub.?) is a 2D rotational matrix, which is a function of the angle ?.sub.?.

[0106] However, of course, the target may be mobile and, between two times, it can move in the frame of reference of the world.

[0107] The following may be written:

[00016]$\{\begin{array}{c}==0\\ ==0\end{array}$

and it may be injected into the preceding equation in order to obtain the equation describing the location of a mobile target with respect to a tracking vehicle:

[00017]$\left(\begin{array}{c}{x}_{p,k+1}^r\\ y_{p,k+1}^r\end{array}\right)={R?\left({?}_{?}\right)}^T.Math.\left(\begin{array}{c}{x}_{p,k}^r+{?}_D.Math..Math.\sin ?\left({?}_{?}/2\right)\\ y_{p,k}^r-{?}_D.Math..Math.\cos ?\left({?}_{?}/2\right)\end{array}\right)+v_{\mathrm{xy}}$

in which v.sub.xy follows a two-dimensional centered normal law N(0,Q.sub.xy) whose covariance matrix Q.sub.xy is defined by the equation

Q.sub.xy=I(?.sub.xy.sup.r.Math.?.sub.T).sup.2

with ?.sub.xy.sup.r the standard deviation of the movement that can be made by the target in one second, and ?.sub.T the period of time (in seconds) between the times k and k+1.

[0108] Considering now that the location frame of reference is that of the tracking vehicle, it may be defined that the location (x.sub.p.sup.r, y.sub.p.sup.r) is equivalent to the location (x,y) previously used.

[0109] It is then possible to estimate the location of the target by the Kalman filter with constraints using the equation hereinabove.

[0110] The prediction equations of the Kalman filter thus defined become:

[00018]$X_{k/k-1}={R?\left({?}_{?}\right)}^T.Math.\left(X_{k-1/k-1}-\left(\begin{array}{c}-{?}_D.Math..Math.\sin ?\left({?}_{?}/2\right)\\ {?}_D.Math..Math.\cos ?\left({?}_{?}/2\right)\end{array}\right)\right)$$P_{k/k-1}={R?\left({?}_{?}\right)}^T.Math.\left(P_{k-1/k-1}+G_k.Math.Q_u.Math.G_k^T\right).Math.R?\left({?}_{?}\right)+Q_{\mathrm{xy}}$

where Q.sub.u is the covariance matrix associated with the uncertainties in the proprioceptive information coming from the tracking vehicle, such that:

[00019]$Q_u=\left(\begin{array}{cc}{?}_{v_r}^2& 0\\ 0& {?}_{{?}_r}^2\end{array}\right).Math..Math.\mathrm{and}.Math.$$G_k=\left(\begin{array}{cc}-{?}_T.Math..Math.\sin ?\left({?}_{?}/2\right)& -0.5.Math..Math.{?}_D.Math..Math.\cos ?\left({?}_{?}/2\right)\\ {?}_T.Math..Math.\cos ?\left({?}_{?}/2\right)& -0.5.Math..Math.{?}_D.Math..Math.\sin ?\left({?}_{?}/2\right)\end{array}\right)$

with ?.sub.v.sub.r.sup.2 and ?.sub.?.sub.r.sup.2 given by the characteristics of the odometric sensors of the vehicle. These quantities are therefore to be determined for each vehicle.

[0111] Knowing the location of the target with respect to which the vehicle must or should be closed-loop controlled, it is possible to determine a control command aimed at adapting the speed and the direction of the tracking vehicle. It is furthermore possible to impose a setpoint distance between the tracking vehicle and the target.

[0112] Various mechanisms are possible for exploiting the location of the target determined by the method according to some embodiments, and allowing given behaviors of the tracking vehicle to be defined.

[0113] According to one particular embodiment, a control command is determined in order for the tracking vehicle to be directed toward the target while at the same time complying with a inter-distance setpoint ?.sub.c.

[0114] In the case of a tracking vehicle having a steering carriage and a drive train, the control vector is composed of the speed at the center of the rear axle v.sub.r and of the orientation of the steering carriage ?.sub.r. Considering the location (?, ?) of the target to be tracked in a system of polar coordinates such that:

[00020]$\{\begin{array}{c}?=\sqrt{x^2+y^2}\\ ?=\mathrm{arc}.Math..Math.\tan .Math..Math.2.Math.\left(y,x\right)\end{array}$

[0115] The latter respectively describes the inter-distance measured between the tracking vehicle and the target, together with the orientation that the tracking vehicle must or should follow in order to go and place itself behind the target. In order for the vehicle to follow the target, the control vector must or should be found that ensures that both p tends toward ?.sub.c and that ? tends toward 0. The problem is generally treated separately. The heading of the tracking vehicle may be corrected by using a proportional corrector:

?.sub.r,k=Kp.sub.??.sub.k

in which Kp.sub.? is the proportional gain of the corrector. The inter-distance between the vehicle B and the target A may be regulated by an integral-proportional corrector:

[00021]$v_{r,k}={\mathrm{Kp}}_{?}?\left({?}_k-{?}_c\right)+{\mathrm{Ki}}_{?}.Math.\underset{i=0}{\overset{k}{.Math.}}.Math.\left({?}_i-{?}_c\right)$

[0116] in which Kp.sub.? and Ki.sub.? are respectively the proportional and integral gains of the corrector. The equation ?.sub.r,k=Kp.sub.??.sub.k shows that, in order to correct the heading of the vehicle, it suffices to orient the wheels of the steering carriage of the vehicle toward the target. The preceding equation shows that an integral action may be necessary for regulating the linear speed of the vehicle in order for the inter-distance to be adhered to, in other words for the quantity ?.sub.k??.sub.c to tend toward 0.

[0117] It is noted that, if the target is coming toward the vehicle, the quantity ?.sub.k??.sub.c becomes negative and will cause the vehicle to reverse since the speed v.sub.r,k will also become negative.

[0118] It may be desirable to prevent the vehicle from reversing, notably for reasons of safety. In this case, it is possible, according to one embodiment, to introduce the following constraint:

[00022]$v_{r,k}=\{\begin{array}{cc}{\mathrm{Kp}}_{?}?\left({?}_k-{?}_c\right)+{\mathrm{Ki}}_{?}.Math.\underset{i=0}{\overset{k}{.Math.}}.Math.\left({?}_i-{?}_c\right),& \mathrm{if}.Math..Math.{?}_k>{?}_c-3.Math.{?}_{?,k}\\ 0,& \mathrm{or}.Math..Math.\mathrm{else}\end{array}$

where ?.sub.r,k is the standard deviation of the distance measurement ?.sub.k separating the vehicle and the target such that:

?.sub.?,k.sup.2=J.sub.rhoP.sub.kJ.sub.rho.sup.T

with

[00023]$J_{\mathrm{rho}}=\left(\frac{x_k}{{?}_k}.Math.\frac{y_k}{{?}_k}\right)$

and P.sub.k is the matrix supplied at the time k by the module for locating the target. This matrix may be deduced a posteriori or a priori depending on whether the control command is carried out at the same time as the update of the Kalman filter or otherwise.

[0119] The preceding description relates to an embodiment in which the tracking vehicle disposes has 2 communications modules S.sub.1, S.sub.2, but it is also possible to consider more than 2 communications modules.

[0120] It goes without saying that the presently disclosed subject matter is not limited to the examples and to the embodiment described and shown, but numerous variants are possible which are accessible to those of ordinary skill in the art.