JP2012203721A - Relative position estimation device and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To accurately estimate a relative position to other devices by a little data communication amount.SOLUTION: A position/velocity vector calculation section 24 calculates position and velocity vector of one's own vehicle. A radio communication section 18 receives position and velocity vector of a partner vehicle. A position difference relative position calculation section 26 calculates a relative position of the partner vehicle for each time from the received position of the partner vehicle. A locus calculation section 32 integrates the velocity vector of one's own vehicle to calculate a locus. A partner locus calculation section 34 integrates the received velocity vector to calculate a locus of the partner vehicle. A locus use relative position estimation section 36 estimates an optimum value of the locus of the partner vehicle and estimates a relative position at a specific point on the locus on the basis of the locus of one's own vehicle, the locus of the partner vehicle, and the relative position calculated for each time.

Description

  The present invention relates to a relative position estimation apparatus and program, and more particularly to a relative position estimation apparatus and program for estimating a relative position between another apparatus and a moving body.

  Conventionally, there is known a relative position detection device that obtains a relative position between three or more moving bodies by GPS interference positioning and efficiently performs reliability determination regarding interference positioning (Patent Document 1). In this relative position detection device, at a predetermined location where three or more mobile bodies can communicate with each other, the reference mobile body transmits data including observation data to the non-reference mobile body, and the non-reference mobile body transmits the observation data and the reference data. Interferometric positioning is performed using data including observation data received from the moving body, and a relative position with respect to the reference moving body is calculated. Then, data including an integer value bias is received from another non-reference moving body, and reliability determination regarding interference positioning is performed.

  Also, a relative position detection system that detects a relative position between an arbitrary moving body and another moving body using a positioning satellite receiving unit and a wireless communication unit is known (Patent Document 2). In this relative position detection system, the usage of the satellite is calculated from the state of the satellite used in the past so that the combination of the satellites is more consistent, and it is commonly used so that the satellites can be easily matched based on the data. The satellite to be selected is selected.

  In addition, a cooperative position estimation method for a vehicle safe driving support application using a GPS receiver, a range sensor, and inter-vehicle communication held by a plurality of vehicles has been proposed (Non-Patent Document 1). In this coordinated position estimation method, GPS and range sensors that are held by different vehicles are observed at different times in order to estimate the positions of the host vehicle and the surrounding vehicles with high accuracy in an environment where vehicles not equipped with onboard equipment are mixed. The result is used for estimation. Further, by obtaining the accuracy of each estimation result based on the central limit theorem, the most accurate estimation result is shared among the vehicles. The idea of this method is basically to apply a probability theory using the measurement values of various sensors to improve the position accuracy of the vehicle (for example, a method using a Kalman filter etc.) Applied to position.

JP 2009-264844 A Japanese Patent Application No. 11-2675

