WO2015186309A1 - 移動距離推定装置 - Google Patents

移動距離推定装置 Download PDF

Info

Publication number
WO2015186309A1
WO2015186309A1 PCT/JP2015/002632 JP2015002632W WO2015186309A1 WO 2015186309 A1 WO2015186309 A1 WO 2015186309A1 JP 2015002632 W JP2015002632 W JP 2015002632W WO 2015186309 A1 WO2015186309 A1 WO 2015186309A1
Authority
WO
WIPO (PCT)
Prior art keywords
speed
value
satellite
estimated
initial
Prior art date
Application number
PCT/JP2015/002632
Other languages
English (en)
French (fr)
Inventor
正剛 隈部
洪二郎 武山
祥子 小島
Original Assignee
株式会社デンソー
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社デンソー filed Critical 株式会社デンソー
Priority to US15/313,596 priority Critical patent/US10365109B2/en
Priority to SG11201609814VA priority patent/SG11201609814VA/en
Publication of WO2015186309A1 publication Critical patent/WO2015186309A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G01C22/02—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers by conversion into electric waveforms and subsequent integration, e.g. using tachometer generator
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/421—Determining position by combining or switching between position solutions or signals derived from different satellite radio beacon positioning systems; by combining or switching between position solutions or signals derived from different modes of operation in a single system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/51—Relative positioning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/14—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by recording the course traversed by the object
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Definitions

  • the present disclosure relates to a moving distance estimation apparatus that estimates the moving distance of a moving body, and more particularly, to a technique that improves the estimation accuracy of the moving distance.
  • the moving distance of the moving body is used together with the moving direction, for example, to estimate the moving trajectory. If the movement trajectory can be estimated, the relative position with respect to the position at a certain time can be determined. Therefore, even when a positioning radio wave transmitted by a satellite positioning system such as GPS cannot be received, the current position can be estimated from the current position and the movement locus measured in the past.
  • Travel distance can be calculated as the product of speed and time.
  • a speed hereinafter referred to as “Doppler speed”
  • Doppler speed a speed
  • the speed calculated from the acceleration is sequentially corrected by the Doppler speed.
  • the speed calculated from the acceleration can be updated in a short period, but an error is more likely to occur than the Doppler speed. Therefore, the speed calculated from the acceleration is sequentially corrected by the Doppler speed. With this correction, an accurate speed can be obtained.
  • Patent Document 1 has an advantage that wiring for acquiring a signal of a vehicle speed sensor becomes unnecessary when applied to a vehicle.
  • Patent Document 2 discloses a technique for accurately estimating a vehicle speed vector. Specifically, in the technique of Patent Document 2, the relationship between the satellite direction vehicle speed, the wheel speed, the line-of-sight vector from the vehicle to the satellite of the positioning system, the vehicle direction, and the clock drift is shown in the estimation of the velocity vector. Use the formula.
  • This formula is a formula obtained by transforming the formula showing the relationship between the satellite direction velocity, the line-of-sight vector, the velocity vector, and the clock drift using a constraint condition.
  • the constraint conditions are a condition that the speed vector is constrained by the speed and yaw rate of the moving body, and a condition that the time variation of the clock drift is linear.
  • Patent Document 2 requires wheel speed. Therefore, the technique of Patent Document 2 cannot be applied to a speed estimation device that cannot be connected to a wheel speed sensor.
  • Patent Document 1 calculates the speed from the acceleration detected by the acceleration sensor provided in the apparatus itself, it is not necessary to connect to the wheel speed sensor.
  • Patent Document 1 corrects the speed calculated from the acceleration by the Doppler speed.
  • the speed calculated from the acceleration is corrected at a speed more accurate than the speed calculated from the acceleration, such as the Doppler speed, until the next correction after correcting the speed calculated from the acceleration.
  • the travel distance is calculated by the product of the speed and time calculated from the acceleration. Since the speed error increases while the speed calculated from the acceleration is used, the error of the moving distance also increases cumulatively. Therefore, the estimation accuracy of the moving distance is not sufficient.
  • the present disclosure has been made based on this situation, and an object of the present disclosure is to provide a movement distance estimation apparatus that can estimate the movement distance more accurately.
  • the travel distance estimation apparatus is determined by a travel direction acceleration determination unit that sequentially determines a travel direction acceleration that is a travel direction component of a detection value of an acceleration sensor that moves with a moving body, and a travel direction acceleration determination unit
  • the integration processing unit that sequentially calculates the acceleration integrated value obtained by integrating the acceleration in the traveling direction, and the speed of the moving body calculated based on the Doppler shift amount from the satellite included in the satellite positioning system, or the speed when the moving body is stopped, Based on the initial set value determination unit determined as the initial velocity value of the moving object, the acceleration integrated value calculated by the integration processing unit, and the initial speed value determined by the initial set value determination unit, the estimated speed of the moving object is sequentially determined.
  • An estimated speed determining unit to be estimated a moving distance calculating unit for sequentially calculating an estimated moving distance of the moving object based on the estimated speed and time determined by the estimated speed determining unit, and an initial set value determination
  • the initial setting value determination unit has previously determined the initial speed value
  • the current initial setting value determination unit has determined the initial speed value
  • the estimated speed determination unit has successively estimated this time.
  • a past speed correction unit that determines a corrected past speed that has been corrected so that the past estimated speed before correction that is an estimated speed is continuous with the initial speed value that has been determined by the initial setting value determination unit, and a past speed correction unit that has corrected the past
  • the initial setting value determination unit determines the initial speed value from the previous time until the initial setting value determination unit determines the initial speed value this time.
  • a movement distance correction unit that recalculates the estimated movement distance.
  • the initial value setting unit moves the speed of the moving body calculated based on the Doppler shift amount of the radio wave from the satellite included in the satellite positioning system or the speed when the moving body is stopped. It is determined as the initial speed of the body.
  • the speed of the moving body calculated based on the Doppler shift amount of the radio wave from the satellite and the speed when the moving body is stopped can be determined with high accuracy. Therefore, the speed initial value accurately represents the speed of the moving object at the time when the speed initial value is determined.
  • the estimated speed is estimated using the initial speed value and the integrated acceleration value. Therefore, after the speed initial value is updated, the accuracy of the estimated speed that is lowered due to the accumulated error of the acceleration integrated value is improved.
  • the initial set value determination unit determines the speed initial value
  • a correction that is an estimated speed from the previous determination of the initial speed value to the current initial speed value is determined.
  • the previous past estimated speed is corrected so as to be continuous with the current initial speed value to obtain a corrected past speed.
  • the accuracy of the past estimated speed which is reduced due to the accumulated error of the acceleration integrated value, is also improved.
  • the corrected past speed is determined, the estimated past distance from the previous determination of the initial speed value to the determination of the initial speed value is recalculated using the corrected past speed. As a result, the accuracy of the estimated moving distance that has been reduced due to a decrease in accuracy of the past estimated speed is also improved.
  • FIG. 1 is a configuration diagram of a current position estimation apparatus according to an embodiment.
  • Figure 2 is a diagram showing an error of the acceleration cumulative value [Delta] V G diverges.
  • FIG. 3 is a detailed configuration diagram of the position update unit of FIG.
  • FIG. 4 is a diagram illustrating speed correction by the past speed correction unit of FIG.
  • FIG. 5 is a flowchart showing the flow of processing performed by the control unit of FIG.
  • FIG. 6 is a detailed flowchart of step S22 of FIG.
  • FIG. 7 is a detailed flowchart of step S28 in FIG.
  • FIG. 8 is a detailed flowchart of step S34 in FIG.
  • a current position estimation apparatus 1 shown in FIG. 1 is an example of a movement distance estimation apparatus.
  • the current position estimation device 1 includes a GPS signal receiving unit 10, an acceleration sensor 20, a yaw rate sensor 30, and a control unit 100, and is mounted on a vehicle (not shown).
  • the GPS signal receiving unit 10 is a receiver that receives GPS radio waves transmitted by GPS satellites included in a global positioning system (hereinafter referred to as GPS) that is one of satellite positioning systems.
  • This GPS radio wave means a GPS signal superimposed on a carrier wave.
  • the GPS signal receiving unit 10 demodulates the received GPS radio wave, extracts the GPS signal, and sends it to the control unit 100.
  • the carrier wave and the received signal strength are also sent to the control unit 100.
  • the GPS signal corresponds to an example of a satellite signal.
  • the GPS signal receiving unit 10 receives GPS radio waves from all GPS satellites that can be received.
  • the GPS signal includes the satellite number of the GPS satellite, the ephemeris which is the orbit information of the GPS satellite, the time when the GPS satellite transmits the radio wave, and the like.
  • the acceleration sensor 20 is a three-axis acceleration sensor such that the z-axis is parallel to the vehicle vertical direction, the x-axis is parallel to the vehicle width direction, and the y-axis is parallel to the vehicle front-rear direction.
  • the direction of the acceleration sensor 20 is fixed.
  • an acceleration sensor that detects the biaxial acceleration of the x axis and the y axis may be used.
  • the acceleration sensor 20 sends a detected value of acceleration of each axis to the control unit 100.
  • the yaw rate sensor 30 passes through the yaw rate sensor 30 and detects the rotational angular velocity around the vertical axis of the vehicle, that is, the yaw rate. Then, the detected yaw rate is supplied to the control unit 100.
  • the vertical axis of the vehicle is an axis perpendicular to the vehicle compartment floor of the vehicle, and is an axis parallel to the vertical axis when the vehicle is positioned on the horizontal ground.
  • the control unit 100 is a computer that includes a CPU, a ROM, a RAM, and the like.
  • the CPU executes a program stored in the ROM while using a temporary storage function of the RAM, whereby the control unit 100 is configured as shown in FIG. Function as the units 102 to 140 shown in FIG.
  • the traveling direction acceleration determining unit 102 determines the traveling direction acceleration of the vehicle from the detection value detected by the acceleration sensor 20.
  • the acceleration sensor 20 is fixed in a predetermined direction with respect to the vehicle, and the y-axis is directed in the vehicle traveling direction. Therefore, the detected value of the y axis detected by the acceleration sensor 20 is extracted as the traveling direction acceleration.
  • the integration processing unit 104 sequentially integrates the traveling direction acceleration determined by the traveling direction acceleration determination unit 102, that is, the detected value of the y-axis of the acceleration sensor 20.
  • the integrated value hereinafter referred to as acceleration integrated value ⁇ V G.
  • the integration processing unit 104 also integrates the detection values of the yaw rate sensor 30 to calculate a relative azimuth angle ⁇ t gyro .
  • the relative azimuth angle ⁇ t gyro is a relative azimuth angle at time t with respect to the traveling azimuth of the vehicle at the reference time point.
  • the relative azimuth angle ⁇ t gyro is calculated from the equation (1).
  • ⁇ t is a time step
  • ⁇ t is a detected value of the yaw rate sensor 30 detected at time t.
  • the acceleration integrated value ⁇ V G , the detected value of the acceleration sensor 20 for calculating the relative azimuth angle ⁇ t gyro and the detected value of the yaw rate sensor 30 are acquired at the same timing.
  • the acquisition timing is, for example, at a certain time or during a certain distance.
  • the satellite information acquisition unit 106 acquires a GPS signal and a carrier wave from the GPS signal reception unit 10.
  • the GPS signal receiving unit 10 receives GPS radio waves from a plurality of GPS satellites
  • the GPS signal receiving unit 10 acquires GPS signals and carrier waves for all GPS satellites that have received the GPS radio waves.
  • the satellite information acquisition unit 106 is an example of a satellite signal acquisition unit.
  • the satellite information acquisition unit 106 is the position coordinates (X si , Y si , Z si ), the pseudo distance ⁇ i , and the Doppler shift amount of the GPS satellite i, which are information on the GPS satellite i obtained from these GPS signals and carrier waves. Di is also calculated.
  • the position coordinates (X si , Y si , Z si ) of each GPS satellite i are calculated based on the ephemeris of each GPS satellite i and the time when the radio wave is transmitted.
  • the pseudo distance ⁇ i is calculated by multiplying the time difference between the time when the GPS satellite i transmits the GPS radio wave and the time when the GPS signal receiving unit 10 receives the GPS radio wave, that is, the radio wave propagation time, by the speed of light.
  • the Doppler shift amount D i is a frequency difference between the frequency of the carrier wave of the radio wave transmitted by the GPS satellite i and the frequency of the carrier wave of the received GPS radio wave.
  • the carrier wave frequency of the radio wave transmitted by the GPS satellite is determined in advance, and this frequency is stored in advance in a predetermined storage unit such as a storage unit (not shown) included in the control unit 100. Therefore, the satellite information acquisition section 106 acquires the frequency of the GPS radio waves of the carrier from the storage unit, and the frequency from the frequency of the carrier wave acquired from the GPS signal receiving unit 10, calculates the Doppler shift quantity D i.
  • the frequency of the carrier wave acquired by the satellite information acquisition unit 106 is determined by a known frequency analysis method, for example, fast Fourier transform.
  • the relative velocity calculation unit 108 based on the Doppler shift amount D i of the satellite information acquisition unit 106 is calculated, to calculate the relative speed Vr i of the vehicle with respect to the GPS satellite i.
  • the relative speed Vr i is calculated from the following equation (2). (2) In the equation, the relative speed of the vehicle with respect to Vr i is the GPS satellite i, D i is the Doppler shift amount satellite information acquisition unit 106 is calculated, C is the speed of light, F is the frequency of a radio wave carrier GPS satellite transmits is there.
  • the satellite velocity calculation unit 110 is a known method that uses the differentiation of Kepler's equation from the time series data of the position coordinates (X si , Y si , Z si ) of each GPS satellite i calculated by the satellite information acquisition unit 106.
  • the velocity vector of each GPS satellite i that is, the three-dimensional velocity Vxs i , Vys i , Vzs i is calculated.
  • the current position calculation unit 112 calculates the current position (X v , Y v , Z v ) of the vehicle using the pseudorange ⁇ i of each GPS satellite i calculated by the satellite information acquisition unit 106.
  • the position of the GPS satellite i (X si , Y si , Z si ) and the pseudo distance ⁇ i between each GPS satellite are determined according to the principle of triangulation according to the principle of triangulation.
  • the current position (X v , Y v , Z v ) is calculated.
  • true distance r i to GPS satellites i is expressed by equation (3).
  • pseudo distance ⁇ i is expressed by the equation (4).
  • s is a distance error due to a clock error.
  • the current position of the vehicle (X v , Y v ) can be obtained by solving the following simultaneous equations (5) obtained from the pseudoranges ⁇ i of four or more GPS satellites. , Z v ) can be calculated.
  • the current position (X v , Y v , Z v ) of the vehicle is used as the final current position, and in the satellite direction calculation unit 114 described later, Also used to determine the angle between the GPS satellite and the vehicle.
  • the current position used for obtaining the angle between the GPS satellite and the host vehicle may have low accuracy.
  • the current position of the vehicle can also be obtained from the estimated moving distance L t e or the corrected moving distance L t co and the estimated direction ⁇ t e .
  • the vehicle position error is in the range of several hundred meters, the speed estimation error is 1 m / sec or less and there is no significant problem. Therefore, for example, the position may be determined from a map or the like, or the current position (X v , Y v , Z v ) of the vehicle may be determined from information such as past position measurement history and beacons. Good.
  • the pseudorange [rho i need not be calculated from a good GPS radio signal quality.
  • the determination criterion that the signal quality is good will be described in the description of the signal quality determination unit 120.
  • the line-of-sight vector calculation unit 113 calculates line-of-sight vectors (Gx i , Gy i , Gz i ) from the vehicle to the GPS satellite i.
  • the x component, y component, and z component of the line-of-sight vector are calculated from equation (6).
  • ⁇ t i is the pseudorange of the GPS satellite i at time t
  • (X t si , Y t si , Z t si ) is the position coordinate of the GPS satellite i at time t.
  • the satellite direction calculation unit 114 includes the current position (X v , Y v , Z v ) calculated by the current position calculation unit 112 and the GPS satellite position coordinates (X si , Y si , Based on Z si ), the direction R i of each GPS satellite i is calculated.
  • Direction R i of each GPS satellite i is when viewing the GPS satellite i from the vehicle, and represents the azimuth angle phi i elevation theta i with respect to the horizontal direction, with respect to the north direction.
  • the satellite direction speed calculator 116 calculates a satellite direction speed Vs i that is the speed of the vehicle in the direction of the GPS satellite i.
  • the satellite direction velocity Vs i is calculated from the following equation (7).
  • the first term on the right side is the relative speed Vr, which is calculated by the relative speed calculation unit 108.
  • Gx, Gy, and Gz are line-of-sight vectors, and are calculated by the line-of-sight vector calculation unit 113.
  • Vxs, Vys, and Vzs are x, y, and z components of the speed of the GPS satellite i, and are calculated by the satellite speed calculation unit 110.
  • the first term on the right side of equation (7) is the vehicle relative speed Vri with respect to the GPS satellite i
  • the second to fourth terms are the speed of the GPS satellite i in the vehicle direction. Since these sums mean the speed of the vehicle in the direction of the GPS satellite i, equation (7) holds.
  • the speed vector calculation unit 118 calculates a vehicle speed vector.
  • Cbv is a clock drift of the timepiece provided in the GPS signal receiving unit 10.
  • the speed vector calculation unit 118 calculates the speed vector of the vehicle from equation (9) when receiving GPS radio waves determined to have good signal quality from four or more GPS satellites. Whether the signal quality is good or bad is determined by a signal quality determination unit 120 described below.
  • the signal quality determination unit 120 determines whether the signal quality of the GPS radio wave received from the GPS satellite i received by the GPS signal reception unit 10 is good. Various known standards can be used for determining the signal quality.
  • (judgment condition 1) S / N is greater than or equal to a predetermined value
  • (judgment condition 2) the residual of pseudo distance ⁇ i is less than or equal to the judgment reference distance
  • (judgment condition 3) elevation angle ⁇ i is a judgment standard. It is determined whether or not the signal quality is good by determining that the angle is equal to or greater than (corner condition 4) or a combination of the determination conditions 1 to 3.
  • the residual of the pseudorange ⁇ i is the distance between the position coordinates (X si , Y si , Z si ) of the GPS satellite i and the current position of the vehicle (X v , Y v , Z v ), It is the difference from the pseudorange ⁇ i . If this residual is large, it is determined that the signal quality is not good because of the influence of multipath or the like. Determination condition 1 determines that the signal quality is good if S / N is equal to or greater than a predetermined value. The determination condition 3 determines that the signal quality is good if the elevation angle ⁇ i is equal to or greater than the determination reference angle.
  • the stop determination unit 122 determines whether or not the vehicle is stopped. And when it determines with the vehicle having stopped, the speed of a vehicle shall be 0 km / h.
  • Various known methods can be used as the stop determination method. For example, if the z-axis detection value of the acceleration sensor 20 is equal to or less than the stop determination value, it is determined that the vehicle is stopped. When the vehicle is traveling, there is some vertical vibration, so the stop determination can be made based on the z-axis detection value. Instead of the z-axis detection value, a y-axis detection value, a change amount of the z-axis detection value, or a change amount of the y-axis detection value may be used.
  • the stop determination is performed based on the detection value of the acceleration sensor 20, there is an advantage that wiring for acquiring a brake signal and a shift position signal is not necessary.
  • the brake signal and the shift position signal may be acquired, and the stop determination may be performed using these signals.
  • the initial set value determination unit 126 determines a speed initial value V 0 used by the estimated speed determination unit 128 and a direction initial value ⁇ 0 used by the estimated direction determination unit 130.
  • the speed vector calculation unit 118 described above can also calculate the speed of the vehicle.
  • the velocity vector calculation unit 118 cannot calculate the vehicle velocity vector (Vx, Vy, Vz) unless it receives GPS radio waves with good signal quality from four or more GPS satellites. Therefore, for example, in an urban area with many high-rise buildings, the time during which the velocity vector (Vx, Vy, Vz) cannot be calculated may last for a long time. Even in an environment where four or more GPS radio waves with good signal quality can be received, the speed vector calculation unit 118 uses the Doppler shift amount D that requires frequency analysis, and therefore, for example, at a constant cycle such as every 100 ms. However, the vehicle speed vector (Vx, Vy, Vz) cannot be calculated.
  • the detection value of the acceleration sensor 20 can be acquired at a constant cycle without depending on the driving environment, and the speed vector calculation unit 118 calculates the speed vector (Vx, Vy, Vz), for example, every 20 ms. It can be set as the structure acquired with a period shorter than this.
  • the speed can be estimated based on G.
  • FIG. 2 is a graph showing this.
  • Figure 2 is a graph of the acceleration cumulative value [Delta] V G in a stopped state. Since the vehicle is in a stopped state, the acceleration cumulative value [Delta] V G in Fig. 2 refers to an error in case of a directly speed acceleration cumulative value [Delta] V G. From Figure 2, if left as it is rate acceleration cumulative value [Delta] V G, it can be seen that the error diverges over time. In FIG. 2, the error is a negative value. On the contrary, the error may be a positive value.
  • the initial speed value V 0 is calculated by an equation (12) obtained by improving the following equation (11) disclosed as a final velocity vector calculation equation in Patent Document 2.
  • the azimuth initial value ⁇ 0 is also calculated.
  • the speed initial value V 0 when the speed vector calculation unit 118 can calculate the speed vector (Vx, Vy, Vz), the speed initial value V 0 is determined from the equation (10). Since the speed vector (Vx, Vy, Vz) calculated by the speed vector calculation unit 118 has high accuracy, the speed initial value V 0 is determined by using the speed vector (Vx, Vy, Vz). This is because the accuracy of 0 is improved. Further, when the stop determination unit 122 determines that the vehicle is stopped, the speed initial value V 0 is set to ⁇ V G.
  • t is the time
  • V wheel is the detected value of the wheel speed sensor
  • ⁇ 0 is the azimuth angle of the vehicle traveling direction at the initial time (hereinafter referred to as initial direction value)
  • ⁇ gyro is the vehicle traveling direction.
  • Cbv 0 is clock drift at the initial time
  • A is the slope of time variation of clock drift
  • Gx, Gy are the x component and y component of the line-of-sight vector from the vehicle to GPS satellite i is there.
  • V 0 is the speed initial value is the vehicle speed at the initial time
  • [Delta] V G is the acceleration cumulative value after the initial time. That is, (12) is an equation of the detected values V wheel of the wheel speed sensors were replaced with the sum of the speed initial value V 0 and the acceleration cumulative value [Delta] V G in (11). This equation (12) corresponds to an example of a speed estimation equation.
  • equation (11) a method for deriving equation (11) will be described.
  • the relationship between the direction of the satellite velocity Vs t i, the velocity vector of the vehicle (Vx, Vy, Vz) and can be expressed by equation (13).
  • This equation (13) has four unknown parameters Vx t , Vy t , Vz t , and Cbv t .
  • Patent Document 2 derives the above-described equation (11) by substituting the following constraint conditions 1 to 3 into the equation (13).
  • Constraint condition 1 is that the magnitudes of the x component and y component of the speed vector to be estimated are constrained by the wheel speed, and the time change of the x component and y component is constrained by the time change of the azimuth.
  • ⁇ 0 in constraint condition 1 is an azimuth angle in the traveling direction of the vehicle at the initial time.
  • the constraint condition 2 assumes that the change in the velocity in the height direction is always minute.
  • constraint condition 3 assumes that the time change of the clock drift is gentle and the change in a short time is linear.
  • Cbv 0 is the clock drift at the initial time
  • A is the slope of the time variation of the clock drift.
  • the satellite direction speed Vs t i is the satellite direction speed in the direction of the GPS satellite i at time t, and is calculated by the satellite direction speed calculation unit 116.
  • ⁇ V t G is an acceleration integrated value at time t, and is calculated by the integration processing unit 104.
  • (Gx t i , Gy t i , Gz t i ) are line-of-sight vectors at time t, and are calculated by the line-of-sight vector calculation unit 113.
  • ⁇ t gyro is a yaw rate integrated value, and is calculated by the integration processing unit 104. Accordingly, there are four unknown parameters, ⁇ 0 , Cbv 0 , A, and V 0 in the equation (12).
  • the initial velocity value V 0 and the initial azimuth value ⁇ 0 that are unknown parameters can be obtained.
  • the unknown parameters ⁇ 0 , Cbv 0 , A, and V 0 are the same even if the times are different as long as they are after the initial time. Therefore, it is not necessary to formulate four formulas at the same time, and an unknown parameter can be obtained if the total number of formulas formulated at a plurality of times is four or more.
  • the velocity initial value V 0 and The initial azimuth value ⁇ 0 can be obtained.
  • the speed initial value V 0 is the vehicle speed at the initial time as described above.
  • the equation (12) for calculating the initial velocity value V0 uses the satellite direction velocity Vs calculated from the Doppler shift amount D. Therefore, the initial set value determination unit 126 also calculates the vehicle speed at the initial time based on the Doppler shift amount.
  • the estimated speed determination unit 128 adds the acceleration integrated value ⁇ V t G calculated by the integration processing unit 104 to the speed initial value V 0 determined by the initial set value determination unit 126, and uses the estimated speed V t e as an acceleration acquisition cycle. Is calculated sequentially.
  • the estimated azimuth determining unit 130 adds the relative azimuth angle ⁇ t gyro calculated by the integration processing unit 104 to the azimuth initial value 0 determined by the initial setting value determining unit 126 to estimate the estimated azimuth ⁇ t indicating the moving direction of the vehicle. e is sequentially determined at a relative azimuth angle determination cycle.
  • the position update unit 140 sequentially updates the current position of the vehicle.
  • a detailed configuration of the position updating unit 140 is shown in FIG.
  • the position update unit 140 includes a movement distance calculation unit 141, a past speed correction unit 142, a movement distance correction unit 143, and a position determination unit 144.
  • the travel distance calculation unit 141 calculates the travel distance of the vehicle every time the vehicle speed is updated.
  • the speed of the vehicle is updated at an acceleration acquisition cycle.
  • the speed of the vehicle is determined by any of the estimated speed determination unit 128, the speed vector calculation unit 118, and the stop determination unit 122.
  • the travel distance calculation unit 141 multiplies the speeds determined by the estimated speed determination unit 128, the speed vector calculation unit 118, and the stop determination unit 122 by the acceleration acquisition cycle, and estimates the distance traveled by the vehicle during the acceleration acquisition cycle. calculating the estimated travel distance L t e is.
  • estimating the speed determination unit 128 corrects the estimated speed V t e which sequentially estimated.
  • the estimated speed V t e that is the correction target hereinafter referred to as pre-correction past estimated velocity V t (i) e.
  • i is an integer from N to 0
  • t (N) is the time when the initial speed value V 0 was last updated
  • t (0) is the time when the initial speed value V 0 was updated this time.
  • a value obtained by correcting the pre-correction past estimated speed V t (i) e is set as a corrected past speed V t (i) co .
  • the corrected past speed V t (i) co is calculated from the following equation (14).
  • the speed initial value V 0 can be regarded as an accurate speed.
  • the speed initial value V 0 from the previous correction at the time that have been updated in the Default speed V 0 past estimated speed V t (0) difference between e (V 0 -V t (0 ) e ) Means an error accumulated in the pre-correction past estimated speed V t (0) e by the time when the speed initial value V 0 is updated.
  • FIG. 4 is a diagram showing a comparison between the pre-correction past estimated speed V t (i) e and the corrected past speed V t (i) co .
  • V t (0) there is a difference (V 0 ⁇ V t (0) e ) between the initial speed value V 0 and the pre-correction past estimated speed V t (0) e.
  • This difference means the error accumulated in the estimated speed V t e.
  • the estimated speed V t e Default speed V 0.
  • the estimated velocity V t e becomes discontinuous at the time of updating the velocity initial value V 0.
  • the corrected past estimated speed V t (i) e is corrected to the corrected past speed V t (i) co using the equation (14).
  • the correction past velocity V t (i) co as shown in FIG. 4, there is a continuity between the estimated speed V t e of t (0) or later. Since the actual speed change of the vehicle has continuity, the corrected past speed V t (i) co calculated using the equation (14) is more than the past estimated speed V t (i) e before correction. It can be said that it is close to the true speed.
  • the moving distance correcting unit 143 moves using the corrected past speed V t (i) co instead of the estimated speed V t e.
  • the estimated movement distance is recalculated in the same manner as the distance calculation unit 141. This estimated moving distance is set as a corrected moving distance L t (i) co .
  • the position determination unit 144 uses the GPS radio waves determined to have good signal quality by the signal quality determination unit 120, and the current position calculation unit 112 uses the equation (5) to determine the current position of the vehicle (X v , Y v , When Z v ) is calculated, the current position (X v , Y v , Z v ) calculated by the current position calculation unit 112 is set as the updated current position.
  • Current position current position calculation unit 112 calculates a (X v, Y v, Z v) if no current position after updating the moving distance L t e the moving distance calculating unit 141 calculates the estimated orientation determined using the estimated orientation theta t e the part 130 is sequentially determined, the current position (X v, Y v) sequentially calculated.
  • the previous current position (X v t ( ⁇ 1) , Y v t ( ⁇ 1) ) and the current estimated movement distance L t ( 0) e and the current estimated direction ⁇ t (0) e are used to calculate the current position (X v t (0) , Y v t (0) ) at the current time t (0).
  • the current position (X v t (0) , Y v t (0) ) is, of course, a relative position with respect to the previous current position (X v t (-1) , Y v t (-1) ). .
  • X v t (0) X v t ( ⁇ 1) + L t (0) e ⁇ cos ⁇ t (0) e
  • Y v t (0) Y v t ( ⁇ 1) + L t (0) e ⁇ sin ⁇ t (0) e
  • the position determination unit 144 starts this time from the time T (N) at which the speed initial value V 0 was updated last time. position up to the point t (0) where V 0 is updated sequentially corrected (X v t (i), Y v t (i)), the current position (X v t (0), Y v t (0 ) )
  • step S2 detection values of the acceleration sensor 20 and the yaw rate sensor 30 are acquired and stored in a storage unit such as a RAM. This processing is performed by the integration processing unit 104, for example.
  • Step S4 is a process performed by the traveling direction acceleration determination unit 102, and the traveling direction acceleration is determined from the detection value of the acceleration sensor 20 acquired in step S2.
  • Step S6 is a process performed by the integration processing unit 104.
  • the detection value of the yaw rate sensor 30 acquired in step S2 is added to the relative azimuth angle ⁇ t ( ⁇ 1) gyro so far to obtain the relative azimuth angle ⁇ t gyro .
  • the acceleration integrated value ⁇ V t G is updated by adding the traveling direction acceleration determined in step S2 to the acceleration integrated value ⁇ V t ( ⁇ 1) G so far.
  • Steps S8 and S10 are processes performed by the satellite information acquisition unit 106.
  • a GPS signal and a carrier wave are acquired from the GPS signal receiving unit 10.
  • the position coordinates (X si , Y si , Z si ), the pseudorange ⁇ i , and the Doppler shift amount D i of the GPS satellite i are calculated from the GPS signal and carrier wave acquired in step S8.
  • Step S12 is a process of the stop determination unit 122. For example, it is determined whether or not the vehicle is stopped from the detected value of the z-axis of the acceleration sensor 20 acquired in step S2. If it is determined that the vehicle is stopped, the process proceeds to step S14. If it is determined that the vehicle is moving, the process proceeds to step S18.
  • Step S14 is also a process of the stop determination unit 122, and the velocity vector is set to (0, 0, 0). Of course, the speed is also zero.
  • Step S16 is a process of the initial set value determination unit 126, and the speed initial value V 0 is set as the latest acceleration integrated value ⁇ V t G. If step S16 is performed, the position update process of step S34 will be performed. The position update process in step S34 will be described later.
  • Step S18 which is executed when it is determined that the vehicle is moving in step S12, is processing of the signal quality determination unit 120, and it is determined whether or not the signal quality of the GPS signal is good based on the above-described determination conditions.
  • the signal quality is determined for all acquired GPS signals.
  • Step S20 is processing of the velocity vector calculation unit 118, and it is determined whether or not the number of GPS signals determined to have good signal quality in step S18 is four or more. When it is 4 or more, the process proceeds to step S22.
  • the Doppler speed is calculated.
  • the Doppler velocity means the velocity vector (Vx, Vy, Vz) calculated by the velocity vector calculation unit 118 or the magnitude of the velocity vector. Since the velocity vector (Vx, Vy, Vz) is calculated using the Doppler shift amount D, it is referred to as a Doppler velocity here.
  • step S222 is a process of the current position calculation unit 112, and the current position (X X, Y si , Z si ) of the vehicle is calculated from the pseudo distance ⁇ i calculated in step S10 and the position coordinates (X si , Y si , Z si ) v 1 , Y v , Z v ) are calculated.
  • Step S224 is a process performed by the satellite direction calculation unit 114.
  • the direction R i ( ⁇ i , ⁇ i ) of each GPS satellite i is calculated from Y si , Z si ).
  • Step S226 is a process performed by the satellite velocity calculation unit 110. From the time-series data of the position coordinates (X si , Y si , Z si ) of each GPS satellite i calculated in step S10, the velocity vector ( Vxs i , Vys i , Vzs i ) are calculated.
  • Steps S228 and S230 are processing of the speed vector calculation unit 118.
  • step S230 four or more simultaneous equations shown in the equation (9) are formed, and the simultaneous equations are solved. As a result, the vehicle speed vector (Vx, Vy, Vz) and the clock drift Cbv t can be obtained.
  • Step S24 is a process of initial setting value determination unit 126, from the velocity vector of the vehicle computed in step S22 (Vx, Vy, Vz), the equation (10), determines the speed initial value V 0. Even when step S24 is executed, the position update process of step S34 is executed. Before describing step S34, step S26 and subsequent steps will be described first.
  • Step S26 is processing of the initial set value determination unit 126, and determines whether or not the initial value can be updated. This determination is specifically using the GPS signals from the update speed initial value V 0, is to determine whether four or more, can stand equation (12). If this judgment is NO, it will progress to Step S30, and if it is YES, it will progress to Step S28.
  • step S28 the speed initial value V 0 and the azimuth initial value ⁇ 0 are determined using a tight coupling type estimation formula.
  • the tight coupling type estimation equation is specifically the equation (12).
  • step S282 is a process performed by the satellite velocity calculation unit 110. From the time series data of the position coordinates (X si , Y si , Z si ) of each GPS satellite i calculated in step S10, each GPS satellite i Velocity vectors (Vxs i , Vys i , Vzs i ) are calculated.
  • Step S284 is a process of relative speed calculating unit 108 is performed, by substituting the Doppler shift amount D i calculated in step S10 described above (2), calculates the relative speed Vr i of the vehicle with respect to the GPS satellite i.
  • Step S286 is a process of the current position calculation unit 112, and the current position (X v , Y v ) of the vehicle is calculated from the pseudo distance ⁇ i calculated in step S10 and the position coordinates (X si , Y si , Z si ) of the GPS satellite i. , Z v ).
  • Step S288 is processing performed by the line-of-sight vector calculation unit 113.
  • step S288 the pseudo-range ⁇ i calculated in step S10, the position coordinates of the GPS satellite i (X si , Y si , Z si ), and the current vehicle position calculated in step S286 ( X v , Y v , Z v ) are substituted to calculate the line-of-sight vector (Gx i , Gy i , Gz i ).
  • Step S290 is processing performed by the satellite direction velocity calculation unit 116.
  • the relative velocity Vr i calculated in step S284, the line-of-sight vector (Gx i , Gy i , Gz i ) calculated in step S288, and the GPS satellite i calculated in step S282 are added to the above-described equation (7).
  • the satellite direction velocity Vs i of the vehicle in the direction of the GPS satellite i is calculated.
  • Step S292 is processing performed by the initial set value determination unit 126.
  • the satellite direction velocity Vs i calculated in step S290, the acceleration integrated value ⁇ V G updated in step S6, the relative azimuth angle ⁇ gyro , and the line-of-sight vector (Vxs calculated in step S288) are added to the above-described equation (12).
  • i , Vys i , Vzs i ) are substituted for four or more equations.
  • simultaneous equations composed of these four or more equations are solved. Thereby, the initial velocity value V 0 and the initial azimuth value ⁇ 0 that are unknown parameters in the equation (12) can be obtained simultaneously.
  • step S30 is a process of estimating speed determining unit 128 performs, the latest velocity initial value V 0, the acceleration cumulative value [Delta] V G updated in step S6, to calculate the estimated speed V e.
  • acceleration cumulative value [Delta] V G may calculate the estimated velocity V e by adding the velocity initial value V 0 in the acceleration cumulative value [Delta] V G To do.
  • Step S32 is a process for the estimated orientation determining unit 130 performs, the latest azimuth initial value theta 0, and a relative azimuth angle theta Gyro updated in step S6, to calculate the estimated azimuth theta e.
  • a specific method for calculating the estimated direction ⁇ e is a method of calculating the estimated speed V e from the speed initial value V 0 and the acceleration integrated value ⁇ V G , replacing the speed initial value V 0 with the direction initial value ⁇ 0 , and integrating the acceleration. is the same as when replacing the values [Delta] V G to the relative azimuth angle theta Gyro.
  • step S32 is executed, the position update process of step S34 is executed.
  • the position update unit 140 executes this step S34.
  • step S34 Detailed processing in step S34 is shown in FIG. In FIG. 8, step S348 is executed by the movement distance calculation unit 141, step S352 is executed by the past speed correction unit 142, and step S354 is executed by the movement distance correction unit 143. The other steps are executed by the position determination unit 144.
  • step S342 it is determined whether the current GPS position has been determined.
  • the GPS current position is the current position (X v , Y v , Z v ) calculated from the equation (5) by using the GPS radio wave determined to have good signal quality by the signal quality determination unit 120. ). If this determination is YES, the process proceeds to step S344. In step S344, the GPS current position (X v , Y v , Z v ) is set as the latest current position.
  • step S342 determines whether the determination in step S342 is no. If the determination in step S342 is no, the process proceeds to step S346.
  • step S346 it is determined whether to update the speed initial value V 0. If any of steps S16, S24, and S28 in FIG. 5 is being executed, the determination in step S346 is YES. On the other hand, if the determination in step S26 of FIG. 5 is NO, the determination in step S346 is also NO. If the determination in step S346 is no, the process proceeds to step S348.
  • step S348 the estimated moving distance L e is calculated by multiplying the estimated speed V e calculated in step S30 by the acceleration acquisition cycle.
  • step S350 the latest current position (X v , Y v ) is calculated using the estimated moving distance L e calculated in step S348, the estimated azimuth ⁇ e estimated in step S32, and equations 15 and 16. .
  • the z coordinate is not changed.
  • step S352 the determination in step S346 is executed when was YES, the last, from the time to update the velocity initial value V 0 t (N), this time to time to update the velocity initial value V 0 t (0) the estimated speed V t e, i.e., the uncorrected past estimated velocity V t (i) e, ( 14) is corrected by the equation to calculate the corrected past velocity V t (i) co.
  • step 354 the corrected travel speed L t (i) co is calculated by multiplying the corrected past speed V t (i) co calculated in step S352 by the acceleration acquisition cycle.
  • step S356 using the corrected moving distance L t (i) co calculated in step S354, the estimated azimuth ⁇ t (i) e estimated in step S32, and the equations 17 and 18, the current position (X v , Yv ) is recalculated. The z coordinate is not changed.
  • the estimated velocity V e is estimated using the In this way the acceleration cumulative value and the speed initial value V 0 was determined ⁇ V G (S30).
  • the Default speed V 0 was determined ⁇ V G (S30).
  • the speed initial value V 0 is updated last time using the corrected past speed V t (i) co , and then the current speed initial value V 0 is determined.
  • the corrected moving distance L t (i) co is calculated by recalculating the estimated moving distance until updating (S354).
  • the current position is corrected using the corrected movement distance L t (i) (S356). Therefore, even in a situation where the GPS current position cannot be determined, the current position can be determined with high accuracy.
  • the vehicle satellite direction velocity Vs i is a line-of-sight vector (Gx, Gy, Gz) from the vehicle to the GPS satellite i, a vehicle velocity vector (Vx, Vy, Vz), It can be calculated based on the clock drift Cbv.
  • the x component and y component of the speed vector are constrained by the time change of the azimuth of the vehicle speed and the traveling direction of the vehicle, that is, the relative azimuth angle ⁇ gyro .
  • the initial set value determination unit 126 determines the speed initial value V 0 using the equation (12) obtained thereby.
  • the expression (12) using GPS signals at a plurality of different observation points can be used as simultaneous equations. Since there are unknown parameters other than the initial velocity value V 0 in the equation (12), it is necessary to use a plurality of equations (12) corresponding to the number of unknown parameters as simultaneous equations. However, in the present embodiment, since the equation (12) using GPS signals at a plurality of different observation points can be used as simultaneous equations, simultaneous equations composed of a number of equations (12) that can solve unknown parameters are obtained. Easy to stand up.
  • the initial set value determination unit 126 can obtain the speed initial value V 0 with high frequency. Then, by updating the Default speed V 0 to a high frequency, since the frequency to eliminate the effects of drift from the detected value of the acceleration sensor 20 is higher, it is calculated from the acceleration cumulative value [Delta] V G and the speed initial value V 0 estimated estimation accuracy of velocity V e is improved. Therefore, the accuracy of the current position determined using the estimated speed V e is also improved.
  • the equation (12) is derived using the constraint condition 3 in which the time change of the clock drift is linear.
  • the number of unknown parameters in equation (12) can be further reduced.
  • it becomes easier to formulate simultaneous equations for solving unknown parameters so that the update frequency of the initial speed value V 0 can be further increased.
  • the initial speed value V 0 can be obtained directly by solving the simultaneous equations consisting of the number of unknown parameters (12).
  • the present embodiment includes the velocity vector without using the acceleration cumulative value ⁇ V G (Vx, Vy, Vz ) of the velocity vector calculating unit 118 for calculating a.
  • the velocity vector calculation unit 118 calculates the velocity vector (Vx, Vy, Vz)
  • the velocity initial value V 0 is determined using the velocity vector (Vx, Vy, Vz) calculated by the velocity vector calculation unit 118 ( S24). Since the speed vector (Vx, Vy, Vz) calculated by the speed vector calculation unit 118 has high accuracy, the accuracy of the speed initial value V 0 is improved by doing so.
  • the stop determination unit 122 when it is determined that the vehicle is stopped, as the speed of the vehicle is zero, determines the speed initial value V 0 (S16). This also improves the accuracy of the speed initial value V 0. As a result of improving the accuracy of the speed initial value V 0, also improves the accuracy of the estimated speed Ve calculated from Default speed V 0 and the acceleration cumulative value [Delta] V G.
  • embodiment is not limited to the above-mentioned embodiment
  • the following modified example is also included in embodiment, and also changes variously within the range which does not deviate from a summary other than the following. be able to.
  • the constraint condition 3 may be left, and the constraint due to the temporal change of the azimuth angle in the constraint condition 1 may be omitted.
  • the constraint condition 1 is only a condition where the magnitude of the speed vector is constrained by the vehicle speed, and ⁇ t gyro in the equation (12) becomes an unknown parameter.
  • the GPS is used as the satellite positioning system, but other satellite positioning systems may be used. Moreover, you may use both the satellite with which GPS is equipped, and the satellite with which another satellite positioning system is equipped.
  • the moving body is a car, but the moving distance estimation apparatus can be applied to a moving body other than the car.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

