WO2017144808A1 - Localisation d'une cible pour véhicule suiveur - Google Patents

Localisation d'une cible pour véhicule suiveur Download PDF

Info

Publication number
WO2017144808A1
WO2017144808A1 PCT/FR2017/050381 FR2017050381W WO2017144808A1 WO 2017144808 A1 WO2017144808 A1 WO 2017144808A1 FR 2017050381 W FR2017050381 W FR 2017050381W WO 2017144808 A1 WO2017144808 A1 WO 2017144808A1
Authority
WO
WIPO (PCT)
Prior art keywords
location
follower vehicle
vehicle
target
moving target
Prior art date
Application number
PCT/FR2017/050381
Other languages
English (en)
Inventor
Roland CHAPUIS
Jean LANEURIT
Christophe Debain
Original Assignee
Université Clermont Auvergne
Centre National De La Recherche Scientifique - Cnrs -
Institut National De Recherche En Sciences Et Technologies Pour L'environnement Et L'agriculture - Irstea -
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Université Clermont Auvergne, Centre National De La Recherche Scientifique - Cnrs -, Institut National De Recherche En Sciences Et Technologies Pour L'environnement Et L'agriculture - Irstea - filed Critical Université Clermont Auvergne
Priority to EP17710337.1A priority Critical patent/EP3420373A1/fr
Priority to US16/078,775 priority patent/US20190137617A1/en
Publication of WO2017144808A1 publication Critical patent/WO2017144808A1/fr

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/74—Systems using reradiation of radio waves, e.g. secondary radar systems; Analogous systems
    • G01S13/76—Systems using reradiation of radio waves, e.g. secondary radar systems; Analogous systems wherein pulse-type signals are transmitted
    • G01S13/765—Systems using reradiation of radio waves, e.g. secondary radar systems; Analogous systems wherein pulse-type signals are transmitted with exchange of information between interrogator and responder
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/87—Combinations of radar systems, e.g. primary radar and secondary radar
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
    • H04W4/46—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P] for vehicle-to-vehicle communication [V2V]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/9318—Controlling the steering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/932—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles using own vehicle data, e.g. ground speed, steering wheel direction
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G01S2013/9325—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles for inter-vehicle distance regulation, e.g. navigating in platoons
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/12—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves by co-ordinating position lines of different shape, e.g. hyperbolic, circular, elliptical or radial
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services

