JP2012098185A - Azimuth angle estimation device and program - Google Patents

Abstract

An object of the present invention is to appropriately determine a multipath and accurately estimate an azimuth angle of a moving object.
GPS information transmitted from each of a plurality of GPS satellites is acquired, and an azimuth angle estimation unit 30 estimates an azimuth angle (estimated azimuth angle) of the host vehicle based on the GPS information. The optimum estimation unit 62 calculates the azimuth angle (observation azimuth angle) of the host vehicle based on the detection value of the gai sensor 14, and estimates the optimum value by integrating the estimated azimuth angle and the observation azimuth angle. The accuracy determination unit 64 determines the accuracy of the optimum value based on whether or not the variance of the residual between the optimum value and the estimated azimuth is equal to or less than a threshold value. When it is determined that the accuracy of the optimum value is high, the velocity vector calculation unit 66 adopts the optimum value as an azimuth angle and calculates the velocity vector using the optimum value and the velocity detected by the velocity sensor 16. If it is determined that the accuracy is low, the velocity vector is calculated by using the azimuth angle calculated by integrating the detected values of the gyro sensor 14 using the azimuth angle estimated in the past.
[Selection] Figure 1

Description

  The present invention relates to an azimuth angle estimation apparatus and program, and more particularly to an azimuth angle estimation apparatus and program for estimating an azimuth angle of a moving object.

  2. Description of the Related Art Conventionally, a system that uses a satellite signal to calculate an optimum velocity vector of a moving object has been proposed even in an environment where the influence of multipath is large such as a high-rise building street (see, for example, Patent Document 1). The velocity vector determination system described in Patent Document 1 uses two satellites using satellite elevation angle information based on satellite orbit information and signal level information based on tracking information from a plurality of satellites tracked by a satellite signal tracking device. Are included in common and other satellites are included, and a plurality of combinations of three or more satellite signals are obtained. Then, speed vector solutions for each of the plurality of combinations are calculated to obtain speed vector solution candidates, and an optimum speed vector is obtained based on the degree of coincidence between these speed vector solution candidates.

  In addition, there has been proposed a moving body positioning apparatus that obtains a navigation position of a moving body by using a plurality of pseudo-ranges from a positioning satellite, a positioning navigation unit, an inertial navigation unit, and a Kalman filter (see, for example, Patent Document 2). The moving body positioning apparatus described in Patent Document 2 determines multipath by comparing the absolute value of the difference between the Doppler estimation value obtained from the output of the Kalman filter and the Doppler observation value under a predetermined condition.

  In addition, a GPS navigation device that selectively uses GPS data that is less affected by errors has been proposed (see, for example, Patent Document 3). In the GPS navigation device described in Patent Document 3, the GPS direction is obtained from the movement vector from the GPS speed measurement unit, and the moving body is obtained from the GPS direction, the rotational angular velocity of the moving body detected by the gyro sensor, and the road link information in the map memory. Each time new GPS data is detected, the GPS azimuth calculated based on the data is compared with the estimated azimuth obtained at that time, so that the error included in the data can be reduced. The magnitude of the influence is determined, and if the influence is large, control is performed so that the data is not used for position estimation.