　移動距離推定装置を提供する。移動距離推定装置は、加速度積算値と速度初期値Ｖ０とに基づいて、移動体の推定速度Ｖｔ ｅを逐次推定する推定速度決定部（１２８）と、推定速度Ｖｔ ｅと時間とに基づいて移動体の推定移動距離Ｌｔ ｅを逐次算出する移動距離算出部（１４１）と、前回、初期設定値決定部（１２６）が速度初期値Ｖ０を決定してから、今回、速度初期値Ｖ０を決定するまでに逐次推定した推定速度Ｖｔ ｅである補正前過去推定速度Ｖｔ（ｉ） ｅを、今回の速度初期値Ｖ０と連続するように補正した補正過去速度Ｖｔ（ｉ） ｃｏを決定する過去速度補正部（１４２）と、補正過去速度Ｖｔ（ｉ） ｃｏを決定した場合に、補正過去速度Ｖｔ（ｉ） ｃｏと時間とに基づいて、前回、速度初期値Ｖ０を決定してから、今回、速度初期値Ｖ０を決定するまでの推定移動距離を再計算する移動距離補正部（１４３）とを備える。

Description

移動距離推定装置 関連出願の相互参照
　本出願は、２０１４年６月４日に出願された日本国特許出願２０１４－１１５９５４号に基づくものであり、この開示をここに参照により援用する。
　本開示は、移動体の移動距離を推定する移動距離推定装置に関し、特に、その移動距離の推定精度を向上させる技術に関する。
　移動体の移動距離は、たとえば、移動方向とともに用いて、移動軌跡を推定するために用いられる。移動軌跡が推定できれば、ある時点における位置に対する相対位置を決定することができる。したがって、ＧＰＳ等の衛星測位システムが送信する測位用の電波を受信できない場合であっても、過去に測位した現在位置と移動軌跡から現在位置を推定することができる。
　移動距離は、速度と時間の積で計算することができる。移動体の速度を推定する方法として、特許文献１には、加速度を計測し、この加速度から速度を算出することに加え、ＧＰＳ搬送波のドップラーシフト量からも速度（以下、ドップラー速度）を算出する技術が開示されている。この特許文献１では、ドップラー速度により、加速度から算出した速度を逐次補正している。
　加速度から算出した速度は、短周期で更新できるが、ドップラー速度よりも誤差が生じやすい。そのため、ドップラー速度により、加速度から算出した速度を逐次補正するのである。この補正により、精度のよい速度が得られる。
　特許文献１の技術は、車両に適用する場合に、車速センサの信号を取得するための配線が不要になるメリットがある。
　特許文献２には、車両の速度ベクトルを精度よく推定する技術が開示されている。具体的には、特許文献２の技術では、速度ベクトルの推定に、衛星方向車速と、車輪速と、車両から測位システムの衛星への視線ベクトルと、車両方位と、クロックドリフトとの関係を示す式を用いる。この式は、衛星方向速度、視線ベクトル、速度ベクトル、クロックドリフトの関係を示す式を、拘束条件を用いて変形した式である。拘束条件は、速度ベクトルが移動体の速度およびヨーレートにより拘束されるという条件、および、クロックドリフトはその時間変化が線形であるという条件である。
特開２０１２－４２３１８号公報 特開２０１３－１１３７８９号公報
　本願発明者は、上述の背景技術に関し以下を見出した。
特許文献２の技術は、車輪速が必要となる。そのため、特許文献２の技術は、車輪速センサと接続することができない速度推定装置には適用できない。
　これに対して、特許文献１の技術は、自装置内に備えている加速度センサが検出した加速度から速度を算出しているため、車輪速センサと接続する必要がない。
　広く知られているようにセンサにはドリフトという現象があるため、センサ検出値の誤差は時間とともに発散する。そこで、特許文献１の技術は、加速度から算出した速度をドップラー速度により補正している。
　しかし、ドップラー速度など、加速度から算出した速度よりも精度のよい速度で、加速度から算出した速度を逐次補正するとしても、加速度から算出した速度を補正してから次に補正するまでの間は、加速度から算出した速度と時間の積で移動距離を算出する。加速度から算出した速度を用いている間は速度誤差が増加していくので、移動距離の誤差も累積的に大きくなってしまう。したがって、移動距離の推定精度が十分ではなかった。
　本開示は、この事情に基づいて成されたものであり、その目的とするところは、より精度よく移動距離を推定することができる移動距離推定装置を提供することにある。
　本開示の一例に係る移動距離推定装置は、移動体とともに移動する加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部と、進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部と、衛星測位システムが備える衛星からのドップラーシフト量に基づいて算出する移動体の速度、または、移動体の停止時の速度を、移動体の速度初期値として決定する初期設定値決定部と、積算処理部が算出した加速度積算値と、初期設定値決定部が決定した速度初期値とに基づいて、移動体の推定速度を逐次推定する推定速度決定部と、推定速度決定部が決定した推定速度と時間とに基づいて移動体の推定移動距離を逐次算出する移動距離算出部と、初期設定値決定部が速度初期値を決定した場合、前回、初期設定値決定部が速度初期値を決定してから、今回、初期設定値決定部が速度初期値を決定するまでに推定速度決定部が逐次推定した推定速度である補正前過去推定速度を、今回、初期設定値決定部が決定した速度初期値と連続するように補正した補正過去速度を決定する過去速度補正部と、過去速度補正部が補正過去速度を決定した場合に、補正過去速度と時間とに基づいて、前回、初期設定値決定部が速度初期値を決定してから、今回、初期設定値決定部が速度初期値を決定するまでの推定移動距離を再計算する移動距離補正部と、を備える。
　この移動距離推定装置によれば、初期値設定部により、衛星測位システムが備える衛星からの電波のドップラーシフト量に基づいて算出する移動体の速度、または、移動体の停止時の速度を、移動体の速度初期値として決定している。衛星からの電波のドップラーシフト量に基づいて算出する移動体の速度や、移動体の停止時の速度は、精度よく決定することができる。したがって、速度初期値は、速度初期値を決定した時点における移動体の速度を精度のよく表している。
　そして、推定速度は、この速度初期値と加速度積算値とを用いて推定している。したがって、速度初期値を更新した以後は、加速度積算値の累積誤差により低下している推定速度の精度が向上する。
　加えて、上記移動距離推定装置では、初期設定値決定部が速度初期値を決定した場合、前回、速度初期値を決定してから、今回、速度初期値を決定するまでの推定速度である補正前過去推定速度を、今回の速度初期値と連続するように補正して補正過去速度とする。これにより、加速度積算値の累積誤差により低下している過去の推定速度の精度も向上する。そして、補正過去速度を決定した場合、その補正過去速度を用いて、前回、速度初期値を決定してから、今回、速度初期値を決定するまでの推定移動距離を再計算する。これにより、過去の推定速度の精度低下により低下していた推定移動距離の精度も向上する。
　本開示についての上記および他の目的、特徴や利点は、添付の図面を参照した下記の詳細な説明から、より明確になる。添付図面において
図１は、実施形態の現在位置推定装置の構成図である。 図２は、加速度積算値ΔＶＧの誤差が発散することを示す図である。 図３は、図１の位置更新部の詳細構成図である。 図４は、図３の過去速度補正部による速度の補正を説明する図である。 図５は、図１の制御部が行う処理の流れを示すフロチャートである。 図６は、図５のステップＳ２２の詳細フロチャートである。 図７は、図５のステップＳ２８の詳細フロチャートである。 図８は、図５のステップＳ３４の詳細フロチャートである。
　＜実施形態の構成＞
　以下、実施形態を図面に基づいて説明する。図１に示す現在位置推定装置１は、移動距離推定装置の一例である。この現在位置推定装置１は、ＧＰＳ信号受信部１０、加速度センサ２０、ヨーレートセンサ３０、および、制御部１００を備えており、図示しない車両に搭載される。
　ＧＰＳ信号受信部１０は、衛星測位システムの一つであるグローバルポジショニングシステム（以下、ＧＰＳ）が備えるＧＰＳ衛星が送信するＧＰＳ電波を受信する受信機である。このＧＰＳ電波は、搬送波にＧＰＳ信号が重畳されたものを意味する。ＧＰＳ信号受信部１０は、受信したＧＰＳ電波を復調してＧＰＳ信号を取り出し、制御部１００に送る。また、搬送波、受信信号強度も制御部１００に送る。ＧＰＳ信号は衛星信号の一例に相当する。
　周知のように、ＧＰＳ衛星は複数存在している。ＧＰＳ信号受信部１０は、受信できるすべてのＧＰＳ衛星からのＧＰＳ電波を受信する。ＧＰＳ信号には、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報であるエフェメリス、ＧＰＳ衛星が電波を送信した時刻などが含まれている。
　加速度センサ２０は、３軸の加速度センサであり、ｚ軸が車両の上下方向に平行になり、ｘ軸が車両の幅方向と平行になり、ｙ軸が車両の前後方向に平行になるように、加速度センサ２０は向きが固定されている。なお、３軸の加速度センサに代えて、ｘ軸、ｙ軸の２軸の加速度を検出する加速度センサを用いてもよい。加速度センサ２０は各軸の加速度の検出値を制御部１００に送る。
　ヨーレートセンサ３０は、このヨーレートセンサ３０を通り、車両の垂直軸周りの回転角速度、すなわち、ヨーレートを検出する。そして、検出したヨーレートを制御部１００に供給する。なお、車両の垂直軸とは、車両の車室床に対して垂直な軸であり、車両が水平な地面に位置している時の鉛直軸と平行になる軸である。
　制御部１００は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、ＣＰＵが、ＲＡＭの一時記憶機能を利用しつつＲＯＭに記憶されているプログラムを実行することで、制御部１００は、図１に示す各部１０２～１４０として機能する。
　進行方向加速度決定部１０２は、加速度センサ２０が検出した検出値から、車両の進行方向加速度を決定する。前述したように、加速度センサ２０は車両に対する向きが予め定められた向きに固定されており、ｙ軸が車両進行方向を向いている。したがって、加速度センサ２０が検出したｙ軸の検出値を、進行方向加速度として抽出する。
　積算処理部１０４は、進行方向加速度決定部１０２が決定した進行方向加速度、すなわち、加速度センサ２０のｙ軸の検出値を、逐次、積算する。積算した値を、以下、加速度積算値ΔＶＧという。また、この積算処理部１０４は、ヨーレートセンサ３０の検出値を積算して、相対方位角θｔ ｇｙｒｏも算出する。相対方位角θｔ ｇｙｒｏは、基準時点における車両の進行方位に対する時刻ｔの相対方位角である。この相対方位角θｔ ｇｙｒｏは、（１）式から算出する。（１）式において、Δｔはタイムステップ、ωｔは時刻ｔに検出されたヨーレートセンサ３０の検出値である。
Figure JPOXMLDOC01-appb-M000001
 　なお、これら加速度積算値ΔＶＧ、相対方位角θｔ ｇｙｒｏを算出するための加速度センサ２０の検出値、ヨーレートセンサ３０の検出値は、同じタイミングで取得する。取得するタイミングは、たとえば、一定時間時、あるいは、一定距離走行時である。