Ayae Fujii, Hirozumi Yamaguchi, Teruo Higashino, Shigeru Kaneda, Mineo Takai, “Cooperative Vehicle Location Estimation Method Using Inter-Vehicle Communication”, Information Processing Society of Japan, 43rd Advanced Traffic Systems Study Group (ITS), November 2010 Moon

  However, the technique described in Patent Document 1 has a problem that it is necessary to communicate a received carrier wave in order to perform GPS interference positioning. Further, in Patent Document 1 described above, a method for reducing the amount of communication data when calculating the relative positions of three or more vehicles is proposed, but there is a problem that it cannot be applied to the case of two vehicles. In addition, a relative position reliability calculation method using calculation of an integer value bias of a carrier wave has been proposed, but there is a problem that it is difficult to estimate an integer value bias in the first place when receiving multipath. .

  Further, in the technique described in Patent Document 2, the accuracy of the relative position can be improved by sharing the satellite, but positioning cannot be performed when the number of the common satellites is 4 or less. There is a problem.

  Further, the technique described in Non-Patent Document 1 has a problem that a correct value cannot be estimated when multipath occurs.

  The present invention has been made to solve the above problems, and provides a relative position estimation device and a program capable of accurately estimating the relative position with other devices with a small amount of data communication. Objective.

  In order to achieve the above object, a relative position estimation device according to the present invention is a relative position estimation device mounted on a moving body for estimating a relative position between the moving body and another device, and a plurality of information transmissions Information on the position of each of the information transmission sources transmitted from each of the sources, 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 Based on the acquisition means for acquiring the transmission source information, the detection means for detecting the movement state of the moving body, the transmission source information acquired by the acquisition means, and the movement state detected by the detection means Calculating means for calculating the position of the moving object and the speed vector of the moving object, or speed information indicating the speed of the moving object and the azimuth angle of the moving object, and the position and speed of the other device. From the difference between the receiving means for receiving the information, the position of the other device received by the receiving means, and the position of the moving body calculated by the calculating means, the moving body for each time or each point Relative position calculating means for calculating a relative position between the moving body and the other device, and the speed vector or the speed information of the moving body calculated by the calculating means is accumulated for a predetermined time, and the moving body at the predetermined time A first trajectory calculating means for calculating a trajectory of the position of the second device, and a second trajectory for calculating the position trajectory of the other device at the predetermined time by integrating the speed information received by the receiving means for the predetermined time. A trajectory calculating means; a trajectory of the position of the moving body at the predetermined time calculated by the first trajectory calculating means; and the predetermined time calculated by the second trajectory calculating means. The relative position at a specific point on the trajectory is calculated based on the trajectory of the position of the other device and the relative position calculated for each time or each point within the predetermined time by the relative position calculating means. Relative position estimating means for estimating.

  The relative position estimation program according to the present invention is a relative position estimation program for causing a computer to function as each unit of a relative position estimation device mounted on the moving body, which estimates a relative position between the moving body and another device. The information is transmitted from each of a plurality of information transmission sources, the information about the position of each of the information transmission sources, the information about the distance between each of the information transmission sources and the mobile body, and the information transmission Detected by acquisition means for acquiring transmission source information including information on the relative speed of the moving body with respect to each of the sources, the transmission source information acquired by the acquisition means, and detection means for detecting the motion state of the moving body. A speed indicating the position of the moving object and the velocity vector of the moving object, or the speed of the moving object and the azimuth angle of the moving object, based on the state of motion From the difference between the position of the other device received by the calculating means for calculating the information, the position of the other device and the receiving means for receiving the speed information, and the position of the moving body calculated by the calculating means The relative position calculation means for calculating the relative position between the moving body and the other device at each time or each point, the speed vector of the moving body calculated by the calculation means or the speed information is accumulated for a predetermined time. A first trajectory calculating means for calculating a trajectory of the position of the moving body at the predetermined time, and the speed information received by the receiving means is accumulated for the predetermined time, and the other device at the predetermined time Second trajectory calculating means for calculating a trajectory of the position of the moving object at the predetermined time calculated by the first trajectory calculating means, and calculating the second trajectory. On the trajectory based on the trajectory of the position of the other device at the predetermined time calculated by the stage and the relative position calculated for each time or each point within the predetermined time by the relative position calculating means. It is a relative position estimation program for functioning as a relative position estimation means for estimating the relative position at a specific point.

  According to the present invention, the information relating to the position of each of the information transmission sources transmitted from each of a plurality of information transmission sources by the acquisition means, the information regarding the distance between each of the information transmission sources and the mobile body, and Source information including information on the relative speed of the moving body with respect to each information source is acquired. The motion state of the moving body is detected by the detecting means. 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, a magnetic azimuth meter that detects an azimuth angle, or a speed sensor that detects the speed of the host vehicle can be used.

  Then, based on the transmission source information acquired by the acquisition unit and the motion state detected by the detection unit by the calculation unit, the position of the mobile unit, the velocity vector of the mobile unit, or the mobile unit Speed and speed information indicating the azimuth angle of the moving body are calculated. The receiving means receives the position of the other device and the speed information. From the difference between the position of the other device received by the receiving unit and the position of the moving unit calculated by the calculating unit by the relative position calculating unit, for each time or each point, the mobile unit and the point The relative position with other devices is calculated.

  Then, the first trajectory calculating means integrates the speed vector or the speed information of the moving body calculated by the calculating means for a predetermined time to calculate the trajectory of the position of the moving body at the predetermined time. The second trajectory calculating means integrates the speed information received by the receiving means for the predetermined time, and calculates the trajectory of the position of the other device at the predetermined time.

  Then, the relative position estimating means calculates the position of the moving body at the predetermined time calculated by the first locus calculating means and the other apparatus at the predetermined time calculated by the second locus calculating means. The relative position at a specific point on the trajectory is estimated based on the trajectory of the position and the relative position calculated for each time or each point within the predetermined time by the relative position calculating means.

  As described above, the position and speed information of the other device is received, the locus of the position of the moving body, the locus of the position of the other device, and the relative position between the moving body and the other device are calculated and moved. By estimating the relative position at a specific point on the trajectory based on the trajectory of the position of the body, the trajectory of the position of another device, and the relative position of each point or each point, the amount of data communication can be reduced. It is possible to accurately estimate the relative position to the device.

  The relative position according to the present invention is a relative position of the other device based on the position of the moving body, and the relative position estimating means is the other position at the predetermined time calculated by the second locus calculating means. The position trajectory of the apparatus, the trajectory of the position of the moving object at the predetermined time calculated by the first trajectory calculating means, and the relative position calculating means are calculated for each time or each point within the predetermined time. Further, a position trajectory of the other apparatus that minimizes a sum of differences from the position of the other apparatus at each time or each point obtained from the relative position is obtained, and the obtained position of the other apparatus is obtained. The relative position at a specific point on the trajectory can be estimated based on the trajectory and the trajectory of the position of the moving body. Thereby, it is possible to accurately estimate the relative position with respect to another device.

  The relative position according to the present invention is a relative position of the moving body with respect to a position of the other device, and the relative position estimating means is the movement at the predetermined time calculated by the first trajectory calculating means. The locus of the body position, the locus of the position of the other device at the predetermined time calculated by the second locus calculating means, and the time or each point within the predetermined time are calculated by the relative position calculating means. A trajectory of the position of the moving body that minimizes the sum of differences from the position of the moving body at each time or each point obtained from the relative position; and The relative position at a specific point on the locus can be estimated based on the locus of the position of the other device. Thereby, it is possible to accurately estimate the relative position with respect to another device.

  Further, the relative position estimation device calculates a relative position error calculation that calculates an error of the relative position estimated by the relative position estimation unit based on each of the differences when the sum of the differences is minimized. Means can be further included. Thereby, the error of the estimated relative position can be calculated with high accuracy.

  In addition, the other device includes a second acquisition unit that acquires the source information transmitted from each of a plurality of information transmission sources, a second detection unit that detects a motion state of the other device, Second calculation means for calculating the position of the other device and the speed information based on the source information acquired by the second acquisition means and the motion state detected by the second detection means; Transmission means for transmitting the position information of the other device calculated by the second calculation means, the speed information, and identification information of the information transmission source that has transmitted the transmission source information acquired by the second acquisition means. The receiving unit receives the position of the other device, the speed information, and the identification information of the information source transmitted by the transmitting unit, and the relative position estimating unit receives the information received by the receiving unit. The difference obtained for the relative position based on the position of the moving body calculated based on the source information from a plurality of information source including all of the information source indicated by the identification information is obtained for other relative positions. A trajectory that minimizes the sum of the differences can be obtained so as to be smaller than the obtained difference, and the relative position at a specific point on the trajectory can be estimated. This makes it possible to estimate the relative position with other devices with higher accuracy.

  In addition, the calculation unit is configured to calculate the moving body based on the position of the moving body obtained from the information regarding the position of each of the information transmitting sources and the information regarding the distance between each of the information transmitting sources and the moving body. Calculate the direction of each of the information transmission sources viewed from the time, calculate the speed of each of the information transmission sources based on the information on the position of each of the information transmission sources in time series, and viewed from the mobile body Based on information about each direction of the information transmission 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, each direction of the information transmission source of the mobile body The speed can be calculated, and the speed 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.

  Further, satellite orbit information is used as information relating to the position of each of the information transmission sources, pseudorange information is used as information relating to the distance between each of the information transmission sources and the mobile body, and the information transmission is provided. It can be a GPS satellite that transmits Doppler frequency information as information on the relative speed of the moving body to each of the sources. 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.

  The storage medium for storing the relative position estimation 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 relative position estimation apparatus and program of the present invention, the position and speed information of another apparatus is received, the locus of the position of the moving body, the locus of the position of the other apparatus, and the movement Calculate the relative position of the body and the other device, based on the trajectory of the position of the moving body, the trajectory of the position of the other device, and the relative position of each time or each point, at a specific point on the trajectory By estimating the relative position, it is possible to accurately estimate the relative position with another apparatus with a small amount of data communication.

