JP2013113789A - Speed estimation device and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To stably estimate a speed vector of a traveling object with high accuracy even in environment having a few observation satellites.SOLUTION: Vehicle speed data V, yaw rate data ω, and GPS data are acquires in time series and stored in a time series vehicle speed storage part 20, a time series yaw rate storage part 22 and a time series GPS data storage part 24, respectively. An azimuth angle time change calculation part 26 calculates azimuth angle time change θon the basis of the time series yaw rate data ω, and a satellite direction speed calculation part 28 calculates a satellite direction speed Vs. An own vehicle speed estimation part 30 substitutes an observed value for an equation for estimating a speed vector on the basis of GPS data and obtained by restricting time change of an unknown parameter changing in each time with a time change in an azimuth angle and a time change in a clock drift, forms an equation for the number of observation satellites of time number×each time, and estimates a speed vector of the own vehicle by using the acquired parameter.

Description

  The present invention relates to a speed estimation apparatus and program, and in particular, estimates a speed vector of a moving body based on GPS (Global Positioning System) information and INS (Inertial Navigation System) information. The present invention relates to a speed estimation device and a program.

  Conventionally, a technique for integrating the Doppler frequency received by the GPS receiver and the wheel speed data of the host vehicle and estimating the speed of the host vehicle has been proposed (for example, see Non-Patent Document 1). In the technique described in Non-Patent Document 1, wheel speed data and Doppler frequency data at the current time are acquired, the vehicle speed is constrained by the wheel speed data, and the speed vector is estimated using the Doppler frequency. When speed estimation is performed using only the Doppler frequency, because there are four unknown parameters, it is necessary to observe four or more GPS satellites. If three or more GPS satellites are observed, the speed can be estimated.

  Also, an autonomous navigation means for detecting the position of the own vehicle by integrating the autonomous movement vectors detected by the turning angle sensor and the distance sensor, and a GPS navigation means for detecting the position of the own vehicle by receiving radio waves from a GPS satellite are provided. In the navigation apparatus provided, GPS movement vectors at a plurality of measurement points indicating the traveling direction and speed of the vehicle by GPS navigation means, and autonomous movement vectors at a plurality of corresponding measurement points indicating the traveling direction and speed of the vehicle by autonomous navigation means. There has been proposed an apparatus for determining the reliability of data detected by GPS navigation means based on the difference between a plurality of difference data indicating the difference between (see, for example, Patent Document 1). In the apparatus described in Patent Document 1, when it is determined that the reliability of the data detected by the GPS navigation means is low, position estimation is performed using the detection value of the autonomous navigation means.

JP 2002-323551 A