　衛星情報取得部１０６は、ＧＰＳ信号受信部１０からＧＰＳ信号および搬送波を取得する。ＧＰＳ信号受信部１０が複数のＧＰＳ衛星からＧＰＳ電波を受信している場合、ＧＰＳ信号受信部１０がＧＰＳ電波を受信したすべてのＧＰＳ衛星についてのＧＰＳ信号および搬送波を取得する。この衛星情報取得部１０６は衛星信号取得部の一例である。
　さらに、衛星情報取得部１０６は、これらＧＰＳ信号および搬送波から得られるＧＰＳ衛星ｉに関する情報である、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、擬似距離ρｉ、ドップラーシフト量Ｄｉも算出する。
　各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）は、各ＧＰＳ衛星ｉのエフェメリスおよび電波を送信した時刻に基づいて算出する。擬似距離ρｉは、ＧＰＳ衛星ｉがＧＰＳ電波を送信した時刻と、ＧＰＳ信号受信部１０がＧＰＳ電波を受信した時刻との時刻差すなわち電波伝播時間に、光速を乗じることで算出する。
　ドップラーシフト量Ｄｉは、ＧＰＳ衛星ｉが送信した電波の搬送波の周波数と、受信したＧＰＳ電波の搬送波の周波数の周波数差である。ＧＰＳ衛星が送信する電波の搬送波周波数は予め定まっており、この周波数は、制御部１００が備える図示しない記憶部など、所定の記憶部に予め記憶されている。したがって、衛星情報取得部１０６は、記憶部からＧＰＳ電波の搬送波の周波数を取得し、この周波数と、ＧＰＳ信号受信部１０から取得した搬送波の周波数から、ドップラーシフト量Ｄｉを算出する。なお、衛星情報取得部１０６が取得した搬送波の周波数は、公知の周波数解析手法、たとえば、高速フーリエ変換により決定する。
　相対速度算出部１０８は、衛星情報取得部１０６が算出したドップラーシフト量Ｄｉに基づいて、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒｉを算出する。相対速度Ｖｒｉは、下記（２）式から算出する。（２）式において、ＶｒｉはＧＰＳ衛星ｉに対する車両の相対速度、Ｄｉは衛星情報取得部１０６が算出したドップラーシフト量、Ｃは光速、ＦはＧＰＳ衛星が送信する電波の搬送波の周波数である。
Figure JPOXMLDOC01-appb-M000002
 　衛星速度算出部１１０は、衛星情報取得部１０６が算出した各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、ケプラーの方程式の微分を用いる公知の方法で、各ＧＰＳ衛星ｉの速度ベクトル、すなわち、三次元速度Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉを算出する。