JP 2006-208392 A Japanese Patent No. 3875714 JP-A-8-334338

  However, in the technique described in Patent Document 1 described above, multipaths may occur even from satellites with high elevation angles in urban areas and the like, but this point is not taken into consideration, and an optimal velocity vector is estimated. There is a problem that it may not be possible. Similarly, in urban areas, the majority of satellites received by multipath satellites are often more than half, and in such cases, the optimum velocity vector is estimated by changing the combination. There is a problem that can not be. Furthermore, there is a problem that when the satellite signal cannot be received, the velocity vector cannot be estimated.

  In order to cope with such a problem, it is conceivable to estimate a physical quantity related to a moving body by integrating a satellite signal and a detection value of an inertial sensor as in the technique described in Patent Document 2. However, in the technique described in Patent Document 2, an optimum estimated value is obtained by a Kalman filter and used for multipath determination, but the information of the satellite signal input to the Kalman filter includes many multipath effects. In some cases, the possibility that the optimal estimated value will deviate from the true value is increased, and there is a problem that the multipath error may not be detected even when compared with the observed value in the situation where the error has occurred. is there. In addition, there is a problem that when there is an error in the estimated value and the observed value is correct, the correct observed value is likely to be erroneously determined as a multipath error. In addition, the technique disclosed in Patent Document 2 calculates a physical quantity such as speed and position using a pseudo distance, and thus has a problem that the estimated physical quantity is greatly affected by the accuracy of the pseudo distance.

  Further, in the technique described in Patent Document 3, the traveling direction of the vehicle is estimated from the pseudo-range change rate of the GPS data, and the time series data is compared with the measured value of the gyro sensor, so that the error of the GPS data is reduced. Judging the few points. However, since the multipath error depends on the location, it is highly likely that the same tendency error has occurred in the two azimuth angles measured in a short time at the location where the multipath occurs. There is a problem that a path error may not be detected. Even in a place where no multipath occurs, Gaussian errors occur in the GPS data. For this reason, at least a random error occurs in the measured azimuth angle of the vehicle. Even if the multipath error is determined using the difference between the measured value and the estimated value, erroneous detection or non-detection may occur depending on the threshold setting. There is a problem that may occur frequently. Even if the threshold value is reduced in order to prevent erroneous determination, it is not possible to prevent undetected multipath errors for the reasons described above.

  The present invention has been made to solve the above-described problems, and provides an azimuth angle estimation apparatus and program that can appropriately determine multipath and estimate the azimuth angle of a moving object with high accuracy. With the goal.

  In order to achieve the above object, an azimuth angle estimation device of the present invention is an azimuth angle estimation device mounted on a moving body, and each position of the information transmission source transmitted from each of a plurality of information transmission sources. Acquisition means for acquiring transmission source information including information on information, information on the distance between each of the information transmission sources and the mobile body, and information on a relative speed of the mobile body with respect to each of the information transmission sources, and the mobile body Detecting means for detecting a physical quantity related to the azimuth angle of the mobile body, calculating means for calculating the azimuth angle of the moving body from the velocity vector of the moving body obtained based on the source information acquired by the acquiring means, and the calculation An optimum value estimation means for estimating an optimum value of the azimuth angle of the moving body based on the azimuth angle calculated by the means and the azimuth angle obtained from the physical quantity detected by the detection means; The determination means for determining the accuracy of the optimum value of the azimuth angle based on the distribution of the difference between the optimum value estimated by the optimum value estimation means and the azimuth angle calculated by the calculation means, and the accuracy by the determination means Azimuth angle estimation means for estimating the azimuth angle of the moving body using an optimum value when it is determined that is higher than a predetermined threshold value.

  According to the azimuth angle estimation apparatus of the present invention, the acquisition means is an azimuth angle estimation apparatus mounted on a moving body, and information on the position of each information transmission source transmitted from each of a plurality of information transmission sources, Source information including information on the distance between each information source and the moving body and information on the relative speed of the mobile body with respect to each information source is acquired, and the detection means detects a physical quantity related to the azimuth angle of the moving body. To do. The detecting means is an inertial sensor, and for example, a gyro sensor that detects the angular velocity or angular acceleration of the host vehicle or a magnetic azimuth meter that detects the azimuth angle can be used.

  Then, the calculating means calculates the azimuth angle of the moving object from the velocity vector of the moving object obtained based on the transmission source information acquired by the acquiring means, and the optimum value estimating means calculates the azimuth angle calculated by the calculating means, Based on the azimuth angle obtained from the physical quantity detected by the detection means, the optimum value of the azimuth angle of the moving body is estimated. The azimuth angle of the moving object calculated by the calculating means is an absolute amount, the azimuth angle of the moving object based on the physical quantity detected by the detecting means is a relative amount, and when these are integrated to estimate the optimum value, Variations in absolute quantities due to multipaths, etc. affect the accuracy of optimal values. Therefore, the determination unit determines the accuracy of the optimum value of the azimuth angle based on the distribution of the difference between the estimated optimum value and the azimuth angle calculated by the calculation unit. Then, the azimuth angle estimation means estimates the azimuth angle of the mobile object using the optimum value when the determination means determines that the accuracy is higher than a predetermined threshold.

  In this way, the azimuth angle of the moving object is integrated by integrating the azimuth angle of the moving object, which is an absolute amount calculated based on the transmission source information, and the azimuth angle of the moving object, which is a relative amount based on the detection value of the detection means. In order to determine the optimal value accuracy based on the distribution of the difference between the optimal value and the calculated azimuth angle, the multipath is properly determined to accurately estimate the azimuth angle of the moving object. can do.

  Further, the determination means determines the accuracy of the optimum value of the azimuth by determining whether the variation of the difference is equal to or less than a predetermined threshold value, or whether the distribution of the difference is compatible with a Gaussian distribution. Can be determined. If the variation in the difference between the optimum value and the calculated azimuth angle is small, or if the difference distribution matches the Gaussian distribution, it can be determined that the accuracy of the optimum value is high.

  The azimuth angle estimating means estimates the optimum value as the azimuth angle of the mobile object when the accuracy is determined to be higher than a predetermined threshold by the determining means, and the accuracy is determined by the determining means. When it is determined that the value is lower than the predetermined threshold value, the azimuth angle of the moving body can be estimated based on the azimuth angle estimated in the past and the physical quantity detected by the detection means. Thereby, the azimuth angle calculated as an absolute amount is interpolated with the azimuth angle detected as a relative amount, and the azimuth angle of the mobile object can be estimated with high accuracy.

  Further, the optimum value estimation means calculates an azimuth angle obtained from the physical quantity detected by the detection means by integrating the physical quantity detected by the detection means for a predetermined time with reference to a predetermined initial value, The optimum value of the azimuth angle can be estimated by searching for the initial value that minimizes the sum of the difference between the angle and the azimuth angle calculated by the calculation means in a certain time.

  In addition, the calculation means is based on the position of the mobile body obtained from information on the position of each of the information transmission sources and information on the distance between each of the information transmission sources and the mobile body. Calculate the direction of each of the information transmission sources as seen, calculate the speed of each of the information transmission sources based on the information about the position of each of the information transmission sources in time series, and view from the mobile body Based on information about each direction of the information transmission source, each speed of the information transmission source, and relative speed of the mobile body with respect to each of the information transmission sources, the speed of each direction of the information transmission source of the mobile body And the velocity vector of the moving body can be calculated based on the speed in each direction of the information transmission sources of the plurality of moving bodies.

  In addition, the information transmission source, satellite orbit information as information regarding the position of each of the information transmission sources, pseudorange information as information regarding the distance between each of the information transmission sources and the mobile body, and the information transmission source It is possible to use a GPS satellite that transmits Doppler frequency information as information relating to the relative speed of the moving body with respect to each of the above. The information transmission source may be any source that transmits transmission source information, such as a pseudo satellite or a beacon, but can be typically a GPS satellite.

  An azimuth angle estimation program according to the present invention is an azimuth angle estimation program for causing a computer to function as each unit of an azimuth angle estimation device mounted on a mobile body. Information on the position of each of the information transmission sources transmitted from each, information on the distance between each of the information transmission sources and the mobile body, and information on the relative speed of the mobile body with respect to each of the information transmission sources Obtaining means for obtaining transmission source information, calculating means for calculating an azimuth angle of the moving body from a velocity vector of the moving body obtained based on the transmission source information obtained by the obtaining means, and calculated by the calculating means. Based on the azimuth obtained from the physical quantity detected by the detection means for detecting the physical quantity relating to the azimuth angle and the azimuth angle of the moving object. An optimum value estimating means for estimating an optimum value of the azimuth angle of the mobile body, based on a distribution of differences between the optimum value estimated by the optimum value estimating means and the azimuth angle calculated by the calculating means; Determining means for determining the accuracy of the optimum value, and azimuth angle estimating means for estimating the azimuth angle of the moving body using the optimum value when the determining means determines that the accuracy is higher than a predetermined threshold value It is a program to make it.

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

  As described above, according to the azimuth angle estimation apparatus and program of the present invention, the azimuth angle of the moving body, which is an absolute amount calculated based on the transmission source information, and the relative amount based on the detection value of the detection means. In order to estimate the optimum value of the azimuth angle of the moving object by integrating the azimuth angle of the moving object, and to determine the accuracy of the optimum value based on the distribution of the difference between the optimum value and the calculated azimuth angle, It is possible to obtain an effect that it is possible to appropriately determine and accurately estimate the azimuth angle of the moving body.