Definitions

  • the invention relates to the field of follower vehicles and more particularly to a method and a device for precisely locating a moving target that the follower vehicle must follow.
  • the field of the follower vehicles is in full expansion and knows very varied fields of application.
  • the follower vehicle may be a motorized caddy automatically following a golf player.
  • a carrier vehicle following a worker on an industrial site or in a factory, workshop or warehouse.
  • Another example concerns the agricultural field, in which one or more motorized agricultural vehicles automatically follow a farmer.
  • the follower vehicles may follow another vehicle.
  • a classic constraint for follower vehicles is to maintain a constant distance with the target tracked (pedestrian or other vehicle) and to follow its path, that is to say to be able to react to changes in speed and direction.
  • this tracking can not be done without a location of the target to follow with respect to the follower vehicle. This location can including understanding the distance and an angle to the direction of the follower vehicle.
  • a conventional technique relies on laser rangefinders and / or cameras used either in the visible spectrum or in the infrared.
  • a shape recognition mechanism in the images acquired by the cameras and / or the range finder is then set up to detect the target to be tracked and estimate its relative position.
  • Such methods are sensitive to external disturbances such as, in particular, climatic variations (rain, smoke, fog, snow, etc.), illumination changes, temperature variations (greatly disturbing infra-red cameras), dazzling, the presence of obstacles (which can break the contact between the target and the sensors), etc.
  • the target is a pedestrian
  • the sensor that can best differentiate two pedestrians is the camera in the visible range, but this sensor is also the most sensitive to environmental conditions. For example, it can not work at night or in poor lighting. In these conditions, the rangefinders and infrared cameras give better performance, but they do not make it easy to distinguish one pedestrian from another.
  • the rangefinder only allows to determine silhouettes "in depth" and can distinguish silhouettes only on their size or size.
  • the target to be followed may be temporarily obscured by an obstacle (tree, corner of a street, other pedestrian, etc.).
  • the target can no longer be located.
  • Other mechanisms are based on radio or ultrasound links. This is for example the case of those disclosed in US Patent 5,810,105 or in EP application 2,590,041.
  • the location is ensured by two communication modules on the follower vehicle and a module on the target. From measurements of the flight time of the information exchanged between the module on the target and each of the modules of the following vehicle, a relative location can be estimated by trilateration.
  • the accuracy is strongly impacted by the lack of synchronization between the two communication streams: the first between the pedestrian module and the first module of the vehicle and the second between the pedestrian module and the second module of the vehicle.
  • the communications with the first and with the second module on board the follower vehicle the latter was able to move, so that a bias is inserted into the calculations of the trilateration.
  • the location thus obtained lacks precision and may even lead, in some situations, to significant aberrations.
  • the object of the present invention is to provide a solution at least partially overcoming the aforementioned drawbacks.
  • the present invention proposes a method of locating a moving target by a follower vehicle, comprising the determination of minus a first distance measurement between said moving target and a first location on said follower vehicle, taken at a first instant, and a second distance measurement between said moving target and a second location on said follower vehicle, taken in a second instant, characterized in that said method determines a prediction of the displacement of said follower vehicle between said first and second instants, and determines a location of said moving target relative to the follower vehicle from said first and second distance measurements, taking into account said prediction, so as to compensate for said displacement between said first and second instants.
  • the invention comprises one or more of the following features which can be used separately or in partial combination with one another or in total combination with one another:
  • said prediction and said location are determined by a Kalman filter
  • said displacement prediction is determined from linear velocity measurement and steering gear orientation of said follower vehicle
  • Q u is the covariance matrix associated with the uncertainties of the proprioceptive information from said follower vehicle, such as:
  • ⁇ is the time between instants k and k-1.
  • v r , k is the linear velocity of said follower vehicle at time k;
  • r , k is the orientation of the steering gear of said follower vehicle at time k, and,
  • the method further comprises a step of determining a command to adapt the speed and direction of said follower vehicle to direct it according to said location location of said target n respecting a set distance.
  • Another subject of the invention relates to a device for locating a moving target for a follower vehicle, comprising a first communication module at a first location on said follower vehicle to determine a first distance measurement between said first location and said target. mobile at a first instant, and a second communication module at a second location on said follower vehicle to determine a second distance measurement between said second location and said moving target at a second time, and a calculation module for determining a forecast of the moving said follower vehicle between said first and second instants, and determining a location of said moving target relative to said follower vehicle from said first and second distance measurements, taking into account said forecast, so as to compensate said displacement between said first and second moments.
  • this device comprises one or more of the following characteristics which can be used separately or in partial combination with one another or in total combination with one another:
  • said calculation module determines said prediction of the tracking robot's displacement and said location of the target by a Kalman filter
  • said calculation module determines said displacement prediction from the proprioceptive measurements of said follower vehicle.
  • Another object of the invention relates to a follower vehicle comprising such a device.
  • Another object of the invention relates to a system comprising a tracking vehicle as defined above and a communication module fitted to the moving target.
  • Figure 1 shows schematically an example of context in which the invention can be inserted.
  • Figure 2 schematizes the principle of localization by trilateration.
  • FIGS. 3a, 3b, 3c schematize different mechanisms of distance measurements.
  • Figure 1 schematically illustrates the context in which the invention is inserted.
  • the invention allows the location of a moving target A by a follower vehicle B.
  • the moving target A is typically a human being, which may be a pedestrian or a vehicle, including a robotic vehicle.
  • the following vehicle B is preferably a robotic vehicle, that is to say, whose control is provided by an automatic mechanism.
  • the moving target A is equipped with an S3 communication module. It will be seen later that it may include more, but only one is sufficient to implement the invention in its generality.
  • the follower vehicle B is equipped with at least two communication modules SI, S2, located at two distinct locations on the vehicle. It is important that these two modules be far enough apart to obtain the best possible localization results. They may for example be near the two side edges of the follower vehicle. They must be in solidarity, that is, their relative distance must not vary. They must be integral with the chassis of the following vehicle.
  • the communication modules SI, S2, S3 are suitable for determining distance measurements d1, d2 respectively between the module SI and the module S3 and between the module S2 and the module S3.
  • This protocol is a standard specifying that the communication modules SI, S2, S3 incorporate a physical layer capable of performing distance measurements.
  • This protocol has two communication formats: the IRUWB (for "Impulse Radio Ultra Wide Band” in English) and the CSSS (for "Chirp Spread Spectrum Signais”).
  • the invention does not lie in the mechanism of measuring a distance, but, as we shall see later, in the operation of these distance measurements.
  • the distance measurement can be carried out in different ways, including those included in the state of the art.
  • distance measurement methods are based on the principle of time of arrival ("Time of Arrival") required for a message to go from one communication module to another.
  • TOA Time Of Arrival
  • mobile station and “base station”, which may represent, in the context of the invention, respectively, the follower vehicle B and the target to be followed. It should be noted that the target itself may be mobile and that the terms “mobile station” and “base station” must be understood in a relative sense: the mobile station is mobile in the base station repository ( whose repository can itself be mobile with respect to the terrestrial reference system for example).
  • FIGS. 3a, 3b, 3c which schematize the protocol exchanges between two stations A, B, with a view to measuring the distance between them.
  • TOA Time of Arrival
  • the mobile station A sends an RFRAME message to the base station B on the transmission date t e , this date being transmitted in the message.
  • the base station receives the message and notes the date of acquisition t a .
  • the base station A sends a request RFRrame_req to a mobile station B and records the date of issue t e of this message,
  • the mobile station responds to the request sending the RFRrame rep message to the base station,
  • the base station receives the response message and records the receipt date t a .
  • the base and mobile stations are therefore both transmitters and receivers.
  • the time T r being measured by the base station A, there is no need to synchronize their clocks. Nevertheless, the response of the mobile station can not be immediate. Indeed, it must decode the request message sent by the base station, create a response message and finally send it. This process introduces a delay T "" and biases the distance measurement. For example, an error of a few nanoseconds introduces decimetre errors. It is therefore important to be able to estimate this delay very precisely.
  • the base station A sends a RFRrame request req to the mobile station B and records the transmission date of the last byte of the SFD ("Start Frame Delimeter") of the message RFRrame req.
  • the mobile station B responds to the request sending the message RFRrame rep to the base station, 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 response message and records the receipt date of the last SFD byte of the RFRrame rep message
  • the mobile station sends a second message to the base station containing the value of the estimated T using the counter,
  • the base station receives the message containing the estimate of the delay a ,
  • the base station sends an acknowledgment to the mobile station.
  • the distance measurement dsDs is as follows:
  • the distance measurement is a few tens of meters at most, T r will not exceed 100 nanoseconds. However, the time T required to process your request message and reply is of the order of a millisecond. This means that the transmission time is well below the data processing time which accounts for most of the distance measurement error: T r A (_l + e A ) - T t B a (_l + e B )
  • the communication modules SI, S2, S3 can implement such distance measuring mechanisms. Concretely, these mechanisms can be implemented using communication modules available on the market and that the method and the device according to the invention can use.
  • Such modules available on the market may be those of the Decawave company, and more particularly the DW1000 sensor.
  • This sensor complies with the previously mentioned IEEE 802.15.4a communication protocol and operates in a frequency range from 3.5 GHz to 6.5 GHz with a bandwidth of 1 GHz. It allows measurement of distance with a precision of 10 cm.
  • This sensor has among other advantages those of having a relatively low cost and being of small size, for example 23 mm x 13 mm for the model DWM1000.
  • Figure 2 schematizes the principle of localization by trilateration.
  • the problem to be solved is to locate the communication module S3 with coordinates (x, y) by knowing the locations of the communication modules S1 and S2, respectively of coordinates (x1, y1) and (x2, y2). and the measured distances d1, d2 between the communication module S3 and, respectively, the communication modules S1 and S2.
  • the position of the S3 module being the point of intersection between two circles centered on each of the modules SI, S2 and di radius and d 2, this problem can be easily solved analytically.
  • measurement noise introduces very important discontinuities over time. Indeed, measurement noise introduces large variations in the localization results, especially on the measurement of heading. These variations are abrupt, hence the discontinuities, but the notion of discontinuity can be omitted.
  • the heading estimate can vary by plus or minus 5 ° from one measurement to the next because of the noise of distance measurements.
  • the Kalman filter implemented by one embodiment of the invention alleviates this problem while providing an original solution to synchronization problems.
  • the communication module S3 generally has a communication interface that allows it to communicate only alternatively with the modules. SI, S2 communication. In other words, the distance measurements d1, d2 are necessarily not synchronized.
  • the module S3 of the moving target communicates with a first module SI of the follower vehicle for a certain duration, for example 5 ms. It is also possible to set a measurement frequency at 50 Hz. In such a situation, the measurements are spaced at least 20 ms apart.
  • the follower vehicle is mobile and therefore between the time t1 at which the distance measurement dl is determined and the time t2 at which the distance measurement d2 is determined, it will have moved by a distance determined by the speed of the vehicle, its management and the inter-measurement period.
  • the method according to the invention provides steps of: determining at least a first distance measurement d1 between the moving target A and a first location in (or on) the tracking vehicle B, typically corresponding to a first communication module S1, taken at a first instant tl, determination of a second distance measurement d2 between the moving target A and a second location S2 in (or on) the tracking vehicle B, corresponding typically to a second communication module S2, taken at a second time t2,
  • the determination of the displacement can typically be made from the linear velocity measurement of the follower vehicle and the orientation of its steering gear. Preferably, these measurements are provided by the control bodies of the follower vehicle in particular by the proprioceptive sensors on board the latter.
  • Proprioceptive sensors or proprioception sensors are the measurement sensors on the state of the vehicle itself. They oppose sensors on external information.
  • An example of a proprioceptive sensor is a speed sensor. This term is commonly understood by those skilled in the art as evidenced by the page wikipedia devoted to robotics: https://en.wikipedia.org/wiki/Autonomous_robot
  • the method according to the invention can take into account its estimated displacement in order to compensate for it.
  • the measurement data d1, d2 can then be exploited valid way, and accurate despite their asynchronism. The same is obviously true if more than two measurement data are provided.
  • Kalman filters and particulate filters are often the most effective, but others would be quite applicable if they incorporate the features mentioned above.
  • a Kalman filter is used to determine the displacement prediction and the location of the moving target relative to the follower vehicle. This Kalman filter makes it possible to infer and filter the location at any time.
  • the state vector of the Kalman filter represents the parameters that one wants to estimate.
  • the state vector Xk of the Kalman filter reflects the position (x, y) of the moving target A at time k such that
  • the Kalman filter also estimates the accuracy of the estimate at all times. This is represented by the covariance matrix Qk.
  • each measurement d n , k is compared with its a priori measurement d n , kk i and the location of the mobile target is updated based on their difference.
  • the update equation of the Kalman filter at time k is as follows:
  • x n and y n are the coordinates of the location of the location of the communication module S n on the follower vehicle.
  • ⁇ 5d is the standard deviation for distance measurements.
  • Kalman filtering with state constraints a survey of linear and nonlinear algorithms
  • IET Contril Theory and applications 1303-1318
  • 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
  • (x, y) is the location of the follower vehicle in the world referential
  • the kinematic model of the follower vehicle must then be defined.
  • An example of such a kinematic model can be:
  • v r is the linear speed of the follower vehicle
  • ⁇ ⁇ is the orientation of the train
  • L is the track of the follower vehicle.
  • ⁇ ⁇ v
  • ⁇ is the time elapsed between the instants k and k-1, that is to say the sampling period.
  • R (Ae) is a 2D rotation matrix, a function of the angle ⁇ .
  • the target can be mobile, and between two instants it can move in the repository of the world.
  • v xy follows a normal two-dimensional centered law N (, Q xy ) whose covariance matrix Q xy is defined by the equation with ⁇ ⁇ the standard deviation of the displacement that can be made by the target in one second, and ⁇ ⁇ the elapsed time (in seconds) between the instants k and k + 1.
  • the location reference is that of the follower vehicle, it can be defined that the location ( ⁇ , j) is equivalent to the location (x, y) previously used.
  • Q u is the covariance matrix associated with the uncertainties of proprioceptive information from the follower vehicle, such as: - A r sin (A e / 2) - 0.5 ⁇ ⁇ cos (A e 12
  • a command is determined so that the follower vehicle moves towards the target while respecting an inter-distance setpoint p c .
  • control vector is composed of the speed at the center of the rear axle v r and the orientation of the steering gear ⁇ ⁇ .
  • Kpe is the proportional gain of the corrector.
  • V r, t K P P (A " Pc) + Ki p ⁇ (- Pc)
  • Kp p and Ki p are respectively the proportional and integral gains of the corrector.
  • equation shows that in order to correct the course of the vehicle, it is sufficient to orient the wheels of the steering gear of the vehicle towards the target.
  • the preceding equation shows that an integral action is necessary to regulate the linear speed of the vehicle so that the distance is always respected, that is to say that the quantity p k -p c tends towards 0. Note that if the target is moving towards the vehicle, the quantity p k - p c becomes negative and will cause the vehicle to fall back because the speed v r , k will also become negative.
  • Pk is the matrix provided at the instant k by the target location module. This matrix can be deduced a posteriori or a priori depending on whether the command is performed at the same time as updating the Kalman filter or not.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Signal Processing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

L'invention concerne un dispositif de localisation d'une cible mobile (A) pour un véhicule suiveur (B), comportant un premier module de communication (S1) en un premier emplacement sur ledit véhicule suiveur (B) pour déterminer une première mesure de distance (d1) entre le premier emplacement et la cible mobile (A) en un premier instant, et un second module de communication (S2) en un second emplacement sur le véhicule suiveur (B) pour déterminer une seconde mesure de distance (d2) entre le second emplacement et la cible mobile (A) en un second instant, et un module de calcul pour déterminer une prévision du déplacement du véhicule suiveur entre les premier et second instants, et déterminer une localisation de ladite cible mobile par rapport au dit véhicule suiveur à partir des première et seconde mesures de distance, en prenant en compte la prévision, de sorte à compenser le déplacement entre les premier et second instants.

Description

LOCALISATION D'UNE CIBLE POUR VÉHICULE SUIVEUR
DOMAINE DE L'INVENTION
L'invention concerne le domaine des véhicules suiveurs et plus particulièrement d'une méthode et d'un dispositif permettant la localisation précise d'une cible mobile que le véhicule suiveur doit suivre.
CONTEXTE DE L'INVENTION
Le domaine des véhicules suiveurs est en pleine expansion et connaît des champs d'application très variés.
Par exemple, le véhicule suiveur peut être un caddy motorisé suivant de façon automatique un joueur de golf. Un autre exemple est un véhicule porteur suivant un ouvrier sur un chantier industriel ou au sein d'une usine, d'un atelier ou d'un entrepôt de stockage.
Un autre exemple concerne le domaine agricole, dans lequel un ou plusieurs véhicules agricoles motorisés suivent de façon automatique un agriculteur.
Dans certaines applications, les véhicules suiveurs peuvent suivre un autre véhicule.
Une contrainte classique pour les véhicules suiveurs est de maintenir une distance constante avec la cible suivie (piéton ou autre véhicule) et de suivre sa trajectoire, c'est-à-dire de pouvoir réagir à des changements de vitesse et de direction.
Différents mécanismes ont été proposés pour permettre ce suivi. D'une façon générale, ce suivi ne peut se faire sans une localisation de la cible à suivre par rapport au véhicule suiveur. Cette localisation peut notamment comprendre la distance et un angle par rapport à la direction du véhicule suiveur.
Une technique classique repose sur des télémètres lasers et/ou des caméras utilisées soit dans le spectre visible soit dans l'infrarouge. Un mécanisme de reconnaissance de forme dans les images acquises par les caméras et/ou par le télémètre est ensuite mis en place pour détecter la cible à suivre et estimer sa position relative.
De telles méthodes sont toutefois sensibles à des perturbations extérieures comme, notamment, des variations climatiques (pluie, fumée, brouillard, neige...), des changements d'illumination, des variations de température (perturbant fortement les caméras infra-rouge), les éblouissements, la présence d'obstacles (pouvant rompre le contact entre la cible et les capteurs), etc.
En outre, même dans le cas idéal d'une perception parfaite de l'environnement, il reste difficile de différentier la cible des autres objets présents dans l'environnement.
Par exemple, si la cible est un piéton, il est techniquement difficile de le distinguer d'un autre piéton présent dans la scène observée. Le capteur le plus à même de différencier deux piétons est la caméra dans le domaine visible, mais ce capteur est par ailleurs le plus sensible aux conditions environnementales. Il ne peut par exemple fonctionner de nuit ou par mauvais éclairage. Dans ces conditions, les télémètres et les caméras infrarouges donnent de meilleures performances, mais ils ne permettent pas de distinguer facilement un piéton d'un autre. Le télémètre ne permet que de déterminer des silhouettes « en profondeur » et ne peut distinguer des silhouettes que sur leur corpulence ou leur taille.
Par ailleurs, la cible à suivre peut être temporairement occultée par un obstacle (arbre, angle d'une rue, autre piéton, etc.). La cible ne peut alors plus être localisée. D'autres mécanismes se basent sur des liaisons radios ou ultrasons. C'est par exemple le cas de ceux exposés dans le brevet US 5,810,105 ou dans la demande EP 2,590,041.
La localisation est assurée par deux modules de communication sur le véhicule suiveur et un module sur la cible. A partir des mesures du temps de vol des informations échangées entre le module sur la cible et chacun des modules du véhicule suiveur, une localisation relative peut être estimée par trilatération.
Toutefois, ces mécanismes fournissent une précision de la localisation insuffisante. En effet, si la connexion radio est rompue entre les modules de communication, de la même façon qu'avec les solutions basées sur des caméras ou télémètre, la détermination de la localisation ne peut plus être assurée.
En outre, même sans une telle rupture, la précision est fortement impactée par l'absence de synchronisation entre les deux flots de communication : le premier entre le module du piéton et le premier module du véhicule et le second entre le module du piéton et le deuxième module du véhicule. Or, comme observé par la Demanderesse, entre les communications avec le premier et avec le second module embarqué sur le véhicule suiveur, ce dernier a pu se déplacer, de sorte qu'un biais est inséré dans les calculs de la trilatération. La localisation ainsi obtenue manque donc de précision et peut même aboutir, dans certaines situations, à des aberrations importantes.
RÉSUMÉ DE L'INVENTION
Le but de la présente invention est de fournir une solution palliant au moins partiellement les inconvénients précités.
A cette fin, la présente invention propose un procédé de localisation d'une cible mobile par un véhicule suiveur, comportant la détermination d'au moins une première mesure de distance entre ladite cible mobile et un premier emplacement sur ledit véhicule suiveur, prise en un premier instant, et d'une seconde mesure de distance entre ladite cible mobile et un second emplacement sur ledit véhicule suiveur, prise en un second instant, caractérisé en ce que ledit procédé détermine une prévision du déplacement dudit véhicule suiveur entre lesdits premier et second instants, et détermine une localisation de ladite cible mobile par rapport au véhicule suiveur à partir desdites première et seconde mesures de distance, en prenant en compte ladite prévision, de sorte à compenser ledit déplacement entre lesdits premier et second instants.
Suivant des modes de réalisation préférés, l'invention comprend une ou plusieurs des caractéristiques suivantes qui peuvent être utilisées séparément ou en combinaison partielle entre elles ou en combinaison totale entre elles :
ladite prévision et ladite localisation sont déterminés par un filtre de Kalman ;
ladite prévision du déplacement est déterminée à partir de mesure de vitesse linéaire et de l'orientation du train directeur dudit véhicule suiveur ;
les équations de prédiction dudit filtre de Kalman, entre un instant k et un instant k-1 sont
Figure imgf000006_0001
dans lesquelles Qu est la matrice de covariance associée aux incertitudes des informations proprioceptives provenant dudit véhicule suiveur, telle que :
Figure imgf000006_0002
et f- AT sm(Ae / 2) - 0.5Δΰ οο8(Δθ / 2)
k ~ { Ar cos(Ae / 2) - 0.5Δΰ 8ίη(Δθ / 2)
avec
AD = vr>JtAr , et
ΔΓ tan <?
= ^r'*
Δτ est le temps écoulé entre les instants k et k-1. vr,k est la vitesse linéaire dudit véhicule suiveur à l'instant k ; ôr,k est l'orientation du train directeur dudit véhicule suiveur à l'instant k, et,
L est la voie du véhicule suiveur,
le procédé comporte outre une étape de détermination d'une commande visant à adapter la vitesse et la direction dudit véhicule suiveur afin de le diriger en fonction de ladite location localisation de ladite cible n respectant une distance de consigne.
Un autre objet de l'invention concerne un dispositif de localisation d'une cible mobile pour un véhicule suiveur, comportant un premier module de communication en un premier emplacement sur ledit véhicule suiveur pour déterminer une première mesure de distance entre ledit premier emplacement et ladite cible mobile en un premier instant, et un second module de communication en un second emplacement sur ledit véhicule suiveur pour déterminer une seconde mesure de distance entre ledit second emplacement et ladite cible mobile en un second instant, et un module de calcul pour déterminer une prévision du déplacement dudit véhicule suiveur entre lesdits premier et second instants, et déterminer une localisation de ladite cible mobile par rapport audit véhicule suiveur à partir desdites première et seconde mesures de distance, en prenant en compte ladite prévision, de sorte à compenser ledit déplacement entre lesdits premier et second instants. Suivant des modes de réalisation préférés, ce dispositif selon l'invention comprend une ou plusieurs des caractéristiques suivantes qui peuvent être utilisées séparément ou en combinaison partielle entre elles ou en combinaison totale entre elles :
ledit module de calcul détermine ladite prévision du déplacement du robot suiveur et ladite localisation de la cible par un filtre de Kalman ;
ledit module de calcul détermine ladite prévision du déplacement à partir des mesures proprioceptives dudit véhicule suiveur.
Un autre objet de l'invention concerne un véhicule suiveur comportant un tel dispositif.
Un autre objet de l'invention concerne un système comportant un véhicule suiveur tel que précédemment défini et un module de communication équipant la cible mobile.
Système comportant un véhicule suiveur (B) selon la revendication précédente, et un module de communication équipant ladite cible mobile (A).
D'autres caractéristiques et avantages de l'invention apparaîtront à la lecture de la description qui suit d'un mode de réalisation préféré de l'invention, donnée à titre d'exemple et en référence aux dessins annexés.
BRÈVE DESCRIPTION DES DESSINS
La figure 1 représente schématiquement un exemple de contexte dans lequel peut s'insérer l'invention.
La figure 2 schématise le principe de la localisation par trilatération.
Les figures 3a, 3b, 3c schématisent différents mécanismes de mesures de distance. DESCRIPTION DÉTAILLÉE DE L'INVENTION
La figure 1 illustre schématiquement le contexte dans lequel s'insère l'invention.
L'invention permet la localisation d'une cible mobile A par un véhicule suiveur B. La cible mobile A est typiquement un être humain, qui peut être un piéton ou un véhicule, y compris un véhicule robotisé. Le véhicule suiveur B est préférentiellement un véhicule robotisé, c'est-à-dire dont le pilotage est assuré par un mécanisme automatique.
La cible mobile A est équipée d'un module de communication S3. On verra ultérieurement qu'elle peut en comporter davantage, mais un seul suffit pour mettre en œuvre l'invention dans sa généralité.
Le véhicule suiveur B est équipé d'au moins deux modules de communication SI, S2, situés à deux emplacements distincts sur le véhicule. Il est important que ces deux modules soient suffisamment distants pour obtenir les meilleurs résultats de localisation possible. Ils peuvent par exemple être à proximité des deux bords latéraux du véhicule suiveur. Ils doivent être solidaires, c'est-à-dire que leur distance relative ne doit pas varier. Ils doivent être solidaires du châssis du véhicule suiveur.
Les modules de communication SI, S2, S3 sont adaptés pour déterminer des mesures de distance dl, d2, respectivement entre le module SI et le module S3 et entre le module S2 et le module S3.
Ces modules de communication peuvent notamment s'appuyer sur le protocole normalisé IEEE 802.15.4a. Ce protocole est un standard spécifiant que les modules de communication SI, S2, S3 incorporent une couche physique capable de réaliser des mesures de distance. Ce protocole a deux formats de communication : l'IRUWB (pour « Impulse Radio Ultra Wide- Band », en langue anglaise) et le CSSS (pour « Chirp Spread Spectrum Signais ».) L'invention ne réside pas dans le mécanisme de mesure d'une distance, mais, comme nous le verrons par la suite, dans l'exploitation de ces mesures de distances. La mesure de distance peut être effectuée de différentes façons, y compris celles comprises dans l'état de la technique.
Dans le cadre du protocole IEEE 802.15.4a, les méthodes de mesure de distance sont basées sur le principe de mesure du temps de vol (« Time of Arrivai ») nécessaire à un message pour aller d'un module de communication à un autre.
Ainsi, la méthode « Time Of Arrivai » (TOA), appelée en français temps de vol, est une méthode très simple qui permet de calculer la distance entre une station mobile et une station de base.
Pour la généralité de l'exposé, on utilise dans les paragraphes suivants les expressions « station mobile » et « station de base », qui peuvent représenter dans le cadre de l'invention, respectivement, le véhicule suiveur B et la cible à suivre A. Il est à noter que la cible peut elle-même être mobile et que ces termes « station mobile » et « station de base » doivent être compris dans un sens relatif : la station mobile est mobile dans le référentiel de la station de base (dont le référentiel peut lui-même être mobile par rapport au référentiel terrestre par exemple).
Les mécanismes exposés sont illustrés par les figures 3a, 3b, 3c, qui schématisent les échanges protocolaires entre deux stations A, B, en vue de la mesure de distance entre elles.
La méthode « Time Of Arrivai » (TOA) est illustrée par la figure 3 a.
La station mobile A envoie un message RFRAME à la station de base B à la date d'émission te, cette date étant transmise dans le message. La station de base reçoit le message et note la date d'acquisition ta.
La distance d qui sépare la station de base de la station mobile est alors donnée par l'expression
d = C.(te-ta)
dans laquelle c est la vitesse de la lumière. Le principal inconvénient est qu'il faut que les horloges des deux stations soient parfaitement synchronisées pour obtenir une mesure très précise. Cela demande une infrastructure très difficile à mettre en œuvre.
Pour s'affranchir des problèmes de synchronisation des horloges entre les stations de base et les stations mobiles, la méthode « Two Way Time Of Arrivai » a été proposée. Celle-ci est illustrée par la figure 3b. Elle consiste à faire dialoguer la station de base et la station mobile de la manière suivante :
La station de base A envoie une requête RFRrame_req à une station mobile B et enregistre la date d'émission te de ce message,
La station mobile répond à la requête envoyant le message RFRrame rep à la station de base,
La station de base reçoit le message de réponse et enregistre la date de réception ta.
La station de base A calcule le temps écoulé durant cet échange par l'expression Tr=te-ta, et détermine la distance d qui sépare la station de base de la station mobile par d=c.Tr/2
Les stations de base et mobiles sont donc à la fois émettrices et réceptrices. Le temps Tr étant mesuré par la station de base A, il n'y a pas besoin de synchroniser leurs horloges. Néanmoins, la réponse de la station mobile ne peut être immédiate. En effet, il faut qu'elle décode le message de requête envoyé par la station de base, créée un message de réponse et enfin envoie ce dernier. Ce processus introduit un retard T?„ et biaise la mesure de distance. Par exemple, une erreur de quelques nanosecondes introduit des erreurs de l'ordre du décimètre. Il est donc important de pouvoir estimer très précisément ce retard.
Il est possible d'utiliser une technique plus élaborée pour obtenir l'estimation de T et donc obtenir une mesure plus précise de la distance. Cette fois le dialogue entre la station de base et la station mobile est le suivant :
La station de base A envoie une requête RFRrame req à la station mobile B et enregistre la date d'émission du dernier octet du SFD (« Start Frame Delimeter ») du message RFRrame req, La station mobile B répond à la requête envoyant le message RFRrame rep à la station de base, en parallèle elle lance un compteur dès que le dernier octet du SFD du message RFRrame rep est lu et l'arrête quand le dernier octet du SFD du message RFRrame rep est envoyé ;
La station de base reçoit le message de réponse et enregistre la date de réception du dernier octet du SFD du message RFRrame rep,
La station mobile envoie un second message à la station de base contenant la valeur du T estimée à l'aide du compteur,
La station de base reçoit le message contenant l'estimation du retard a ,
La station de base envoie un accusé de réception à la station mobile.
Cette fois, il est possible d'obtenir une mesure de distance plus précise en calculant :
Figure imgf000012_0001
Une source majeure d'erreur dans la méthode TW-TOA est le décalage de fréquence d'horloge entre la station de base et la station mobile. Les horloges embarquées dans ces modules utilisent des quartz qui ne fonctionnent pas exactement à la même fréquence. Ainsi des retards ou "des avances" apparaissent dans la mesure du temps de vol qui, multipliés par la vitesse de la lumière, peuvent introduire des erreurs conséquentes dans la mesure de la distance. La méthode « Symétrie Double Sides Two Way Time Of Arrivai » (SDS-TW-ToA) permet de résoudre ce problème. Une telle technique est illustrée par la figure 3c.
L'idée est cette fois d'estimer aussi le temps T nécessaire à la station de base pour décoder le message RFRAMErep provenant de la station mobile et de réémettre un second message RFRAMEreq à la station mobile. Ainsi la mesure de distance dsDs est la suivante :
Figure imgf000013_0001
Pour montrer l'apport de cette méthode, on définit les erreurs de fréquences eA et eB des horloges embarquées dans la station de base et dans la station mobile, telles que :
¾ = NfA
et
Figure imgf000013_0002
où RÎA et Nfk. représentent respectivement les fréquences réelle et nominale de chacune des horloges. En les introduisant dans les équations précédentes, on obtient
. _ J Ç 1 + ¾ ) - 7 ( 1 + ¾ )
— c ^
Figure imgf000013_0003
Dans le cadre du suivi de piéton, la mesure de distance est de quelques dizaines de mètres au plus, Tr ne dépassera pas les 100 nanosecondes. En revanche, le temps Tta nécessaire pour traiter le message de requête et lui répondre est de l'ordre de la milliseconde. Cela signifie que le temps de transmission est bien inférieur au temps de traitements des données qui représente la majeure partie de l'erreur sur la mesure de distance : Tr A (_l + eA) - Tt B a(_l + eB)
έ TW c
â, = C
Nous pouvons donc voir que l'erreur due au décalage de fréquence des horloges est compensée avec la méthode SDS-TW-TOA si
Figure imgf000014_0001
Selon l'invention, les modules de communication SI, S2, S3 peuvent mettre en œuvre de telles mécanismes de mesure de distance. Concrètement, ces mécanismes peuvent être mis en œuvre à l'aide de modules de communication disponibles sur le marché et que le procédé et le dispositif selon l'invention peuvent utiliser.
De tels modules disponibles sur le marché peuvent notamment être ceux de la société Decawave, et plus particulièrement le capteur DW1000. Ce capteur est conforme au protocole de communication IEEE 802.15.4a précédemment évoqué et fonctionne dans une gamme de fréquence de 3.5 GHz à 6.5 GHz avec une largeur de bande de 1 GHz. Il permet la mesure de distance avec une précision de 10 cm. Ce capteur présente entre autres avantages ceux d'avoir un relativement faible coût et d'être de faible dimension, par exemple 23 mm x 13 mm pour le modèle DWM1000.
La figure 2 schématise le principe de la localisation par trilatération.
Le problème que l'on cherche à résoudre est de localiser le module de communication S3 de coordonnées (x, y) en connaissant les localisations des modules de communication SI et S2, respectivement de coordonnées (xl, yl) et (x2, y2) et les distances mesurées dl , d2 entre le module de communication S3 et, respectivement, les modules de communication SI et S2. La position du module S3 étant le point d'intersection entre deux cercles centrés sur chacun des modules SI, S2 et de rayon di et d2, ce problème peut être très facilement résolu de manière analytique.
Soit :
(d 2 = (x - x1)2 + (y - yï)2 = x2 + y2 et
(_d2)2 = (_x - x 2) 2 + y 2
La position du module S3 par rapport celle du module SI est alors :
Figure imgf000015_0001
et
y = ±V(¾2 - 002
La position du module S3 dans le référentiel du "monde" étant :
x = \ + x1
y = y + i
Il y a bien sûr deux solutions à ce problème, le signe de l'ordonnée y doit être déterminé soit par une contrainte de l'utilisateur soit en ajoutant un troisième module de communication sur le véhicule suiveur.
Ainsi, théoriquement il est possible d'estimer la localisation du module de communication S 3 en toutes circonstances.
Cependant en pratique, à cause des bruits de mesure, il s'avère que dans certaines circonstances il n'existe aucune intersection entre les deux cercles et donc aucune solution au problème de localisation.
En outre, les bruits de mesure introduisent des discontinuités très importantes dans le temps. En effet, les bruits de mesure introduisent de fortes variations dans les résultats de localisation, surtout sur la mesure du cap. Ces variations sont brusques, d'où les discontinuités, mais la notion de discontinuité peut être omise. Pour donner un exemple, dans le cadre d'essais expérimentaux effectués par la demanderesse, quand les véhicules sont statiques, l'estimation du cap peut varier de plus ou moins 5° d'une mesure à l'autre à cause des bruits de mesures sur les distances. Le filtre de Kalman mis en œuvre par un mode de réalisation de l'invention permet d'atténuer ce problème tout en fournissant une solution originale aux problèmes de synchronisation.
Par ailleurs, et surtout, les mesures de distance dl, d2 ne sont pas fournies de façon synchrones, alors que les mécanismes décrits ci-dessus supposent une synchronisation des mesures. Comme nous le verrons plus loin, ce phénomène amplifie encore davantage les erreurs sur l'estimation du cap.
D'une façon générale, il est difficile de prévoir une synchronisation des flux de messages transmettant les mesures de distance, mais en outre le module de communication S3 dispose généralement d'une interface de communication ne lui permettant de dialoguer qu'alternativement avec les modules de communication SI, S2. Autrement dit, les mesures de distance dl, d2 sont obligatoirement non synchronisées.
Par exemple, le module S3 de la cible mobile dialogue avec un premier module SI du véhicule suiveur pendant une certaine durée, par exemple 5 ms. On peut par ailleurs fixer une fréquence de mesure à 50 Hz. Dans une telle situation, les mesures sont espacées d'au moins 20 ms.
Or, le véhicule suiveur est mobile et par conséquent entre l'instant tl auquel la mesure de distance dl est déterminée et l'instant t2 auquel la mesure de distance d2 est déterminée, il se sera déplacé d'une distance déterminée par la vitesse du véhicule, de sa direction et de la période inter- mesures.
Par conséquent, les mesures de distance dl et d2 ne correspondent pas à une même position du véhicule suiveur et les méthodes de trilatération ne peuvent plus fonctionner de manière satisfaisante. Le procédé selon l'invention prévoit des étapes de : détermination d'au moins une première mesure de distance dl entre la cible mobile A et un premier emplacement dans (ou sur) le véhicule suiveur B, correspondant typiquement à un premier module de communication SI, prise en un premier instant tl, détermination d'une seconde mesure de distance d2 entre la cible mobile A et un second emplacement S2 dans (ou sur) le véhicule suiveur B, correspondant typiquement à un second module de communication S2, prise en un second instant t2,
détermination d'une prévision du déplacement du véhicule suiveur entre les premier et second instants, tl, t2 et
détermination d'une localisation de la cible mobile B par rapport au véhicule suiveur A à partir de ces première et seconde mesures de distance, dl, d2, en prenant en compte la prévision de déplacement, de sorte à compenser le mouvement du véhicule suiveur entre les premier et second instants.
La détermination du déplacement peut typiquement être faite à partir de la mesure de vitesse linéaire du véhicule suiveur et de l'orientation de son train directeur. Préférentiellement, ces mesures sont fournies par les organes de contrôle du véhicule suiveur notamment par les capteurs proprioceptifs embarqués sur ce dernier.
On appelle capteurs proprioceptifs ou capteurs de proprioception les capteurs de mesures sur l'état du véhicule lui-même. Ils s'opposent aux capteurs sur les informations extérieures. Un exemple de capteur proprioceptif est un capteur de vitesse. Ce terme est communément compris par l'homme du métier ainsi que l'atteste la page wikipedia consacrée à la robotique : https://en.wikipedia.org/wiki/Autonomous_robot
Ainsi, en prévoyant la localisation du véhicule suiveur, le procédé selon l'invention peut prendre en compte son déplacement estimé afin de le compenser. Les données de mesure dl, d2 peuvent alors être exploitées de façon valide, et précise malgré leur asynchronisme. Il en va évidemment de même si plus de deux données de mesure sont fournies.
Différents procédés peuvent être mis en place pour prendre en compte Γ asynchronisme des mesures de distances ainsi que la précision des données dl et d2. Ces procédés font appel à des techniques de filtrage dont les caractéristiques permettent d'intégrer :
1) Γ asynchronisme des données issues des balises
2) le déplacement du véhicule entre les temps tl et t2
3) l'incertitude de déplacement du véhicule entre les temps tl et t2
4) l'incertitude associée aux mesure dl et d2
Parmi les techniques de filtrage les plus répandues les filtres de Kalman et les filtres particulaires sont souvent les plus efficaces mais d'autres seraient tout à fait applicables pour peu qu'elles intègrent les caractéristiques précédemment citées.
Selon un mode de réalisation préférentiel, un filtre de Kalman est utilisé pour déterminer la prévision de déplacement et la localisation de la cible mobile par rapport au véhicule suiveur. Ce filtre de Kalman permet d'inférer et filtrer la localisation à tout instant.
Le vecteur d'état du filtre de Kalman représente les paramètres que l'on veut estimer. Dans le cadre de l'invention, le vecteur d'état Xk du filtre de Kalman reflète la position (x,y) de la cible mobile A à l'instant k tel que
xk = Γ¾Ί
J
Par construction, le filtre de Kalman estime également la précision de l'estimation à tout instant. Celle-ci est représentée par la matrice de covariance Qk.
Sachant que les mesures de distance entre la cible mobile A et le véhicule suiveur B sont asynchrones, l'une après l'autre, chaque mesure dn,k est comparée à sa mesure a priori dn,kk i et la localisation de la cible mobile est mise à jour en fonction de leur différence. A chaque nouvelle mesure de distance provenant du module de communication Sn (avec n=l ou n=2, dans le cas de deux modules), l'équation de mise à jour du filtre de Kalman à l'instant k est la suivante :
Figure imgf000019_0001
dans laquelle xn et yn sont les coordonnées de la localisation de l'emplacement du module de communication Sn sur le véhicule suiveur.
La mise à jour a posteriori de l'état est effectuée par les équations traditionnelles du filtre de Kalman :
Figure imgf000019_0002
dans lesquelles <5d est l'écart type sur les mesures de distances. La
Jacobienne de la fonction d'observation est décrite par :
Figure imgf000019_0003
Expérimentalement, on peut montrer que, dans certaines circonstances, le filtre de Kalman peut considérer plus probables des localisations avec une ordonnée négative que celles avec une ordonnée positive. Pour pallier ce problème, on peut mettre en œuvre une estimation sous contrainte, afin d'imposer que l'ordonnée des solutions trouvées soit toujours positive : yk/k-i>yn, où yn est l'ordonnée des modules Si, S2 du véhicule suiveur. Cette hypothèse implique que le référentiel est choisi de sorte que les deux modules Si, S2 se situent sur une droite parallèle à l'axe x.
Il existe de nombreuses méthodes pour appliquer une telle contrainte dans un filtre de Kalman. On peut, à titre d'exemples, citer celles exposées dans les articles « Kalman filtering with state constraints: a survey ofi linear and nonlinear algorithms » de D. Simon, in IET Contril Theory and applications, 1303-1318, 2010, et « Constrained Kalman filtering via density function truncation for turbofan engine health estimation" de Dan Simon et Donald L. Simon, in Int. J. Systems Science, 41(2), 159-171,
2010. Afin d'utiliser la technologie des filtres de Kalman pour la localisation de la cible par rapport au véhicule suiveur, nous devons définir les nouvelles variables suivantes :
(x , y ) est la localisation du véhicule suiveur dans le référentiel du monde ;
est l'orientation du véhicule suiveur dans le référentiel du monde ; (χρ , γ™) est la localisation de la cible dans le référentiel du monde ; ( ^, j ) est la localisation de la cible dans le référentiel du véhicule suiveur. On peut définir ce référentiel centré sur le milieu de l'essieu arrière du véhicule, par exemple.
Le modèle cinématique du véhicule suiveur doit ensuite être défini. Un exemple d'un tel modèle cinématique peut être :
Figure imgf000020_0001
dans lequel
vr est la vitesse linéaire du véhicule suiveur ;
δΓ est l'orientation du train directeur, et,
L est la voie du véhicule suiveur.
On appelle « voie » la distance entre les roues droite et gauche d'un même essieu.
Il est alors possible de définir le modèle dynamique simple, dit modèle d'Ackermann :
Figure imgf000020_0002
dans lequel
et
Δ Θ = v, Δτ est le temps écoulé entre les instants k et k-1, c'est-à-dire la période d'échantillonnage.
Les équations ci-dessus décrivent le déplacement du véhicule suiveur dans le monde. Or, notre problème est de connaître le déplacement de la cible dans le repère du véhicule suiveur.
On définit ( ^, j )k la localisation de la cible dans le référentiel du véhicule suiveur à l'instant k. On suppose pour le moment que la cible est immobile. A l'instant k+1, la localisation de la cible dans le référentiel du véhicule suiveur
Figure imgf000021_0001
En utilisant le système d'équations précédent, nous obtenons :
■
Figure imgf000021_0002
où R(Ae) est une matrice de rotation 2D, fonction de l'angle ΔΘ.
Toutefois, bien évidemment, la cible peut être mobile, et entre deux instants elle peut se déplacer dans le référentiel du monde.
Nous pouvons écrire :
Figure imgf000021_0003
et l'injecter dans l'équation précédente afin d'obtenir l'équation décrivant la localisation d'une cible mobile par rapport à un véhicule suiveur :
Figure imgf000021_0004
dans laquelle vxy suit une loi normale centrée à deux dimensions N ( ,Qxy) dont la matrice de covariance Qxy est définie par l'équation
Figure imgf000021_0005
avec στ l'écart-type du déplacement pouvant être effectué par la cible en une seconde, et ΔΓ le temps écoulé (en secondes) entre les instants k et k+1.
En considérant maintenant que le référentiel de localisation est celui du véhicule suiveur, on peut définir que la localisation ( ^, j ) est équivalente à la localisation (x,y) précédemment utilisée.
Il est alors possible d'estimer la localisation de la cible à l'aide du filtre de Kalman avec contraintes en utilisant l'équation ci-dessus.
Les équations de prédiction du filtre de Kalman ainsi défini deviennent :
Figure imgf000022_0001
Pklk_x = + (¾&(¾ Δβ) + Qxy
où Qu est la matrice de covariance associée aux incertitudes des informations proprioceptives provenant du véhicule suiveur, telle que :
Figure imgf000022_0002
- Ar sin(Ae / 2) - 0.5Δΰ cos(Ae 12
k ~ Ar cos(Ae / 2) - 0.5Δΰ 8ίη(Δθ / 2)
avec a et σδ 2 données par les caractéristiques des capteurs odométriques du véhicule. Ces grandeurs sont donc à déterminer pour chaque véhicule.
Connaissant la localisation de la cible par rapport à laquelle le véhicule doit s'asservir, il est possible de déterminer une commande visant à adapter la vitesse et la direction du véhicule suiveur. Il est en outre possible d'imposer une distance de consigne entre le véhicule suiveur et la cible.
Différents mécanismes sont possibles pour exploiter la localisation de la cible déterminée par le procédé selon l'invention, et permettant de définir des comportements donnés du véhicule suiveur. Selon une mise en œuvre particulière, on détermine une commande afin que le véhicule suiveur se dirige vers la cible tout en respectant une inter-distance de consigne pc.
Dans le cas d'un véhicule suiveur ayant un train directeur et un train propulseur, le vecteur de commande est composé de la vitesse au centre de l'essieu arrière vr et de l'orientation du train directeur δΓ. Considérons la location (p, Θ) de la cible à suivre dans un système de coordonnées polaires tel que :
Figure imgf000023_0001
Celle-ci décrit respectivement l'inter-distance mesurée entre le véhicule suiveur et la cible, ainsi que l'orientation que le véhicule suiveur doit suivre pour venir se placer derrière la cible. Pour que le véhicule suive la cible, il faut trouver le vecteur de commande qui assure à la fois que p tende vers pc et que Θ tende vers 0. Le problème est en général traité séparément. On peut corriger le cap du véhicule suiveur en utilisant un correcteur proportionnel :
Figure imgf000023_0002
dans lequel Kpe est le gain proportionnel du correcteur. On peut réguler l'inter-distance entre le véhicule B et la cible A à l'aide d'un correcteur proportionnel intégral :
k
Vr,t = KPP (A " Pc ) + Kip∑ ( - Pc )
:=0
dans lequel Kpp et Kip sont respectivement les gains proportionnel et intégral du correcteur. L'équation
Figure imgf000023_0003
montre que pour corriger le cap du véhicule, il suffit d'orienter les roues du train directeur du véhicule vers la cible. L'équation précédente montre qu'une action intégrale est nécessaire pour réguler la vitesse linéaire du véhicule afin que l'inter- distance soit toujours respectée, c'est-à-dire que la quantité pk -pc tende vers 0. On remarque que si la cible se dirige vers le véhicule, la quantité pk - pc devient négative et provoquera le recul du véhicule car la vitesse vr,k deviendra également négative.
On peut souhaiter empêcher le véhicule de reculer, notamment pour 5 des raisons de sécurité. Auquel cas, il est possible, selon un mode de réalisation, d'introduire la contrainte suivante : si pk > pc - 3ap k
Figure imgf000024_0001
où Cr,k est Γ écart-type sur la mesure de distance pk séparant le véhicule et la cible tel que :
1 l0J σ up2,k = J T rho1 P kJ rho
avec
Figure imgf000024_0002
Et Pk est la matrice fournie à l'instant k par le module de localisation de la cible. Cette matrice peut être déduite a posteriori ou a priori selon si la commande est effectuée en même temps que la mise à jour du filtre de Kalman ou non.
La description qui précède concerne une mise en œuvre dans laquelle le véhicule suiveur dispose de 2 modules de communication Si, S2, mais il est également possible de considérer plus de 2 modules de communication. 0
Bien entendu, la présente invention n'est pas limitée aux exemples et au mode de réalisation décrits et représentés, mais elle est susceptible de nombreuses variantes accessibles à l'homme de l'art. 5

Claims

REVENDICATIONS
1. Procédé de localisation d'une cible mobile (A) par un véhicule suiveur (B), comportant la détermination d'au moins une première mesure de distance (dl) entre ladite cible mobile (A) et un premier emplacement (SI) sur ledit véhicule suiveur, prise en un premier instant, et d'une seconde mesure de distance (d2) entre ladite cible mobile (A) et un second emplacement (S2) sur ledit véhicule suiveur, prise en un second instant, caractérisé en ce que ledit procédé détermine une prévision du déplacement dudit véhicule suiveur entre lesdits premier et second instants, et détermine une localisation de ladite cible mobile par rapport au véhicule suiveur à partir desdites première et seconde mesures de distance, en prenant en compte ladite prévision, de sorte à compenser ledit déplacement entre lesdits premier et second instants.
2. Procédé selon la revendication précédente, dans lequel ladite prévision et ladite localisation sont déterminés par un filtre de Kalman.
3. Procédé selon la revendication précédente, dans lequel ladite prévision du déplacement est déterminée à partir de mesure de vitesse linéaire et de l'orientation du train directeur dudit véhicule suiveur.
4. Procédé selon la revendication précédente, dans lequel les équations de prédiction dudit filtre de Kalman, entre un instant k et un instant k-1 sont
Figure imgf000025_0001
Pklk_x =
Figure imgf000025_0002
+ GkQuGk T)R(Ae) + Q dans lesquelles Qu est la matrice de covariance associée aux incertitudes des informations proprioceptives provenant dudit véhicule suiveur, telle que :
Figure imgf000026_0001
et
^- Ar sin(Ae / 2) - 0.5Δΰ cos(Ae / 2)>
Ar cos(Ae / 2) - 0.5Δΰ 8ίη(Δθ / 2)
avec
v, tAr , et
Figure imgf000026_0002
Δτ est le temps écoulé entre les instants k et k-1.
vr,k est la vitesse linéaire dudit véhicule suiveur à l'instant k ;
ôr,k est l'orientation du train directeur dudit véhicule suiveur à l'instant k, et,
L est la voie du véhicule suiveur.
5. Procédé selon l'une des revendications précédentes, comportant en outre une étape de détermination d'une commande visant à adapter la vitesse et la direction dudit véhicule suiveur (B) afin de le diriger en fonction de ladite location localisation de ladite cible (A) en respectant une distance de consigne.
6. Dispositif de localisation d'une cible mobile (A) pour un véhicule suiveur (B), comportant un premier module de communication (SI) en un premier emplacement sur ledit véhicule suiveur (B) pour déterminer une première mesure de distance (dl) entre ledit premier emplacement et ladite cible mobile (A) en un premier instant, et un second module de communication (S2) en un second emplacement sur ledit véhicule suiveur (B) pour déterminer une seconde mesure de distance (d2) entre ledit second emplacement et ladite cible mobile (A) en un second instant, et un module de calcul pour déterminer une prévision du déplacement dudit véhicule suiveur entre lesdits premier et second instants, et déterminer une localisation de ladite cible mobile par rapport audit véhicule suiveur à partir desdites première et seconde mesures de distance, en prenant en compte ladite prévision, de sorte à compenser ledit déplacement entre lesdits premier et second instants.
7. Dispositif de localisation selon la revendication précédente, dans lequel ledit module de calcul détermine ladite prévision du déplacement du robot suiveur et ladite localisation de la cible par un filtre de Kalman.
8. Dispositif de localisation selon la revendication précédente, dans lequel ledit module de calcul détermine ladite prévision du déplacement à partir des mesures proprioceptives dudit véhicule suiveur.
9. Véhicule suiveur (B) comportant un dispositif selon l'une des revendications 6 à 8.
10. Système comportant un véhicule suiveur (B) selon la revendication précédente, et un module de communication équipant ladite cible mobile (A).
PCT/FR2017/050381 2016-02-22 2017-02-21 Localisation d'une cible pour véhicule suiveur WO2017144808A1 (fr)

Priority Applications (2)

Application Number Priority Date Filing Date Title
EP17710337.1A EP3420373A1 (fr) 2016-02-22 2017-02-21 Localisation d'une cible pour véhicule suiveur
US16/078,775 US20190137617A1 (en) 2016-02-22 2017-02-21 Location of a target by a tracking vehicle

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
FR1651429A FR3048148B1 (fr) 2016-02-22 2016-02-22 Localisation d'une cible pour vehicule suiveur
FR1651429 2016-02-22

Publications (1)

Publication Number Publication Date
WO2017144808A1 true WO2017144808A1 (fr) 2017-08-31

Family

ID=56372953

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/FR2017/050381 WO2017144808A1 (fr) 2016-02-22 2017-02-21 Localisation d'une cible pour véhicule suiveur

Country Status (4)

Country Link
US (1) US20190137617A1 (fr)
EP (1) EP3420373A1 (fr)
FR (1) FR3048148B1 (fr)
WO (1) WO2017144808A1 (fr)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA3037458A1 (fr) * 2016-09-21 2018-03-29 Nissan Motor Co., Ltd. Procede de commande de deplacement et dispositif de commande de deplacement de vehicule
KR102038317B1 (ko) * 2017-11-21 2019-10-30 주식회사 티티엔지 정확한 위치정보를 기반으로 자율주행이 가능한 골프카트 시스템 및 그 시스템을 이용한 골프카트 제어방법
CN111061228B (zh) * 2018-10-17 2022-08-09 长沙行深智能科技有限公司 基于目标追踪的货箱自动转运控制方法
FR3094256B1 (fr) * 2019-03-29 2021-07-02 Norcan robot motorisé amélioré
EP3865967A1 (fr) * 2020-02-11 2021-08-18 Cart Technology, S.L. Système de suivi basé sur un chariot
CN112017171B (zh) * 2020-08-27 2021-10-26 四川云从天府人工智能科技有限公司 一种图像处理指标评估方法、系统、设备和介质
CN113008222B (zh) * 2021-02-20 2023-03-31 西北工业大学 一种基于连续时间轨迹函数的航迹约束目标跟踪方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5810105A (en) * 1995-03-15 1998-09-22 Technologietransfer-Anstalt Tetra Ultrasonic tracking control for an automotive vehicle
US6327219B1 (en) * 1999-09-29 2001-12-04 Vi&T Group Method and system for directing a following device toward a movable object
EP1708065A2 (fr) * 2005-03-31 2006-10-04 Deere & Company Procédé et système de suivi d'un véhicule de tête
EP2590041A2 (fr) * 2011-11-07 2013-05-08 Willem-Dirk Hendrik Udo Véhicule de suivi de cible

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5810105A (en) * 1995-03-15 1998-09-22 Technologietransfer-Anstalt Tetra Ultrasonic tracking control for an automotive vehicle
US6327219B1 (en) * 1999-09-29 2001-12-04 Vi&T Group Method and system for directing a following device toward a movable object
EP1708065A2 (fr) * 2005-03-31 2006-10-04 Deere & Company Procédé et système de suivi d'un véhicule de tête
EP2590041A2 (fr) * 2011-11-07 2013-05-08 Willem-Dirk Hendrik Udo Véhicule de suivi de cible

Also Published As

Publication number Publication date
US20190137617A1 (en) 2019-05-09
FR3048148A1 (fr) 2017-08-25
FR3048148B1 (fr) 2018-12-07
EP3420373A1 (fr) 2019-01-02

Similar Documents

Publication Publication Date Title
EP3420373A1 (fr) Localisation d&#39;une cible pour véhicule suiveur
KR102210271B1 (ko) 차량 위치 결정을 위한 기술
EP2922040B1 (fr) Pilotage de vehicules en convoi
EP2626723B1 (fr) Procédé d&#39;estimation de la direction d&#39;arrivée de signaux de navigation sur un récepteur après réflexion par des parois dans un système de positionnement par satellite
WO2010063844A1 (fr) Procede de geo-localisation d&#39;un objet par multitelemetrie
EP3388914A1 (fr) Procédé de suivi de cible par un drone, programme d&#39;ordinateur, système électronique et drone associés
EP3060881A1 (fr) Procede de localisation en interieur et exterieur et dispositif portatif mettant en oeuvre un tel procede.
EP3126864B1 (fr) Procédé de géo-localisation de l&#39;environnement d&#39;un porteur
WO2010020625A1 (fr) Procédé et un dispositif de commande, à distance, d&#39;une caméra embarquée dans une station mobile
FR3100895A1 (fr) Procédé et système de positionnement automatique de drones en essaim
WO2023094347A1 (fr) Système et procédé de navigation autonome base vision d&#39;un satellite
FR3064073A1 (fr) Procede et dispositif de determination d&#39;une position
FR3043467B1 (fr) Procede et systeme de geolocalisation d&#39;une balise par horodatage
EP2626725B1 (fr) Procédé de détermination d&#39;un indicateur de confiance relatif à la trajectoire empruntée par un mobile
FR3077549A1 (fr) Procede de determination de la trajectoire d’un vehicule automobile en absence de marquage au sol.
EP4184208B1 (fr) Procédé de localisation d&#39;un véhicule
FR3075949A1 (fr) Procede de determination sur une distance d’anticipation de la trajectoire d’un vehicule automobile.
FR3079608A1 (fr) Procede de geolocalisation d&#39;un drone, systeme de geolocalisation et produit programme d&#39;ordinateur associes
WO2024009048A1 (fr) Procede de recalage d&#39;une attitude fournie par un systeme de navigation a l&#39;estime au moyen d&#39;un systeme de positionnement relatif
FR3041766A1 (fr) Procede de localisation passive d&#39;emetteurs electromagnetiques au sol, utilisation, et dispositif associe
FR3039342A1 (fr) Procede et dispositif pour localiser des mobiles se deplacant en suivant une trajectoire predetermine
EP4024089A1 (fr) Procédé pour la détermination de la position d&#39;un leurre à partir d&#39;au moins un récepteur
FR3129216A1 (fr) Procédé d’estimation de la vitesse d’un véhicule ferroviaire et centrale inertielle associée
EP4184193A1 (fr) Procédé de détermination d&#39;une distance corrigée
WO2019201845A1 (fr) Dispositif de synchronisation d&#39;une position primaire d&#39;un objet mobile avec une position secondaire

Legal Events

Date Code Title Description
NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 2017710337

Country of ref document: EP

ENP Entry into the national phase

Ref document number: 2017710337

Country of ref document: EP

Effective date: 20180924

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 17710337

Country of ref document: EP

Kind code of ref document: A1