　現在位置算出部１１２は、衛星情報取得部１０６が算出した各ＧＰＳ衛星ｉの擬似距離ρｉを用いて、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。
　ＧＰＳ信号を用いた測位では、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）と、各ＧＰＳ衛星との間の擬似距離ρｉとに基づいて、三角測量の原理に従って、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。ここで、ＧＰＳ衛星ｉまでの真の距離ｒｉは（３）式で表される。一方、擬似距離ρｉは（４）式で表される。なお、（４）式において、ｓは時計誤差による距離誤差である。
Figure JPOXMLDOC01-appb-M000003
 　上記（３）式、（４）式より、４つ以上のＧＰＳ衛星の擬似距離ρｉから得られる以下の（５）式の連立方程式を解くことによって、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）が算出できる。
Figure JPOXMLDOC01-appb-M000004
 　なお、本実施形態では、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）は、最終的な現在位置として用いる以外に、後述する衛星方向算出部１１４において、ＧＰＳ衛星ｉの方向、すなわち、ＧＰＳ衛星と自車両との角度を求めるためにも用いる。
　ＧＰＳ衛星は遠方に存在するため、ＧＰＳ衛星と自車両との角度を求めるために用いる現在位置は、精度が低くてもよい。また、車両の現在位置は、詳細は後述するが、推定移動距離Ｌｔ ｅあるいは補正移動距離Ｌｔ ｃｏと推定方位θｔ ｅから求めることもできる。
　したがって、常に擬似距離ρｉを用いて現在位置を決定する必要はなく、擬似距離ρｉを用いた位置決定以外の精度の低い方法で車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を決定してもよい。システム等で許容される推定精度に依存するが、車両の位置誤差が数百ｍの範囲であれば、速度推定誤差は１ｍ／ｓｅｃ以下となり大きな問題はない。そのため、たとえば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を決定してもよい。また、擬似距離ρｉを用いて現在位置を決定する場合にも、その擬似距離ρｉは、信号品質のよいＧＰＳ電波から算出する必要はない。信号品質がよいことの判定基準は、信号品質判定部１２０の説明において、説明する。
　視線ベクトル算出部１１３は、車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘｉ、Ｇｙｉ、Ｇｚｉ）を算出する。視線ベクトルのｘ成分、ｙ成分、ｚ成分は、（６）式から算出する。
Figure JPOXMLDOC01-appb-M000005
 　（６）式において、ρｔ ｉは時刻ｔにおけるＧＰＳ衛星ｉの擬似距離、（Ｘｔ ｓｉ、Ｙｔ ｓｉ、Ｚｔ ｓｉ）は時刻ｔにおけるＧＰＳ衛星ｉの位置座標である。これらは、衛星情報取得部１０６が算出している。（Ｘｔ Ｖ、Ｙｔ Ｖ、Ｚｔ Ｖ）は時刻ｔにおける現在位置であり、現在位置算出部１１２が算出している。