It is a block diagram which shows the relative position estimation apparatus and onboard equipment which concern on the 1st Embodiment of this invention. It is a block diagram which shows the position and velocity vector calculation part of the relative position estimation apparatus which concerns on the 1st Embodiment of this invention. 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 GPS error in the own vehicle and an other party vehicle. It is an image figure which shows a mode that the locus | trajectory of a partner vehicle is fitted. It is an image figure which shows a mode that the locus | trajectory of an other party vehicle is fitted again after rejecting the relative position with a large residual. It is an image figure which shows a mode that a locus | trajectory has an error and the dispersion | variation in a residual becomes large. It is an image figure which shows a mode that the multipass generate | occur | produced in the whole region and the dispersion | variation in a residual becomes large. It is a flowchart which shows the content of the position speed calculation process routine in the computer of the relative position estimation apparatus which concerns on 1st 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 relative position estimation apparatus which concerns on 1st Embodiment. It is a flowchart which shows the content of the speed vector calculation process routine in the computer of the relative position estimation apparatus which concerns on 1st Embodiment. It is a flowchart which shows the content of the relative position estimation process routine in the computer of the relative position estimation apparatus which concerns on 1st Embodiment. It is a block diagram which shows the position and speed vector calculation part of the relative position estimation apparatus which concerns on the 2nd Embodiment of this invention. (A) The figure for demonstrating the case where the GPS satellite to be used cannot be shared, and (A) The figure for demonstrating the case where the GPS satellite to be used can be shared. It is a flowchart which shows the content of the position speed calculation process routine in the computer of the relative position estimation apparatus which concerns on 2nd Embodiment. It is a block diagram which shows the roadside machine which concerns on the 3rd Embodiment of this invention. It is an image figure which shows a mode that the locus | trajectory of a roadside machine is fitted.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the first embodiment, GPS information mounted on a vehicle and transmitted from a GPS satellite is acquired, and inter-vehicle communication is performed with another vehicle to estimate a relative position of the other vehicle with respect to the own vehicle. A case where the present invention is applied to a relative position estimation apparatus will be described as an example.

  As shown in FIG. 1, a relative position estimation apparatus 10 according to the first 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, and the host vehicle. A speed sensor 16 for detecting the speed of the vehicle, a wireless communication unit 18 for performing inter-vehicle communication with other vehicles, a received signal from a GPS satellite received by the GPS receiving unit 12, and a data received by the wireless communication unit 18 And a computer 20 that executes a process of estimating the relative position of the other vehicle with respect to the host vehicle based on the detection values of the gyro sensor 14 and the speed sensor 16. The gyro sensor 14 and the speed sensor 16 are examples of detection means, and the wireless communication unit 18 is an example of reception means.

  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, etc. are acquired and output to the computer 20.

  The wireless communication part 18 receives the data transmitted from the onboard equipment 80 mounted in the other vehicle. Hereinafter, the other vehicle is referred to as an opponent vehicle.

  Here, as in the relative position estimation device 10, the vehicle-mounted device 80 detects a GPS receiver 82 that receives radio waves from GPS satellites, a gyro sensor 84 that detects the yaw rate of the opponent vehicle, and the speed of the opponent vehicle. A speed sensor 86, a wireless communication unit 88 for performing vehicle-to-vehicle communication, and a computer 90 for executing processing for calculating the position and speed vector of the opponent vehicle are provided. Based on the received signal from the GPS satellite received by the GPS receiving unit 82, the data received by the wireless communication unit 88, and the detected values of the gyro sensor 84 and the speed sensor 86, the computer 90 detects the position and speed vector of the opponent vehicle. A position / velocity vector calculation unit 92 is provided. The position / velocity vector calculation unit 92 has the same configuration as that of the position / velocity vector calculation unit 24 to be described later, and a description thereof will be omitted. The gyro sensor 84 and the speed sensor 86 are an example of a second detection unit, the wireless communication unit 88 is an example of a transmission unit, and the position / speed vector calculation unit 92 is a second acquisition unit and a second calculation unit. It is an example of a means.

  The in-vehicle device 80 calculates the position and velocity vector of the opponent vehicle at predetermined time intervals, and for each time, measurement data including the position and velocity vector and the satellite number of the GPS satellite used in the process of calculating the position. Is transmitted by the wireless communication unit 88. The measurement data further includes a time indicating when the calculated position is, that is, a time at which the in-vehicle device 80 receives a radio wave from a GPS satellite. Table 1 shows data transmitted by the wireless communication unit 88.

  The computer 20 includes a storage device such as a CPU, a ROM that stores a program for realizing a later-described position / velocity calculation processing routine and a relative position estimation processing routine, a RAM that temporarily stores data, and an HDD. Yes.

  When the computer 20 is represented by functional blocks according to a position / velocity calculation processing routine and a relative position estimation processing routine described below, as shown in FIG. 1, received data that stores measurement data from the opponent vehicle received by the wireless communication unit 18. Based on the received signals from the GPS satellites received by the storage unit 22 and the GPS receiver 12 and the detection values of the gyro sensor 14 and the speed sensor 16, the position and speed vector of the host vehicle at each time are calculated. For each time, the speed vector calculation unit 24 calculates the difference between the calculated position of the host vehicle and the position of the partner vehicle included in the received measurement data, and calculates the relative position of the partner vehicle based on the host vehicle. Position difference relative position calculation unit 26, the satellite number included in the GPS satellite information acquired at each time, and the received total Based on the satellite number of the GPS satellite included in the data, a weight calculation unit 28 for calculating a weight for each time, and data for storing the position, speed vector, relative position, and weight of the host vehicle calculated for each time And a storage unit 30. The position / velocity vector calculation unit 24 is an example of a calculation unit, and the position difference relative position calculation unit 26 is an example of a relative position calculation unit.

  The reception data storage unit 22 stores and accumulates measurement data every time measurement data from the partner vehicle is received by the wireless communication unit 18.

  As shown in FIG. 2, the position / velocity vector calculation unit 24 acquires GPS satellite information for all GPS satellites that have received radio waves from the GPS reception unit 12, and includes GPS pseudorange data, Doppler frequency, and A GPS information acquisition unit 50 that calculates and acquires the position coordinates of the GPS satellite, a position calculation unit 51 that calculates the position of the host vehicle based on the acquired GPS information, and the vehicle's position based on the acquired GPS information. An azimuth angle estimation unit 52 that estimates the azimuth angle, a data storage unit 54 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, and stores the data in the data storage unit 54 The speed vector estimation unit 56 that estimates the speed vector of the host vehicle based on the various data thus obtained can be represented by a configuration. The GPS information acquisition unit 50 is an example of an acquisition unit.

  The GPS information acquisition unit 50 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 50 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 50 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. Also, the GPS information acquisition unit 50 outputs the satellite number of each GPS satellite included in the acquired GPS satellite information to the weight calculation unit 28.

  The position calculation unit 51 calculates the position of the host vehicle using the GPS pseudorange data of each GPS satellite acquired by the GPS information acquisition unit 50 as follows.

  In the positioning using GPS, as shown in FIG. 3, based on the position coordinates of the 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 (1), and the pseudorange ρ _j observed by GPS is expressed by the following equation (2).

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 (1) and (2), by solving the simultaneous equations of the following equation (3) obtained from the GPS pseudorange data of four or more GPS satellites, the position of the host vehicle (x, y, z ) Is calculated.

  Further, the azimuth angle estimation unit 52 further includes a relative speed calculation unit 60 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, the satellite velocity calculation unit 62 that calculates the velocity vector of each GPS satellite, and the direction (angle of each GPS satellite) based on the calculated position of the host vehicle and the position coordinates of each GPS satellite. A satellite direction calculating unit 66 for calculating the relationship), and calculating the speed of the vehicle in each GPS satellite direction based on the calculated relative velocity, the velocity vector of each GPS satellite, and the direction of each GPS satellite. A vehicle speed calculation unit 68, a host vehicle speed calculation unit 70 that calculates a speed vector of the host vehicle based on the calculated speeds of the host vehicle in a plurality of GPS satellite directions, and a calculated host vehicle A vehicle azimuth calculation unit 72 that calculates the azimuth of the vehicle based on the velocity vector can be represented by configurations with.

  The relative speed calculation unit 60 calculates the relative speed of the host vehicle with respect to each GPS satellite from the Doppler frequency of the received signal from each GPS satellite according to the following equation (4) 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 62 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 differentiation 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 satellite direction calculation unit 66, based on the calculated position of the own vehicle and the position coordinates of each GPS satellite, 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. 4, the satellite direction own vehicle speed calculation unit 68 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 70 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 vCb 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 host vehicle azimuth calculating unit 72 calculates the azimuth angle of the host vehicle from the velocity vector of the host vehicle calculated by the host vehicle speed calculating unit 70 using a trigonometric function, and the azimuth angle estimation unit 52 estimates the azimuth angle. As a result, the data is stored in the data storage unit 54. 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 54, the observation azimuth allowing calculation.

  The data storage unit 54 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 velocity vector estimation unit 56 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 76 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 76, and the velocity vector is calculated using the estimated azimuth angle It can be expressed by a configuration including a speed vector calculation unit 78 to calculate.

  The optimum estimation unit 74 reads the yaw rate of the host vehicle, which is the detection value of the gyro sensor 14 for a predetermined time, from the data storage unit 54, integrates the yaw rate for each observation and 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 54.