It is a block diagram which shows the vehicle-mounted speed vector estimation apparatus which concerns on this Embodiment. It is an image figure which shows a mode that the position of the own vehicle is estimated according to the principle of triangulation. It is an image figure which shows a mode that the speed of the own vehicle of a GPS satellite direction is calculated based on the velocity vector of each GPS satellite, and the direction of each GPS satellite. It is a figure for demonstrating the estimation of the optimal value in case there is no influence of (a) multipath, and (b) there exists the influence of multipath. It is a figure which shows distribution of the residual of the optimal value and estimated azimuth in the case where there is no influence of (a) multipath, and (b) the influence of multipath. It is a flowchart which shows the content of the speed vector estimation process routine in the computer of the vehicle-mounted speed vector estimation apparatus which concerns on this Embodiment. It is a flowchart which shows the content of the own vehicle azimuth angle estimation process routine based on GPS information in the computer of the vehicle-mounted speed vector estimation apparatus which concerns on this Embodiment. It is a flowchart which shows the content of the speed vector calculation process routine in the computer of the vehicle-mounted speed vector estimation apparatus which concerns on this Embodiment.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the present embodiment, an in-vehicle speed vector that is mounted on a vehicle, acquires GPS information transmitted from a GPS satellite, estimates the azimuth angle of the host vehicle, and estimates the speed vector of the host vehicle from the estimated azimuth angle. A case where the present invention is applied to an estimation apparatus will be described as an example.

  As shown in FIG. 1, an in-vehicle speed vector estimation device 10 according to the present embodiment includes a GPS receiver 12 that receives radio waves from GPS satellites, a gyro sensor 14 that detects the yaw rate of the host vehicle, Based on the speed sensor 16 that detects the speed, the received signal from the GPS satellite received by the GPS receiver 12, and the detected values of the gyro sensor 14 and the speed sensor 16, the process of estimating the speed vector of the host vehicle is executed. And a computer 18 that performs processing.

  The GPS receiver 12 receives radio waves from a plurality of GPS satellites, and from the received signals from all the received GPS satellites, as GPS satellite information, the satellite number of the GPS satellite, orbit information of the GPS satellite (ephemeris) The time when the GPS satellite transmits the radio wave, the intensity of the received signal, the frequency, and the like are acquired and output to the computer 18.

  The computer 18 includes a CPU, a ROM that stores a program for realizing a speed vector estimation processing routine, which will be described later, a RAM that temporarily stores data, and a storage device such as an HDD.

  When the computer 18 is represented by functional blocks in accordance with a speed vector estimation processing routine described below, as shown in FIG. 1, GPS satellite information is acquired from the GPS receiver 12 for all GPS satellites that have received radio waves. A GPS information acquisition unit 20 that calculates and acquires GPS pseudorange data, Doppler frequency, and GPS satellite position coordinates; and an azimuth angle estimation unit 30 that estimates the azimuth angle of the host vehicle based on the acquired GPS information; Based on various data stored in the data storage unit 40, the data storage unit 40 that stores the estimated azimuth angle, the detection value of the gyro sensor 14 and the detection value of the speed sensor 16 in association with each other, It can be expressed by a configuration including a velocity vector estimation unit 60 that estimates a velocity vector.

  The GPS information acquisition unit 20 acquires GPS satellite information for all GPS satellites that have received radio waves from the GPS reception unit 12, and at the time when the GPS satellites transmit radio waves and the time at which the vehicle receives radio waves. Based on this, GPS pseudorange data is calculated. Further, the GPS information acquisition unit 20 sets the Doppler frequency of the received signal from each GPS satellite based on the known frequency of the signal transmitted from each GPS satellite and the frequency of the received signal received from each GPS satellite. calculate. The Doppler frequency is obtained by observing the Doppler shift amount of the carrier frequency due to the relative speed between the GPS satellite and the own vehicle. Further, the GPS information acquisition unit 20 calculates the position coordinates of the GPS satellites based on the orbit information of the GPS satellites and the time at which the GPS satellites transmitted radio waves.

  In addition, the azimuth angle estimation unit 30 further includes a relative speed calculation unit 42 that calculates a relative speed of the host vehicle with respect to each GPS satellite based on the acquired Doppler frequency of each GPS satellite, and a positional coordinate of each acquired GPS satellite. Based on the time series data, a satellite velocity calculation unit 44 that calculates a velocity vector of each GPS satellite, and a vehicle position calculation unit that calculates the position of the vehicle based on the acquired GPS pseudorange data of each GPS satellite 46, a satellite direction calculation unit 48 that calculates the direction (angle relationship) of each GPS satellite based on the calculated position of the own vehicle and the position coordinates of each GPS satellite, the calculated relative velocity, and each GPS satellite Satellite direction own vehicle speed calculation unit 50 for calculating the speed of the own vehicle in the direction of each GPS satellite based on the velocity vector of each and the direction of each GPS satellite, and a plurality of calculated GPS satellites The own vehicle speed calculation unit 52 that calculates the speed vector of the own vehicle based on the speed of the host vehicle in the direction, and the own vehicle azimuth calculation that calculates the azimuth angle of the own vehicle based on the calculated speed vector of the own vehicle It can represent with the structure provided with the part 54. FIG.

  The relative speed calculation unit 42 calculates the relative speed of the vehicle with respect to each GPS satellite from the Doppler frequency of the received signal from each GPS satellite according to the following equation (1) representing the relationship between the Doppler frequency and the relative speed with respect to the GPS satellite. calculate.