Y. Kojima, "Proposal for a new localization method using tightly coupled integration based on a precise estimation of trajectory from GPS Doppler", Proceedings of AVEC2010, Laughborough UK, 2010.

  However, since the technique described in Non-Patent Document 1 uses only the observation data for one time and uses only the constraint condition “vehicle speed constraint by wheel speed”, the number of GPS satellites that can be used is limited. And there are few constraints when performing speed estimation. For this reason, there is a problem that speed estimation cannot be performed in an environment (less than 3) where the number of observed GPS satellites (hereinafter referred to as the number of observed satellites) is small.

  In the technique described in Patent Document 1, when the number of observation satellites is less than 4, GPS data is rejected and autonomous navigation (trajectory estimation using only INS) is performed. Therefore, when a place where the number of observation satellites is less than 4 continues for a long time (autonomous navigation continues for a long time), there is a problem that the position of the vehicle may be greatly shifted.

  The present invention has been made to solve the above problems, and provides a speed estimation apparatus and program capable of stably and accurately estimating the velocity vector of a moving object even in an environment where the number of observation satellites is small. For the purpose.

  In order to achieve the above object, a speed estimation device according to a first aspect of the present invention includes an acquisition unit that acquires GPS data transmitted from a GPS satellite and a yaw rate of a moving object in time series, and a time acquired by the acquisition unit. Based on the yaw rate of the series, the calculation means for calculating the time change of the azimuth angle in the traveling direction of the moving body, and at each time that is formulated based on the plurality of GPS data acquired in time series by the acquisition means An equation for estimating the velocity vector of the moving object, based on an equation in which an unknown parameter that changes at each time is constrained by a time change of the azimuth angle calculated by the calculating means. And estimation means for estimating a vector.

  According to the speed estimation apparatus of the first aspect of the invention, the acquisition unit acquires the GPS data transmitted from the GPS satellite and the yaw rate of the moving object in time series, and the calculation unit acquires the time series acquired by the acquisition unit. Based on the yaw rate, the time change of the azimuth of the moving body in the traveling direction is calculated. The estimating means is an equation for estimating the velocity vector of the moving body at each time, which is formulated based on a plurality of GPS data acquired in time series by the acquiring means, and is unknown that changes at each time The velocity vector of the moving object is estimated based on an equation in which the parameter is constrained by the time change of the azimuth calculated by the calculating means.

  In order to achieve the above object, a speed estimation device according to a second aspect of the present invention includes an acquisition unit that acquires, in time series, GPS data transmitted from a GPS satellite and the magnitude of the speed of a moving object, and the acquisition unit. An equation for estimating a velocity vector of the moving body at each time, which is formulated based on a plurality of GPS data acquired in time series by the unknown parameter that changes at each time, And estimating means for estimating a velocity vector of the moving body based on an equation constrained by linearizing the magnitude of the velocity obtained by the above and the time variation of the clock drift.

  According to the speed estimation device of the second invention, the acquisition means acquires the GPS data transmitted from the GPS satellite and the magnitude of the speed of the moving body in time series. And the estimation means is an equation for estimating the velocity vector of the moving body at each time, which is formulated based on a plurality of GPS data acquired in time series by the acquisition means, and changes every time The unknown parameter estimates the velocity vector of the moving object based on an equation constrained by the magnitude of the velocity acquired by the acquisition means and linearization of the time variation of the clock drift.

  In this way, when estimating the velocity vector of a moving body based on a plurality of GPS data acquired in time series, it is possible to reduce the number of unknown parameters by constraining the time variation of unknown parameters in the equation. it can. Thereby, even in an environment where the number of observation satellites at each time is small, there is a high possibility that an equation greater than the number of unknown parameters can be established, and the velocity vector of the moving object can be estimated stably and accurately.

  In addition, the speed estimation device of the second invention includes a calculation unit that calculates a time change of the azimuth angle in the traveling direction of the moving body based on the time-series yaw rate acquired by the acquisition unit. The estimating means can estimate the velocity vector of the moving body based on an equation in which the unknown parameter is further constrained by a time change of the azimuth angle calculated by the calculating means. Thus, the number of unknown parameters can be reduced as the constraint condition increases.

  The speed estimation device according to the first or second aspect of the invention is a yaw rate use availability determination unit that determines whether the yaw rate is usable in the calculation unit and the estimation unit based on the accuracy of the yaw rate acquired by the acquisition unit. Can be configured.

  The speed estimation device according to the first or second aspect of the present invention determines whether or not the speed magnitude is usable in the estimation means based on the accuracy of the speed magnitude obtained by the obtaining means. It can comprise including a determination means.

  The speed estimation device according to the first or second aspect of the invention is based on at least one of the reception state of GPS data acquired by the acquisition unit, the residual of pseudorange, and the residual of Doppler frequency. The GPS selection means for selecting the GPS data used for the above-mentioned standing can be configured.

  In addition, the speed estimation device according to the first or second aspect of the present invention provides the number of unknown parameters included in the equation according to the number of types of GPS satellites that transmitted each of a plurality of GPS data at a plurality of times acquired by the acquiring unit. The number of unknown parameter selection means for selecting can be configured.

  In this way, by determining the magnitude of speed and availability of yaw rate, selecting GPS data with good reception status, or selecting the number of unknown parameters included in the equation according to the number of types of GPS satellites Thus, the velocity vector of the moving body can be estimated with higher accuracy.

  According to a third aspect of the present invention, there is provided a speed estimation program for obtaining, in a time series, GPS data transmitted from a GPS satellite and a yaw rate of a moving object in a time series, a time series yaw rate obtained by the obtaining means. Based on a plurality of GPS data acquired in chronological order by the acquisition means, and calculating means for calculating the time change of the azimuth angle in the traveling direction of the mobile object, An equation for estimating a velocity vector, wherein the velocity vector of the moving object is estimated based on an equation in which an unknown parameter that changes at each time is constrained by a time change of the azimuth calculated by the calculation means. It is a program for functioning as estimation means.

  According to a fourth aspect of the present invention, there is provided a speed estimation program for acquiring a computer in time series by acquiring the GPS data transmitted from a GPS satellite and the magnitude of the speed of the moving object in time series. An equation for estimating a velocity vector of the moving body at each time, which is formulated based on a plurality of GPS data, and an unknown parameter that changes at each time is acquired by the acquisition means And a program for functioning as an estimation means for estimating the velocity vector of the moving body based on an equation constrained by linearization of the time variation of the clock drift.

  The storage medium for storing the program of the present invention is not particularly limited, and may be a hard disk or a ROM. Further, it may be a CD-ROM, a DVD disk, a magneto-optical disk or an IC card. Furthermore, the program may be downloaded from a server or the like connected to the network.

  As described above, according to the speed estimation apparatus and program of the present invention, when estimating the speed vector of a moving body based on a plurality of GPS data acquired in time series, the time variation of the unknown parameter in the equation. By restricting, the number of unknown parameters can be reduced. This increases the possibility that an equation greater than the number of unknown parameters can be established even in an environment where the number of observation satellites at each time is small, and the speed vector of the moving object can be estimated stably and accurately. It is done.