Further, the optimum estimation unit 74 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 76 calculates a residual between the optimum value of the azimuth angle estimated by the optimal estimation unit 74 and each of the estimated azimuth angles, and a variation (variance or standard deviation) of the residual for a predetermined time 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.

  When the accuracy determining unit 76 determines that the optimum value of the azimuth angle estimated by the optimum estimating unit 74 is high in accuracy, the velocity vector calculating unit 78 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 78 outputs the calculated speed vector as a speed vector estimation result.

  Further, the position difference relative position calculation unit 26 uses the difference between the calculated position of the own vehicle and the position of the partner vehicle included in the received measurement data for each time as a reference for calculating the relative position. The relative position is calculated. As described above, the position difference relative position calculation unit 26 calculates the relative position by simply calculating the difference between the positions of the two vehicles. At this time, in the case of calculating the relative position of two vehicles at a distance of about 100 m or less, as shown in FIG. 5, errors caused by the GPS stratosphere and troposphere are shared. Relative position is more accurate than absolute position. Therefore, by simply calculating the difference in position, it is possible to calculate an accurate relative position although there is a variation in a location where multipath does not occur.

  Further, the weight calculation unit 28 is common to the satellite numbers of the GPS satellites included in the GPS satellite information acquired by the host vehicle among the satellite numbers of the GPS satellites included in the received measurement data for each time. The larger the ratio of the satellite numbers is, the larger the weight used in the weighted least square method described later is calculated. That is, the maximum weight is calculated when the satellite numbers included in the GPS satellite information acquired by the host vehicle include all the satellite numbers of the GPS satellites included in the received measurement data.

  In addition, the computer 20 further accumulates the speed vectors of the host vehicle for a predetermined time, calculates a trajectory of the position of the host vehicle at the predetermined time, and a predetermined time obtained from the received measurement data. Based on the relative position calculated from the difference between the trajectory and the position of the own vehicle and the opponent vehicle, and the other vehicle's speed vector. Thus, a trajectory-based relative position estimation unit 36 that estimates the relative position of the opponent vehicle with respect to the host vehicle and a relative position error calculation unit 38 that calculates an error in the estimated relative position are provided. The trajectory calculation unit 32 is an example of a first trajectory calculation unit, and the opponent trajectory calculation unit 34 is an example of a second trajectory calculation unit.

  The trajectory calculation unit 32 acquires the calculated speed vector of the host vehicle for the predetermined time from the data storage unit 30, integrates the acquired speed vector of the host vehicle for the predetermined time, and The locus of the position is calculated. Further, the opponent trajectory calculation unit 34 acquires the speed vector of the partner vehicle for the same predetermined time from the measurement data stored in the received data storage unit 22, integrates the acquired speed vector of the partner vehicle, The trajectory of the position of the opponent vehicle in time is calculated.

  The trajectory use relative position estimation unit 36 estimates the relative position of the opponent vehicle with respect to the host vehicle, as described below.

  First, the trajectory of the host vehicle and the opponent vehicle and the relative position based on the difference in position at each time on the trajectory are known. Here, although the trajectory is correct, the relative position due to the position difference may include a variation due to noise and a large error due to multipath.

  Accordingly, the trajectory use relative position estimation unit 36 estimates the optimal value of the trajectory by fitting the trajectory to the relative position based on the difference between the trajectory and the position by an optimal calculation represented by the least square method or the like. . As shown in FIG. 6, first, the predicted position of the opponent vehicle at each time is calculated from the relative position based on the position difference at each time with reference to the trajectory of the own vehicle. Then, by fitting the trajectory of the opponent vehicle to the predicted position of the opponent vehicle by the least square method, the optimum value of the trajectory of the opponent vehicle is obtained, and the trajectory is obtained using the obtained optimum value of the trajectory of the opponent vehicle. The relative position of the opponent vehicle with respect to the host vehicle at the upper specific point (for example, the end point) is calculated.

  Here, in particular, when positioning is performed using a satellite common to two vehicles, the error of GPS is more common, and thus the error is reduced. Using this feature, when calculating the least squares, the weight calculation unit 28 acquires the weight calculated according to the ratio of the shared satellites from the data storage unit 30 and applies the acquired weight. Then, according to the following equation (10), the locus of the opponent vehicle is fitted by the weighted least square method.

  However, Δy is a difference (residual) between the observed amount (position on the trajectory of the opponent vehicle) and a predicted value (predicted position of the opponent vehicle), Δx is an unknown, and G is a general matrix. is there. W is a weight matrix indicating the weight of each time.

  In the above equation (10), the unknown Δx is calculated so that the difference (residual) Δy between the observed amount and the predicted value is minimized according to a preset weight matrix W. It is known that the weight matrix W can be estimated in consideration of the magnitude of the error by setting it based on a known error covariance, and the inverse of the covariance is set in the weight matrix G. Techniques are often used. Since it is known that the relative position error decreases when positioning is performed using a common satellite for two vehicles, the weight matrix W is calculated based on the satellite number of the GPS satellite used for position calculation. By setting, a more accurate relative position can be estimated. Note that the weight matrix W does not need to be calculated at the ratio of the number of satellites that match, and may be set in two types, that is, whether or not they completely match. For example, the weight “1” may be set for the time when the satellite numbers are completely matched, and the weight “0.5” may be set for the time when the satellite numbers are not completely matched.

  In addition, the trajectory use relative position estimation unit 36 determines a threshold value for a residual (residual at the time of least squares) between the optimum value of the trajectory of the opponent vehicle and the predicted value of the position of the opponent vehicle at each time on the trajectory. As shown in FIG. 7, after rejecting the relative position due to the position difference for the time when the residual is equal to or greater than the threshold, the fitting is performed again with respect to the relative position due to the difference between the trajectory and the position, Multipath effects are eliminated by estimating the optimal value of the trajectory.