Here, v _j is a relative velocity with respect to the GPS satellite j, and D1 _j is a Doppler frequency (a Doppler shift amount) obtained from the GPS satellite j. C is the speed of light, and F ₁ is a known L1 frequency of a signal transmitted from a GPS satellite.

The satellite velocity calculation unit 44 calculates the velocity vector (three-dimensional velocity VX _j , VY _j , VZ _j ) of each GPS satellite from the acquired time-series data of the position coordinates of each GPS satellite using the differential of Kepler's equation. calculate. For example, using the method described in a non-patent document (Pratap Misra and Per Eng, the original Japanese Navigational Society GPS Study Group translation: “Sophisticated GPS Basic Concept, Positioning Principle, Signal and Receiver”, Shoyo Bunko, 2004.) The velocity vector of each GPS satellite can be calculated.

  The own vehicle position calculation unit 46 calculates the position of the own vehicle using the GPS pseudorange data of each GPS satellite acquired by the GPS information acquisition unit 20 as follows.

  In the positioning using GPS, as shown in FIG. 2, based on the position coordinates of known GPS satellites and the pseudo distance that is the propagation distance of the received signal received from each GPS satellite, according to the principle of triangulation, The position of the host vehicle is estimated.

Here, the true distance r _j to the GPS satellite is expressed by the following equation (2), and the pseudorange ρ _j observed by GPS is expressed by the following equation (3).

However, (X _j , Y _j , Z _j ) is the position coordinate of the GPS satellite j, and (x, y, z) is the position coordinate of the host vehicle. s is a distance error due to a clock error of the GPS receiver 12.

  From the above equations (2) and (3), by solving the simultaneous equations of the following equation (4) obtained from the GPS pseudorange data of four or more GPS satellites, the position of the host vehicle (x, y, z ) Is calculated.

  In the present embodiment, the position of the host vehicle is calculated in order to obtain the direction of the GPS satellite (the angle between the GPS satellite and the host vehicle), but is the direction of the GPS satellite existing in the distance. The positioning may be rough, and the position determination using the pseudo distance may not be performed. Although the influence of time is small and depends on the estimation accuracy allowed by the system etc., if the positioning error of the host vehicle is in the range of several hundred meters, the speed estimation error is about 1 m / sec or less and there is no major problem. For example, the position may be determined from a map or the like, or the position of the host vehicle may be determined from information such as a past measurement history or a beacon.