It is a block diagram which shows the speed estimation apparatus which concerns on 1st Embodiment. It is a figure for demonstrating estimation of the speed vector based on GPS data. It is a flowchart which shows the content of the speed estimation process routine in 1st Embodiment. It is a block diagram which shows the speed estimation apparatus which concerns on 2nd Embodiment. It is a flowchart which shows the content of the speed estimation process routine in 2nd Embodiment.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the present embodiment, a case where the present invention is applied to a speed estimation device that is mounted on a vehicle and estimates a speed vector of the host vehicle will be described as an example.

  As shown in FIG. 1, a speed estimation apparatus 10 according to the first embodiment includes a vehicle speed sensor 12 that detects the magnitude of the speed of the host vehicle, a yaw rate sensor 14 that detects the yaw rate of the host vehicle, and a GPS satellite. The GPS device 16 that receives GPS data transmitted from the vehicle and the computer 18 that estimates the speed vector of the host vehicle are provided.

  The vehicle speed sensor 12 detects vehicle speed data output from the wheel speed. As the vehicle speed sensor 12, a sensor other than the wheel speed sensor other than the GPS device 16, such as a laser radar, a millimeter wave radar, and a stereo camera, may be used.

  The yaw rate sensor 14 may be a gyro sensor or the like, and detects yaw rate data of the host vehicle. As the yaw rate sensor 14, a sensor other than the gyro sensor may be used as long as it is other than the GPS device 16, such as a laser radar or a moving stereo.

  The GPS device 16 receives GPS data from a GPS satellite including a pseudo distance, a Doppler frequency, a satellite number, a satellite position, a satellite speed, and the like necessary for positioning and speed estimation.

  The computer 18 includes a storage device such as a CPU, a ROM that stores a program for realizing a speed estimation processing routine described later, a RAM that temporarily stores data, and an HDD. When this computer 18 is represented by functional blocks in accordance with a speed estimation processing routine described below, as shown in FIG. 1, a time-series vehicle speed accumulating unit 20 that accumulates vehicle speed data acquired in time series, and time-series acquired. A time-series yaw rate storage unit 22 that stores the yaw rate data, a time-series GPS data storage unit 24 that stores GPS data acquired in time series, and an azimuth angle that calculates temporal changes in azimuth from the time-series yaw rate data Based on a time change calculation unit 26, a satellite direction speed calculation unit 28 that calculates a velocity vector from the own vehicle to the GPS satellite direction using GPS data, and an equation that constrains the time change of each observed value and unknown parameter. The vehicle speed estimation unit 30 that estimates the speed vector of the vehicle can be represented by a configuration including the vehicle speed estimation unit 30.

The time-series vehicle speed accumulation unit 20 stores the vehicle speed data detected by the vehicle speed sensor 12 in time series. The trigger for storing data may be when a certain time has elapsed or when traveling for a certain distance. Incidentally, when the vehicle speed data detected by the vehicle speed sensor 12 to the time t and V ^t _wheel, finally estimated to velocity vector (Vx ^t, Vy ^t, Vz ^t) the relationship between the V ^t _wheel, following (1 ) Can be set as shown below.

  In the time series yaw rate accumulation unit 22, the yaw rate data detected by the yaw rate sensor 14 is stored in time series. The trigger for data storage is the same timing as the time-series vehicle speed accumulation unit 20.

  The time series GPS data storage unit 24 stores GPS data acquired by the GPS device 16 in time series. The trigger for data storage is the same timing as the time-series vehicle speed accumulation unit 20.