The relative position error calculation unit 38 calculates a standard deviation of a residual between the optimal value of the trajectory of the opponent vehicle obtained by the trajectory relative position estimation unit 36 and the predicted value of the position of the opponent vehicle at each time on the trajectory. And output as an indicator of relative position error.
When errors occur in the method of calculating the relative position using the trajectory, “when there is an error in the trajectory” as shown in FIG. 8 and “multipath occurs in the entire area” as shown in FIG. Case ". In both cases, the dispersion of the residual in the case of least squares becomes large, so the standard deviation of the residual can be used as an indicator of the relative position error.

  Note that an error in the calculated relative position may be calculated by using the relative position calculated using the optimal value of the locus and the relative position calculated simply by the position difference.

  Next, the operation of the relative position estimation apparatus 10 according to the first embodiment will be described.

  First, every time the wireless communication unit 18 receives measurement data including the position, velocity vector, and satellite number transmitted from the vehicle-mounted device 80 of the opponent vehicle, the computer 20 of the relative position estimation apparatus 10 receives the received measurement. Data is stored in the received data storage unit 22.

  Further, when the GPS receiving unit 12 repeatedly receives radio waves from a plurality of GPS satellites, the computer 20 repeatedly executes the position / velocity calculation processing routine shown in FIG.

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

  In step 102, the position of the host vehicle is calculated according to the above equations (1) to (3) using the GPS pseudorange data of each GPS satellite.

  Next, in step 104, 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 54.

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

  In step 110, measurement data at the same time (the reception time of radio waves is the same) as the GPS satellite information acquired in step 100 is acquired from the reception data storage unit 22. In step 112, the weight is calculated based on the satellite number obtained from the GPS satellite information obtained in step 100 and the satellite number obtained from the measurement data obtained in step 110.

  In step 114, the relative position of the partner vehicle with respect to the host vehicle is calculated from the difference between the position of the host vehicle calculated in step 102 and the position of the partner vehicle obtained from the measurement data acquired in step 110. In the next step 116, the calculation results in steps 102, 106, 112, and 114 are stored in the data storage unit 30, and the position / velocity calculation processing routine is terminated.

  By repeatedly executing the position / speed calculation processing routine, the position, speed vector, weight, and relative position of the host vehicle calculated for each time are stored in the data storage unit 30.

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

In step 1060, according to the above (4) equation, the Doppler frequency of the received signal from each GPS satellite, and calculates the relative velocity _{v j} of the vehicle with respect to each GPS satellite.

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

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

