Travel speed estimation device and travel speed estimation method

Abstract

Errors other than Gaussian white noise contained in the measured moving speed of the moving object, i.e., errors due to speed jumps, are removed to estimate the appropriate moving speed of the moving object. [Solution] The system comprises an information acquisition unit 4 that acquires the measured moving speed and signal-to-noise ratio of the moving body from a radar speedometer 3 mounted on the moving body, and a moving speed estimation unit 5 that sets an observation noise based on the signal-to-noise ratio, performs Kalman filter processing using the observation noise, and calculates an estimated moving speed of the moving body from the measured moving speed.The moving speed estimation unit 5 is characterized in that when the amount of change in the measured moving speed is less than a predetermined threshold, it performs Kalman filter processing using the observation noise set based on the signal-to-noise ratio, and when the amount of change in the measured moving speed is equal to or greater than the predetermined threshold, it makes the observation noise larger than the observation noise set based on the signal-to-noise ratio, and after a predetermined time has passed, it reduces the observation noise to the observation noise set based on the signal-to-noise ratio and performs Kalman filter processing.

Description

[Technical Field]

[0001] The present invention relates to a moving speed estimation device and a moving speed estimation method, and more particularly to a technique for correcting an error contained in the moving speed of a moving object measured using a radar device and estimating an appropriate moving speed. [Background technology]

[0002] A radar speedometer is a device used to detect the moving speed of railway vehicles. A radar speedometer is installed inside the passenger compartment of a railway vehicle, emits an irradiated wave through the window glass toward the road surface in the direction of travel (diagonally downward), and measures the moving speed of the railway vehicle based on the wave reflected from the irradiated wave. Therefore, there is a problem that errors occur in the measured moving speed if the signal-to-noise ratio (SN ratio) of the reflected wave decreases due to transmission loss caused by the thickness of the window glass or diffuse reflection from water droplets such as rain or snow adhering to the window glass.

[0003] Patent Document 1 describes a vehicle speed detection device equipped with a millimeter wave sensor as a device for detecting the vehicle speed of a railway vehicle. This vehicle speed detection device installs two millimeter wave sensors in different positions under the railway vehicle, compares the vehicle speed information detected by the two sensors, and determines which sensor's reflected waves should be used to calculate the vehicle speed. It explains that this device can detect vehicle speed stably and with high accuracy, regardless of the surface conditions of the tracks on which the vehicle runs. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Application Publication No. 2016-125856 Summary of the Invention [Problem to be solved by the invention]

[0005] A known method for removing errors contained in the moving speed measured by radar equipment is to use a Kalman filter. This method calculates the optimal Kalman gain by setting the observation noise based on the S / N ratio of the reflected wave, and calculates an estimated moving speed complemented by the speed based on the acceleration sensor. Specifically, when the S / N ratio is small (when the noise is large), the observation noise is set large, and when the S / N ratio is large (when the noise is small), the observation noise is set small.

[0006] However, conventional Kalman filter processing using observation noise based on the S / N ratio can remove errors caused by a decrease in the S / N ratio, such as the above-mentioned errors caused by the thickness of window glass or water droplets due to rainfall or snowfall (i.e., considered as normal white noise), but cannot remove errors that do not involve a decrease in the S / N ratio. Therefore, when the irradiated wave from the radar is reflected and returned by a moving object other than the vehicle itself, that is, when the relative speed is detected from somewhere other than the road surface (called "speed jump"), there may be cases where the S / N ratio does not decrease, so conventional Kalman filter processing cannot remove the errors and it is not possible to calculate an appropriate estimated value of the moving speed.

[0007] Furthermore, the method of Patent Document 1 cannot deal with the case where an error occurs in both of the two sensors due to the influence of the external environment.

[0008] Therefore, an object of the present invention is to provide a moving speed estimation device and a moving speed estimation method that can remove errors other than Gaussian white noise contained in the measured moving speed of a moving object, i.e., errors due to speed jumps, and can estimate the appropriate moving speed of a moving object, and that can be realized with only one radar device installed. [Means for solving the problem]

[0009] In order to solve the above problem, the invention described in claim 1 is a moving speed estimation device comprising: an information acquisition means for acquiring a measured moving speed and a signal-to-noise ratio of the moving body from a radar device mounted on the moving body; and a moving speed estimation means for setting an observation noise based on the signal-to-noise ratio, performing Kalman filter processing using the observation noise, and calculating an estimated moving speed of the moving body from the measured moving speed, wherein the moving speed estimation means, when an amount of change in the measured moving speed is less than a predetermined threshold, performs Kalman filter processing using the observation noise set based on the signal-to-noise ratio, and, when an amount of change in the measured moving speed is equal to or greater than the predetermined threshold, makes the observation noise larger than the observation noise set based on the signal-to-noise ratio, and after a predetermined time has elapsed, reduces the observation noise towards the observation noise set based on the signal-to-noise ratio and performs Kalman filter processing.

[0010] A second aspect of the present invention is the moving speed estimation device according to the first aspect, wherein the moving speed estimation means sets the predetermined threshold value based on a variance value of the acceleration of the moving object.

[0011] The invention described in claim 3 is characterized in that, in the moving speed estimation device described in claim 1, it further comprises an acceleration offset estimation means for estimating an acceleration offset value at a predetermined period based on the acceleration of the moving body and the estimated moving speed, the information acquisition means acquires the acceleration from an acceleration sensor mounted on the moving body, and the acceleration offset estimation means stops estimating the acceleration offset value when the signal-to-noise ratio is less than a predetermined threshold or when an amount of change in the measured moving speed is equal to or greater than the predetermined threshold.