The azimuth time change calculation unit 26 calculates the relative azimuth (time change of azimuth) θ ^t _gyro at time t with respect to the azimuth at a certain point in the traveling direction of the _{host vehicle} by the following equation (2). Δt represents a time step, and ω ^t represents yaw rate data detected at time t.

  The satellite direction velocity calculation unit 28 calculates a velocity vector (satellite direction velocity) in the GPS satellite direction from the host vehicle using the Doppler frequency included in the GPS data accumulated in the time series GPS data accumulation unit 24.

  Here, FIG. 2 shows an outline of velocity vector calculation using the Doppler frequency. In the speed estimation using the Doppler frequency, the speed component of each GPS satellite is calculated based on the observed Doppler frequency of each GPS satellite, and the speed vector of the host vehicle is obtained by synthesizing each. For this purpose, the velocity component in the direction of each GPS satellite is calculated by the following equation (3).

Here, Vs ^t _i is the magnitude of the own vehicle speed vector from the own vehicle to the GPS satellite i direction, Vxs ^t _i , Vys ^t _i , and Vzs ^t _i are each speed component of the GPS satellite i, and D ^t _i is the Doppler frequency. , F1 is the frequency of the GPS data carrier, C is the speed of light, r ^t _i is the pseudorange between the host vehicle and the GPS satellite i, Gx ^t _i , Gy ^t _i and Gz ^t _i are the line of sight from the host vehicle to the GPS satellite i. _{^{vector, X t si, Y t si}} , Z t si satellite _{^{position, X t v, Y t v}} , Z t v represents the vehicle position. The subscript t represents data at time t, and the subscript i represents data of the i-th GPS satellite. As the vehicle position, a value calculated by using a general positioning method using a pseudo distance or other positioning methods can be used.

The following equation (5) shows the relational expression between the magnitude of the velocity vector from the own vehicle to each GPS satellite direction shown in equation (3) and the velocity vector of the own vehicle to be finally estimated. Cbv ^t is the time variation (clock drift) of the clock error of the GPS device 16.

By substituting Vs ^t _i , Gx ^t _i , Gy ^t _i, and Gz ^t _i calculated from the GPS data into the simultaneous equations shown in equation (5), the speed vector of the host vehicle can be estimated. The satellite direction velocity calculation unit 28 calculates Vs ^t _i , Gx ^t _i , Gy ^t _i, and Gz ^t _i . The satellite direction velocity Vs ^t _i is the relative velocity of the vehicle with respect to each GPS satellite obtained from the Doppler frequency, the velocity vector of each GPS satellite obtained from the time series data of the satellite position included in the GPS data, and the satellite of each GPS satellite. It can be calculated based on the satellite direction obtained from the position and the vehicle position. Gx ^t _i , Gy ^t _i, and Gz ^t _i can be calculated by equation (4).

  The host vehicle speed estimation unit 30 estimates the speed vector of the host vehicle based on an equation for speed estimation constrained by a constraint condition described later. The range of the time series data of each observed value used for speed estimation is determined based on the elapsed time from the current time, the travel distance, and the like. In the following, the time of the oldest data among the time series data used for speed estimation is treated as t = 0 (initial time).

  Here, the principle of the present embodiment will be described.

In solving the simultaneous equations of equation (5), the condition for obtaining the solution is usually “the number of unknown parameters = the number of equations”. In equation (5), Vx ^t , Vy ^t , Vz ^t , and Cbv ^t Are the unknown parameters, the number of unknown parameters at all times of the time-series data is “number of unknown parameters = number of times × 4”. On the other hand, since “the number of formulas = the number of times × the number of observation satellites at each time”, the speed estimation result cannot be obtained if the average number of observation satellites at each time is not four or more. become. In particular, in an environment where the number of observation satellites is small due to the shielding of buildings, such as in urban areas, it is difficult to satisfy the above conditions.

  Therefore, in the present embodiment, by incorporating the following constraint conditions, the number of unknown parameters is reduced, and the possibility that a solution for speed estimation can be obtained even in an environment where the number of observation satellites is small is improved. The constraint conditions used in the present embodiment are the constraint conditions 1 to 3 shown below.

The constraint condition 1 is that the magnitude of each component of the estimated speed vector is constrained by the vehicle speed data, and the time change is constrained by the time change of the azimuth calculated by the azimuth time change calculator 26. Θ ⁰ in constraint condition 1 is the azimuth angle of the traveling direction of the host vehicle at the initial time. The constraint condition 2 assumes that the change in the velocity in the height direction is always minute. The constraint condition 3 assumes that the time change of the clock drift is gentle and the change in a short time is linear. Cbv ⁰ in constraint condition 3 indicates the clock drift at the initial time, and A indicates the slope of the time variation of the clock drift. Here, the case where only the velocity component in the two-dimensional plane is constrained by the constraint conditions 1 and 2 will be described. However, the velocity component in the three-dimensional space is obtained using the pitch rate acquired by a three-axis gyro sensor or the like. You may be restrained. As a result of incorporating the constraint conditions 1 to 3 into the equation (5), the following equation (6) is obtained.