Next, at step 1068, the relative speed v _j of the own vehicle with respect to each GPS satellite calculated at step 1060, and the velocity vector V _j (VX _j , VY _j , VZ _{j of} each GPS satellite calculated at step 1062 above. ) And the direction R _j (θ _j , φ _j ) of each GPS satellite calculated in step 1066, 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 1070, 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 1072, the azimuth angle of the host vehicle is calculated from the speed vector of the host vehicle calculated in step 1070 using a trigonometric function, and the calculated azimuth angle is stored in the data storage unit 54 as an estimated azimuth angle. And return.

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

  In step 1080, 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 54, and the observation azimuth angle of the host vehicle is calculated by integrating the yaw rates for each observation in time series.

  Next, in step 1082, the estimated azimuth angle estimated within a predetermined period is read from the data storage unit 54, and the time-series data of the observation azimuth angle calculated in step 1080 is translated and searched, and the least square method is used. The optimum value of the azimuth angle is estimated by the optimum calculation represented by the above.

  Next, in Step 1084, a residual between the optimum value of the azimuth angle estimated in Step 1082 and each of the estimated azimuth angles is calculated, and a variance of the residual for a predetermined period is calculated.

  Next, in Step 1086, it is determined whether or not the variance of the residual calculated in Step 1084 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 optimal value of the azimuth is high, and the process proceeds to step 1088. The optimal value of the azimuth estimated in step 1082 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 1090, and the detected value of the gyro sensor 14 using the azimuth estimated in the past. The azimuth calculated by integrating the values is adopted as the estimated value of the azimuth.

  Next, in Step 1092, the speed detected by the speed sensor 16 is read from the data storage unit 54, and the speed vector is calculated using the estimated value of the azimuth angle adopted in Step 1088 or 1090 and the read speed, The calculated speed vector is stored in the data storage unit 30 as a speed vector estimation result, and the process returns.

  Further, when the position, speed vector, weight, and relative position of the host vehicle for a predetermined time are stored in the data storage unit 30, the computer 20 executes a relative position estimation processing routine shown in FIG.

  In step 120, a speed vector for a predetermined time is acquired from the data storage unit 30, and the speed vector of the host vehicle for a predetermined time is integrated to calculate a trajectory of the host vehicle for a predetermined time. In the next step 122, the speed vector of the opponent vehicle corresponding to a predetermined time is acquired from the received data storage unit 22, the speed vector of the opponent vehicle for a predetermined time is integrated, and the trajectory of the opponent vehicle for a predetermined period is calculated. To do.

  In step 124, the relative position calculated for each time within the corresponding predetermined time is acquired from the data storage unit 30. In step 126, based on the trajectory of the host vehicle calculated in step 120 and the relative position of each time acquired in step 124, the predicted position of the opponent vehicle at each time within the corresponding predetermined time is calculated. .

  In the next step 128, the weight calculated for each time within the corresponding predetermined time is acquired from the data storage unit 30, and the opponent vehicle calculated in step 126 is calculated by the least square method using the acquired weight. The trajectory of the opponent vehicle calculated in step 122 is fitted to the predicted position, and the optimum value is estimated.

  In step 130, the least squares residual is calculated for each time based on the optimal value of the trajectory of the opponent vehicle estimated in step 128 and the predicted position of the opponent vehicle at each time. Then, it is determined whether or not there is a point (time) at which the residual calculated in step 130 is equal to or greater than a threshold. If there is a point (time) at which the residual is equal to or greater than the threshold, in step 134, the relative position of the point (time) is rejected, and the process proceeds to step 136.

  In step 136, a response is made based on the trajectory of the host vehicle calculated in step 120 and a relative position other than the relative position rejected in step 134 among the relative positions obtained at step 124. The predicted position of the opponent vehicle at each time within a predetermined time is calculated.

  In the next step 138, the trajectory of the opponent vehicle calculated in step 122 is compared with the predicted position of the opponent vehicle calculated in step 136 by the least square method using weights, as in step 128. Fit and estimate the optimal value.

  In step 140, the least squares residual is calculated for each time based on the optimum value of the trajectory of the opponent vehicle estimated in step 138 and the predicted position of the opponent vehicle at each time, and step 142 Migrate to

  On the other hand, if it is determined in step 132 that there is no point (time) at which the residual is equal to or greater than the threshold, the process proceeds to step 142.

  In step 142, the relative position at the end point on the trajectory is calculated based on the optimum value of the trajectory of the opponent vehicle estimated in step 128 or 138 and the trajectory of the host vehicle calculated in step 120. ,Output.

  In the next step 144, the relative position error calculated in step 142 is calculated and output based on the residual of each time calculated in step 130 or 140, and the relative position estimation processing routine is terminated. To do.

  As described above, according to the relative position estimation device according to the first embodiment, the position and speed vectors of the opponent vehicle are received, the locus of the own vehicle position, the locus of the opponent vehicle position, Calculate the relative position of the opponent vehicle with respect to the own vehicle, and estimate the optimum value of the opponent vehicle's locus based on the locus of the own vehicle's position, the locus of the opponent's vehicle, and the relative position at each time. By estimating the relative position at the end point on the trajectory, it is possible to accurately estimate the relative position with the opponent vehicle with a smaller amount of data communication than Patent Document 1 that communicates GPS observation data. GPS observation data is composed of the pseudorange, Doppler frequency, carrier phase, etc. of each satellite, and its communication capacity is proportional to the satellite. In order to perform positioning by GPS, four or more satellites are required, so it is necessary to communicate at least 4 × 3 (pseudorange, Doppler frequency, carrier phase) = 12 pieces of data. On the other hand, in the present invention, the communication data is limited to the data shown in Table 1, and may be a position (three-dimensional), a velocity vector (three-dimensional), a time, and a satellite number. If the satellite number is represented by a bit indicating the usage status of each satellite, a maximum of 32 satellites are assumed in the case of GPS, so a capacity of 32 bits = 4 bytes is sufficient. Therefore, the relative position can be estimated with a small and constant communication capacity as compared with the conventional method.

  Further, by combining a GPS Doppler and an inertial sensor, a highly accurate trajectory can be calculated. An accurate trajectory can be calculated even in a place where multipath occurs. In addition, since the satellites used for positioning are often shared at a relative position of about 100 m, there is variation even if the GPS positioning result difference is simply used in a place where there is no multipath. Can measure the relative position with high accuracy. Based on the above technical background, based on the relative position due to the difference in position and the trajectory of the two vehicles, the relative position with reduced variation can be estimated by fitting using the least square method, and the threshold value can be used as the residual. Multipath can be rejected by setting. In addition, it is possible to increase the accuracy of the estimated relative position by increasing the weight used in the weighted least square method for the location where the satellites are shared.

  In addition, by calculating the relative position using the trajectories of the two vehicles, it is possible to eliminate the influence of multipath using the residual and to measure the relative position even in a place where GPS cannot be received. it can.

  In addition, since it is not necessary to wirelessly communicate GPS observation data (pseudorange, Doppler, carrier wave phase) and the like, the amount of communication data can be reduced.

  Next, a relative position estimation apparatus according to the second embodiment will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the second embodiment, when the position of the host vehicle is calculated, the GPS satellite to be used is selected so as to be the same as the GPS satellite used in the calculation of the position of the opponent vehicle. This is different from the embodiment.

  In the relative position estimation apparatus according to the second embodiment, as shown in FIG. 14, the position / velocity vector calculation unit 224 includes a GPS information acquisition unit 50, a satellite number included in the acquired GPS satellite information, and Based on the satellite number obtained from the received measurement data, a satellite selection unit 250 that selects a GPS satellite to be used in position calculation, a position calculation unit 51, an azimuth angle estimation unit 52, a data storage unit 54, a speed It can be expressed by a configuration including the vector estimation unit 56.

  The satellite selection unit 250 acquires the satellite number from the GPS satellite information acquired by the GPS information acquisition unit 50, and calculates the position of the opponent vehicle from the measurement data stored at the same time stored in the reception data storage unit 22. Get the satellite number of the used GPS satellite. Then, the satellite selection unit 250 completes when all the satellite numbers of the GPS satellites used in the calculation of the position of the opponent vehicle are included in the satellite numbers included in the GPS satellite information acquired by the own vehicle. All the GPS satellites used in the calculation of the position of the opponent vehicle are selected as the GPS satellites used in the calculation of the position so that the satellites are shared. On the other hand, when at least one of the satellite numbers of the GPS satellites used in the calculation of the position of the opponent vehicle is not included in the satellite numbers included in the GPS satellite information acquired by the own vehicle, the satellite selection unit 250 Do not select GPS satellites.

  For example, as shown in FIG. 15A, when at least one of the satellite numbers of the GPS satellites used in the calculation of the position of the opponent vehicle is not included in the satellite numbers of the GPS satellite information acquired by the own vehicle. The position calculation unit 51 calculates the position of the host vehicle using all GPS satellites that have received radio waves from the host vehicle.

  On the other hand, as shown in FIG. 15B, when all the satellite numbers of the GPS satellites used in the calculation of the position of the opponent vehicle are included in the satellite numbers of the GPS satellite information acquired by the own vehicle In the position calculation unit 51, the position of the host vehicle is calculated using the same GPS satellite as that used in the calculation of the position of the opponent vehicle.

  As a result, it is possible to increase the number of cases where positioning is performed using a GPS satellite common to the host vehicle and the opponent vehicle.

  The position calculation unit 51 uses the GPS pseudorange data of each GPS satellite acquired by the GPS information acquisition unit 50 or the GPS pseudorange data of each GPS satellite selected by the satellite selection unit 250 to determine the position of the host vehicle. calculate.

  Next, a position / velocity calculation processing routine in the second embodiment will be described with reference to FIG. In addition, about the process similar to 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  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. In the next step 110, measurement data at the same time as the acquired GPS satellite information is acquired from the received data storage unit 22.

  In step 270, it is determined whether or not the satellite numbers included in the GPS satellite information acquired in step 100 include all the satellite numbers included in the measurement data acquired in step 110. If the satellite number included in the GPS satellite information acquired by the host vehicle does not include at least one of the satellite numbers included in the measurement data, the process proceeds to step 102. On the other hand, when the satellite numbers included in the GPS satellite information acquired by the own vehicle include all the satellite numbers included in the measurement data, in step 272, as the GPS satellites used for calculating the position of the own vehicle, GPS satellites of all satellite numbers included in the measurement data acquired in step 110 are selected.

  In step 102, the position of the host vehicle is calculated using the GPS pseudorange data of each GPS satellite selected in step 272 or the GPS pseudorange data of all GPS satellites for which GPS information has been acquired.

  Next, in step 104, the yaw rate of the host vehicle and the speed of the host vehicle are acquired and stored in the data storage unit 54. Next, in step 106, the host vehicle azimuth processing based on the GPS information is executed to estimate the estimated azimuth, and in step 108, the speed vector calculation processing is executed to estimate the speed vector.

  In step 112, the weight used in the least square method is calculated based on the satellite number obtained from the GPS satellite information obtained above and the satellite number obtained from the measurement data obtained above.

  In step 114, the relative position of the partner vehicle with respect to the host vehicle is calculated. In the next step 116, the calculation results in steps 102, 106, 112, and 114 are stored in the data storage unit 30, and the position / velocity calculation processing routine is terminated.

  In addition, about the other structure and effect | action of the relative position estimation apparatus which concern on 2nd Embodiment, since it is the same as that of 1st Embodiment, description is abbreviate | omitted.

  As described above, according to the relative position estimation apparatus according to the second embodiment, the number of cases in which positioning is performed using the GPS satellite common to the host vehicle and the partner vehicle can be increased. The relative position can be estimated with high accuracy.

  In the first embodiment and the second embodiment described above, the case where the host vehicle and the partner vehicle are moving has been described. However, the present invention is not limited to this. Either one may stop. In this case, since the velocity vector of the stopped vehicle is 0, the calculated trajectory becomes a point. However, as with the trajectory use relative position estimation unit 36 described above, the relative position of the opponent vehicle with respect to the own vehicle. Can be estimated.

  Next, a relative position estimation apparatus according to the third embodiment will be described. In addition, since the structure of the relative position estimation apparatus which concerns on 3rd Embodiment is the same as that of 1st Embodiment, it attaches | subjects the same code | symbol and abbreviate | omits description.

  The third embodiment is different from the first embodiment in that the relative position of the roadside machine with respect to the host vehicle is estimated by communicating with the roadside machine by road-to-vehicle communication.

  In the third embodiment, the relative position estimation device 10 performs road-to-vehicle communication with a roadside machine 380 as shown in FIG. The roadside device 380 includes a GPS receiving unit 82, a wireless communication unit 88, and a computer 390 that executes a process of calculating the position of the roadside device 380 based on the received signal from the GPS satellite received by the GPS receiving unit 82. It is equipped with.

  The computer 390 includes a position calculation unit 392. The position calculation unit 392 acquires GPS satellite information for all GPS satellites that have received radio waves from the GPS reception unit 82, and the GPS satellites transmit radio waves. GPS pseudorange data is calculated based on the received time and the time when the roadside device 380 receives the radio wave. The position calculation unit 392 calculates the position of the roadside device 380 using the acquired GPS pseudorange data of each GPS satellite, outputs the position to the wireless communication unit 88, and sets the vehicle speed to 0 as a wireless communication unit. Output to 88.

  The wireless communication unit 88 transmits measurement data including the position and velocity vector and the satellite number of the GPS satellite used in the position calculation process for each time at predetermined time intervals. The measurement data further includes a time indicating when the calculated position is, that is, a time when the roadside device 380 receives a radio wave from a GPS satellite.

  In addition, about the structure and effect | action of the relative position estimation apparatus 10 which concern on 3rd Embodiment, since it is the same as that of 1st Embodiment, description is abbreviate | omitted.

  When the relative position estimation device 10 according to the third embodiment estimates a relative position using a trajectory, the trajectory of the roadside machine 380 as a communication partner becomes a point as shown in FIG.

  Then, based on the trajectory of the host vehicle, the predicted position of the roadside machine 380 at each time is calculated from the relative position based on the position difference at each time. And the optimal value of the locus | trajectory (point) of the roadside machine 380 is calculated | required by fitting the locus | trajectory (point) of the roadside machine 380 with the least squares method with respect to the estimated position of the roadside machine 380, and the calculated roadside machine Using the optimum value of the trajectory (point) of 380, the relative position of the roadside device 380 with respect to the host vehicle at the terminal point on the trajectory is calculated.

  As described above, even when the target for calculating the relative position with respect to the host vehicle is a stationary object, the relative position can be estimated with high accuracy.

  In the first to third embodiments, the velocity vector is calculated using the Doppler frequency to calculate the azimuth angle of the host vehicle, and the azimuth angle of the host vehicle for a predetermined period. In the above description, the example of calculating the velocity vector by performing the optimal estimation using the calculation result is described, but the present invention is not limited to this. The velocity vector may be calculated by another method using the Doppler frequency. For example, the calculation result by the own vehicle speed calculation unit 70 may be used as it is as the speed vector of the own vehicle.

  In the above embodiment, the case where the speed vector is calculated has been described as an example. However, the present invention is not limited to this, and the azimuth angle of the host vehicle is calculated and the speed of the host vehicle is detected. Speed information consisting of the azimuth angle and speed may be used. In this case, the trajectory may be calculated from the azimuth angle and speed of the host vehicle for a predetermined time.

  Further, in the above embodiment, the case where the relative position and the velocity vector are calculated at the predetermined time interval to obtain the relative position and the velocity vector at each time has been described as an example. However, the present invention is not limited to this. The relative position and velocity vector may be calculated at predetermined distance intervals to obtain the relative position and velocity vector of each point.

  In the above embodiment, the case where the relative position is estimated with respect to the own vehicle has been described as an example. However, the present invention is not limited to this, and the relative position of the own vehicle with respect to the opponent vehicle or the roadside device is described. May be estimated. In this case, the predicted position of the host vehicle at each time is calculated from the relative position based on the position difference at each time with reference to the trajectory of the opponent vehicle or the roadside machine. Then, by fitting the trajectory of the own vehicle to the predicted position of the own vehicle by the least square method, the optimum value of the trajectory of the own vehicle is obtained, and the optimum value of the obtained trajectory of the own vehicle is used. What is necessary is just to make it estimate the relative position of the own vehicle with respect to an other party vehicle or a roadside machine.

  In the above embodiment, the case where the optimum value of the trajectory of the opponent vehicle is obtained by using the weighted least square method is described as an example. However, the present invention is not limited to this, and the least square method without weight is used. The optimum value of the trajectory of the opponent vehicle may be obtained. In this case, the optimum value of the trajectory of the opponent vehicle may be obtained using the following equations (11) and (12).