[0012] The invention described in claim 4 is characterized in that, in the travel speed estimation device described in claim 3, the acceleration offset estimation means resumes estimating the acceleration offset value after a predetermined time has elapsed since the estimation of the acceleration offset value was stopped.

[0013] The invention described in claim 5 is a movement speed estimation method comprising the steps of: acquiring a measured movement speed and a signal-to-noise ratio of the moving body from a radar device mounted on the moving body; setting an observation noise based on the signal-to-noise ratio, performing Kalman filter processing using the observation noise, and calculating an estimated movement speed of the moving body from the measured movement speed; and, when an amount of change in the measured movement speed is less than a predetermined threshold, performing Kalman filter processing using the observation noise set based on the signal-to-noise ratio; and, when an amount of change in the measured movement speed is equal to or greater than the predetermined threshold, making the observation noise larger than the observation noise set based on the signal-to-noise ratio, and, after a predetermined time has elapsed, reducing it to the observation noise set based on the signal-to-noise ratio, and performing Kalman filter processing.

[0014] A sixth aspect of the present invention is characterized in that the moving speed estimation method according to the fifth aspect further comprises a step of setting the predetermined threshold value based on a variance value of the acceleration of the moving object.

[0015] The invention described in claim 7 is characterized in that, in the moving speed estimation method described in claim 5, it comprises the steps of acquiring the acceleration of the moving body from an acceleration sensor mounted on the moving body, and estimating an acceleration offset value at a predetermined period based on the acceleration and the estimated moving speed, and when the signal-to-noise ratio is less than a predetermined threshold or when the amount of change in the measured moving speed is equal to or greater than the predetermined threshold, the estimation of the acceleration offset value is stopped.

[0016] An eighth aspect of the present invention is characterized in that, in the travel speed estimation method of the seventh aspect, the estimation of the acceleration offset value is resumed after a predetermined time has elapsed since the estimation of the acceleration offset value was stopped. [Effects of the Invention]

[0017] According to the inventions of claims 1 and 5, when calculating an estimated moving speed from the measured moving speed of a moving object by Kalman filtering, it is determined whether or not there is a speed jump (appears as an increase in the amount of change in the measured moving speed). If there is a speed jump, the observation noise is increased to a value greater than the observation noise set based on the signal-to-noise ratio (SN ratio) before performing Kalman filtering. This makes it possible to remove errors other than Gaussian white noise contained in the measured moving speed of the moving object, thereby enabling the moving speed of the moving object to be estimated with high accuracy. Furthermore, since the temporarily increased observation noise is reduced after a predetermined time has elapsed before performing Kalman filtering, it is possible to prevent divergence of the ex-post estimated value of Kalman filtering. Furthermore, it is possible to estimate an appropriate moving speed simply by installing one radar device.

[0018] According to the inventions of claims 2 and 6, the magnitude of the threshold (threshold of the amount of change in the measured moving speed) that serves as a criterion for determining whether or not to increase the observation noise is determined based on the variance value of the acceleration of the moving body. This makes it possible to use more appropriate observation noise in the Kalman filter processing, and ultimately makes it possible to estimate the moving speed of the moving body with higher accuracy.

[0019] According to the inventions of claims 3 and 7, when an estimated moving speed is calculated by Kalman filter processing from the measured moving speed of a moving body, estimation of an acceleration offset value is continued at a predetermined cycle, so it is possible to estimate the acceleration offset value of the moving body regardless of the moving state (whether the moving body is stopped or moving) or the moving speed (moving speed). Moreover, when the SN ratio is less than a predetermined threshold (when the SN ratio is poor) or when the measured moving speed is equal to or greater than a predetermined threshold (when there is a speed jump), the estimated moving speed calculated by Kalman filter processing may contain an error, but the acceleration offset value is not estimated (updated) based on the estimated moving speed that may contain such an error. Therefore, it is possible to accurately and highly accurately estimate the acceleration offset value of the moving body, and ultimately to highly accurately estimate the moving speed of the moving body.

[0020] According to the inventions of claims 4 and 8, the estimation of the acceleration offset value is resumed after a predetermined time has elapsed since the estimation of the acceleration offset value was stopped. This means that a sudden change in speed, such as a speed jump that is likely to occur immediately after the S / N ratio is recovered, does not need to be used in the estimation of the acceleration offset, and the acceleration offset value of the moving body can be estimated more accurately and with higher precision, and ultimately the moving speed of the moving body can be estimated with higher precision. [Brief explanation of the drawings]