Based on the calculated position of the own vehicle and the position coordinates of each GPS satellite, the satellite direction calculation unit 48 determines the angular relationship between the position of each GPS satellite j and the position of the own vehicle (elevation angle θ _{j with} respect to the horizontal direction, north direction) the phi _j) azimuth angle to be calculated as the direction of each GPS satellite.

As shown in FIG. 3, the satellite direction own vehicle speed calculation unit 50 calculates the relative speed v _j of the own vehicle with respect to each GPS satellite, the velocity vectors (VX _j , VY _j , VZ _j ) of each GPS satellite, Based on the GPS satellite direction R _j (θ _j , φ _j ), the speed Vv _j of the host vehicle in the direction of each GPS satellite _j is calculated according to the following equation (5).

v _j is a relative speed of the host vehicle with respect to the GPS satellite j (relative speed with respect to the GPS satellite in the satellite direction). Vs _j is the speed of the GPS satellite j in the direction of the vehicle, and is obtained by Vs _j = R _j [VX _j , VY _j , VZ _j ] ^T. Vv _j is the vehicle speed in the direction of the GPS satellite j, and vCb is a clock bias fluctuation.

  As described above, the own vehicle speed in the direction of the GPS satellite is calculated not only by the three-dimensional position of the GPS satellite position but only by the orientation relationship with the GPS satellite. The GPS satellite is far away, and the angle change per minute is 0.5 degrees because it makes two rounds of the earth in one day. Usually, since the clock error between the GPS satellite and the GPS receiver is usually 1 msec or less, the azimuth relationship with the GPS satellite is not greatly affected. Similarly, since the GPS satellite is far away, even if an error of about several hundred meters occurs in determining the position of the own vehicle, the orientation relationship with the GPS satellite is not greatly affected. For this reason, even if it is a situation where an error is likely to ride on the pseudo distance, the vehicle speed in the direction of the GPS satellite can be accurately calculated as compared with the pseudo distance.

  The host vehicle speed calculation unit 52 performs optimum estimation of the speed vector of the host vehicle, as will be described below.

First, when the velocity vector of the vehicle and (Vx, Vy, Vz), the relationship between the speed Vv _j of the GPS satellites direction of the vehicle is expressed by the following equation (6).

  From the above equation (6) obtained for each GPS satellite j, simultaneous equations represented by the following equation (7) with Vx, Vy, Vz and Cb as estimated values are obtained.

  When the number of GPS satellites that have received radio waves is four or more, the optimal value of the velocity vector (Vx, Vy, Vz) of the host vehicle is calculated by solving the simultaneous equation (7).