ただし、Δｙは、観測量（相手車両の軌跡上の位置）と予測値（予測された相手車両の位置）との差（残差）であり、Δｘは未知数であり、Ｇは、一般行列である。上記（１１）式は観測量と予測値との差（残差）Δｙと未知数Δｘ、一般行列Ｇの関係を示している。上記（１２）式は、一般的な未知数Δｘを推定する方法である。

However, Δy is a difference (residual) between the observed amount (position on the trajectory of the opponent vehicle) and a predicted value (predicted position of the opponent vehicle), Δx is an unknown, and G is a general matrix. is there. The above equation (11) shows the relationship between the difference (residual) Δy between the observed quantity and the predicted value, the unknown Δx, and the general matrix G. The above equation (12) is a method for estimating a general unknown Δx.

  Further, the position of the host vehicle may be calculated using a composite navigation result obtained by combining a GPS measurement value, a gyroscope, and a speed sensor.

  Moreover, although the said embodiment demonstrated the case where the azimuth angle of the own vehicle was estimated based on GPS information and used for accuracy determination, 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 70, and the speed detected by the speed sensor 16 and the absolute speed of the calculated speed are calculated by the optimum estimation unit 74. 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 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 addition, the satellite positioning system is generally called GNSS (Global Navigation Satellite System), and not only GPS but also Russia's Glonus, Europe's Galileo, and Japan's Quasi-Zenith Satellite System are planned. Some have already been applied. The present invention is not limited to GPS, but is a technology that can be widely applied to GNSS. Therefore, information transmitted from the GNSS may be received.

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