[0021] [Figure 1] 1 is a functional block diagram showing a schematic configuration of a travel speed estimation system including a travel speed estimation device according to first and second embodiments of the present invention. [Figure 2] 5 is a flowchart showing the procedure of an observation noise determination process in a travel speed estimation unit (observation noise determination unit) of the travel speed estimation device in accordance with Embodiment 1 of the present invention. [Figure 3] 3 is a diagram showing an example of observation noise used in process A of the observation noise determination process shown in FIG. 2. FIG. [Figure 4] 3 is a diagram showing an example of observation noise used in process B of the observation noise determination process shown in FIG. 2. FIG. [Figure 5] 2 is a diagram showing data of a measured moving speed measured by a radar speedometer and data of an estimated moving speed estimated from the measured moving speed using the moving speed estimation device of FIG. 1; FIG. [Figure 6] 10 is a flowchart showing the procedure of an observation noise determination process in a travel speed estimation unit (observation noise determination unit) of a travel speed estimation device in accordance with Embodiment 2 of the present invention. [Figure 7] 10 is a functional block diagram showing a schematic configuration of a travel speed estimation system including a travel speed estimation device in accordance with Embodiment 3 of the present invention. FIG. [Figure 8] 8 is a flowchart showing the procedure of an acceleration offset value estimation process in an acceleration offset estimation unit of the moving speed estimation device of FIG. 7. [Figure 9]FIG. 10 is a diagram showing data of measured moving speeds measured by a radar speedometer, and data of estimated moving speeds and acceleration offset values ​​calculated from the measured moving speeds (Example and Comparative Example). DETAILED DESCRIPTION OF THE INVENTION

[0022] The present invention will be described below based on the illustrated embodiments.

[0023] (Embodiment 1) FIG. 1 is a functional block diagram showing a schematic configuration of a travel speed estimation system including a travel speed estimation device 1 according to this embodiment.

[0024] The moving speed estimation system S includes a moving speed estimation device 1, one radar speedometer (radar device) 3, and an acceleration sensor 6.

[0025] The travel speed estimation device 1 is a device that is mounted on a moving body and estimates the travel speed (traveling speed) of the moving body, and mainly includes a control unit 2, an information acquisition unit (information acquisition means) 4, and a travel speed estimation unit (travel speed estimation means) 5. Examples of moving bodies on which the travel speed estimation device 1 is mounted include vehicles such as railway cars, agricultural machinery, and construction machinery.

[0026] The radar speedometer 3 is mounted on a moving object, transmits irradiated waves (radio waves) at a depression angle in the moving direction of the moving object, receives reflected waves from the road surface and surrounding reflecting objects, and measures (calculates) the measured moving speed of the moving object (meaning the measured moving speed) and the signal-to-noise ratio (SN ratio) of the reflected waves. The radio waves transmitted from the radar speedometer 3 are, for example, radio waves having a frequency in the 79 GHz band, 76 GHz band, or 60 GHz band (also called "millimeter waves"). Furthermore, the radar method used is, for example, an FMCW (Frequency Modulated Continuous Wave) method. However, these are just examples, and other frequencies and radar methods can also be used.

[0027] Specifically, the measured moving speed of the moving body is determined by frequency analysis of the waves reflected from the road surface and surrounding reflecting objects to calculate the relative speed of the reflecting objects relative to the moving body. The method of calculating the relative speed is not particularly limited, and known methods can be used. For example, when a moving body and an object that reflects irradiated waves are moving relative to each other, the frequency of the radio waves (i.e., reflected waves) reflected by the surface of the object and received by the receiving antenna can be calculated using known methods, such as a method that utilizes the fact that, when the radio waves are reflected by the surface of the object relative to the moving body, the frequency of the radio waves transmitted from the transmitting antenna (i.e., irradiated waves) is affected by the relative speed between the moving body and the object due to the Doppler effect, and shifts depending on the relative speed between the moving body and the object.

[0028] Possible objects that reflect the irradiated waves transmitted from the radar speedometer 3 include, for example, stationary objects such as the surface of a road or a railroad track on which a moving object travels, and other moving objects that move relative to the moving object (host vehicle). In the latter case, speed jumps occur, increasing the amount of change in the measured moving speed, which causes a problem that the moving speed of the moving object cannot be estimated appropriately. As will be described later, the moving speed estimation device and moving speed estimation method according to this embodiment enable appropriate estimation of the moving speed of the moving object even in such cases.

[0029] The acceleration sensor 6 is a sensor for measuring the acceleration of a moving object. The acceleration sensor 6 may be an individual sensor, or may be an acceleration sensor included in an IMU (Inertial Measurement Unit), for example. The measured acceleration is used in the moving speed estimation unit 5 to determine observation noise and for Kalman filter processing.

[0030] The control unit 2 of the moving speed estimation device 1 is a device for controlling each part of the moving speed estimation device 1, and is mainly composed of a central processing unit 21 (CPU: Central Processing Unit) that performs calculations related to detecting the moving speed, a ROM 22 (ROM: Read Only Memory) that is a readable storage device, and a RAM 23 (RAM: Random Access Memory) that is a readable / writable storage device.

[0031] The information acquisition unit 4 acquires the measured moving speed of the moving object and the SN ratio of the reflected wave from the radar speedometer 3, acquires the acceleration of the moving object from the acceleration sensor 6, and outputs this acquired information to the moving speed estimation unit 5.

[0032] The moving speed estimation unit 5 is a device for calculating an estimated moving speed (meaning an estimated moving speed) of a moving object by Kalman filter processing, using the measured moving speed and the SN ratio input from the information acquisition unit 4 as observation values. The moving speed estimation unit 5 mainly includes an observation noise determination unit 51 that determines the observation noise to be used in the Kalman filter processing, and a Kalman filter unit 52 that performs the Kalman filter processing.

[0033] The observation noise determination unit 51 determines the observation noise to be used in Kalman filter processing. As a preliminary step, the value of the observation noise is set in advance based on the expected S / N ratio. Specifically, the observation noise is set to be large when the S / N ratio is small (when the noise is large), and small when the S / N ratio is large (when the noise is small). For example, as shown in the comparison table of FIG. 3, the S / N ratio may be divided into several levels, and a value of the observation noise corresponding to each level may be set.