　衛星方向算出部１１４は、現在位置算出部１１２が算出した現在位置（Ｘｖ，Ｙｖ，Ｚｖ）、および、衛星情報取得部１０６が算出したＧＰＳ衛星の位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）に基づいて、各ＧＰＳ衛星ｉの方向Ｒｉを算出する。各ＧＰＳ衛星ｉの方向Ｒｉは、車両からＧＰＳ衛星ｉを見たときの、水平方向に対する仰角θｉ、北方向に対する方位角φｉで表すものとする。
　衛星方向速度算出部１１６は、ＧＰＳ衛星ｉの方向への車両の速度である衛星方向速度Ｖｓｉを算出する。衛星方向速度Ｖｓｉは、下記（７）式から算出する。
Figure JPOXMLDOC01-appb-M000006
 　（７）式において、右辺第１項は相対速度Ｖｒであり、相対速度算出部１０８が算出している。Ｇｘ、Ｇｙ、Ｇｚは、視線ベクトルであり、視線ベクトル算出部１１３が算出している。Ｖｘｓ、Ｖｙｓ、Ｖｚｓは、ＧＰＳ衛星ｉの速度のｘ、ｙ、ｚ成分であり、衛星速度算出部１１０が算出している。（７）式の右辺の第１項は、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒｉであり、第２～第４項は、ＧＰＳ衛星ｉの車両方向への速度である。これらの和は、ＧＰＳ衛星ｉの方向への車両の速度を意味することから、（７）式が成り立つのである。
　速度ベクトル算出部１１８は、車両の速度ベクトルを算出する。車両の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、衛星方向速度Ｖｓｉと、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、以下の（８）式で表される。
Figure JPOXMLDOC01-appb-M000007
 　各ＧＰＳ衛星ｉについて得られる上記（８）式より、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）およびＣｂｖを未知数とした以下の（９）式で表される連立方程式が得られる。
　（９）式において、Ｖｓａｔｉは、車両方向のＧＰＳ衛星ｉの速度であり、Ｖｓａｔｉ＝Ｒｉ［Ｖｘｓｉ，Ｖｙｓｉ，Ｖｚｓｉ］Ｔにより求める。Ｔは行列の転置を意味する。なお、（７）式の右辺第２～第４項を計算して車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔｉを求めてもよい。ＣｂｖはＧＰＳ信号受信部１０が備える時計のクロックドリフトである。
Figure JPOXMLDOC01-appb-M000008
 　ＧＰＳ電波を受信したＧＰＳ衛星が４個以上である場合に、上記（９）式の連立方程式を解くことができる。ただし、ＧＰＳ電波を受信していても、信号品質がよいと判定できないＧＰＳ電波は除外する。よって、速度ベクトル算出部１１８は、信号品質がよいと判定したＧＰＳ電波を４個以上のＧＰＳ衛星から受信した場合に、（９）式から車両の速度ベクトルを算出する。信号品質の良否は、次に説明する信号品質判定部１２０が判定する。
　信号品質判定部１２０は、ＧＰＳ信号受信部１０が受信したＧＰＳ衛星ｉからのＧＰＳ電波の信号品質がよいか否かを判定する。信号品質の判定には、公知の種々の基準を用いることができる。
　たとえば、（判定条件１）Ｓ／Ｎが所定値以上であること、（判定条件２）擬似距離ρｉの残差が判定基準距離以下であること、（判定条件３）仰角θｉが判定基準角以上であること、（判定条件４）判定条件１～３の組み合わせ、などにより信号品質がよいか否かを判定する。
　なお、擬似距離ρｉの残差とは、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）と車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）との間の距離と、擬似距離ρｉとの差である。この残差が大きい場合には、マルチパス等の影響が考えられるので、信号品質が良くないと判定する。判定条件１は、Ｓ／Ｎが所定値以上であれば信号品質はよいと判定する。判定条件３は、仰角θｉが判定基準角以上であれば信号品質はよいと判定する。
　停止判定部１２２は、車両が停止しているか否かを判定する。そして、車両が停止していると判定した場合には、車両の速度を０ｋｍ／ｈとする。停止判定の方法は、公知の種々の手法を用いることができる。たとえば、加速度センサ２０のｚ軸の検出値が停止判定値以下であれば、車両が停止していると判定する。車両が走行している場合には、多少の上下振動があるので、ｚ軸の検出値により停止判定を行うことができるのである。ｚ軸の検出値に代えて、ｙ軸の検出値や、ｚ軸の検出値の変化量、ｙ軸の検出値の変化量を用いてもよい。加速度センサ２０の検出値により停止判定を行えば、ブレーキ信号やシフト位置信号を取得するための配線が不要となる利点がある。もちろん、ブレーキ信号やシフト位置信号を取得できるようにして、それらの信号を用いて停止判定をしてもよい。
　初期設定値決定部１２６は、推定速度決定部１２８で用いる速度初期値Ｖ０、推定方位決定部１３０で用いる方位初期値θ０決定する。
　本実施形態では、すでに説明した速度ベクトル算出部１１８でも車両の速度を算出することができる。しかし、速度ベクトル算出部１１８は、信号品質がよいＧＰＳ電波を４個以上のＧＰＳ衛星から受信しなければ、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。したがって、たとえば、高層ビルの多い都市部では、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できない時間が長く続くこともある。また、信号品質がよいＧＰＳ電波を４個以上受信できる環境であっても、速度ベクトル算出部１１８では、周波数解析を必要とするドップラーシフト量Ｄを用いるため、たとえば、１００ｍｓ毎などの一定周期でしか、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。
　これに対して、加速度センサ２０の検出値は、走行環境に依存せずに一定周期で取得でき、しかも、たとえば２０ｍｓごとなど、速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するよりも短い周期で取得する構成とすることができる。
　そこで、速度ベクトル算出部１１８が車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出してから、次に、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するまでの間、加速度積算値ΔＶＧをもとにして速度推定を行えるようにする。
　加速度センサ２０の検出値には常に誤差があり、その誤差は発散する性質を有する。図２はそのことを示すグラフである。図２は停止状態における加速度積算値ΔＶＧのグラフである。車両が停止状態であることから、この図２における加速度積算値ΔＶＧは、加速度積算値ΔＶＧをそのまま速度とした場合の誤差を意味する。図２から、加速度積算値ΔＶＧをそのまま速度とする場合、誤差が時間の経過により発散することが分かる。なお、図２では、誤差は負の値であるが、これとは反対に誤差が正の値となることもある。
　このように、加速度積算値ΔＶＧをそのまま速度とすると、誤差が時間の経過により発散する。そこで、精度の高い速度を求めた時点で、その精度の高い速度を速度初期値Ｖ０とする。
　この時点での加速度積算値ΔＶＧを、精度の高い速度から引いた値を速度初期値Ｖ０とする。このようにして求める速度初期値Ｖ０は、加速度積算値ΔＶＧを真の速度とみなす精度の高い速度にするためのオフセット分を意味する。よって、ΔＶＧ－Ｖ０により推定速度Ｖｅを求める。
　加速度積算値ΔＶＧと真の速度との誤差は、時間の経過とともに増大する。したがって、速度初期値Ｖ０の更新周期が短いほど、加速度積算値ΔＶＧを用いた速度の推定精度が向上する。
　そこで、本実施形態では、特許文献２において最終的な速度ベクトル算出式として開示されている下記（１１）式を改良した（１２）式により速度初期値Ｖ０を算出する。また、同時に方位初期値θ０も算出する。
　ただし、速度初期値Ｖ０については、速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できたときは、（１０）式から速度初期値Ｖ０を決定する。速度ベクトル算出部１１８が算出する速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は精度がよいので、この速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を用いて速度初期値Ｖ０を決定したほうが、速度初期値Ｖ０の精度が向上するからである。また、停止判定部１２２が、車両が停止していると判定したときは、速度初期値Ｖ０を－ΔＶＧとする。
Figure JPOXMLDOC01-appb-M000009
 　（１１）式において、ｔは時刻、Ｖｗｈｅｅｌは車輪速センサの検出値、θ０は初期時刻における車両の進行方向の方位角（以下、方位初期値）、θｇｙｒｏは、車両の進行方向の方位角の積算値すなわち相対方位角、Ｃｂｖ０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾き、Ｇｘ、Ｇｙは、車両からＧＰＳ衛星ｉへの視線ベクトルのｘ成分、ｙ成分である。
　また、（１２）式において、Ｖ０は初期時刻における車速である速度初期値、ΔＶＧは初期時刻以降の加速度積算値である。すなわち、（１２）式は、（１１）式における車輪速センサの検出値Ｖｗｈｅｅｌを、速度初期値Ｖ０と加速度積算値ΔＶＧの和に置き換えた式である。この（１２）式が速度推定式の一例に相当する。
　まず、（１１）式の導出方法を説明する。特許文献２にも開示されているように、衛星方向速度Ｖｓｔ ｉと、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、（１３）式で表すことができる。
Figure JPOXMLDOC01-appb-M000010
 　この（１３）式には、Ｖｘｔ、Ｖｙｔ、Ｖｚｔ、Ｃｂｖｔの４つの未知パラメータがある。この未知パラメータの数を少なくするために、特許文献２では、下記に示す拘束条件１～３を（１３）式に代入することで、前述した（１１）式を導出している。
Figure JPOXMLDOC01-appb-M000011
 　拘束条件１は、推定する速度ベクトルのｘ成分、ｙ成分の大きさを車輪速で拘束し、かつｘ成分、ｙ成分の時間変化分を方位角の時間変化で拘束したものである。拘束条件１のθ０は初期時刻における車両の進行方向の方位角である。拘束条件２は、高さ方向速度の変化は常に微小であると仮定するものである。