In equation (6), the unknown parameters are θ ⁰ , Cbv ⁰ , and A, and the speed vector of the host vehicle can be estimated by obtaining these unknown parameters. In equation (6), the number of unknown parameters is reduced by constraining the time variation of unknown parameters by a constraint condition using time variation of azimuth and clock drift. In equation (6), the number of unknown parameters is reduced to three, so if “the number of equations = the number of times × the number of observation satellites at each time” is three or more, a speed estimation result can be obtained. For example, even if the number of observation satellites at three times (t ₀ , t ₁ , t ₂ ) is 1, the speed estimation result can be calculated using GPS data from the observed GPS satellites.

Therefore, the host vehicle speed estimation unit 30 estimates the speed vector of the host vehicle using the equation (6) in which the time change is constrained in accordance with the principle of the present embodiment. Specifically, the time-series vehicle speed data V ^t _wheel accumulated in the time-series vehicle speed accumulation unit 20, the azimuth angle change θ ^t _gyro calculated by the azimuth angle change calculation unit 26, and the satellite direction velocity calculation unit Substituting the satellite direction speeds Vs ^t _i and Gx ^t _i , Gy ^t _i , and Gz ^t _i calculated in 28 into the equation (6), equations for the number of times × the number of observation satellites at each time are formed. By solving these equations, the unknown parameters θ ⁰ , Cbv ⁰ , and A are obtained. Then, the obtained parameter θ ⁰ , the vehicle speed data V ^t _wheel , and the time change θ ^t _gyro of the azimuth are substituted into constraint condition 1 to calculate the speed vector of the _host vehicle.

  Next, the operation of the speed estimation device 10 according to the first embodiment will be described.

At each time t, the vehicle speed sensor 12 detects the vehicle speed data V ^t _wheel , the yaw rate sensor 14 detects the yaw rate data ω ^t , and the GPS device 16 receives the GPS data. A speed estimation processing routine shown in FIG.

In step 100, the vehicle speed data V ^t _wheel detected by the vehicle speed sensor 12, the yaw rate data ω ^t detected by the yaw rate sensor 14, and the GPS data received by the GPS device 16 are acquired in time series as observed values. Each of them is stored in the time-series vehicle speed accumulation unit 20, the time-series yaw rate accumulation unit 22, and the time-series GPS data accumulation unit 24.

Next, in step 102, the time variation θ ^t _gyro of the azimuth angle is calculated from the time series yaw rate data ω ^t stored in step 100 by the equation (2).

Next, in step 104, the satellite direction velocity Vs ^t _i is calculated based on the time-series GPS data stored in step 100. Further, Gx ^t _i , Gy ^t _i, and Gz ^t _i are calculated by the equation (4).

Next, in step 106, the vehicle speed data V ^t _wheel acquired in step 100, the time change θ ^t _gyro of the azimuth calculated in step 102, and the step 104 are _{added to} equation (6) in which the time change is constrained. Substituting Vs ^t _i , Gx ^t _i , Gy ^t _i , and Gz ^t _i calculated in step 4, equations for the number of times × the number of observation satellites at each time are formed.

Next, in step 108, the equations established in step 106 are solved to obtain unknown parameters θ ⁰ , Cbv ⁰ , and A. Then, the obtained parameter θ ⁰ , the vehicle speed data V ^t _wheel , and the azimuth angle change θ ^t _gyro are substituted into the constraint condition 1 to calculate and output the speed vector of the _host vehicle, and a speed estimation processing routine Exit.

  As described above, according to the speed estimation device of the first embodiment, when estimating the speed vector of the host vehicle using GPS data at a plurality of times, the time of the unknown parameter in the equation for speed estimation By constraining the change, the number of unknown parameters is reduced. Thereby, even in an environment where the number of observation satellites at each time is small, there is a high possibility that an equation greater than the number of unknown parameters can be established, and the speed vector of the host vehicle can be estimated stably and accurately.

  In the first embodiment, a case has been described in which all of the constraints due to the temporal change in the vehicle speed data and the azimuth and the constraints due to the temporal change in the clock bias are used. However, the present invention is not limited to this. The constraint condition may be only the vehicle speed data, the time change of the azimuth angle, or the time change of the clock bias, or may be a constraint condition appropriately combining them. However, from the viewpoint of reducing the number of unknown parameters by constraining the time variation, it is desirable that the constraint conditions use at least one of the time variation of the azimuth angle and the time variation of the clock bias.

