METHOD FOR VEHICLE SPEED ESTIMATION USING MULTIPLE GEOMAGNETIC SENSORS

20220413005 · 2022-12-29

    Inventors

    Cpc classification

    International classification

    Abstract

    The present disclosure provides a method for vehicle speed estimation using multiple geomagnetic sensors, and mainly solves the problem of low accuracy of vehicle speed estimation in existing single-geomagnetic, dual-geomagnetic and multi-geomagnetic scenarios. There are M vehicle detection modules (1), and the vehicle detection modules are deployed along a roadside at equal intervals and configured to detect processes that a vehicle approaches and leaves a detection point, record the time when the vehicle approaches the detection point and send time data to a data processing module (2). In order to avoid the situation that the vehicle detection modules do not detect the vehicle and may not perform, vehicle speed estimation, the data processing module (2) performs matching of data association on the time data uploaded by all the vehicle detection modules by using standard deviations of vehicle position estimation and speed estimation, and performs multiple Kalman filter iterations on the matched time data and the positions of the vehicle detection modules, so as to obtain a precise vehicle speed. The present disclosure improves the accuracy of vehicle speed estimation, and may be used for intelligent transportation management.

    Claims

    1. A method for vehicle speed estimation using multiple geomagnetic sensors, comprising the following steps: A) enabling a vehicle running on a road to pass through all vehicle detection modules (1) in sequence, wherein there are M vehicle detection modules (1), and the vehicle detection modules are deployed along a roadside at equal intervals; B) acquiring, by a data processing module (2), real-time data, and sending the real-time data to each of the vehicle detection modules, so as to synchronize the time of all the vehicle detection modules; C) detecting, by the vehicle detection modules, magnetic field strength data in real time, sampling the magnetic field strength data at a sampling frequency F.sub.s to obtain discrete magnetic field strength data, detecting processes that the vehicle approaches and leaves a detection point, according to the discrete magnetic field strength data, recording time data when the vehicle approaches the detection point, and sending the time data to the data processing module; D) performing, by the data processing module, data cleaning on the time data uploaded by all the vehicle detection modules, and performing matching of data association by using standard deviations of vehicle position estimation and speed estimation: D1) traversing each piece of the time data, and judging a format of the time data: if the format of the time data does not conform to a normal time format, deleting the time data directly; and if the format of the time data conforms to the normal time format, retaining the time data. and until all the time data is traversed, executing the step D2); D2) setting an initial value k=1 of a loop variable; D3) opening a time window for the time when the vehicle approaches a k+1.sub.th vehicle detection module by the time when the vehicle approaches a k.sub.th vehicle detection module according to the following formula: [ t k + Δ x k v ˆ k - 3 2 σ k , x ( v ˆ k ) 2 σ k , y , t k + Δ x k v ˆ k + 3 2 σ k , x ( v ˆ k ) 2 σ k , y ] wherein t.sub.k is the time when the vehicle approaches the k.sub.th vehicle detection module, Δx.sub.k is a distance between the k.sub.th vehicle detection module and the k+1.sub.th vehicle detection module, {circumflex over (v)}.sub.k is a speed at which the vehicle approaches the k.sub.th vehicle detection module, and σ.sub.k,x and σ.sub.k,v are standard deviations of position estimation and speed estimation of the vehicle approaching the k th vehicle detection module respectively; D4) determining the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module according to the number of the time data in the above time window: if there is only one piece of the time data in the time window, takin, the time data as the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module; if there are multiple pieces of the time data in the time window, arranging the multiple pieces of time data from small to large, and taking the intermediate time data as the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module; and if there is no time data in the time window, taking a median of the time window as the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module; and D5) judging the loop variable k and the total number M of the vehicle detection modules: if the loop variable k is less than the total number of the vehicle detection modules, that is, k<M, adding 1 to the loop variable k and returning to the step D3); and if the loop variable k is equal to the total number of the vehicle detection modules, that is, k=M ending iterations, so as to obtain the time for the vehicle to pass through all the vehicle detection modules; and E) performing, by the data processing module, multiple Kalman filter iterations on the matched time data of the vehicle approaching all the vehicle detection modules and the positions of the vehicle detection modules, so as to obtain a precise vehicle speed: E1) setting an initial value θ ˆ 1 | 1 = [ 0 v max 2 ] of, a state of the vehicle approaching a first vehicle detection module and an initial value P 1 | 1 = [ 1 0 0 ( v max 4 ) 2 ] of a covariance matrix, setting a maximum number of iterations to be N according to the accuracy of the vehicle speed required for an actual test, and setting the initial value k=1 of the loop variable, wherein v.sub.max is a maximum vehicle speed allowed by the road; E2) predicting a state of the vehicle approaching the k+1.sub.th vehicle detection module by a state of the vehicle approaching the k.sub.th vehicle detection module: θ ˆ k + 1 | k = [ x ˆ k + 1 .Math. k v ˆ k + 1 .Math. k ] = F k θ ˆ k .Math. k wherein θ ˆ k | k = [ x ˆ k v ˆ k ] is the state of the vehicle approaching the k.sub.th vehicle detection module, {circumflex over (x)}.sub.k is an estimated position of the vehicle approaching the k.sub.th vehicle detection module, and {circumflex over (v)}.sub.k is an estimated speed of the vehicle approaching the k.sub.th vehicle detection module; {circumflex over (θ)}.sub.k+1|k is a predicted state of the vehicle approaching the k+1.sub.th vehicle detection module, {circumflex over (x)}.sub.k+1|k is a predicted position of the vehicle approaching the k+1.sub.th vehicle detection module, and {circumflex over (v)}.sub.k+1|k is a predicted speed of the vehicle approaching the k+1.sub.th vehicle detection module; and F k = [ 1 Δ t k 0 1 ] is a state transition matrix, and Δt.sub.k=t.sub.k+1−t.sub.k is a time difference between the time when the vehicle approaches the k.sub.th vehicle detection module and the time when the vehicle approaches the k+1.sub.th vehicle detection module; E3) estimating, by a state prediction result obtained in the step E2) and a real position of the k+1.sub.th vehicle detection module, a speed of the vehicle approaching the k+1.sub.th vehicle detection module according to the following formula: θ ˆ k + 1 | k + 1 = [ x ˆ k + 1 v ˆ k + 1 ] = θ ˆ k + 1 .Math. k + W k + 1 λ k + 1 wherein {circumflex over (θ)}.sub.k+1|k+1 is a state of the vehicle approaching the k+1.sub.th vehicle detection module, {circumflex over (x)}.sub.k+1 is an estimated position of the vehicle approaching the k+1.sub.th vehicle detection module, {circumflex over (v)}.sub.k+1 is an estimated speed of the vehicle approaching the k+1.sub.th vehicle detection module; λ.sub.k+1=n.sub.k+1−{circumflex over (x)}.sub.k+1|k is a distance difference between the real position n.sub.k+1 of the k+1.sub.th vehicle detection module and a predicted position {circumflex over (x)}.sub.k−1|k of the vehicle approaching the k+1.sub.th vehicle detection module; and W.sub.k+1 is a gain of Kalman filtering, with a calculation formula as follows:
    W.sub.k+1=P.sub.k+1|k(h.sub.k).sup.T(S.sub.k+1).sup.−1 wherein P.sub.k+1|k=F.sub.kP.sub.k|kF.sub.k.sup.T+Q.sub.k is a state prediction covariance matrix, P.sub.k|k is a covariance matrix when the vehicle approaches the k th vehicle detection module, and Q k = [ 2 ( v ^ k .Math. k ) 2 σ t 2 0 0 Q v .Math. "\[LeftBracketingBar]" t k + 1 - t k .Math. "\[RightBracketingBar]" ] is a noise covariance matrix; h.sub.k=[1 0]is an observation matrix provided by the vehicle detection modules; and S.sub.k+1=h.sub.kP.sub.k+1|k(h.sub.k).sub.T+R.sub.k is a measurement covariance matrix provided by the vehicle detection modules, and R.sub.k is observation noise of the vehicle detection modules; E4) updating a covariance matrix when the vehicle approaches the k+1.sub.th vehicle detection module according to the following formula:
    P.sub.k+1|k+1=P.sub.k+1|k−W.sub.k+1S.sub.k+1W.sub.k+1.sup.T wherein P.sub.k+1|k+1 is the covariance matrix when the vehicle approaches the k+1.sub.th vehicle detection module; and E5) judging the loop variable k and the set maximum number N of iterations: if the loop variable k is less than the set maximum number of iterations, that is, k<N, adding 1 to the loop variable k and returning to the step E2); and if the loop variable k is equal to the set maximum number of iterations, that is, k=N, ending the iterations, so as to obtain the vehicle speed.

    2. The method according to claim 1, wherein in the step A), each of the vehicle detection modules (1) comprises a geomagnetic sensor submodule (11), a controller submodule (12), and a data transceiver submodule (13), and the controller submodule (12) is connected with the geomagnetic sensor submodule (11) and the data transceiver submodule (13); the geomagnetic sensor submodule (11) is configured to detect the magnetic field strength data in real time and send the magnetic field strength data to the controller submodule (12) the controller submodule (12) is configured to acquire the magnetic field strength data of the geomagnetic sensor submodule and send the time data when the vehicle approaches the detection point to the data transceiver submodule (13); and the data transceiver submodule (13) is configured to send the time data to the data processing module (2).

    3. The method according to claim 1, wherein in the step B), the data, processing module (2) comprises a data transceiver submodule (21), a controller submodule (22), and a GPS submodule (23), and the controller submodule (22) is connected with the data transceiver submodule (21) and the UPS submodule (23); the GPS subrnodule (23) is configured to acquire the real-time data and send the real-time data, to each of the vehicle detection modules (1), so as to synchronize the time of all the vehicle detection modules; and the data transceiver submodule (21) is configured to receive the time data uploaded by the vehicle detection modules (1).

    4. The method according to claim 1, wherein in the step C), the frequency at which the magnetic field strength data is sampled by the vehicle detection modules is set to be 100 Hz˜500 Hz so as to obtain the discrete magnetic field strength data.

    5. The method according to claim 1, wherein in the step C), detecting, by the vehicle detection modules, the processes that the vehicle approaches and leaves the detection point according to the discrete magnetic field strength data comprises: C1) setting a threshold Th according to the actually tested magnetic field strength data; C2) comparing, by the vehicle detection modules, the discrete magnetic field strength data obtained by sampling with the threshold Th, so as to judge the process that the vehicle approaches the detection point: if the discrete magnetic field strength data is higher than the threshold Th, judging whether there are K pieces of continuously increased data all higher than the threshold Th subsequently: if yes, indicating, that the vehicle has approached the detection point, recording, by the vehicle detection modules, the time data, and executing the step C3); and if not, indicating that the data is interference data, and skipping recording the time data; and if the discrete magnetic field strength data is lower than the threshold Th, indicating that the vehicle does not approach the detection point, and skipping performing processing; and C3) comparing, by the vehicle detection modules, the above discrete magnetic field strength data with the set threshold Th again, so as to judge the process that the vehicle leaves the detection point: if the discrete magnetic field strength data is lower than the threshold Th, judging whether there are L pieces of continuously reduced data all lower than the threshold Th subsequently: if yes, indicating that the chick has left the detection point; and if not, indicating that the vehicle has not left a detection range of the sensors; and if the discrete magnetic field strength data is higher than the threshold Th, indicating that the vehicle has not left the detection range of the sensors, and making a system continue to wait, until the magnetic field strength data is lower than the threshold Th, wherein values of K and L are determined according to the actually tested magnetic field strength data.

    Description

    BRIEF DESCRIPTION OF DRAWINGS

    [0038] FIG. 1 is a flowchart for implementing a method provided by the present disclosure; and

    [0039] FIG. 2 is a deployment diagram of a plurality of vehicle detection modules in the present disclosure.

    DETAILED DESCRIPTION OF EMBODIMENTS

    [0040] The embodiments of the present disclosure are further described below with reference to the accompanying drawings.

    [0041] Referring to FIG. 1, a method for vehicle speed estimation using multiple geomagnetic sensors, including the following steps:

    [0042] step 1: a plurality of vehicle detection modules are deployed according to actual needs, wherein

    [0043] referring to FIG. 2, there are M vehicle detection modules deployed along a roadside at equal intervals, and a vehicle running on a road passes through all the vehicle detection modules in sequence; each of the vehicle detection modules 1 includes a geomagnetic sensor submodule 11, a controller submodule 12, and a data transceiver submodule 13, and the controller submodule 12 is connected with the geomagnetic sensor submodule 11 and the data transceiver submodule 13; the geomagnetic sensor submodule 11 is configured to detect magnetic field strength data in real time and send the magnetic field strength data to the controller submodule 12; the controller submodule 12 is configured to acquire the magnetic field strength data of the geomagnetic sensor submodule and send the time data when the vehicle approaches a detection point to the data transceiver submodule 13; and the data transceiver submodule 13 is configured to send the time data to a data processing module 2;

    [0044] step 2: the time of all the vehicle detection modules is synchronized, wherein

    [0045] the data processing module acquires the real-time data and sends it to each of the vehicle detection modules, so as to synchronize the time of all the vehicle detection modules; the data processing module 2 includes a data transceiver submodule 21, a controller submodule 22, and a GPS submodule 23, and the controller submodule 22 is connected with the data transceiver submodule 21 and the GPS submodule 23; the GPS submodule 23 is configured to acquire the real-time data and send the real-time data to each of the vehicle detection modules 1, so as to synchronize the time of all the vehicle detection modules; and the data transceiver submodule 21 is configured to receive the time data uploaded by the vehicle detection modules 1;

    [0046] step 3: the vehicle detection modules detect the magnetic field strength data in real time, sample the magnetic field strength data at a sampling frequency F.sub.s=100 Hz˜500 Hz to obtain discrete magnetic field strength data, detect, processes that the vehicle approaches and leaves the detection point according to the discrete magnetic field strength data, record the time data when the vehicle approaches the detection point, and send the time data to the data processing module:

    [0047] 3.1) the controller submodule 12 sets a threshold Th according to the actually tested magnetic field strength data, and, compares the discrete magnetic field strength data obtained by sampling with the threshold Th, so as to judge the process that the vehicle approaches the detection point:

    [0048] if the discrete magnetic field strength data is higher than the threshold Th, whether there are K pieces of continuously increased data all higher than the threshold Th subsequently is judged: if yes, it indicates that the vehicle has approached the detection point, the controller submodule 12 records the time data, and the step 3.2) is executed; and if not, it indicates that the data is interference data, and the time data is not recorded; and

    [0049] if the discrete magnetic field strength data is lower than the threshold Th, it indicates that the vehicle does not approach the detection point, and processing is not performed; and

    [0050] 3.2) the controller submodule 12 compares the above discrete magnetic field strength data with the set threshold Th again, so as to judge the process that the vehicle leaves the detection point:

    [0051] if the discrete magnetic field strength data is lower than the threshold Th, whether there are L pieces of continuously reduced data all lower than the threshold Th subsequently is judged: if yes, it indicates that the vehicle has left the detection point, the controller submodule 12 sends the time data to the data transceiver submodule 13, and the data transceiver submodule 13 forwards the time data to the data processing module 2; and if not, it indicates that the vehicle has not left a detection range of the sensors; and

    [0052] if the discrete magnetic field strength data is higher than the threshold Th, it indicates that the vehicle has not left the detection range of the sensors, and a system continues to wait, until the magnetic field strength data is lower than the threshold Th,

    [0053] wherein, values of K and L are determined according to the actually tested magnetic field strength data;

    [0054] step 4: the data processing module performs data cleaning on the time data uploaded by all the vehicle detection modules, and performs matching of data association by using standard deviations of vehicle position estimation and speed estimation:

    [0055] 4.1) each piece of the time data is traversed, and a format of the time data is judged:

    [0056] if the format of the time data does not conform to a normal time format, the time data is deleted directly; and

    [0057] if the format of the time data conforms to the normal time format, the time data is retained, and until all the time data is traversed, the step 4.2) is executed;

    [0058] 4.2) an initial value k=1 of a loop variable is set;

    [0059] 4.3) a time window is opened for the time when the vehicle approaches the k+1.sub.th vehicle detection module by the time when the vehicle approaches the k.sub.th vehicle detection module according to the following formula:

    [00009] [ t k + Δ x k v ˆ k - 3 2 σ k , x ( v ˆ k ) 2 σ k , v , t k + Δ x k v ˆ k + 3 2 σ k , x ( v ˆ k ) 2 σ k , v ]

    [0060] wherein, t.sub.k is the time when the vehicle approaches the k.sub.th vehicle detection module, Δx.sub.k is a distance between the k.sub.th vehicle detection module and the k+1.sub.th vehicle detection module, {circumflex over (v)}.sub.k is a speed at which the vehicle approaches the k th vehicle detection module, and σ.sub.k,x and σ.sub.k,v are standard deviations of position estimation, and speed estimation of the vehicle approaching the k th vehicle detection module respectively;

    [0061] 4.4) the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module is determined according to the number of the time data in the above time window:

    [0062] if there is only one piece of the time data in the time window, the time data is taken as the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module;

    [0063] if there are multiple pieces of the time data in the time window, the multiple pieces of time data are arranged from small to large, and the intermediate time data is taken as the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module; and

    [0064] if there is no time data in the time window, a median of the time window is taken as the time t.sub.k+1 when the vehicle approaches the k+1.sub.th vehicle detection module; and

    [0065] 4.5) the loop variable k and the total number M of the vehicle detection modules are judged:

    [0066] if the loop variable k is less than the total number of the vehicle detection modules, that is, k<M, 1 is added to the loop variable k and the flow returns to the step 4.3); and

    [0067] if the loop variable k is equal to the total number of the vehicle detection modules, that is, k=M, iterations are ended, so as to obtain the time for the vehicle to pass through all the vehicle detection modules;

    [0068] step 5: the data, processing module performs multiple Kalman filter iterations on the matched time data of the vehicle approaching all the vehicle detection modules and the positions of the vehicle detection modules, so as to obtain a precise vehicle speed:

    [0069] 5.1) an initial value

    [00010] θ ˆ 1 | 1 = [ 0 v max 2 ]

    of a state of the vehicle approaching a first vehicle detection module and an initial value

    [00011] P 1 | 1 = [ 1 0 0 ( v max 4 ) 2 ]

    of a covariance matrix are set, a maximum number of iterations is set to be according to the accuracy of the vehicle speed required for an actual test, and the initial value k=1 of the loop variable is set, wherein v.sub.max is a maximum vehicle speed allowed by the road, and this embodiment sets but is not limited to N=7;

    [0070] 5.2) a state of the vehicle approaching the k+1.sub.th vehicle detection module is predicted by a state of the vehicle approaching the k.sub.th vehicle detection module

    [00012] θ ˆ k + 1 | k = [ x ˆ k + 1 .Math. k v ˆ k + 1 .Math. k ] = F k θ ˆ k .Math. k

    [0071] wherein

    [00013] θ ˆ k | k = [ x ˆ k v ˆ k ]

    is the state of the vehicle approaching the k.sub.th vehicle detection module, {circumflex over (x)}.sub.k is an estimated position of the vehicle approaching the k.sub.th vehicle detection module, and {circumflex over (v)}.sub.k is an estimated speed of the vehicle approaching the k.sub.th vehicle detection module; {circumflex over (θ)}.sub.k+1|k is a predicted state of the vehicle approaching the k+1.sub.th vehicle detection module, is a predicted position of the vehicle approaching the k+1.sub.th vehicle detection module, and {circumflex over (v)}.sub.k+1|k is a predicted speed of the vehicle approaching the k+1.sub.th vehicle detection module; and

    [00014] F k = [ 1 Δ t k 0 1 ]

    is a state transition matrix, and Δt.sub.k=t.sub.k+1−t.sub.k is a time difference between the time when the vehicle approaches the k.sub.th vehicle detection module and the time when the vehicle approaches the k+1.sub.th vehicle detection module;

    [0072] 5.3) by a state prediction result obtained in the step 5.2) and a real position of the k+1 th vehicle detection module, a speed of the vehicle approaching the k+1.sub.th vehicle detection module is estimated according to the following formula:

    [00015] θ ˆ k + 1 .Math. k + 1 = [ x ˆ k + 1 v ˆ k + 1 ] = θ ˆ k + 1 .Math. k + W k + 1 λ k + 1

    [0073] wherein {circumflex over (θ)}.sub.k+1|k+1 is a state of the vehicle approaching the k+1.sub.th vehicle detection module, {circumflex over (x)}.sub.k+1 is an estimated position of the vehicle approaching the k+1.sub.th vehicle detection module, and {circumflex over (v)}.sub.k+1 is an estimated speed of the vehicle approaching the k+1.sub.th vehicle detection module; λ.sub.k+1=n.sub.k+1−{circumflex over (x)}.sub.k+1|k is a distance difference between the real position n.sub.k+1 of the k+1.sub.th vehicle detection module and a predicted position {circumflex over (x)}.sub.k−1|k of the vehicle approaching the k+1.sub.th vehicle detection module; and W.sub.k+1 is a gain of Kalman filtering, with a calculation formula as follows:


    W.sub.k+1=P.sub.k+1k(h.sub.k).sup.T(S.sub.k+1).sup.−1

    wherein P.sub.k+1|k=F.sub.kP.sub.k|kF.sub.k.sup.T=Q.sub.k is a state prediction covariance matrix, P.sub.k|k is a covariance matrix when the vehicle approaches the k.sub.th vehicle detection module, and

    [00016] Q k = [ 2 ( v ^ k .Math. k ) 2 σ t 2 0 0 Q v .Math. "\[LeftBracketingBar]" t k + 1 - t k .Math. "\[RightBracketingBar]" ]

    is a noise covariance matrix; h.sub.k=[1 0]is an observation matrix provided by the vehicle detection modules; and S.sub.k+1=h.sub.kP.sub.k+1|k(h.sub.k).sup.T=R.sub.k is a measurement covariance matrix provided by the vehicle detection modules, and R.sub.k is observation noise of the vehicle detection modules:

    [0074] 5.4) a covariance matrix when the vehicle approaches the k+1 th vehicle detection module is updated according to the following formula:


    P.sub.k+1|k+1=P.sub.k+1|k−W.sub.k+1S.sub.k+1W.sub.k+1.sup.T,

    [0075] wherein P.sub.k+1|k+1 is the covariance matrix when the vehicle approaches the k+1.sub.th vehicle detection module: and

    [0076] 5.5) the loop variable k and the set maximum number N of iterations are judged:

    [0077] if the loop variable k is less than the set maximum number of iterations, that is, k<N, 1 is added to the loop variable k and the flow returns to the step 5.2); and

    [0078] if the loop variable k is equal to the set maximum number of iterations, that is, k=N, the iterations are ended, so as to obtain the vehicle speed.

    [0079] The above is merely a specific embodiment of the present disclosure, and does not constitute any limitation on the present disclosure. Apparently, for those skilled in the art, various modifications and changes in form and detail may be made under the idea and spirit of the present disclosure, but these modifications and changes based on the idea of the present disclosure still fall within the scope of protection of claims of the present disclosure.