The own vehicle azimuth calculating unit 54 calculates the azimuth angle of the own vehicle from the velocity vector of the own vehicle calculated by the own vehicle speed calculating unit 52 using a trigonometric function, and the azimuth angle is estimated by the azimuth estimating unit 30. As a result, the data is stored in the data storage unit 40. Since the azimuth angle estimated here is an azimuth angle estimated based on GPS information, it is hereinafter referred to as “estimated azimuth angle”. On the other hand, the azimuth angle of the host vehicle obtained from the detection value of the gyro sensor 14 is referred to as “observation azimuth angle”. However, since the gyro sensor 14 can only measure the change in the azimuth angle, the observation azimuth angle is calculated from the detection value of the gyro sensor 14 by integrating the elapsed time with a certain initial value as a reference. For example, in the following equation (8), the observation azimuth angle ψ _m (t ₀ −k) of k seconds before is calculated based on the initial value ψ _init (t ₀ ) at a certain time t ₀ . That is, by storing the differential value of the detected value [psi _Gyro gyro sensor 14 to the data storage unit 40, the observation azimuth allowing calculation.

  Note that the data storage unit 40 stores the detected value of the gyro sensor 14 and the detected value of the speed sensor 16 detected within a predetermined time, together with the estimated azimuth angle for a predetermined time.

  Further, the speed vector estimation unit 60 further integrates the estimated azimuth angle and the observed azimuth angle to estimate the optimum value of the azimuth angle, and the residual between the estimated optimum value and the estimated azimuth angle. An accuracy determination unit 64 that determines the accuracy of the estimated azimuth angle based on the variance of the vehicle, and the azimuth angle of the host vehicle is estimated based on the determination result of the accuracy determination unit 64, and the velocity vector is calculated using the estimated azimuth angle. It can be expressed by a configuration including a speed vector calculation unit 66 to calculate.

  The optimum estimation unit 62 reads the yaw rate of the host vehicle, which is the detection value of the gyro sensor 14 for a predetermined period, from the data storage unit 40, integrates the yaw rate for each observation, integrates them in time series, and observes the observation direction of the host vehicle. Calculate the corner. Further, the estimated azimuth angle estimated within the corresponding predetermined period is read from the data storage unit 40.

Here, the estimated azimuth angle estimated based on GPS information has a feature that the estimation result varies due to a GPS error factor caused by multipath or the atmosphere. On the other hand, the observation azimuth angle based on the detection value of the gyro sensor 14 has a feature that the continuous angle change can be accurately estimated, but the reliability of the absolute value is unknown. Therefore, the optimum estimation unit 62 estimates the optimum value of the azimuth angle obtained by integrating the estimated azimuth angle and the observation azimuth angle by the optimum calculation represented by the least square method or the like. For example, in the least square method, the following formula (9) is used to calculate the observation azimuth angle ψ _m so that the sum of the difference between the estimated azimuth angle ψ _gps and the observation azimuth angle ψ _m at a certain time is minimized. By searching for the optimal initial value ψ _init (t ₀ ), the optimal value of the azimuth angle is estimated.

  The accuracy determination unit 64 calculates the residual between the optimum value of the azimuth angle estimated by the optimal estimation unit 62 and each of the estimated azimuth angles, and the variation (variance or standard deviation) of the residual for a predetermined period is predetermined. The accuracy of the optimum value of the azimuth angle is determined depending on whether or not it is equal to or less than the threshold value.

  Here, the principle of determining the accuracy of the optimum value of the azimuth using the variation of the residual will be described. FIG. 4A shows the optimum value of the azimuth angle when there is no multipath effect and FIG. 4B shows the multipath effect. When there is a multipath effect, an outlier that increases the residual occurs, and the possibility that the optimum value deviates from the true value increases. Next, FIGS. 5 (a) and 5 (b) show residual histograms in the cases of FIGS. 4 (a) and 4 (b). When there is a multipath effect, the variance value tends to be larger than when there is no multipath effect. Therefore, the accuracy of the optimum value of the azimuth can be determined by comparing the variance value with the threshold value.

  When the accuracy determining unit 64 determines that the optimum value of the azimuth angle estimated by the optimum estimating unit 62 is high in accuracy, the velocity vector calculating unit 66 determines the optimum value of the azimuth angle and the speed sensor. A speed vector is calculated using the speed detected in 16. On the other hand, the optimum value determined to have low accuracy is not used for calculating the velocity vector. The velocity vector in the meantime is calculated using the velocity detected by the velocity sensor 16 by integrating the detected values of the gyro sensor 14 using the azimuth angle estimated in the past and calculating the azimuth angle. The speed vector calculation unit 66 outputs the calculated speed vector as a speed vector estimation result.

  Next, the operation of the in-vehicle speed vector estimation device 10 according to the present embodiment will be described.

  When the GPS receiver 12 receives radio waves from a plurality of GPS satellites, the computer 18 executes a speed vector estimation processing routine shown in FIG.

  In step 100, information on a plurality of GPS satellites is acquired from the GPS receiver 12, and GPS pseudorange data, Doppler frequency, and position coordinates of the GPS satellites are calculated and acquired.

  Next, in step 102, the yaw rate of the host vehicle, which is a detection value of the gyro sensor 14, and the speed of the host vehicle, which is a detection value of the speed sensor 16, are acquired and stored in the data storage unit 40.

  Next, in step 104, the host vehicle azimuth angle process based on GPS information described later is executed to estimate the estimated azimuth angle, and in step 106, a speed vector calculation process described later is executed to obtain a speed vector. Is estimated.

  Next, the own vehicle azimuth angle estimation processing routine based on GPS information will be described with reference to FIG.

In step 1040, the relative speed vj of the _host vehicle with respect to each GPS satellite is calculated from the Doppler frequency of the received signal from each GPS satellite according to the above equation (1).

Next, in step 1042, the velocity vector (VX _j , VY _j , VZ _j ) of each GPS satellite is calculated from the obtained time series data of the position coordinates of each GPS satellite using the differential of Kepler's equation.

  Next, in step 1044, the position of the host vehicle is calculated according to the above equations (2) to (4) using the GPS pseudorange data of each GPS satellite. Here, the position of the own vehicle is calculated in order to obtain the direction of the GPS satellite (the angle between the GPS satellite and the own vehicle), and it is sufficient that a rough position can be determined as the position of the own vehicle. The position may be determined from the above information, or the position of the host vehicle may be determined from information such as past position measurement history and beacons.

Next, in step 1046, based on the position of the own vehicle calculated in step 114 and the acquired position coordinates of each GPS satellite, the angular relationship R _j (position of each GPS satellite j and the position of the own vehicle is determined. The elevation angle θ _{j with} respect to the horizontal direction and the azimuth angle φ _{j with} respect to the north direction are calculated as the directions of the respective GPS satellites.

Next, at step 1048, the relative speed v _j of the own vehicle with respect to each GPS satellite calculated at step 1040, and the velocity vector V _j (VX _j , VY _j , VZ _{j of} each GPS satellite calculated at step 1042 above. ) And the direction R _j (θ _j , φ _j ) of each GPS satellite calculated in step 1046, the speed Vv _j of the host vehicle in the direction of each GPS satellite _j is calculated according to the above equation (5). To do. The velocity Vs _j of the GPS satellite j in the direction of the vehicle in the equation (5) is calculated by Vs _j = R _j [VX _j , VY _j , VZ _j ] ^T.

  Next, in step 1050, the optimum value of the speed vector (Vx, Vy, Vz) of the host vehicle is calculated according to the above equations (6) and (7).

  Next, in step 1052, the azimuth angle of the host vehicle is calculated from the speed vector of the host vehicle calculated in step 1050 using a trigonometric function, and the calculated azimuth angle is stored in the data storage unit 40 as an estimated azimuth angle. And return.

  Next, the speed vector estimation processing routine will be described with reference to FIG.

  In step 1060, the yaw rate of the host vehicle, which is the detection value of the gyro sensor 14 for a predetermined period, is read from the data storage unit 40, and the yaw rate for each observation is integrated and integrated in time series to obtain the observation azimuth angle of the host vehicle. calculate.

  Next, in step 1062, the estimated azimuth angle estimated within a predetermined period from the data storage unit 40 is read from the data storage unit 40, and the time series data of the observation azimuth angle calculated in step 1060 is translated and searched. However, the optimum value of the azimuth angle is estimated by the optimum calculation represented by the least square method.

  Next, in step 1064, a residual between the optimum value of the azimuth angle estimated in step 1062 and each of the estimated azimuth angles is calculated, and a variance of the residual for a predetermined period is calculated.

  Next, in step 1066, it is determined whether the residual variance calculated in step 1064 is equal to or less than a predetermined threshold value, thereby determining the accuracy of the optimum value of the azimuth. If the variance of the residual is less than or equal to the threshold value, it is determined that the accuracy of the optimum value of the azimuth is high, and the process proceeds to step 1068, where the optimum value of the azimuth estimated in step 1062 Adopt as. On the other hand, when the variance of the residual exceeds the threshold value, it is determined that the accuracy of the optimum value of the azimuth is low, the process proceeds to step 1070, and the detected value of the gyro sensor 14 using the azimuth angle estimated in the past. The azimuth calculated by integrating the values is adopted as the estimated value of the azimuth.

  Next, in step 1072, the speed detected by the speed sensor 16 is read from the data storage unit 40, and the speed vector is calculated using the estimated value of the azimuth angle adopted in step 1068 or 1070 and the read speed, The calculated speed vector is output as a speed vector estimation result.

  As described above, according to the in-vehicle speed vector estimation device of the present embodiment, the azimuth obtained by integrating the estimated azimuth angle estimated based on GPS information and the observed azimuth angle based on the detection value of the gyro sensor. In order to determine whether the accuracy of the optimum value, that is, whether the influence of multipath is large or not, depending on whether the variance of the residual between the optimum value of the angle and the estimated azimuth is below the threshold, The azimuth angle of the host vehicle can be estimated with high accuracy. In addition, since the estimated azimuth angle of a predetermined period with high accuracy is used to interpolate with the observation azimuth angle, even when satellite signals from GPS satellites cannot be received or the number of satellite signals is insufficient, The azimuth angle can be estimated.

  In the above embodiment, a case has been described in which the azimuth angle of the host vehicle is estimated based on GPS information and used for accuracy determination. However, speed may be used for accuracy determination. Specifically, the absolute value of the speed of the host vehicle is calculated from the speed vector calculated by the host vehicle speed calculation unit 52, and the speed detected by the speed sensor 16 and the absolute speed of the calculated speed are calculated by the optimum estimation unit 62. The values can be integrated to estimate the optimum speed value, and the accuracy can be determined based on the variance of the residual between the optimum value and the calculated absolute value of the speed. Then, an estimated azimuth angle may be calculated from a velocity vector for a predetermined period determined to have high accuracy, and an azimuth angle integrated with the observation azimuth angle may be estimated.

  Moreover, although the said embodiment demonstrated the case where accuracy was determined based on the dispersion | distribution of the residual of an optimal value and an estimated azimuth angle, it is not limited to this. For example, the accuracy may be determined based on whether or not the residual distribution is compatible with a Gaussian distribution. In this case, in step 1064 of the velocity vector estimation process (FIG. 8), the residual distribution is calculated. In step 1066, the compatibility between the residual distribution and the Gaussian distribution is determined using, for example, the Kolmogorov-Smirnov test. Just test. If the fitness is high, it can be determined that the accuracy of the optimum value is also high.

  In the above-described embodiment, the case where the gyro sensor is used to obtain the observation azimuth angle has been described. However, it may be an inertial navigation system sensor, and may be a magnetic azimuth meter.

  Moreover, although the case where GPS information of a GPS satellite is used has been described in the above embodiment, information on the position of the information transmission source, information on the distance between the information transmission source and the moving body, and the information transmission source and the moving body It is sufficient that information from an information transmission source that transmits transmission source information including information on relative speed can be acquired. For example, information transmitted from a pseudo satellite may be received.

  In the above embodiment, the case where the velocity vector is estimated and output from the estimated azimuth angle has been described. However, the estimated azimuth angle may be output as the estimation result without calculating the velocity vector. The travel locus of the host vehicle may be further estimated from the vector and output.

  Moreover, although the speed vector apparatus mounted in a vehicle was demonstrated in the said embodiment, the mobile body in which the azimuth angle estimation apparatus of this invention is mounted is not limited to a vehicle. For example, the azimuth estimation device may be mounted on a robot, or the azimuth estimation device may be configured as a portable terminal so that a pedestrian can carry it.

DESCRIPTION OF SYMBOLS 10 In-vehicle speed vector estimation apparatus 12 GPS receiving part 14 Gyro sensor 16 Speed sensor 18 Computer 20 GPS information acquisition part 30 Azimuth angle estimation part 40 Data storage part 42 Relative speed calculation part 44 Satellite speed calculation part 46 Own vehicle position calculation part 48 Satellite Direction calculation unit 50 Satellite direction own vehicle speed calculation unit 52 Own vehicle speed calculation unit 54 Own vehicle azimuth angle calculation unit 60 Speed vector estimation unit 62 Optimal estimation unit 64 Accuracy determination unit 66 Speed vector calculation unit

Claims (8)

An azimuth estimation device mounted on a moving body,
Information relating to the position of each of the information transmission sources transmitted from each of a plurality of information transmission sources, information relating to the distance between each of the information transmission sources and the mobile body, and the mobile body relative to each of the information transmission sources Obtaining means for obtaining source information including information on relative speed;
Detecting means for detecting a physical quantity related to the azimuth angle of the moving body;
Calculating means for calculating an azimuth angle of the moving body from a velocity vector of the moving body obtained based on the transmission source information acquired by the acquiring means;
An optimum value estimation means for estimating an optimum value of the azimuth angle of the mobile body based on the azimuth angle calculated by the calculation means and the azimuth angle obtained from the physical quantity detected by the detection means;
Determination means for determining the accuracy of the optimum value of the azimuth based on the distribution of the difference between the optimum value estimated by the optimum value estimation means and the azimuth calculated by the calculation means;
Azimuth angle estimating means for estimating the azimuth angle of the moving body using an optimum value when it is determined by the determining means that the accuracy is higher than a predetermined threshold;
An azimuth estimation device including   The determination means determines the accuracy of the optimum value of the azimuth angle by determining whether the variation in the difference is equal to or less than a predetermined threshold value or whether the difference distribution matches a Gaussian distribution. The azimuth angle estimation apparatus according to claim 1.   The azimuth angle estimation means estimates the optimum value as the azimuth angle of the mobile body when the determination means determines that the accuracy is higher than a predetermined threshold, and the accuracy is predetermined by the determination means. 3. The azimuth angle of the mobile body is estimated based on an azimuth angle estimated in the past and a physical quantity detected by the detection unit when it is determined that the azimuth angle is lower than the threshold value. Azimuth estimation device.   The optimum value estimation means calculates an azimuth angle obtained from the physical quantity detected by the detection means by integrating the physical quantity detected by the detection means for a predetermined time with reference to a predetermined initial value, 4. The optimum value of the azimuth angle is estimated by searching for the initial value that minimizes the sum of differences from the azimuth angle calculated by the calculation means over a predetermined time. 5. The azimuth angle estimation apparatus according to item.   The calculation means is viewed from the moving body based on the position of the moving body obtained from information on the position of each of the information transmitting sources and information on the distance between each of the information transmitting sources and the moving body. The direction of each of the information transmission sources is calculated, the speed of each of the information transmission sources is calculated based on the information on the position of each of the information transmission sources in time series, and the information transmission viewed from the mobile body Based on information on each direction of the source, each speed of the information transmission source, and a relative speed of the mobile body with respect to each of the information transmission sources, a speed in each direction of the information transmission source of the mobile body is calculated. The azimuth angle estimation apparatus according to any one of claims 1 to 4, wherein a velocity vector of the moving body is calculated based on a speed in each direction of the information transmission sources of the plurality of moving bodies.   The information transmission source, satellite orbit information as information regarding the position of each of the information transmission sources, pseudorange information as information regarding the distance between each of the information transmission sources and the mobile body, and each of the information transmission sources The azimuth estimation device according to any one of claims 1 to 5, wherein the GPS satellite is a GPS satellite that transmits Doppler frequency information as information relating to a relative speed of the moving body with respect to the vehicle. An azimuth angle estimation program for causing a computer to function as each means of an azimuth angle estimation device mounted on a moving body,
The computer,
Information relating to the position of each of the information transmission sources transmitted from each of a plurality of information transmission sources, information relating to the distance between each of the information transmission sources and the mobile body, and the mobile body relative to each of the information transmission sources Obtaining means for obtaining source information including information on relative speed;
Calculating means for calculating an azimuth angle of the moving body from a velocity vector of the moving body obtained based on the transmission source information acquired by the acquiring means;
Based on the azimuth angle calculated from the calculation means and the azimuth angle obtained from the physical quantity detected by the detection means for detecting the physical quantity related to the azimuth angle of the mobile object, the optimum value of the azimuth angle of the mobile object is estimated. Optimal value estimation means,
Determining means for determining the accuracy of the optimum value of the azimuth angle based on the distribution of the difference between the optimum value estimated by the optimum value estimating means and the azimuth angle calculated by the calculating means; An azimuth angle estimation program for functioning as an azimuth angle estimation means for estimating an azimuth angle of the moving body using an optimum value when it is determined that is higher than a predetermined threshold.   The azimuth angle estimation program for functioning a computer as each means of the azimuth angle estimation apparatus of any one of Claims 1-6.

Images