Method, device and computer program for estimating a speed of a wheeled vehicle
The present invention relates to a method for estimating a speed (v) of movement of a wheeled vehicle, wherein a frequency trajectory (ξ t ) representative of the speed (v) of a wheel (RO) of the vehicle (VE) in a filtered spectrogram (S(f t , t)) is estimated (E 5 ) as follows: a probability (p Zt|ξt ) of observation of the trajectory (ft) is estimated (E 51 ) on the basis of a computed amplitude of the filtered spectrogram (S(f t , t)), an a posteriori observation law (p Zt|ξt ) proportional to the product of the probability (p ξ ) and of the probability (p ξ|Z) ′ is estimated (E 52 ), the trajectory (ft) is estimated (E 53 ) on the basis of the law (p ξ|Z ), and the speed (v) of movement of the wheel of the vehicle is estimated (E 6 ) on the basis of the trajectory (ξ t ).
1 . A method for estimating a travel speed of a vehicle having at least one wheel for rolling on a ground, comprising the following steps:
measuring a signal being inertial or vibratory by at least one inertial or vibratory sensor of the vehicle,
calculating by at least one calculator a plurality of spectra of the signal over a plurality of successive time windows defined with respect to instants being successive, the plurality of spectra forming at least one spectrogram as a function of the instants,
filtering the at least one spectrogram by at least one noise attenuation and/or deterministic feature extraction filter, to obtain at least one filtered spectrogram of the signal on the instants,
estimating by the at least one calculator a frequency trajectory representative of the travel speed of the at least one wheel of the vehicle in the at least one filtered spectrogram on the instants, by:
estimating by the at least one calculator a probability of observation of the frequency trajectory from a calculated amplitude of the at least one filtered spectrogram for each instant,
estimating by the at least one calculator an a posteriori law of observation of the frequency trajectory as being proportional to a product of a prescribed probability of transition of the frequency trajectory and of the probability of observation of the frequency trajectory,
estimating by the at least one calculator the frequency trajectory for the instants from the a posteriori law of observation of the frequency trajectory,
estimating by the at least one calculator the travel speed of the at least one wheel of the vehicle for the instants from the frequency trajectory, that has been estimated,
controlling real-time navigation of the vehicle by an inertial navigation device of the vehicle using the estimated travel speed.
2 . The method according to claim 1 , comprising determining by the at least one calculator a support of frequency values, estimating by the at least one calculator the frequency trajectory as being for each instant in the support the frequency value that maximizes the a posteriori law of observation of the frequency trajectory until this instant.
3 . The method according to claim 1 , comprising determining by the at least one calculator a support of frequency values, estimating by the at least one calculator a frequency for each instant as being a barycenter of the a posteriori law of observation of the frequency, taken at the frequency values of the support and subjected to these frequency values.
4 . The method according to claim 1 , comprising determining by the at least one calculator a support 2 of frequency values, estimating by the at least one calculator the probability of observation knowing the frequency trajectory as dependent on the calculated amplitude of the at least one filtered spectrogram being normalized for each instant.
5 . The method according to claim 4 , comprising estimating by the at least one calculator the probability p Z t |ξ t of observation is according to the following equation:
p Z t |ξ t =f t =( z t )=| S ( f t ,t )|·1 [f min (t)/,f max (t)] ( f t )
where |S(f t , t)| is the calculated amplitude of the at least one spectrogram S(f t , t) filtered for each instant t,
f min (t) is a minimum frequency trajectory that has been measured,
f max (t) is a maximum frequency trajectory that has been measured,
f min (t) and f max (t) are such that f min (t)≤ξ t ≤f max (t),
1 [f min (t),f max (t)] (f t ) is a function taking the value 1 for f t between f min (t) and f max (t) and taking the value 0 for f t below f min (t) or above f max (t).
6 . The method according to claim 4 , comprising estimating by the at least one calculator the probability p Z t |ξ t of observation according to the following equation:
p
Z
t
❘
"\[LeftBracketingBar]"
ξ
t
=
f
t
(
z
t
)
=
∏
j
❘
"\[LeftBracketingBar]"
j
·
f
t
<
F
s
2
1
c
t
❘
"\[LeftBracketingBar]"
S
(
j
·
f
t
,
t
)
❘
"\[RightBracketingBar]"
1
[
f
min
(
t
)
,
f
max
(
t
)
]
(
f
t
)
where j is a positive integer or real number, satisfying
j
·
f
t
<
F
s
2
where f t denotes the frequencies of each at least one filtered spectrogram S(f t , t) at the instant t,
and where F S is a threshold frequency that has been predetermined as a function of the at least one filtered spectrogram S(f t , t).
7 . The method according to claim 1 , comprising determining by the at least one calculator a support Ω of frequency values,
prescribing by the at least one calculator the prescribed probability p ξ (f 1 , . . . , f N s ) of transition of the frequency trajectory ξ t for the instant N S starting from a given initial probability p ξ 1 of the instant 1, for t∈{1, . . . , N S } and for each frequency value f t ∈Ω according to the following equation:
p
ξ
(
f
1
,
…
,
f
N
s
)
=
p
ξ
1
(
f
1
)
∏
t
=
1
N
s
-
1
p
ξ
t
+
1
❘
"\[LeftBracketingBar]"
ξ
t
=
f
t
(
f
t
+
1
)
where N S is a prescribed integer, greater than or equal to two, of the instants t being prescribed,
where p ξ t+1 |ξ t =f t (f t+1 ) is the prescribed probability of transition from a frequency ξ t of the frequency trajectory of the instant t being present to a frequency ξ t+1 of the frequency trajectory of the instant t+1 being next.
8 . The method according to claim 7 , wherein the prescribed probability p ξ t+1 |ξ t =f t (f t+1 ) of transition from the frequency ξ t of the frequency trajectory of the instant t to the frequency ξ t+1 of the frequency trajectory of the instant t+1 follows a conditional Gaussian law centered on the frequency f t of the at least one filtered spectrogram at the instant t.
9 . The method according to claim 8 , wherein the conditional Gaussian law centered on the frequency f t has a standard deviation σ t =Δt·γ t where Δt is a calculated time resolution of the at least one spectrogram of the instant t, γ t is a calculated average speed with which the frequency trajectory ξ t evolves between the instant t and the instant t+1.
10 . The method according to claim 8 , wherein the noise attenuation and/or deterministic feature extraction filter comprises a sliding median filter and/or a deterministic feature separation filter with respect to random features.
11 . The method according to any claim 1 , comprising estimating by the at least one calculator the travel speed of the at least one wheel of the vehicle by multiplying the frequency trajectory by 2πR, where R is a radius of the at least one wheel and is predetermined or estimated.
12 . A non-transitory computer readable medium containing computer instructions stored therein for causing a computer processor to perform the method for estimating the travel speed of the vehicle according to the claim 1 .
13 . A device for estimating a travel speed of a vehicle having at least one wheel for rolling on a ground, wherein the device comprises:
an inertial or vibratory sensor of the vehicle configured to measure a signal being inertial or vibratory,
a calculator configured to calculate a plurality of spectra of the signal over a plurality of successive time windows defined with respect to instants being successive, the plurality of spectra forming at least one spectrogram as a function of the instants,
a filter configured to attenuate noise and/or extracting deterministic features in the at least one spectrogram, to obtain at least one filtered spectrogram of the signal over the instants,
the calculator configured to estimate a frequency trajectory, representative of the travel speed of the at least one wheel of the vehicle in the at least one filtered spectrogram over the instants, by:
estimation, by the calculator, of a probability of observation of the frequency trajectory from a calculated amplitude of the at least one filtered spectrogram for each instant,
estimation, by the calculator, of an a posteriori law of observation of the frequency trajectory as being proportional to a product of a prescribed probability of transition of the frequency trajectory and of the probability of observation of the frequency trajectory,
estimation, by the calculator, of the frequency trajectory for the instants from the a posteriori law of observation of the frequency trajectory,
the calculator configured to estimate the travel speed of the at least one wheel of the vehicle for the instants from the frequency trajectory,
the device configured to control real-time navigation of the vehicle using the estimated travel speed, the device being integrated into an inertial navigation device.