When only the time change of the azimuth is used as the constraint condition, since there is no constraint of V ^t _wheel , θ ⁰ , and Cbv ^t (Cbv ^t = Cbv ⁰ + At in equation (6), equation (6) “Cbv ⁰ -At” of “Cbv ^t ”) is an unknown parameter, and the number of unknown parameters at all times of the time-series data is “time number × 2 + 1”. Although the number of unknown parameters is larger than when all constraint conditions are applied, the number of unknown parameters can be reduced as compared with the case where there is no constraint condition (number of unknown parameters = time number × 4).

Further, when only the time change of the clock drift is used as the constraint condition, since V ^t _wheel , θ ⁰ , θ ^t _gyro , Cbv ⁰ , and A are unknown parameters in the equation (6), the time series The number of unknown parameters at all times of the data is “number of times × 2 + 3”, and the number of unknown parameters can be reduced as described above.

In addition, when time variation of clock drift and vehicle speed data are used as restraint conditions, θ ⁰ , θ ^t _gyro , Cbv ⁰ , and A are unknown parameters in the equation (6). The number of unknown parameters at all times is “time number × 1 + 3”, and the number of unknown parameters can be reduced in the same manner as described above.

  Next, a second embodiment will be described. Note that in the speed estimation device of the second embodiment, the same components as those of the speed estimation device 10 of the first embodiment are denoted by the same reference numerals, and detailed description thereof is omitted.

  As illustrated in FIG. 4, the speed estimation device 210 according to the second embodiment includes a vehicle speed sensor 12, a yaw rate sensor 14, a GPS device 16, and a computer 218. When this computer 218 is expressed in functional blocks according to a speed estimation processing routine described below, as shown in FIG. 4, a vehicle speed availability determination unit 32 that determines availability of vehicle speed data detected by the vehicle speed sensor 12, and a yaw rate A yaw rate availability determination unit 34 for determining whether the yaw rate data detected by the sensor 14 is usable; a satellite selection unit 36 for selecting an rGPS satellite in a good reception state from the observed GPS satellites; and a time-series vehicle speed accumulation unit 20; The time series yaw rate accumulation unit 22, the time series GPS data accumulation unit 24, the azimuth time change calculation unit 26, the satellite direction velocity calculation unit 28, and the number of types of GPS satellites included are included in the equation. An unknown parameter number selection unit 38 that selects the number of unknown parameters, and a selection that constrains each observed value and the amount of time variation of the unknown parameters. Based on the equation of the number of unknown parameters, the vehicle speed estimation unit 230 that estimates the speed vector of the vehicle can be represented.

  The vehicle speed availability determination unit 32 determines the reliability of the vehicle speed data detected by the vehicle speed sensor 12 and determines whether the vehicle speed data is usable. For example, when the vehicle speed data is detected from the wheel speed, the accuracy of the vehicle speed data is deteriorated due to a decrease in the wheel speed resolution capability at a low speed, so that it is determined that vehicle speed data below a certain value cannot be used. Further, at the time of a curve, an error in the vehicle speed data increases due to tire slip, so that it is determined that the vehicle speed data detected when the yaw rate data is a certain value or more cannot be used.

  The yaw rate availability determination unit 34 determines the reliability of the yaw rate data detected by the yaw rate sensor 14 and determines whether the yaw rate data is usable. For example, at the time of a curve, since a slip occurs between the traveling direction of the vehicle and the direction of the vehicle due to tire slip, it is determined that yaw rate data of a certain value or more cannot be used.

  The satellite selection unit 36 selects GPS data from a GPS satellite with a good reception state from the received GPS data. In central Tokyo, GPS signals may be reflected (multipath) by buildings, and the accuracy of pseudorange and Doppler frequency may deteriorate. For example, the SN ratio, pseudorange or Doppler frequency when GPS data is received If the residual of is large, it is determined that the GPS data is erroneous and is eliminated.

  The unknown parameter number selection unit 38 selects the number of unknown parameters included in the equation used by the host vehicle speed estimation unit 230. Normally, increasing the number of unknown parameters makes it possible to estimate the speed with higher accuracy, but if the number of observed GPS satellites is less than the number of unknown parameters, the solution increases instability and is more accurate. The speed estimation result may not be obtained. Therefore, the unknown parameter number selection unit 38 uses the vehicle speed estimation unit 230 according to the number of types of GPS satellites observed at all times in the time series GPS data stored in the time series GPS data storage unit 24. Select the number of unknown parameters for speed estimation. The number of types of GPS satellites can be obtained by counting the number of GPS satellites at all times without duplication. For example, the number of types of observed GPS satellites is divided into “3”, “4”, “5 or more”, and the number of unknown parameters in each case is set to “3”, “4”, “5”, respectively. Can be selected.