DESCRIPTION OF SYMBOLS 10 Relative position estimation apparatus 12, 82 GPS receiving part 14, 84 Gyro sensor 16, 86 Speed sensor 18, 88 Wireless communication part 20, 90, 390 Computer 24, 56, 92, 224 Position / velocity vector calculation part 26 Position difference relative Position calculation unit 28 Weight calculation unit 30 Data storage unit 32 Trajectory calculation unit 34 Trajectory calculation unit 36 Trajectory use relative position estimation unit 38 Relative position error calculation unit 50 Information acquisition unit 51 Position calculation unit 52 Azimuth angle estimation unit 80 In-vehicle device 250 Satellite selector 380 Roadside machine 392 Position calculator

Claims (9)

A relative position estimation device mounted on the moving body for estimating a relative position between the moving body and another device,
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 motion state of the moving body;
Based on the transmission source information acquired by the acquisition means and the motion state detected by the detection means, the position of the moving body, the velocity vector of the moving body, or the speed of the moving body and the moving body. Calculating means for calculating speed information indicating the azimuth angle of
Receiving means for receiving the position of the other device and the speed information;
Based on the difference between the position of the other device received by the receiving means and the position of the moving body calculated by the calculating means, the relative position of the moving body and the other device for each time or each point is determined. A relative position calculating means for calculating a position;
First trajectory calculating means for calculating a trajectory of the position of the moving body at the predetermined time by accumulating the speed vector or the speed information of the moving body calculated by the calculating means for a predetermined time;
Second trajectory calculating means for calculating the trajectory of the position of the other device at the predetermined time by accumulating the speed information received by the receiving means for the predetermined time;
The locus of the position of the moving body at the predetermined time calculated by the first locus calculating means, the locus of the position of the other device at the predetermined time calculated by the second locus calculating means, and the relative position Relative position estimating means for estimating the relative position at a specific point on the trajectory based on the relative position calculated for each time or each point within the predetermined time by the calculating means;
A relative position estimation device. The relative position is a relative position of the other device with respect to the position of the moving body,
The relative position estimation means includes a locus of the position of the other device at the predetermined time calculated by the second locus calculation means, and a position of the moving body at the predetermined time calculated by the first locus calculation means. And the sum of the difference between each time or each position obtained from the relative position calculated for each time or each point within the predetermined time by the relative position calculation means is minimized. And determining the relative position at a specific point on the locus based on the obtained locus of the position of the other device and the locus of the position of the moving body. The relative position estimation apparatus according to claim 1. The relative position is a relative position of the moving body with respect to a position of the other device,
The relative position estimation means includes a locus of the position of the moving body at the predetermined time calculated by the first locus calculation means and a position of the other device at the predetermined time calculated by the second locus calculation means. And the sum of the difference between each time or each position obtained from the relative position calculated for each time or each point within the predetermined time by the relative position calculation means is minimized. Obtaining a trajectory of the position of the moving body and estimating the relative position at a specific point on the trajectory based on the trajectory of the position of the moving body and the trajectory of the position of the other device. Item 2. The relative position estimation device according to Item 1.   4. The relative position error calculating means for calculating an error of the relative position estimated by the relative position estimating means based on each of the differences when the sum of the differences is minimized. Relative position estimation device. The other apparatus includes second acquisition means for acquiring the transmission source information transmitted from each of a plurality of information transmission sources, second detection means for detecting a movement state of the other apparatus, and the second acquisition. Second calculation means for calculating the position of the other device and the speed information based on the source information acquired by the means and the motion state detected by the second detection means; and the second A transmission unit that transmits the position information of the other device calculated by the calculation unit, the speed information, and identification information of the information transmission source that has transmitted the transmission source information acquired by the second acquisition unit;
The receiving means receives the position of the other device transmitted by the transmitting means, the speed information, and the identification information of the information transmission source,
The relative position estimation means is based on the position of the moving body calculated based on the transmission source information from a plurality of information transmission sources including all the information transmission sources indicated by the identification information received by the reception means. A trajectory that minimizes the sum of the differences is obtained so that the difference obtained for the relative position is smaller than the difference obtained for another relative position, and the relative position at a specific point on the trajectory is estimated. The relative position estimation apparatus according to any one of claims 2 to 4.   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 relative position estimation device according to any one of claims 1 to 5, 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 relative position estimation apparatus according to any one of claims 1 to 6, wherein the GPS satellite transmits Doppler frequency information as information relating to a relative velocity of the moving body with respect to the position. A relative position estimation program for causing a computer to function as each means of a relative position estimation device mounted on the moving body, for estimating a relative position between the moving body and another device,
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;
Based on the source information acquired by the acquisition means and the motion state detected by the detection means for detecting the motion state of the mobile body, the position of the mobile body and the velocity vector of the mobile body, or the Calculating means for calculating speed of the moving body and speed information indicating the azimuth angle of the moving body;
From the difference between the position of the other device and the position of the other device received by the receiving means for receiving the speed information and the position of the moving body calculated by the calculating means, for each time or each point, A relative position calculating means for calculating a relative position between the movable body and the other device;
First trajectory calculating means for calculating a trajectory of the position of the moving body at the predetermined time by accumulating the speed vector or the speed information of the moving body calculated by the calculating means for a predetermined time;
The speed information received by the receiving means is integrated for the predetermined time, and is calculated by a second trajectory calculating means for calculating a trajectory of the position of the other device at the predetermined time, and calculated by the first trajectory calculating means. Further, the locus of the position of the moving body at the predetermined time, the locus of the position of the other device at the predetermined time calculated by the second locus calculating means, and the relative position calculating means within the predetermined time. A relative position estimation program for functioning as a relative position estimation means for estimating the relative position at a specific point on the trajectory based on time or the relative position calculated for each point.   The relative position estimation program for functioning a computer as each means of the relative position estimation apparatus of any one of Claims 1-7.

Images