　拘束条件３は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件３のＣｂｖ０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾きを示す。
　なお、拘束条件１および２では、二次元平面内の速度成分のみ時間変化で拘束しているが、３軸ジャイロセンサなどにより取得したピッチレートを用いて、ｚ軸成分も時間変化で拘束してもよい。上記拘束条件１～３を（１３）式に取り入れることで（１１）式が得られる。そして、（１１）式における車輪速センサの検出値Ｖｗｈｅｅｌを、速度初期値Ｖ０と加速度積算値ΔＶＧの和に置き換えることで、（１２）式が得られる。
　（１２）式において、衛星方向速度Ｖｓｔ ｉは、時刻ｔにおけるＧＰＳ衛星ｉ方向の衛星方向速度であり、衛星方向速度算出部１１６が算出する。ΔＶｔ Ｇは、時刻ｔにおける加速度積算値であり、積算処理部１０４が算出する。（Ｇｘｔ ｉ、Ｇｙｔ ｉ、Ｇｚｔ ｉ）は、時刻ｔにおける視線ベクトルであり、視線ベクトル算出部１１３が算出する。θｔ ｇｙｒｏは、ヨーレート積算値であり、積算処理部１０４が算出する。よって、（１２）式において未知パラメータは、θ０、Ｃｂｖ０、Ａ、および、Ｖ０の４つである。
　したがって、４つの式を立式できれば、未知パラメータになっている速度初期値Ｖ０および方位初期値θ０を求めることができる。しかも、未知パラメータθ０、Ｃｂｖ０、Ａ、Ｖ０は、初期時刻以降であれば、時刻が異なっても同じである。そのため、同一時刻で４つの式を立式する必要はなく、複数の時刻において立式した式数が合計４式以上であれば、未知パラメータを求めることができる。たとえば、例えば、４時刻（ｔ０、ｔ１、ｔ２、ｔ３）の観測衛星数がそれぞれ１であったとしても、観測されたＧＰＳ衛星からのデータを用いて、速度初期値Ｖ０および方位初期値θ０を求めることができる。
　なお、速度初期値Ｖ０は、前述したように、初期時刻における車速である。また、速度初期値Ｖ０を算出する（１２）式は、ドップラーシフト量Ｄから算出する衛星方向速度Ｖｓを用いる。したがって、初期設定値決定部１２６は、ドップラーシフト量に基づいて、初期時刻における車両の速度を算出していることにもなる。
　推定速度決定部１２８は、初期設定値決定部１２６が決定した速度初期値Ｖ０に、積算処理部１０４が算出した加速度積算値ΔＶｔ Ｇを加算して、推定速度Ｖｔ ｅを加速度取得周期で逐次算出する。
　推定方位決定部１３０は、初期設定値決定部１２６が決定した方位初期値０に、積算処理部１０４が算出した相対方位角θｔ ｇｙｒｏを加算して、車両の移動方位を示す推定方位θｔ ｅを、相対方位角決定周期で逐次決定する。
　位置更新部１４０は、車両の現在位置を逐次更新する。この位置更新部１４０の詳細構成を図３に示している。図３に示すように、位置更新部１４０は、移動距離算出部１４１、過去速度補正部１４２、移動距離補正部１４３、位置決定部１４４を備える。
　移動距離算出部１４１は、車両の速度が更新されるごとに、車両の移動距離を算出する。車両の速度は、加速度取得周期で更新される。車両の速度は、推定速度決定部１２８、速度ベクトル算出部１１８、停止判定部１２２のいずれかが決定する。移動距離算出部１４１は、それら推定速度決定部１２８、速度ベクトル算出部１１８、停止判定部１２２が決定した速度に加速度取得周期を乗じて、加速度取得周期の間に車両が移動した距離の推定値である推定移動距離Ｌｔ ｅを算出する。
　過去速度補正部１４２は、初期設定値決定部１２６が速度初期値Ｖ０を更新するごとに、前回、速度初期値Ｖ０が更新されてから、今回、速度初期値Ｖ０が更新されるまでに推定速度決定部１２８が逐次推定した推定速度Ｖｔ ｅを補正する。この補正対象となっている推定速度Ｖｔ ｅを、以下、補正前過去推定速度Ｖｔ（ｉ） ｅとする。なお、ｉはＮ～０までの整数であり、ｔ（Ｎ）は、前回、速度初期値Ｖ０が更新された時刻、ｔ（０）は、今回、速度初期値Ｖ０が更新された時刻を意味する。また、補正前過去推定速度Ｖｔ（ｉ） ｅを補正した値を補正過去速度Ｖｔ（ｉ） ｃｏとする。この補正過去速度Ｖｔ（ｉ） ｃｏは、下記式（１４）式から算出する。
Figure JPOXMLDOC01-appb-M000012
 　速度初期値Ｖ０は精度のよい速度であるとみなすことができる。したがって、（１４）式において、速度初期値Ｖ０とその速度初期値Ｖ０に更新された時点における補正前過去推定速度Ｖｔ（０） ｅとの差分（Ｖ０－Ｖｔ（０） ｅ）は、速度初期値Ｖ０を更新した時点までに補正前過去推定速度Ｖｔ（０） ｅに累積した誤差を意味する。