[0034] The observation noise determination unit 51 sets the observation noise in advance based on the S / N ratio as described above, and then determines the observation noise to be used in the subsequent Kalman filter processing each time a new observation value (the current measured moving speed of the moving body and the current S / N ratio) is input from the information acquisition unit 4.

[0035] 2 is a flowchart showing the processing steps for determining the observation noise in the observation noise determiner 51. When new observation values ​​(the current measured travel speed of the moving object and the current S / N ratio) are input from the information acquirer 4, the observation noise determiner 51 first determines whether or not the amount of change in the measured travel speed is equal to or greater than a predetermined threshold (step S1). Here, the amount of change in the measured travel speed means the difference between the measured travel speed used in the current process (the current measured travel speed) and the estimated travel speed (the output value of the Kalman filter) estimated from the measured travel speed used in the previous process (the immediately preceding measured travel speed).

[0036] If the change in the measured moving speed is less than a predetermined threshold (step S1: No), the observation noise determination unit 51 refers to a comparison table, such as that shown in FIG. 3, and selects (determines) the observation noise corresponding to the SN ratio (observation value) of this processing (present) from the observation noises set based on the SN ratio, and outputs it to the subsequent Kalman filter unit 52 (step S2, processing A).

[0037] Here, the predetermined threshold value is not particularly limited and may be set appropriately to the amount of speed change that is considered to correspond to a speed jump. For example, if the moving body is a railway vehicle, it may be considered to be set to around 1 to 5 m / s, taking into account the amount of speed change that can normally occur.

[0038] On the other hand, if the change in the measured moving speed is equal to or greater than a predetermined threshold (step S1: Yes), the observation noise determination unit 51 temporarily increases the observation noise to be greater than the observation noise corresponding to the SN ratio (observation value) of the current process (present), and after a predetermined time has elapsed, outputs the observation noise that has been reduced to the observation noise corresponding to the SN ratio (observation value) to the subsequent Kalman filter unit 52 (step S3, process B).

[0039] Specifically, a case where the change in the measured moving speed exceeds a predetermined threshold occurs when the irradiated wave from the radar speedometer 3 is reflected by a moving object other than the vehicle itself and returns. In this case, simply performing Kalman filter processing using the observation noise corresponding to the SN ratio (observation value) of this processing (current) is not enough to sufficiently remove the error contained in the measured moving speed, and an appropriate estimated moving speed cannot be calculated. Therefore, a process is performed to temporarily increase the observation noise to a value greater than the observation noise based on the SN ratio.

[0040] The degree of increase in observation noise is not particularly limited, and can be set appropriately to a level of observation noise that can eliminate speed jump errors. For example, it can be set to about 100 times the maximum observation noise level set based on the SNR.

[0041] The observation noise, which is set to a value greater than the observation noise corresponding to the SN ratio (observed value) of the current process, is exponentially reduced toward the SN ratio (observed value) of the current process after a predetermined time has elapsed, as shown in FIG. 4. This is to prevent divergence of the posterior estimated value in the Kalman filter process. In other words, if the observation noise is increased, the integrated value of the acceleration sensor becomes dominant in the estimated speed, but the integrated error of the acceleration sensor increases over time. Therefore, by reducing the observation noise after a predetermined time has elapsed, the integrated error caused by the acceleration sensor is suppressed. Here, the predetermined time is not particularly limited, but it can be set to about 2 to 3 seconds if the moving object is a railway vehicle, for example.

[0042] Here, the series of processes from steps S1 to S3 are executed at each radar scan cycle by the radar speedometer 3, and "current process" refers to the series of processes from steps S1 to S3 that are currently being executed, and "previous process" refers to the series of processes from steps S1 to S3 that were executed immediately before / most recently from the series of processes from steps S1 to S3 that are currently being executed.

[0043] The Kalman filter unit 52 performs Kalman filtering using the observation noise input from the observation noise determination unit 51, and calculates an estimated moving speed of the moving object from the measured moving speed (observation value) of the moving object. The Kalman filtering will be described below.

[0044] First, let us consider a state model of the moving object at time t, where the moving object is equipped with a radar speedometer 3 and an acceleration sensor 6. The state quantity v to be estimated is t|t is the speed of the moving object (estimated moving speed), and the observed quantity z t Let us consider modeling the speed of the radar speedometer 3 (measured moving speed).

[0045] Assuming that the moving object's speed at time t+1 is the speed at time t multiplied by the acceleration (measured value), the state equation can be expressed as follows:

[0046]

number

[0047] Here, the variables in equation (1) are as follows:

[0048] v t : Speed ​​of the moving object at time t (m / s) v t+1 : Velocity of the moving object at time t+1 (m / s) a t : Acceleration in the direction of travel (measured value) at time t (m / s / s) ε - t : Offset value A of acceleration in the forward direction offset (m / s / s) (Note: "ε - " is written directly above "ε". - " is used to indicate that the item is marked with ".) q t : System noise (m / s / s) (for a moving object with a speed q t Δ t (m / s) noise is superimposed) Δ t:Transition time interval (s) from time t to time t+1

[0049] Next, the radar speedometer 3 calculates the speed (true value) of the moving object at time t by adding the observation noise w t (m / s) is the superimposed speed (measured moving speed) z t (m / s) is observed. Therefore, the observation equation can be expressed as follows: (The observation equation means that the observed value is expressed as "theoretical value + noise").

[0050]

number