The host vehicle speed estimation unit 230 estimates an equation corresponding to the number of unknown parameters selected by the number of unknown parameter selection unit 38 and then estimates a speed vector of the host vehicle. For example, when the number of selected parameters is “3”, the equation is established by the equation (6) as in the first embodiment. In this case, there are three unknown parameters, θ ⁰ , Cbv ⁰ , and A. When the number of selected unknown parameters is “4”, an equation is established by the following equation (7). In equation (7), the offset error ω _offset of the yaw rate sensor 14 is added as an unknown parameter. That is, there are four unknown parameters, θ ⁰ , Cbv ⁰ , A, and ω _offset . When the number of selected unknown parameters is “5”, the equation is established by the following equation (8). In equation (8), the scale error S of the vehicle speed sensor 12 is further added as an unknown parameter. That is, there are five unknown parameters: θ ⁰ , Cbv ⁰ , A, ω _offset , and S.

  Next, the operation of the speed estimation device 210 according to the second embodiment will be described.

At each time t, the vehicle speed sensor 12 detects the vehicle speed data V ^t _wheel , the yaw rate sensor 14 detects the yaw rate data ω ^t , and the GPS device 16 receives the GPS data. A speed estimation processing routine shown in FIG. In addition, about the process same as the speed estimation process routine of 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

In step 100, each observation value is acquired in time series. Next, in step 200, it is determined whether or not the vehicle speed data V ^t _wheel at each time t acquired in step 100 is usable. The vehicle speed data V ^t _wheel determined to be usable is stored in the time-series vehicle speed storage unit 20, and the vehicle speed data V ^t _wheel determined to be unusable is excluded.

Next, in Step 202, it is determined whether or not the yaw rate data ω ^t at each time t acquired in Step 100 is usable. The yaw rate data ω ^t determined to be usable is stored in the time-series yaw rate accumulation unit 22, and the yaw rate data ω ^t determined to be unusable is excluded.

  Next, in step 204, in the GPS data from each GPS satellite obtained at step t acquired in step 100, based on the SN ratio at the time of receiving the GPS data, the pseudorange or the residual of the Doppler frequency, Select GPS data with good reception. GPS data determined to have a good reception state is stored in the time-series GPS data storage unit 24, and GPS data determined to have a poor reception state is excluded.

Next, in step 102, the time variation θ ^t _gyro of the azimuth angle is calculated. Next, in step 104, the satellite direction velocity s ^t _i , and Gx ^t _i , Gy ^t _i and Gz ^t _{i in the} equation (4) Is calculated.

  Next, in step 206, the number of types of GPS satellites observed at all times in the time series data is obtained, and the number of unknown parameters included in the equation established in step 208 in the subsequent stage according to the number of types of GPS satellites. Select.

Next, at step 208, the vehicle speed data V ^t _wheel stored at step 200 and the time variation θ ^t _gyro of the azimuth calculated at step 102 are _added to the equation corresponding to the number of unknown parameters selected at step 206. , And Vs ^t _i , Gx ^t _i , Gy ^t _i , and Gz ^t _i calculated in the above step 104 are substituted, and equations for the number of times × the number of observation satellites at each time are formed. At this time, the equation is not established for the time t when the vehicle speed data V ^t _wheel or yaw rate data ω ^t determined to be unusable in step 200 or 202 is present.