　そして、（ｔ（ｉ）－ｔ（Ｎ））／（ｔ（０）－ｔ（Ｎ））は、前回、速度初期値Ｖ０が更新されてから、今回、速度初期値Ｖ０が更新されるまでの期間（ｔ（０）－ｔ（Ｎ））に対する、前回、速度初期値Ｖ０が更新されてから、それぞれの補正前過去推定速度Ｖｔ（ｉ） ｅが推定されるまでの期間の比率を表す係数となっている。（１４）式では、差分（Ｖ０－Ｖｔ（０） ｅ）にこの係数を乗じている。したがって、（１４）式から算出する補正過去速度Ｖｔ（ｉ） ｃｏは、補正前過去推定速度Ｖｔ（ｉ） ｅを、その補正前過去推定速度Ｖｔ（ｉ） ｅの誤差が線形に増加すると仮定して補正した値である。
　図４は、補正前過去推定速度Ｖｔ（ｉ） ｅと補正過去速度Ｖｔ（ｉ） ｃｏとを比較して示す図である。この図４に示すように、ｔ（０）時点では、速度初期値Ｖ０と補正前過去推定速度Ｖｔ（０） ｅとの間に差分（Ｖ０－Ｖｔ（０） ｅ）がある。この差分は、推定速度Ｖｔ ｅに累積した誤差を意味する。速度初期値Ｖ０を更新した時点では、推定速度Ｖｔ ｅ＝速度初期値Ｖ０となる。したがって、推定速度Ｖｔ ｅは速度初期値Ｖ０を更新した時点で不連続になる。
　本実施形態では、（１４）式を用いて補正前過去推定速度Ｖｔ（ｉ） ｅを補正して補正過去速度Ｖｔ（ｉ） ｃｏとする。この補正過去速度Ｖｔ（ｉ） ｃｏは、図４に示すように、ｔ（０）以降の推定速度Ｖｔ ｅとの間に連続性がある。実際の車両の速度変化には連続性があることから、（１４）式を用いて算出した補正過去速度Ｖｔ（ｉ） ｃｏは、補正前過去推定速度Ｖｔ（ｉ） ｅよりも、より真の速度に近いと言える。
　移動距離補正部１４３は、過去速度補正部１４２が補正過去速度Ｖｔ（ｉ） ｃｏを算出した場合、その補正過去速度Ｖｔ（ｉ） ｃｏを推定速度Ｖｔ ｅの代わりに用いて、移動距離算出部１４１と同様にして推定移動距離を再計算する。この推定移動距離を補正移動距離Ｌｔ（ｉ） ｃｏとする。
　位置決定部１４４は、信号品質判定部１２０で信号品質がよいと判定されたＧＰＳ電波を用いて、現在位置算出部１１２が（５）式を用いて車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出した場合には、現在位置算出部１１２が算出した現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を、更新後の現在位置とする。
　現在位置算出部１１２が算出した現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を更新後の現在位置としない場合には、移動距離算出部１４１が算出した移動距離Ｌｔ ｅと、推定方位決定部１３０が逐次決定した推定方位θｔ ｅを用いて、現在位置（Ｘｖ，Ｙｖ）を逐次算出する。
　具体的には、下記（１５）式、（１６）式を用いて、前回の現在位置（Ｘｖ ｔ（－１），Ｙｖ ｔ（－１））と、今回の推定移動距離Ｌｔ（０） ｅと、今回の推定方位θｔ（０） ｅとから、現時点ｔ（０）の現在位置（Ｘｖ ｔ（０），Ｙｖ ｔ（０））を算出する。なお、この現在位置（Ｘｖ ｔ（０），Ｙｖ ｔ（０））は、もちろん、前回の現在位置（Ｘｖ ｔ（－１），Ｙｖ ｔ（－１））に対する相対位置でもある。
　Ｘｖ ｔ（０）＝Ｘｖ ｔ（－１）＋Ｌｔ（０） ｅ×ｃｏｓθｔ（０） ｅ　（１５）
　Ｙｖ ｔ（０）＝Ｙｖ ｔ（－１）＋Ｌｔ（０） ｅ×ｓｉｎθｔ（０） ｅ　（１６）
　さらに、位置決定部１４４は、移動距離補正部１４３が補正移動距離Ｌｔ ｃｏを算出した場合には、前回、速度初期値Ｖ０が更新された時点Ｔ（Ｎ）から、今回、速度初期値Ｖ０が更新される時点ｔ（０）までの位置（Ｘｖ ｔ（ｉ），Ｙｖ ｔ（ｉ））を逐次補正して、現在位置（Ｘｖ ｔ（０），Ｙｖ ｔ（０））を補正する。
　Ｘｖ ｔ（ｉ）＝Ｘｖ ｔ（ｉ－１）＋Ｌｔ（ｉ） ｅ×ｃｏｓθｔ（ｉ） ｅ　（１７）
　Ｙｖ ｔ（ｉ）＝Ｙｖ ｔ（ｉ－１）＋Ｌｔ（ｉ） ｅ×ｓｉｎθｔ（ｉ） ｅ　（１８）
　＜制御部１００の処理の流れ＞
　次に、制御部１００の処理の流れの一例を図５～図８のフロチャートを用いて説明する。図５に示すフロチャートは、センサ値を取得する周期で繰り返し実行する。なお、特に明記している場合を除き、各パラメータは、最新の時刻、すなわち、時刻ｔ＝０における値を意味する。
　図５において、ステップＳ２では、加速度センサ２０、ヨーレートセンサ３０の検出値を取得し、ＲＡＭなどの記憶部に記憶する。この処理は、たとえば積算処理部１０４が行う。
　ステップＳ４は進行方向加速度決定部１０２が行う処理であり、ステップＳ２で取得した加速度センサ２０の検出値から、進行方向加速度を決定する。
　ステップＳ６は積算処理部１０４が行う処理であり、ステップＳ２で取得したヨーレートセンサ３０の検出値を、これまでの相対方位角θｔ（－１） ｇｙｒｏに加算して相対方位角θｔ ｇｙｒｏを更新する。また、ステップＳ２で決定した進行方向加速度をこれまでの加速度積算値ΔＶｔ（－１） Ｇに加算して加速度積算値ΔＶｔ Ｇを更新する。
　ステップＳ８、Ｓ１０は衛星情報取得部１０６が行う処理である。ステップＳ８では、ＧＰＳ信号受信部１０からＧＰＳ信号および搬送波を取得する。ステップＳ１０では、ステップＳ８で取得したＧＰＳ信号および搬送波から、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、擬似距離ρｉ、ドップラーシフト量Ｄｉを算出する。
　ステップＳ１２は、停止判定部１２２の処理であり、たとえば、ステップＳ２で取得した加速度センサ２０のｚ軸の検出値から、車両が停止しているか否かを判断する。停止中と判断した場合にはステップＳ１４へ進み、移動中であると判断した場合にはステップＳ１８へ進む。
　ステップＳ１４も停止判定部１２２の処理であり、速度ベクトルを（０、０、０）とする。もちろん、速度も０となる。
　ステップＳ１６は初期設定値決定部１２６の処理であり、速度初期値Ｖ０を、最新の加速度積算値－ΔＶｔ Ｇとする。ステップＳ１６を実行したらステップＳ３４の位置更新処理を実行する。ステップＳ３４の位置更新処理は後述する。
　ステップＳ１２において移動中と判断した場合に実行するステップＳ１８は信号品質判定部１２０の処理であり、前述した判定条件に基づいて、ＧＰＳ信号の信号品質がよいか否かを判定する。信号品質の判定は、取得したすべてのＧＰＳ信号に対して行う。
　ステップＳ２０は速度ベクトル算出部１１８の処理であり、ステップＳ１８で信号品質がよいと判定したＧＰＳ信号の数が４以上であるか否かを判断する。４以上である場合にはステップＳ２２へ進む。
　ステップＳ２２では、ドップラー速度を算出する。このドップラー速度とは、速度ベクトル算出部１１８が算出する速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）またはその速度ベクトルの大きさを意味する。この速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、ドップラーシフト量Ｄを用いて算出することから、ここでは、ドップラー速度と称している。
　ステップＳ２２の詳細処理は図６に示す。図６において、ステップＳ２２２は現在位置算出部１１２の処理であり、ステップＳ１０で算出した擬似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。
　ステップＳ２２４は衛星方向算出部１１４が行う処理であり、ステップＳ２２２で算出した車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）、および、ステップＳ１０で算出したＧＰＳ衛星の位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、各ＧＰＳ衛星ｉの方向Ｒｉ（θｉ、φｉ）を算出する。
　ステップＳ２２６は衛星速度算出部１１０が行う処理であり、ステップＳ１０で算出した各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を算出する。
　ステップＳ２２８、Ｓ２３０は速度ベクトル算出部１１８の処理である。ステップＳ２２８では、ステップＳ２２４で算出した各ＧＰＳ衛星ｉの方向Ｒｉと、ステップＳ２２６で算出した各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）から、Ｖｓａｔｉ＝Ｒｉ［Ｖｘｓｉ，Ｖｙｓｉ，Ｖｚｓｉ］Ｔにより、車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔｉを求める。
　ステップＳ２３０では、（９）式に示した連立方程式を４つ以上立式し、その連立方程式を解く。これにより、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）およびクロックドリフトＣｂｖｔを求めることができる。
　説明を図５に戻す。ステップＳ２４は初期設定値決定部１２６の処理であり、ステップＳ２２で算出した車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）から、（１０）式により、速度初期値Ｖ０を決定する。ステップＳ２４を実行した場合にも、ステップＳ３４の位置更新処理を実行する。ステップＳ３４を説明する前に、先に、ステップＳ２６以下を説明する。
　ステップＳ２０において品質のよい信号が３以下であると判断した場合にはステップＳ２６へ進む。ステップＳ２６は初期設定値決定部１２６の処理であり、初期値を更新することができるか否かを判断する。この判断は、具体的には、速度初期値Ｖ０を更新してからのＧＰＳ信号を用いて、（１２）式を４つ以上、立式できるか否かを判断するものである。この判断がＮＯであればステップＳ３０へ進み、ＹＥＳであればステップＳ２８へ進む。
　ステップＳ２８では、タイトカップリング型推定式を用いて速度初期値Ｖ０および方位初期値θ０を決定する。タイトカップリング型推定式とは、具体的には（１２）式のことである。
　ステップＳ２８の詳細処理は図７に示す。図７において、ステップＳ２８２は衛星速度算出部１１０が行う処理であり、ステップＳ１０で算出した各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を算出する。
　ステップＳ２８４は相対速度算出部１０８が行う処理であり、ステップＳ１０で算出したドップラーシフト量Ｄｉを前述した（２）式に代入して、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒｉを算出する。
　ステップＳ２８６は現在位置算出部１１２の処理であり、ステップＳ１０で算出した擬似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。
　ステップＳ２８８は視線ベクトル算出部１１３が行う処理である。このステップＳ２８８では、前述した（６）式に、ステップＳ１０で算出した擬似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、ステップＳ２８６で算出した車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を代入して、視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）を算出する。
　ステップＳ２９０は衛星方向速度算出部１１６が行う処理である。このステップＳ２９０では、前述した（７）式に、ステップＳ２８４で算出した相対速度Ｖｒｉ、ステップＳ２８８で算出した視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）、ステップＳ２８２で算出したＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を代入して、ＧＰＳ衛星ｉの方向への車両の衛星方向速度Ｖｓｉを算出する。
　ステップＳ２９２は初期設定値決定部１２６が行う処理である。このステップＳ２９２では、前述した（１２）式に、ステップＳ２９０で算出した衛星方向速度Ｖｓｉ、ステップＳ６で更新した加速度積算値ΔＶＧ、相対方位角θｇｙｒｏ、ステップＳ２８８で算出した視線ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を代入した式を４つ以上立式する。そして、それら４つ以上の式からなる連立方程式を解く。これにより、（１２）式において未知パラメータとなっている速度初期値Ｖ０、方位初期値θ０を同時に求めることができる。
　説明を図５に戻す。ステップＳ２８を実行した場合、または、ステップＳ２６の判断がＮＯであった場合には、ステップＳ３０を実行する。ステップＳ３０は推定速度決定部１２８が行う処理であり、最新の速度初期値Ｖ０と、ステップＳ６で更新した加速度積算値ΔＶＧとから、推定速度Ｖｅを算出する。
　図５のように、速度初期値Ｖ０を更新しても加速度積算値ΔＶＧをリセットしない場合には、加速度積算値ΔＶＧに速度初期値Ｖ０を加算することで推定速度Ｖｅを算出する。なお、速度初期値Ｖ０を更新したときに加速度積算値ΔＶＧをリセットする場合には、速度初期値Ｖ０に加速度積算値ΔＶＧを加算することで、推定速度Ｖｅを算出する。
　ステップＳ３２は推定方位決定部１３０が行う処理であり、最新の方位初期値θ０と、ステップＳ６で更新した相対方位角θｇｙｒｏとから、推定方位θｅを算出する。具体的な推定方位θｅの計算方法は、速度初期値Ｖ０と加速度積算値ΔＶＧから推定速度Ｖｅを算出する方法において、速度初期値Ｖ０を方位初期値θ０に置き換え、加速度積算値ΔＶＧを相対方位角θｇｙｒｏに置き換えた場合と同じである。ステップＳ３２を実行した場合にも、ステップＳ３４の位置更新処理を実行する。このステップＳ３４は、位置更新部１４０が実行する。
　ステップＳ３４の詳細処理は図８に示す。なお、図８において、ステップＳ３４８は移動距離算出部１４１が実行し、ステップＳ３５２は過去速度補正部１４２が実行し、ステップＳ３５４は移動距離補正部１４３が実行する。その他のステップは位置決定部１４４が実行する。
　ステップＳ３４２では、ＧＰＳ現在位置を決定したか否かを判断する。ＧＰＳ現在位置とは、信号品質判定部１２０で信号品質がよいと判定されたＧＰＳ電波を用いて、現在位置算出部１１２が（５）式から算出した現在位置（Ｘｖ，Ｙｖ，Ｚｖ）のことである。この判断がＹＥＳであれば、ステップＳ３４４に進む。ステップＳ３４４では、ＧＰＳ現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を、最新の現在位置とする。
　ステップＳ３４２の判断がＮＯである場合には、ステップＳ３４６に進む。ステップＳ３４６では、速度初期値Ｖ０を更新したか否かを判断する。図５のステップＳ１６、Ｓ２４、Ｓ２８のいずれかを実行している場合には、このステップＳ３４６の判断がＹＥＳになる。これに対して、図５のステップＳ２６の判断がＮＯであった場合には、ステップＳ３４６の判断もＮＯになる。ステップＳ３４６の判断がＮＯである場合には、ステップＳ３４８に進む。
　ステップＳ３４８では、ステップＳ３０で算出した推定速度Ｖｅに加速度取得周期を乗じて推定移動距離Ｌｅを算出する。ステップＳ３５０では、ステップＳ３４８で算出した推定移動距離Ｌｅと、ステップＳ３２で推定した推定方位θｅと、式１５、式１６を用いて、最新の現在位置（Ｘｖ，Ｙｖ）を算出する。なお、ｚ座標は変化なしとする。
　ステップＳ３４６の判断がＹＥＳであった場合に実行するステップＳ３５２では、前回、速度初期値Ｖ０を更新した時点ｔ（Ｎ）から、今回、速度初期値Ｖ０を更新した時点ｔ（０）までの推定速度Ｖｔ ｅ、すなわち、補正前過去推定速度Ｖｔ（ｉ） ｅを、（１４）式で補正して、補正過去速度Ｖｔ（ｉ） ｃｏを算出する。
　ステップ３５４では、ステップＳ３５２で算出した補正過去速度Ｖｔ（ｉ） ｃｏに加速度取得周期を乗じて、補正移動距離Ｌｔ（ｉ） ｃｏを算出する。
　ステップＳ３５６では、ステップＳ３５４で算出した補正移動距離Ｌｔ（ｉ） ｃｏと、ステップＳ３２で推定した推定方位θｔ（ｉ） ｅと、式１７、式１８を用いて、現在位置（Ｘｖ，Ｙｖ）を再算出する。なお、ｚ座標は変化なしとする。
　＜実施形態の効果＞
　本実施形態では、ドップラー速度を算出した場合（Ｓ２２）には、そのドップラー速度を速度初期値Ｖ０としており（Ｓ２４）、車両の速度が０ｋｍ／ｈである場合（Ｓ１４）には、その速度を速度初期値Ｖ０としている（Ｓ１６）。また、タイトカップリング型推定式である（１２）式を解くことができる場合、（１２）式から求まる初期時刻における速度を速度初期値Ｖ０としている（Ｓ２８）。これらの方法で決定する速度初期値Ｖ０は精度がよい。
　そして、推定速度Ｖｅは、このようにして決定した速度初期値Ｖ０と加速度積算値ΔＶＧとを用いて推定している（Ｓ３０）。したがって、速度初期値Ｖ０を更新した以後は、加速度積算値ΔＶＧの累積誤差により低下している推定速度Ｖｅの精度が向上する。
　加えて、本実施形態では、速度初期値Ｖ０を決定した場合、前回、速度初期値Ｖ０を更新した時点ｔ（Ｎ）から、今回、速度初期値Ｖ０を更新する時点ｔ（０）までの推定速度Ｖｅである補正前過去推定速度Ｖｔ（ｉ） ｅを、今回の速度初期値Ｖ０と連続するように補正して補正過去速度Ｖｔ（ｉ） ｃｏとする（Ｓ３５２）。これにより、加速度積算値ΔＶＧの累積誤差により低下している過去の推定速度Ｖｅの精度も向上する。そして、補正過去速度Ｖｔ（ｉ） ｃｏを決定した場合、その補正過去速度Ｖｔ（ｉ） ｃｏを用いて、前回、速度初期値Ｖ０を更新してから、今回、速度初期値Ｖ０を更新するまでの推定移動距離を再計算した補正移動距離Ｌｔ（ｉ） ｃｏを決定する（Ｓ３５４）。これにより、過去の推定速度Ｖｅの精度低下により低下していた推定移動距離の精度も向上する。
　また、本実施形態では、補正移動距離Ｌｔ（ｉ）を算出した場合には、その補正移動距離Ｌｔ（ｉ）を用いて、現在位置を補正する（Ｓ３５６）。したがって、ＧＰＳ現在位置が決定できない状況が継続している状況でも、現在位置を精度よく決定し続けることができる。
　このように、速度初期値Ｖ０の更新時に、過去の推定速度である補正前過去推定速度Ｖｔ（ｉ） ｅを補正して、現在位置を補正することに加えて、本実施形態では、以下の効果も得られる。
　車両の衛星方向速度Ｖｓｉは、（１３）式に示したように、車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘ，Ｇｙ，Ｇｚ）と、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）と、クロックドリフトＣｂｖに基づいて算出することができる。
　本実施形態では、この（１３）式に対して、速度ベクトルのｘ成分、ｙ成分を、車両の速度、および車両の進行方向の方位角の時間変化すなわち相対方位角θｇｙｒｏで拘束している（拘束条件１）。初期設定値決定部１２６は、これにより得られる（１２）式を用いて速度初期値Ｖ０を決定する。
　このように（１２）式は時間変化に関する条件で拘束されていることから、異なる複数の観測時点のＧＰＳ信号を用いた（１２）式を連立方程式とすることができる。（１２）式には、速度初期値Ｖ０以外にも未知パラメータがあるため、未知パラメータの数に相当する複数の（１２）式を連立方程式とする必要がある。しかし、本実施形態では、異なる複数の観測時点のＧＰＳ信号を用いた（１２）式を連立方程式とすることができるので、未知パラメータを解くことができる数の（１２）式からなる連立方程式を立式しやすい。
　よって、初期設定値決定部１２６は、速度初期値Ｖ０を高頻度に求めることができる。そして、速度初期値Ｖ０を高頻度に更新することで、加速度センサ２０の検出値からドリフトの影響を除去する頻度が高くなるので、加速度積算値ΔＶＧと速度初期値Ｖ０から算出する推定速度Ｖｅの推定精度が向上する。従って、推定速度Ｖｅを用いて決定する現在位置の精度も向上する。
　また、本実施形態では、拘束条件１に加えて、クロックドリフトの時間変化が線形であるとする拘束条件３も用いて（１２）式を導出している。これにより、（１２）式における未知パラメータの数をより少なくすることができる。その結果、未知パラメータを解く数の連立方程式を立式しやすくなるので、速度初期値Ｖ０の更新頻度をさらに高くすることができる。
　また、（１２）式は、車両の速度の項として、速度初期値Ｖ０と加速度積算値ΔＶＧの項とを含んでいる。よって、未知パラメータの数の（１２）式からなる連立方程式を解くことで、直接、速度初期値Ｖ０を得ることができる。
　また、本実施形態では、加速度積算値ΔＶＧを用いないで速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出する速度ベクトル算出部１１８を備える。この速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出した場合、速度ベクトル算出部１１８が算出した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を用いて速度初期値Ｖ０を決定する（Ｓ２４）。速度ベクトル算出部１１８が算出した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は精度がよいことから、このようにすれば、速度初期値Ｖ０の精度が向上する。
　また、本実施形態では、停止判定部１２２が、車両が停止していると判定したときは、車両の速度がゼロであるとして、速度初期値Ｖ０を決定する（Ｓ１６）。これによっても、速度初期値Ｖ０の精度が向上する。そして、速度初期値Ｖ０の精度が向上する結果、速度初期値Ｖ０と加速度積算値ΔＶＧから算出する推定速度Ｖｅの精度も向上する。
　以上、実施形態を例示したが、実施形態上述の実施形態に限定されるものではなく、次の変形例も実施形態に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変形することができる。
　＜変形例１＞
　前述の実施形態では、拘束条件３により、時間変化が線形であるという条件でクロックドリフトを拘束していたが、この拘束条件３をなしにしてもよい。拘束条件３をなしにする場合、（１２）式におけるＣｂｖ０＋Ａｔを、未知パラメータであるＣｂｖｔに置き換えることになる。
　＜変形例２＞
　また、拘束条件３は残し、拘束条件１における方位角の時間変化による拘束をなしにしてもよい。この場合には、拘束条件１が、速度ベクトルの大きさを車両の速度で拘束したのみの条件となり、（１２）式におけるθｔ ｇｙｒｏが未知パラメータになる。
　＜変形例３＞
　前述の実施形態では、衛星測位システムとしてＧＰＳを利用していたが、その他の衛星測位システムを利用してもよい。また、ＧＰＳが備える衛星と、とその他の衛星測位システムが備える衛星を両方用いてもよい。
　＜変形例４＞
　前述の実施形態では、移動体は車であったが、車以外の移動体にも、移動距離推定装置は適用できる。

 