[0051] Essentially, based on the principles of the Kalman filter, the speed (measured moving speed, observed value) of the radar speedometer 3 must be assumed to be the true speed plus normal white noise with a mean of 0. However, in the moving object environment assumed here, it is necessary to also deal with sudden "speed jumps," and the variance of the observation noise cannot simply be set as a constant. For this reason, the variance of the observation noise is dynamically varied according to the following policy.

[0052] When the SN ratio is poor, the variance of the observation noise is set to a large value. When the SN ratio is good, the variance of the observation noise is set small. The most recent posterior estimate v t-1|t-1 and the current observation z t If the difference between the two is equal to or greater than a predetermined value, the variance of the observation noise is set to a large value. Hereafter, the variance of the observation noise is treated as a parameter that varies with time, so r t This is expressed as follows.

[0053] Applying the Kalman filter algorithm to the above model results in a one-dimensional Kalman filter being calculated, with the calculation steps being as follows:

[0054]

number

[0055]

number

[0056] Here, equations (3) to (4) are equations for the prediction step, equations (5) to (7) are equations for the filtering step, and the variables in equations (3) to (7) are as follows:

[0057] v t|t-1 :Predicted speed at time t (prior estimate) estimated using information up to time t-1 v t|t : Estimated speed at time t (posterior estimate) estimated using information up to time t p t|t-1 : prior error covariance matrix p t|t :Posterior error covariance matrix z t :Measured movement speed (observed value) σ q : System noise deviation (constant) r t : Observation noise K t : Kalman gain

[0058] 5A and 5B are diagrams comparing measured moving speed data of a moving object with estimated moving speed data estimated from the measured moving speed by a moving speed estimation device according to this embodiment. As shown in FIG. 5A, even if the measured moving speed data contains errors due to a decrease in the S / N ratio (circled in the figure), such as those caused by the thickness of window glass or water droplets due to rain or snow, the moving speed estimation device 1 according to this embodiment performs Kalman filtering using appropriate observation noise based on the S / N ratio, as described above, thereby estimating a moving speed from which the errors have been removed. Also, as shown in FIG. 5B, even if the measured moving speed data contains errors due to speed jumps (circled in the figure), the moving speed estimation device 1 according to this embodiment performs Kalman filtering by temporarily increasing the observation noise set based on the S / N ratio, as described above, thereby removing the errors and estimating a moving speed supplemented by the speed based on the acceleration sensor. Furthermore, as shown in FIG. 5(C), even if significant errors due to speed jumps appear in the data of measured moving speeds during snowfall, the moving speed estimation device 1 according to this embodiment temporarily increases the observation noise set based on the S / N ratio and performs Kalman filter processing, as described above, so that the errors are removed and a moving speed supplemented by the speed based on the acceleration sensor is estimated.

[0059] As described above, the travel speed estimation device 1 and travel speed estimation method according to this embodiment determine whether or not there is a speed jump (appears as an increase in the amount of change in the measured travel speed), and if there is a speed jump, perform Kalman filter processing by increasing the observation noise to a level higher than the observation noise set based on the signal-to-noise ratio (SN ratio). This makes it possible to remove errors other than Gaussian white noise contained in the measured travel speed of the moving object, and to estimate the travel speed of the moving object with high accuracy. Furthermore, since the temporarily increased observation noise is reduced after a predetermined time has elapsed before performing Kalman filter processing, it is possible to prevent the posterior estimated value of Kalman filter processing from diverging.

[0060] (Embodiment 2) Next, a description will be given of a travel speed estimation device 1 and a travel speed estimation method according to Embodiment 2 of the present invention. Note that the same components as those in Embodiment 1 are designated by the same reference numerals and detailed description thereof will be omitted.

[0061] The second embodiment differs from the first embodiment in that the observation noise determination unit 51 sets the threshold value of the amount of change in the measured moving speed, which is the criterion for determining the observation noise, based on the variance value of the acceleration of the moving object.

[0062] FIG. 6 is a flowchart showing the processing procedure in the observation noise determination unit 51 of the moving speed estimation device 1 of this embodiment.

[0063] The observation noise determination unit 51 first receives the acceleration of the moving object from the acceleration sensor 6, calculates the variance of the acceleration, and determines whether the variance is large or small (step S4). The reason for checking the magnitude of the acceleration variance is to determine whether the moving object is moving (traveling) or stationary. If the variance is large, the moving object is determined to be moving (traveling), and if the variance is small, the moving object is determined to be stationary. This determination is made because, when the moving object is traveling, a force is applied in a zigzag manner in the yaw axis direction, which is thought to cause variance in the measured acceleration. Furthermore, when the moving object is moving, the shaking of the moving object is thought to be greater than when the moving object is stationary, resulting in greater variance in the amount of change in speed. Therefore, when the moving object is moving (i.e., when the acceleration variance is large), the threshold value for the amount of change in the measured moving speed is set to be larger than when the moving object is stationary (i.e., when the acceleration variance is small).

[0064] Specifically, if the variance of acceleration is large (step S4: No), the threshold for the amount of change in measured travel speed used in the subsequent step S1 is set to a large value (step S5), and if the variance of acceleration is small (step S4: Yes), the threshold for the amount of change in measured travel speed used in the subsequent step S1 is set to a small value (step S6). Then, using the threshold set in steps S5 and S6, it is determined in step S1 whether the amount of change in measured travel speed (the difference between the measured travel speed of the current process and the estimated travel speed estimated using the measured travel speed of the previous process) is equal to or greater than the threshold. The subsequent processing procedure is the same as in embodiment 1.