Next, in step 108, the equation formed in step 208 is solved to obtain unknown parameters for the number of selected unknown parameters, and the obtained parameter θ ⁰ , vehicle speed data V ^t _wheel , and azimuth angle are calculated. Substituting time variation θ ^t _gyro into constraint condition 1, calculates and outputs the speed vector of the _host vehicle, and ends the speed estimation processing routine.

  As described above, according to the speed estimation device of the second embodiment, the availability of vehicle speed data and yaw rate data is determined, GPS data with a good reception state is selected, and the observed GPS satellites are By selecting the number of unknown parameters included in the equation according to the number of types, the accuracy of estimating the velocity vector is further improved.

  In the above embodiment, the speed estimation device mounted on the vehicle has been described. However, the moving body on which the speed estimation device of the present invention is mounted is not limited to the vehicle. For example, the speed estimation device may be mounted on a robot, or the speed estimation device may be configured as a portable terminal so that a pedestrian can carry it.

DESCRIPTION OF SYMBOLS 10,210 Speed estimation device 12 Vehicle speed sensor 14 Yaw rate sensor 16 GPS device 18, 218 Computer 20 Time series vehicle speed accumulation unit 22 Time series yaw rate accumulation unit 24 Time series GPS data accumulation unit 26 Azimuth angle time change calculation unit 28 Satellite direction velocity calculation Units 30 and 230 Own vehicle speed estimation unit 32 Vehicle speed availability determination unit 34 Yaw rate availability determination unit 36 Satellite selection unit 38 Unknown parameter number selection unit

Claims (9)

Acquisition means for acquiring GPS data transmitted from a GPS satellite and yaw rate of a moving object in time series;
Based on the time-series yaw rate acquired by the acquisition means, calculation means for calculating the time change of the azimuth angle of the traveling direction of the moving body;
An equation for estimating a velocity vector of the moving body at each time, which is formulated based on a plurality of GPS data acquired in time series by the acquisition means, and an unknown parameter that changes at each time is calculated Estimating means for estimating a velocity vector of the moving body based on an equation constrained by a time change of the azimuth calculated by the means;
A speed estimation device including: Acquisition means for acquiring, in time series, GPS data transmitted from a GPS satellite and the magnitude of the speed of the moving object;
An equation for estimating a velocity vector of the moving body at each time that is formulated based on a plurality of GPS data acquired in time series by the acquisition means, and an unknown parameter that changes at each time, Estimating means for estimating a velocity vector of the moving body based on an equation constrained by the magnitude of the speed acquired by the acquiring means and linearization of the time variation of clock drift;
A speed estimation device including: Based on the time-series yaw rate acquired by the acquisition means, including a calculation means for calculating the time change of the azimuth of the moving direction of the moving body,
The speed estimation apparatus according to claim 2, wherein the estimation unit estimates the velocity vector of the moving body based on an equation in which the unknown parameter is further constrained by a temporal change in the azimuth angle calculated by the calculation unit.   The speed estimation apparatus according to claim 1 or 3, further comprising a yaw rate availability determination unit that determines whether or not the yaw rate is usable in the calculation unit and the estimation unit based on the accuracy of the yaw rate acquired by the acquisition unit.   4. The speed estimation according to claim 2, further comprising: a speed availability determination unit that determines whether the estimation unit can use the speed magnitude based on the accuracy of the speed magnitude acquired by the acquisition unit. 5. apparatus.   GPS selection means for selecting GPS data to be used for the equation based on at least one of a reception state of GPS data acquired by the acquisition means, a residual of pseudorange, and a residual of Doppler frequency The speed estimation device according to any one of claims 1 to 5.   2. An unknown parameter number selection unit that selects the number of unknown parameters included in the equation according to the number of types of GPS satellites that transmitted each of a plurality of GPS data at a plurality of times acquired by the acquisition unit. Item 7. The speed estimation device according to any one of items 6 to 6. Computer
Acquisition means for acquiring GPS data transmitted from a GPS satellite and yaw rate of a moving object in time series,
Based on the time series yaw rate acquired by the acquisition means, a calculation means for calculating the time change of the azimuth of the moving body in the traveling direction, and based on a plurality of GPS data acquired in time series by the acquisition means The equation for estimating the velocity vector of the moving body at each time is expressed by an equation in which the unknown parameter that changes at each time is constrained by the time change of the azimuth calculated by the calculation means. A speed estimation program for functioning as an estimation means for estimating the speed vector of the moving body based on the above. Computer
GPS data transmitted from GPS satellites, acquisition means for acquiring the magnitude of the velocity of the moving object in time series, and each time established based on a plurality of GPS data acquired in time series by the acquisition means Is an equation for estimating the velocity vector of the moving body at the time, and the unknown parameter that changes with time is constrained by linearizing the magnitude of the velocity acquired by the acquisition means and the time variation of the clock drift. The speed estimation program for functioning as an estimation means which estimates the speed vector of the said moving body based on said equation.

Images