Claims (8)

  1. 　移動体とともに移動する加速度センサ（２０）の検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、
    　前記進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部（１０４）と、
    　衛星測位システムが備える衛星からのドップラーシフト量に基づいて算出する前記移動体の速度、または、前記移動体の停止時の速度を、前記移動体の速度初期値として決定する初期設定値決定部（１２６）と、
    　前記積算処理部が算出した加速度積算値と、前記初期設定値決定部が決定した速度初期値とに基づいて、前記移動体の推定速度を逐次推定する推定速度決定部（１２８）と、
    　前記推定速度決定部が決定した前記推定速度と時間とに基づいて前記移動体の推定移動距離を逐次算出する移動距離算出部（１４１）と、
    　前記初期設定値決定部が前記速度初期値を決定した場合、前回、前記初期設定値決定部が前記速度初期値を決定してから、今回、初期設定値決定部が前記速度初期値を決定するまでに前記推定速度決定部が逐次推定した推定速度である補正前過去推定速度を、今回、初期設定値決定部が決定した前記速度初期値と連続するように補正した補正過去速度を決定する過去速度補正部（１４２）と、
    　前記過去速度補正部が前記補正過去速度を決定した場合に、前記補正過去速度と時間とに基づいて、前回、前記初期設定値決定部が前記速度初期値を決定してから、今回、初期設定値決定部が前記速度初期値を決定するまでの前記推定移動距離を再計算する移動距離補正部（１４３）と、を備える移動距離推定装置。
  2. 　請求項１において、
    　前記移動体の移動方位を逐次推定する推定方位決定部（１３０）と、
    　前記移動距離算出部が算出した前記推定移動距離と、前記推定方位決定部が決定した移動方位とに基づいて、前回の位置決定時に対する前記移動体の相対位置を逐次決定する位置決定部（１４４）とを備え、
    　前記位置決定部は、前記移動距離補正部が前記推定移動距離を再計算した場合には、再計算した前記推定移動距離と、前記推定方位決定部が決定した移動方位とに基づいて、前回、前記初期設定値決定部が前記速度初期値を決定してから、今回、初期設定値決定部が前記速度初期値を決定するまでの前記移動体の前記相対位置を再計算する移動距離推定装置。
  3. 　請求項１または２において、
    　前記過去速度補正部は、
    　前記補正前過去推定速度を、その補正前過去推定速度の誤差が線形に増加するとして、
    　今回、前記初期設定値決定部が決定した速度初期値と最新の前記補正前過去推定速度との差分に、前回、前記初期設定値決定部が前記速度初期値を決定してから、それぞれの前記補正前過去推定速度を推定するまでの期間に応じた係数を乗じた補正量で、それぞれの前記補正前過去推定速度を補正して、前記補正過去速度を決定する移動距離推定装置。
  4. 　請求項１～３のいずれか１項において、
    　前記積算処理部は、前記加速度積算値を逐次算出するとともに、移動体のヨーレートを検出するヨーレートセンサ（３０）が検出したヨーレートを積算することで、基準時点における進行方向に対する相対方位角を逐次算出し、
    　衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、
    　前記衛星からの電波のドップラーシフト量に基づいて、前記移動体の速度の前記衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、
    　前記衛星信号に基づいて、前記移動体から前記衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）とを備え、
    　前記初期設定値決定部は、
    　前記衛星測位システムが備える衛星からの電波のドップラーシフト量に基づいて算出する前記移動体の速度を、前記速度初期値として決定するようになっており、
    　前記衛星方向速度と、前記視線ベクトルと、前記移動体の速度ベクトルとの関係を示す式に対して、前記速度ベクトルの大きさを前記移動体の速度で拘束し、かつ、前記速度ベクトルの向きの時間変化を前記移動体の進行方向の方位角の時間変化で拘束して得られる式であって、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位の関係を示す速度推定式と、
    　前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した視線ベクトル、前記積算処理部が算出した相対方位角から、前記速度初期値を決定する移動距離推定装置。
  5. 　請求項１～３のいずれか１項において、
    　衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、
    　前記衛星からの電波のドップラーシフト量に基づいて、前記移動体の速度の前記衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、
    　前記衛星信号に基づいて、前記移動体から前記衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）とを備え、
    　前記初期設定値決定部は、
    　前記衛星測位システムが備える衛星からの電波のドップラーシフト量に基づいて算出する前記移動体の速度を、前記速度初期値として決定するようになっており、
    　前記衛星方向速度と、前記視線ベクトルと、前記移動体の速度ベクトルと、クロックドリフトとの関係を示す式に対して、前記速度ベクトルの大きさを前記移動体の速度で拘束し、前記クロックドリフトは時間変化が線形であるという拘束条件で拘束して得られる式であって、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位、クロックドリフトの関係を示す速度推定式と、
    　前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した視線ベクトルから、前記移動体の速度初期値を決定する移動距離推定装置。
  6. 　請求項４において、
    　前記速度推定式は、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位に加えて、クロックドリフトを含む式である移動距離推定装置。
  7. 　請求項６において、
    　前記速度推定式は、前記クロックドリフトの時間変化が線形であるという条件でも拘束されている移動距離推定装置。
  8. 　請求項４～７のいずれか１項において、
    　前記速度推定式は、前記移動体の速度の項として、前記速度初期値の項と、前記加速度積算値の項とを含んでおり、
    　前記初期設定値決定部は、前記速度推定式を用いて前記速度初期値を決定する場合、前記速度推定式、前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した前記視線ベクトルに加えて、前記積算処理部が算出した加速度積算値に基づいて、前記速度初期値を決定する移動距離推定装置。

     
PCT/JP2015/002632 2014-06-04 2015-05-25 移動距離推定装置 WO2015186309A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
US15/313,596 US10365109B2 (en) 2014-06-04 2015-05-25 Travel distance estimation device
SG11201609814VA SG11201609814VA (en) 2014-06-04 2015-05-25 Travel distance estimation device

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2014115954A JP6409346B2 (ja) 2014-06-04 2014-06-04 移動距離推定装置
JP2014-115954 2014-06-04

Publications (1)

Publication Number Publication Date
WO2015186309A1 true WO2015186309A1 (ja) 2015-12-10

Family

ID=54766394

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2015/002632 WO2015186309A1 (ja) 2014-06-04 2015-05-25 移動距離推定装置

Country Status (4)

Country Link
US (1) US10365109B2 (ja)
JP (1) JP6409346B2 (ja)
SG (1) SG11201609814VA (ja)
WO (1) WO2015186309A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170199276A1 (en) * 2016-01-13 2017-07-13 Heptagon Micro Optics Pte. Ltd. Power savings through refresh control for distance sensing devices
CN108779986A (zh) * 2016-03-30 2018-11-09 三菱电机株式会社 行进方向推测装置

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6266887B2 (ja) * 2013-03-04 2018-01-24 株式会社デンソー 推定装置
JP6409346B2 (ja) * 2014-06-04 2018-10-24 株式会社デンソー 移動距離推定装置
JP6733619B2 (ja) * 2017-07-07 2020-08-05 株式会社デンソー 測位装置
JP6743777B2 (ja) * 2017-07-07 2020-08-19 株式会社デンソー 測位装置
JP6870507B2 (ja) 2017-07-07 2021-05-12 株式会社デンソー 測位装置
KR102452551B1 (ko) * 2017-11-01 2022-10-07 현대자동차주식회사 횡가속도 제한 장치, 그를 포함한 시스템 및 그 방법
JP7242559B2 (ja) * 2018-01-05 2023-03-20 ソニーセミコンダクタソリューションズ株式会社 情報処理装置および情報処理方法
JP7190500B2 (ja) * 2018-09-21 2022-12-15 古野電気株式会社 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム
JP6685619B1 (ja) * 2018-12-10 2020-04-22 三菱電機株式会社 物体認識装置および物体認識方法
JP6929915B2 (ja) * 2019-10-11 2021-09-01 三菱重工業株式会社 航空機の位置制御システム、航空機および航空機の位置制御方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09196691A (ja) * 1996-01-19 1997-07-31 Matsushita Electric Ind Co Ltd ナビゲーション装置
JP2010249759A (ja) * 2009-04-20 2010-11-04 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2012524273A (ja) * 2009-04-20 2012-10-11 ザ・ボーイング・カンパニー 受信機の位置決定
JP2013113789A (ja) * 2011-11-30 2013-06-10 Toyota Central R&D Labs Inc 速度推定装置及びプログラム
JP2015129714A (ja) * 2014-01-08 2015-07-16 株式会社デンソー 速度推定装置

Family Cites Families (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5928309A (en) * 1996-02-05 1999-07-27 Korver; Kelvin Navigation/guidance system for a land-based vehicle
JPH11271088A (ja) * 1998-03-24 1999-10-05 Chubu Electric Power Co Inc 自動測量装置
US6421622B1 (en) * 1998-06-05 2002-07-16 Crossbow Technology, Inc. Dynamic attitude measurement sensor and method
US6449559B2 (en) * 1998-11-20 2002-09-10 American Gnc Corporation Fully-coupled positioning process and system thereof
US6826478B2 (en) * 2002-04-12 2004-11-30 Ensco, Inc. Inertial navigation system for mobile objects with constraints
US6947880B2 (en) * 2002-04-23 2005-09-20 Motorola, Inc. Method for improving accuracy of a velocity model
US7193559B2 (en) * 2003-01-21 2007-03-20 Novatel, Inc. Inertial GPS navigation system with modified kalman filter
US7239949B2 (en) * 2003-02-26 2007-07-03 Ford Global Technologies, Llc Integrated sensing system
US6879875B1 (en) * 2003-09-20 2005-04-12 American Gnc Corporation Low cost multisensor high precision positioning and data integrated method and system thereof
ES2238936B1 (es) * 2004-02-27 2006-11-16 INSTITUTO NACIONAL DE TECNICA AEROESPACIAL "ESTEBAN TERRADAS" Sistema y metodo de fusion de sensores para estimar posicion, velocidad y orientacion de un vehiculo, especialmente una aeronave.
US7671792B2 (en) * 2004-03-25 2010-03-02 Raytheon Company Global positioning receiver with system and method for parallel synchronization for reducing time to first navigation fix
US7409290B2 (en) * 2004-04-17 2008-08-05 American Gnc Corporation Positioning and navigation method and system thereof
DE102005026853A1 (de) 2005-06-10 2006-12-14 Daimlerchrysler Ag Verfahren und Vorrichtung zur fahrzeugseitigen Berechnung der Länge einer zurückgelegten Wegstrecke
US7702459B2 (en) * 2006-10-17 2010-04-20 Alpine Electronics, Inc. GPS accuracy adjustment to mitigate multipath problems for MEMS based integrated INS/GPS navigation systems
WO2010003437A1 (de) * 2008-07-09 2010-01-14 Pascal Munnix Vorrichtung und verfahren zur bestimmung des fahrzustandes eines fahrzeugs
JP5673071B2 (ja) * 2010-03-10 2015-02-18 株式会社豊田中央研究所 位置推定装置及びプログラム
JP5118177B2 (ja) 2010-08-18 2013-01-16 株式会社小野測器 移動体高精度速度計測装置及び方法
JP6094026B2 (ja) * 2011-03-02 2017-03-15 セイコーエプソン株式会社 姿勢判定方法、位置算出方法及び姿勢判定装置
US9846174B2 (en) * 2013-07-12 2017-12-19 Roy Schwartz Computer-implemented methods and computer systems/machines for identifying dependent and vehicle independent states
JP6409346B2 (ja) * 2014-06-04 2018-10-24 株式会社デンソー 移動距離推定装置
US9243914B2 (en) * 2014-06-17 2016-01-26 Raytheon Company Correction of navigation position estimate based on the geometry of passively measured and estimated bearings to near earth objects (NEOS)
US9886040B1 (en) * 2014-09-24 2018-02-06 Rockwell Collins, Inc. System and method for platform alignment, navigation or targeting
US9650039B2 (en) * 2015-03-20 2017-05-16 Ford Global Technologies, Llc Vehicle location accuracy

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09196691A (ja) * 1996-01-19 1997-07-31 Matsushita Electric Ind Co Ltd ナビゲーション装置
JP2010249759A (ja) * 2009-04-20 2010-11-04 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2012524273A (ja) * 2009-04-20 2012-10-11 ザ・ボーイング・カンパニー 受信機の位置決定
JP2013113789A (ja) * 2011-11-30 2013-06-10 Toyota Central R&D Labs Inc 速度推定装置及びプログラム
JP2015129714A (ja) * 2014-01-08 2015-07-16 株式会社デンソー 速度推定装置

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170199276A1 (en) * 2016-01-13 2017-07-13 Heptagon Micro Optics Pte. Ltd. Power savings through refresh control for distance sensing devices
US10605920B2 (en) * 2016-01-13 2020-03-31 Ams Sensors Singapore Pte. Ltd. Power savings through refresh control for distance sensing devices
CN108779986A (zh) * 2016-03-30 2018-11-09 三菱电机株式会社 行进方向推测装置
CN108779986B (zh) * 2016-03-30 2022-03-01 三菱电机株式会社 行进方向推测装置

Also Published As

Publication number Publication date
SG11201609814VA (en) 2016-12-29
JP2015230218A (ja) 2015-12-21
US20170184403A1 (en) 2017-06-29
US10365109B2 (en) 2019-07-30
JP6409346B2 (ja) 2018-10-24

Similar Documents

Publication Publication Date Title
JP6409346B2 (ja) 移動距離推定装置
JP6201762B2 (ja) 速度推定装置
JP5606656B2 (ja) 測位装置
US8510044B2 (en) Position sensing device and method
US11079494B2 (en) Positioning device
JP6413946B2 (ja) 測位装置
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
JP2011122921A (ja) 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム
JP2014077769A (ja) センサ傾斜判定装置及びプログラム
JP2016206149A (ja) 勾配推定装置及びプログラム
JP2004069536A (ja) データ検定装置および方法
JP5554560B2 (ja) 測位信頼度評価装置、測位信頼度評価方法、および、測位信頼度評価プログラム
JP2012098185A (ja) 方位角推定装置及びプログラム
WO2020110996A1 (ja) 測位装置、速度測定装置、及びプログラム
US10295366B2 (en) Sensor error correcting apparatus and method
JP2008232761A (ja) 移動体用測位装置
US11409006B2 (en) Azimuth estimation device
Komori et al. Initial Study on Spoofing Detection Using IMU and GNSS Compass

Legal Events

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

Ref document number: 15803613

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 15313596

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 15803613

Country of ref document: EP

Kind code of ref document: A1