[0065] Here, the threshold for the amount of change in the measured moving speed is either large or small, but this is not limiting and the threshold may be further subdivided and set according to the variance of the acceleration. When choosing between large and small, for example, the large threshold may be set to about 3 to 5 m / s and the small threshold to about 1 to 2 m / s.

[0066] As described above, according to the moving speed estimation device 1 and the moving speed estimation method of this embodiment, the magnitude of the threshold (threshold of the amount of change in the measured moving speed) that serves as a reference for determining whether or not to increase the observation noise is determined based on the variance value of the acceleration of the moving body. This makes it possible to use more appropriate observation noise in the Kalman filter processing, and ultimately makes it possible to estimate the moving speed of the moving body with higher accuracy. It is possible to prevent the posterior estimated values ​​of the Mann filter processing from diverging.

[0067] (Embodiment 3) Next, a description will be given of a travel speed estimation device 1A and a travel speed estimation method according to Embodiment 3 of the present invention. Note that the same components as those in Embodiment 1 are designated by the same reference numerals and detailed description thereof will be omitted.

[0068] Because the acceleration in the traveling direction measured by the acceleration sensor 6 is superimposed with a gravity component offset, if the measured acceleration is used directly in the speed calculation (more specifically, the above-mentioned Kalman filter processing), the accuracy of estimating the moving speed will be significantly degraded. Therefore, when calculating the speed, it is necessary to estimate an acceleration offset value and remove the estimated offset value from the measured acceleration value (see equations (1) and (3)). Conventionally, the acceleration offset value has been estimated while the vehicle is stopped. However, with this estimation method, even after the vehicle has been traveling for a long time, the speed is calculated using the offset value estimated the previous time the vehicle was stopped, which reduces the accuracy of the moving speed calculation. A device has also been disclosed that estimates the acceleration sensor offset value not only when the moving object is stopped but also when the moving object is traveling (see, for example, JP 2024-092397 A). However, with this device, even when the moving object is traveling, if the speed does not exceed a predetermined speed, the offset estimation is not performed, and the estimation of the acceleration offset value depends on the traveling speed of the moving object.

[0069] Therefore, the moving speed estimation device 1A according to this embodiment makes it possible to estimate the acceleration offset value of a moving object with high accuracy, regardless of the moving state (whether the moving object is stopped or moving) or the moving speed (moving speed) of the moving object. Specifically, as shown in Fig. 7, the moving speed estimation unit 5 further includes an acceleration offset estimation unit 53, which estimates the acceleration offset value of the moving object. In this respect, it differs from the first embodiment.

[0070] The acceleration offset estimation unit 53 estimates the acceleration (measured value) a obtained by the acceleration sensor 6. t and the estimated moving speed (posterior estimated value) v calculated by the Kalman filter unit 52. t|t Based on this, the acceleration offset value A is calculated at a predetermined period. offset Here, the "predetermined period" is the output period of the measured moving speed by the radar speedometer 3.

[0071] 8 is a flowchart showing the procedure of the acceleration offset value estimation process in the acceleration offset estimation unit 53 of the travel speed estimation device 1A of this embodiment. t When the acceleration offset is input, the acceleration offset estimation unit 53 calculates the acceleration (measured value) a of the past N samples as shown in Equation (8). t Using this, acceleration (measured value) a t The average value of these values ​​is calculated (step S11).

[0072]

number

[0073] Here, the variables in equation (8) are as follows:

[0074] a t : Acceleration (measured value) at time t (m / s / s) a - t : The average value (m / s / s) of the acceleration (measurement value) of the past N samples at time t (Note that "a - t " is written with " just above the "a". - " is used to indicate that the item is marked with ".)

[0075] The acceleration offset estimation unit 53 receives a new estimated moving speed (a posteriori estimated value at the current time t) v from the Kalman filter unit 52. t|t When input, the estimated moving speed (post-estimated value at current time t) v is calculated using the least squares method etc. t|t The change in velocity (acceleration, gradient) v t|t is calculated (step S12).

[0076] Next, the acceleration offset estimation unit 53 determines whether the SN ratio (observed value) at the current time t (present) input from the information acquisition unit 4 is equal to or greater than a predetermined threshold (step S13). The reason for checking the SN ratio here is to determine the reliability of the radar speedometer 3. Note that the "predetermined threshold" is not limited to a specific value, and may be estimated appropriately, for example, taking into consideration a noise level that ensures the reliability of the radar speedometer 3.

[0077] If the SN ratio (observed value) at the current time t is equal to or greater than a predetermined threshold (if the SN ratio is good, if the noise is small) (step S13: Yes), it is further determined whether the amount of change in the measured moving speed is equal to or greater than a predetermined threshold (step S14). The reason for checking the magnitude of the amount of change in the measured moving speed is that even if the SN ratio is good, if there are speed jumps (if the amount of change in the measured moving speed is large), the reliability of the radar speedometer 3 cannot be said to be good.

[0078] If the measured moving speed at the current time t is less than a predetermined threshold value (see embodiment 1) (if there is no speed jump) (step S14: No), the acceleration offset estimation unit 53 determines that the measured moving speed at the current time t obtained from the radar speedometer 3 (more specifically, the estimated moving speed of the moving body calculated from the measured moving speed) is reliable, and calculates the acceleration offset value A offset Specifically, as shown in equation (9), the acceleration (measured value) a t The average value of a - t and the velocity change v t|t The difference ε t Calculate the difference ε as shown in equation (10). t The average value of ε - t is calculated and used as the acceleration offset value A offset (step S17).

[0079]

number

[0080] Here, the variables in equations (9) and (10) are as follows:

[0081] v t|t : Estimated movement speed at time t, estimated using information up to time t v· t|t : The amount of change in the estimated moving speed at time t (Note that "v·" indicates that there is a "·" immediately above "v") ε t :Acceleration a at time t t The average value of a - t and the change in estimated moving speed v· t|t Difference from ε - t :Difference ε t is the average value of the acceleration offset value A at time t. offset (m / s / s) (Note: "ε - " is written directly above "ε". - " is used to indicate that the item is marked with ".)

[0082] On the other hand, if the SN ratio (observed value) at the current time t is less than the predetermined threshold (if the SN ratio is poor or if there is a lot of noise) (step S13: No), or if the measured moving speed at the current time t is equal to or greater than the predetermined threshold (if there is a speed jump) (step S14: Yes), the acceleration offset estimation unit 53 determines that the measured moving speed at the current time t obtained from the radar speedometer 3 (more specifically, the estimated moving speed of the moving body calculated from the measured moving speed) is unreliable, and calculates the acceleration offset value A offset In this case, the acceleration offset estimating unit 53 stops estimating (updating) the acceleration offset value ε calculated at the previous time t−1, for example. - t-1 Acceleration offset value A offset (step S18).

[0083] The acceleration offset value A estimated by the acceleration offset estimation unit 53 offsetis output to the Kalman filter unit 52 and is used to calculate the estimated moving speed (posterior estimated value) by the above-mentioned Kalman filter processing (more specifically, to calculate the a priori estimated value).

[0084] The acceleration offset estimation unit 53 estimates the acceleration offset value A when the SN ratio (observed value) at the current time t is less than a predetermined threshold or the measured moving speed at the current time t is equal to or greater than a predetermined threshold. offset If the estimation (update) of is stopped, the acceleration offset value A offset Here, the "predetermined time" is not limited to a specific value, and may be set appropriately in consideration of the time required for the SN ratio to recover and for the correct moving speed to be measured. For example, it may be set to about several seconds, or the difference ε of the past M samples may be set to about 1000. t The average value of ε - t It is conceivable to set it to the time required to calculate (the time required for the process shown in equation (10)).

[0085] In this embodiment, the acceleration offset estimation (update) process (a series of processes from steps S11 to S18) is performed at a predetermined cycle (a measured moving speed output cycle) regardless of whether the moving body is moving or stopped, as described above. When the moving body is stopped, the speed change amount v· of the estimated moving speed at time t is calculated. t|t is zero, so the acceleration offset value A offset is the average value a of the acceleration (measured value) at time t - t This becomes:

[0086] FIG. 9 shows data on the measured moving speed of a moving object measured by a radar speedometer 3 (FIG. 9-1), and data on the estimated moving speed and acceleration offset value calculated from the measured moving speed (Example & Comparative Example) (FIGS. 9-2 and 9-3). The example in FIG. 9-2 uses the moving speed estimation device 1A according to this embodiment to estimate the acceleration offset value A when the SN ratio is less than a predetermined threshold (when the SN ratio is poor) or when the measured moving speed is equal to or greater than a predetermined threshold (when there is a speed jump).offset 9-3 shows an example in which the estimation of the acceleration offset value A is stopped and the estimated moving speed is estimated, and the comparative example of FIG. ... offset An example of estimating the estimated moving speed by continuing the estimation is shown below.

[0087] As shown in Figure 9-1, the measured moving speed data contains errors (the whisker-like parts included in the circled area in the figure) due to a decrease in the signal-to-noise ratio caused by factors such as the thickness of the window glass or water droplets from rain or snowfall, as well as speed jumps.

[0088] In contrast, the moving speed estimation device 1A according to this embodiment calculates the acceleration offset value A when the SN ratio is less than a predetermined threshold (when the SN ratio is poor) or when the measured moving speed is equal to or greater than a predetermined threshold (when there is a speed jump). offset In order to stop the estimation (update) of the acceleration offset, as shown in Figure 9-2(B), the acceleration offset value A is set to a value close to the true value of the acceleration offset (the acceleration offset value predicted from the GNSS velocity and assumed to be correct, dashed line). offset can be estimated, and as a result, the acceleration offset value A offset By performing Kalman filter processing using the above, it is possible to highly accurately estimate a moving speed (estimated moving speed) from which errors contained in the measured moving speed have been removed.

[0089] On the other hand, in the device in which the configuration of the acceleration offset estimation unit 53 of the moving speed estimation device 1A according to this embodiment is changed, the acceleration offset value A is calculated even when the SN ratio is less than the predetermined threshold (when the SN ratio is poor) or when the measured moving speed is equal to or greater than the predetermined threshold (when there is a speed jump). offset In order to continue estimating (updating) the acceleration offset value A, which deviates significantly from the true value of the acceleration offset (dashed line) during periods with poor S / N ratio, as shown in Figure 9-3(B), offset and such an acceleration offset value Aoffset Since the Kalman filter process is performed using the above, the estimated moving speed (estimated moving speed) still contains (or is amplified by) the error in the measured speed, as shown in Figure 9-3(A).

[0090] As described above, according to the moving speed estimation device 1A and the moving speed estimation method of this embodiment, when an estimated moving speed is calculated by Kalman filter processing from the measured moving speed of the moving body, estimation of the acceleration offset value is continued at a predetermined cycle, so it is possible to estimate the acceleration offset value of the moving body regardless of the moving state (whether the moving body is stopped or moving) or the moving speed (moving speed). Moreover, when the SN ratio is less than a predetermined threshold (when the SN ratio is poor) or when the measured moving speed is equal to or greater than a predetermined threshold (when there is a speed jump), an error may be included in the estimated moving speed calculated by Kalman filter processing, but the acceleration offset value A is calculated based on the estimated moving speed that may include such an error. offset Therefore, the acceleration offset value A of the moving object is accurately and highly accurate. offset It is possible to estimate the moving speed of the moving object with high accuracy.

[0091] Furthermore, according to the travel speed estimation device 1A and travel speed estimation method of this embodiment, the acceleration offset value A is offset Therefore, the estimation of the acceleration offset A is restarted. offset This eliminates the need to use the acceleration offset value A of the moving object to estimate the acceleration offset value A more accurately and with higher precision. offset It is possible to estimate the moving speed of the moving object with high accuracy.

[0092] The above describes an embodiment of the present invention, but the specific configuration is not limited to the above embodiment, and even if there are design changes within the scope of the present invention that do not deviate from the gist of the present invention, they are included in the present invention.

[0093] For example, in the above embodiment, the observation noise determination unit 51 presets a value of the observation noise based on an expected S / N ratio, and when it is determined that the amount of change in the measured moving speed is less than a predetermined threshold (step S1: No), it selects the observation noise corresponding to the S / N ratio (observed value) of the current processing (present) from the observation noises preset based on the S / N ratio. However, after it is determined that the amount of change in the measured moving speed is less than the predetermined threshold (step S1: No), it is also possible to set a new observation noise based on the S / N ratio (observed value) of the current processing (present). [Explanation of symbols]

[0094] S Movement Speed ​​Estimation System 1, 1A moving speed estimation device 2. Control section 21 CPU 22 ROM 23 RAM 3. Radar speedometer (radar device) 4 Information acquisition unit (information acquisition means) 5 Traveling speed estimating section (moving speed estimating means) 51 Observation noise determination unit 52 Kalman filter section 53 Acceleration offset estimation unit (acceleration offset estimation means) 6 Acceleration Sensor

Claims

1. an information acquisition means for acquiring a measured moving speed and a signal-to-noise ratio of the moving object from a radar device mounted on the moving object; a moving speed estimating means for setting an observation noise based on the signal-to-noise ratio, performing a Kalman filter process using the observation noise, and calculating an estimated moving speed of the moving object from the measured moving speed; The moving speed estimation means When the change amount of the measured moving speed is less than a predetermined threshold, a Kalman filter process is performed using the observation noise set based on the signal-to-noise ratio. When the amount of change in the measured moving speed is equal to or greater than a predetermined threshold, the observation noise is increased to be greater than the observation noise set based on the signal-to-noise ratio, and after a predetermined time has elapsed, the observation noise is decreased to be closer to the observation noise set based on the signal-to-noise ratio, and a Kalman filter process is performed. A travel speed estimation device characterized by:

2. the moving speed estimation means sets the predetermined threshold value based on a variance value of the acceleration of the moving object; The travel speed estimation device according to claim 1 .

3. an acceleration offset estimation means for estimating an acceleration offset value at a predetermined period based on the acceleration of the moving body and the estimated moving speed, the information acquisition means acquires the acceleration from an acceleration sensor mounted on the moving body; the acceleration offset estimation means stops estimating the acceleration offset value when the signal-to-noise ratio is less than a predetermined threshold or when the amount of change in the measured moving velocity is equal to or greater than the predetermined threshold. The travel speed estimation device according to claim 1 .

4. the acceleration offset estimating means resumes estimating the acceleration offset value after a predetermined time has elapsed since the acceleration offset value estimation was stopped.

4. The moving speed estimation device according to claim 3.

5. acquiring a measured moving speed and a signal-to-noise ratio of the moving object from a radar device mounted on the moving object; setting an observation noise based on the signal-to-noise ratio, performing a Kalman filter process using the observation noise, and calculating an estimated moving speed of the moving object from the measured moving speed; When the change amount of the measured moving speed is less than a predetermined threshold, a Kalman filter process is performed using the observation noise set based on the signal-to-noise ratio. When the amount of change in the measured moving speed is equal to or greater than a predetermined threshold, the observation noise is increased to be greater than the observation noise set based on the signal-to-noise ratio, and after a predetermined time has elapsed, the observation noise is decreased to be closer to the observation noise set based on the signal-to-noise ratio, and a Kalman filter process is performed. A moving speed estimation method characterized by:

6. further comprising a step of setting the predetermined threshold value based on a variance value of the acceleration of the moving object. The method for estimating a moving speed according to claim 5 .

7. acquiring an acceleration of the moving body from an acceleration sensor mounted on the moving body; and estimating an acceleration offset value at a predetermined period based on the acceleration and the estimated moving speed, If the signal-to-noise ratio is less than a predetermined threshold or if the change in the measured moving velocity is equal to or greater than the predetermined threshold, estimating the acceleration offset value is stopped. The method for estimating a moving speed according to claim 5 .

8. restarting the estimation of the acceleration offset value after a predetermined time has elapsed since the estimation of the acceleration offset value was stopped; The method for estimating a moving speed according to claim 7 .

Images