JP6143474B2 - Position detection apparatus and program - Google Patents

Description

  The present invention relates to a position detection device and a program.

  Patent Document 1 listed below discloses a position detection device that can perform map matching in consideration of an error in the position of the host vehicle caused by a sensor error in radio navigation technology and inertial navigation technology. The sensor error is represented by an error covariance matrix calculated in the update process of the Kalman filter that estimates the position and orientation of the dynamic system.

  Further, in Patent Document 2 below, a system equation is simplified by modeling an error generation process using the Langevin equation, and a colored observation noise is modeled by a Markov process and converted into white noise, thereby updating the update period of the Kalman filter. A hybrid navigation device that can solve the problem of the limitation is disclosed.

JP 2011-2324 A JP 2001-174275 A

  By the way, in the Kalman filter, it is assumed that the nature of noise accompanying the system follows a Gaussian distribution, but the gyro sensor has an accumulated error, and the measurement error of the GPS signal has an autocorrelation. As described above, the error included in the actually measured information often has the property of colored noise instead of Gaussian distribution.

  In this respect, since the position detection device of Patent Document 1 calculates the sensor error as white noise, the calculated error amount tends to be estimated lower than the actual error. The position calculation accuracy may be lowered.

  Further, the navigation device of Patent Document 2 models colored noise and estimates an error alone in a Kalman filter, and an offset of the colored noise is taken into account for the estimated error. However, in the navigation device of Patent Document 2, since a colored noise component is added as an element of a determinant in the Kalman filter, the size (number of elements) of the matrix used for the calculation becomes large. Therefore, a large processing load is required for the arithmetic processing, and a large amount of computer resources are required, such as a large memory space.

  Since the position detecting device for detecting the position of the moving body moves together with the moving body, it is required to be small and lightweight, and it is often impossible to mount a large battery as a power source. Therefore, such a position detection device is required to have a low processing load and a small memory capacity.

  The present invention has been made in view of the above circumstances, and an object of the present invention is to be able to calculate an error in consideration of colored noise and to keep consumption of computing resources low.

A first aspect of the present invention for solving the above problem is, for example, a position detection device that detects the position of a moving body, and calculates the position of the moving body based on a GPS signal and outputs the position as a GPS position A GPS position calculation unit, a sensor information acquisition unit that acquires information output from each sensor provided in the moving body, the GPS position information calculated by the GPS position calculation unit, and the sensor information acquisition Information indicating the position and orientation of the moving body based on the sensor information acquired by the unit, a position error calculation unit for calculating the error covariance matrix, and an error vector indicating the accumulation of errors, an offset calculating section that calculates the information indicating the length of the calculated error vector as an offset, based on the calculated offset by the offset calculation section, The error covariance matrix correction unit that corrects the error covariance matrix so that the area of the error ellipse obtained from the error covariance matrix calculated by the position error calculation unit increases, and the position error calculation unit Using the information indicating the position and orientation of the moving object and the error covariance matrix corrected by the error covariance matrix correction unit, the probability that the moving object exists on the road included in the map data is calculated, A map match processing unit that estimates the position of the moving object on a road included in the map data based on the calculated probability.

The second aspect of the present invention is a program that causes a computer to function as a position detection device that detects the position of a moving object, for example, and causes the computer to calculate the position of the moving object based on a GPS signal. A GPS position calculation function for outputting as a GPS position, a sensor information acquisition function for acquiring information output from each sensor provided in the moving body, and the GPS position information calculated by the GPS position calculation function Information indicating the position and orientation of the moving body based on the sensor information acquired by the sensor information acquisition function, a position error calculation function for calculating the error covariance matrix, and an accumulation of errors calculates an error vector, and the offset calculation function of calculating the information indicating the length of the calculated error vector as an offset, the An error covariance matrix correction function for correcting the area of the error ellipse obtained from the error covariance matrix calculated by the position error calculation function based on the offset calculated by the offset calculation function; and the position error calculation Using the information indicating the position and orientation of the moving object calculated by the function and the error covariance matrix corrected by the error covariance matrix correction function, the moving object exists on the road included in the map data And a map match processing function for estimating the position of the moving body on the road included in the map data based on the calculated probability.

  According to the position detection apparatus of the present invention, it is possible to calculate an error in consideration of colored noise and to reduce the consumption of computing resources.

It is a hardware block diagram which shows an example of a structure of the navigation apparatus which concerns on one Embodiment of this invention. It is a conceptual diagram which shows an example of the data structure of the link table stored in the memory | storage device. It is a block diagram which shows an example of a function structure of the position detection function implement | achieved by the arithmetic processing part. It is a flowchart which shows an example of the position detection process performed by a position detection function. It is a flowchart which shows an example of the process in offset calculation (S100). It is a conceptual diagram for demonstrating the time series of the sensor data acquired by the sensor information acquisition part, and the GPS position calculated by the GPS position calculation part. It is a conceptual diagram for _{demonstrating} the calculation process of direction _DDR in the reception time _Gsync . It is a conceptual diagram for _{demonstrating} the calculation process of direction _DGPS in the reception time _Gsync . It is a conceptual diagram for _{demonstrating} the calculation process of difference vector Ei. It is a flowchart which shows an example of a process of correction | amendment of an error covariance matrix (S300). It is a conceptual diagram for demonstrating the effect of correction | amendment of an error covariance matrix.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

  FIG. 1 is a hardware configuration diagram illustrating an example of a configuration of a navigation device 10 that operates as a position detection device according to an embodiment of the present invention. The navigation device 10 includes an arithmetic processing unit 100, a storage device 107, a voice input / output device 108, an input device 111, a ROM device 115, a vehicle speed sensor 116, an acceleration sensor 117, a gyro sensor 118, a GPS ( Global Positioning System) receiving device 119, FM multiplex broadcast receiving device 120, and beacon receiving device 121. The navigation device 10 in this embodiment is mounted on a vehicle such as an automobile.

  The arithmetic processing unit 100 is a central unit that performs various processes. The arithmetic processing unit 100 is based on information output from various sensors (vehicle speed sensor 116, acceleration sensor 117, gyro sensor 118), GPS receiver 119, FM multiplex broadcast receiver 120, etc., for example. Detect position. In addition, the arithmetic processing unit 100 reads map data necessary for display from the storage device 107 or the ROM device 115 based on the obtained current position information.

  In addition, the arithmetic processing unit 100 develops the read map data in graphics, and displays the mark indicating the current position on the display 106 in a superimposed manner. Further, the arithmetic processing unit 100 uses the map data or the like stored in the storage device 107 or the ROM device 115, and the optimal route (recommended) connecting the departure point (or current position) instructed by the user and the destination point. Route). In addition, the arithmetic processing unit 100 guides the user using the speaker 110 and the display 106.

  The arithmetic processing unit 100 includes a CPU (Central Processing Unit) 101 that executes various processes such as numerical calculation and control of each device, and a RAM (Random) that stores map data, arithmetic data, and the like read from the storage device 107. (Access Memory) 102, a ROM (Read Only Memory) 103 for storing programs and data, and an I / F (interface) 104 for connecting various kinds of hardware to the arithmetic processing unit 100. In the arithmetic processing unit 100, each device is connected by a bus 105.

  The display 106 is a unit that displays graphics information generated by the arithmetic processing unit 100 or the like. The display 106 is configured by a liquid crystal display, an organic EL display, or the like. The storage device 107 is configured by at least a readable / writable storage medium such as an HDD or a nonvolatile memory card. This storage medium stores a link table or the like which is map data (including link data of links constituting roads on the map) necessary for a normal route search device.

  The voice input / output device 108 includes a microphone 109 and a speaker 110. The microphone 109 acquires a voice uttered by a passenger such as a driver. The speaker 110 outputs a message or the like to the user generated by the arithmetic processing unit 100 as an audio signal.

  The microphone 109 and the speaker 110 are separately arranged at a predetermined part in the vehicle on which the navigation device 10 is mounted. However, the microphone 109 and the speaker 110 may be housed in the casing of the navigation device 10. The navigation device 10 can include a plurality of microphones 109 and speakers 110.

  The input device 111 is a device that receives an instruction from the user through an operation by the user. The input device 111 includes a direction key 112, a dial switch 113, and a touch panel 114, but other hard switches (for example, a scale change key) may be provided.

  The ROM device 115 is composed of at least a readable storage medium such as a ROM (Read Only Memory) such as a CD-ROM or a DVD-ROM, or an IC (Integrated Circuit) card. In this storage medium, for example, moving image data, audio data, and the like are stored.

  The vehicle speed sensor 116, the acceleration sensor 117, and the gyro sensor 118 are sensors used for detecting the current position in the navigation device 10.

  The vehicle speed sensor 116 is a sensor that outputs a signal used to calculate the vehicle speed. For example, the vehicle speed sensor 116 detects the number of rotations of the wheel, converts it into a pulse signal, and outputs vehicle speed information in the form of the number of pulse signals within a predetermined time.

  The acceleration sensor 117 is a sensor that outputs a predetermined detection signal at a constant period in accordance with the acceleration in the longitudinal direction of the vehicle. For example, the acceleration sensor 117 outputs a positive signal when the acceleration of the vehicle is increasing, and outputs a negative signal when deceleration is occurring.

  The gyro sensor 118 is configured by an optical fiber gyro, a vibration gyro, or the like, and is a sensor that detects an angular velocity due to rotation of a moving body.

  The GPS receiver 119 receives a signal from a GPS satellite and provides the received signal to the arithmetic processing unit 100.

  The FM multiplex broadcast receiving apparatus 120 receives an FM multiplex broadcast signal transmitted from an FM multiplex broadcast station. FM multiplex broadcasting includes VICS (Vehicle Information Communication System: Registered Trademark) information, current traffic information, regulatory information, SA / PA (service area / parking area) information, parking information, weather information, and FM multiplex general information. As text information provided by radio stations.

  The beacon receiving device 121 receives general current traffic information such as VICS information, regulation information, SA / PA information, parking lot information, weather information, emergency alerts, and the like. The beacon receiving device 121 receives, for example, an optical beacon that communicates by light, a radio beacon that communicates by radio waves, and the like.

  FIG. 2 is a conceptual diagram showing an example of the data structure of the link table 20 stored in the storage device 107. The link table 20 has mesh data 21 including information on links indicating roads included in the mesh for each mesh that is a predetermined map area. Each mesh data 21 includes a mesh ID 22 that identifies each mesh and link data 23 that is data related to a link in the mesh.

  Each link data 23 includes a link ID 230 for identifying each link, a start node coordinate 231 of the link, an end node coordinate 232 of the link, a road type 233 of the link, a link length 234 of the link, Travel time 235, start connection link 236 indicating the link ID of another link connected to the start node of the link, and end connection link 237 indicating the link ID of another link connected to the end node of the link Etc. are stored.

  FIG. 3 is a block diagram illustrating an example of a functional configuration of the position detection function 30 realized by the arithmetic processing unit 100. The position detection function 30 includes a GPS position calculation unit 31, a sensor information acquisition unit 32, an offset calculation unit 33, a position error calculation unit 34, an error covariance matrix correction unit 35, and a map match processing unit 36.

  The GPS position calculation unit 31 determines the distance between the vehicle on which the navigation device 10 is mounted and the GPS satellite based on the GPS signal provided from the GPS receiver 119 at predetermined time intervals (for example, every second) By measuring the rate of change for three or more satellites, the position of the vehicle (GPS position), the speed, the position information indicating the direction, the speed information, the direction information, and the certainty of the calculated vehicle position are indicated. Generate error information. The GPS position calculation unit 31 then outputs the generated information to the offset calculation unit 33 and the position error calculation unit 34, respectively.

  The sensor information acquisition unit 32 acquires information output from each of the vehicle speed sensor 116, the acceleration sensor 117, and the gyro sensor 118 at predetermined time intervals (for example, every 0.2 seconds), and acquires these pieces of information. Are output to the offset calculation unit 33 and the position error calculation unit 34, respectively.

  The offset calculation unit 33 calculates an error vector indicating accumulation of errors according to a procedure to be described later, and outputs information indicating the length of the calculated error vector to the error covariance matrix correction unit 35 as an offset.

  The position error calculation unit 34 is configured by, for example, a Kalman filter (including what is called an “extended Kalman filter”), and calculates information that specifies the position and direction of a moving body such as a vehicle using measurement values with errors. . The position error calculation unit 34 according to the present embodiment receives a plurality of pieces of information such as vehicle position information, speed information, azimuth information, error information, and autonomous information, and obtains the average and variance thereof to obtain the vehicle position and Information indicating the direction and an error covariance matrix indicating the error of these pieces of information are calculated. Then, the position error calculation unit 34 outputs the calculated information to the error covariance matrix correction unit 35.

  The error covariance matrix correction unit 35 corrects the error covariance matrix calculated by the position error calculation unit 34 using the offset calculated by the offset calculation unit 33 according to the procedure described later, and the corrected error covariance. The matrix is output to the map match processing unit 36 together with information indicating the position and orientation of the vehicle calculated by the position error calculation unit 34.

  The map match processing unit 36 refers to the link table 20 in the storage device 107, and uses the information indicating the position and orientation of the vehicle received from the error covariance matrix correction unit 35 and the corrected error covariance matrix. Identify the most probable link location that exists. Then, the map match processing unit 36 reads the map data in the vicinity of the specified position from the storage device 107 and develops the graphics, and displays the car mark on the specified position on the display 106 on the displayed map.

  FIG. 4 is a flowchart illustrating an example of the position detection process executed by the position detection function 30. The position detection function 30 executes the processing shown in this flowchart, for example, at predetermined time intervals (for example, every second).

  Separately from this, the GPS position calculation unit 31 calculates the GPS position and the like of the vehicle on which the navigation device 10 is mounted at a predetermined time interval, and sends these information to the offset calculation unit 33 and the position error calculation unit 34. The sensor information acquisition unit 32 acquires information output from each sensor at a predetermined time interval and provides the information to the offset calculation unit 33 and the position error calculation unit 34.

  First, the offset calculation unit 33 calculates an error vector indicating accumulation of errors according to a procedure described later in FIG. 5, calculates information indicating the length of the calculated error vector as an offset, and sets a value indicating the calculated offset. The data is sent to the error covariance matrix correction unit 35 (S100).

  Next, the position error calculation unit 34 uses the position information including the GPS position calculated by the GPS position calculation unit 31, the speed information, the direction information, the error information, and the information output from each sensor. By calculating the mean and variance of the information, information indicating the position and orientation of the vehicle, and an error covariance matrix indicating the error of these information are calculated (S200). Then, the position error calculation unit 34 sends the calculated information to the error covariance matrix correction unit 35.

  Note that the processing in step S100 and the processing in step S200 are not limited to the order shown in this flowchart, as long as they are completed before step S300 is executed.

  Next, the error covariance matrix correction unit 35 corrects the error covariance matrix calculated by the position error calculation unit 34 using the offset calculated by the offset calculation unit 33 according to the procedure described later in FIG. S300), the corrected error covariance matrix is sent to the map match processing unit 36 together with information indicating the position and orientation of the vehicle calculated by the position error calculating unit 34.

  Next, the map match processing unit 36 refers to the link table 20 in the storage device 107 and uses the information indicating the position and orientation of the vehicle received from the error covariance matrix correction unit 35 and the corrected error covariance matrix. Then, the car mark is superimposed on the most likely position on the link where the vehicle is present and displayed on the display 106 (S400), and the position detection function 30 ends the position detection process shown in this flowchart.

  FIG. 5 is a flowchart illustrating an example of processing in offset calculation (S100).

  Here, before explaining the offset calculation process, the temporal relationship between the sensor data measured by each sensor and the GPS position calculated by the GPS position calculation unit 31 is illustrated in FIG. 6, for example. For example, as shown in FIG. 6, the offset calculation unit 33 and the position error calculation unit 34 hold each sensor data acquired by the sensor information acquisition unit 32 and information on the GPS position calculated by the GPS position calculation unit 31. Yes.

In the example shown in FIG. 6, the sensor data and the GPS position information are stored from the top to the bottom of FIG. Also, the reception time of the GPS signal indicating the latest GPS position P _next is G _next , the reception time of the GPS signal indicating the previous GPS position P _sync is G _sync , and the previous GPS position P _prev is The reception time of the indicated GPS signal is defined as G _prev .

First, the offset calculation unit 33, a GPS position of the vehicle at the reception time _{G prev P prev (X gn-} 2, Y gn-2, θ gn-2) standards, from the reception time G _prev to the reception time G _sync Using the sensor data, dead reckoning of the vehicle on which the navigation device 10 is mounted is performed, and the vehicle position P _DR (X _DR , Y _DR ) and the direction D _DR at the reception time G _sync are calculated (S101).

For example, as shown in FIG. 7, the offset calculation unit 33 connects the sensor data from the reception time G _prev to the reception time G _sync in time series, and the vehicle position P _DR (X _DR , Y _DR ) at the reception time G _sync . and calculate the direction D _DR. Orientation D _DR of the vehicle at the reception time G _sync is formed in a direction theta _gn-2 and the angle theta _DR of the vehicle at the reception time G _prev.

Next, offset calculation unit 33, from the position P _prev of the vehicle at the reception time G _prev, from the change of the GPS position to the position P _next of the vehicle at the reception time G _next, calculated from GPS signal reception time G _sync The direction D _GPS at the GPS position P _sync is calculated (S102). For example, as illustrated in FIG. 8, the offset calculation unit 33 calculates the direction of the vector M ₁ from the GPS position P _prev to the GPS position P _sync and the direction of the vector M ₂ from the GPS position P _sync to the GPS position P _next . calculating an intermediate orientation as D _GPS. In the example shown in FIG. 8, a direction that is ½ of the angle θ _GPS formed by the vector M ₁ and the vector M ₂ is calculated as the direction D _GPS at the GPS position P _sync .

Next, the offset calculation unit 33 calculates the difference vector E _i (E _Xi , E _Yi ) (S103). For example, as illustrated in FIG. 9, the offset calculation unit 33 matches the vehicle position P _DR (X _DR , Y _DR ) calculated in step S101 with the GPS position P _sync at the reception time G _sync . The offset calculation unit 33, the direction D _DR of the vehicle calculated in step S101, are rotated to match the calculated orientation D _GPS at step S102.

Then, the offset calculation unit 33 calculates the position after rotation and translation at the reception time G _prev that was the calculation reference of the position P _DR (X _DR , Y _DR ) in step S101 as the position P ′ _prev . . _{Assuming that} the GPS position P _sync at the reception time G _sync is the origin of the XY coordinates, the position P ′ _prev is calculated as a position (−X _DR , −Y _DR ). Then, the offset calculation unit 33 calculates a vector from the GPS position P _prev to the position P ′ _prev as a difference vector E _i (E _Xi , E _Yi ).

Next, the offset calculation unit 33 calculates a difference vector E by calculating a vector sum of a predetermined number (for example, 10) of difference vectors E _i calculated in the past, and sets the length of the calculated difference vector E as d _offset as an error. The offset calculation unit 33 terminates the offset calculation processing shown in the flowchart.

  FIG. 10 is a flowchart illustrating an example of the error covariance matrix correction (S300) process.

First, the error covariance matrix correction unit 35 calculates the lengths of the major axis err _a and the minor axis err _b of the error ellipse based on the error covariance matrix output from the position error calculation unit 34 (S301). .

Next, the error covariance matrix correction unit 35 adds the d _offset calculated by the offset calculation unit 33 in step S100 to the long axis err _a of the error ellipse to calculate the corrected long axis err ′ _a ( S302).

Next, the error covariance matrix correction unit 35 extends the minor axis err _b of the error ellipse at the same ratio as the major axis by the following calculation formula (1), and calculates the corrected minor axis err ′ _b . (S303).
err ′ _b = err _b × err ′ _a / err _a (1)

  In this way, by extending the major axis and the minor axis at the same ratio, it is possible to reduce the influence on the error distribution characteristics by performing correction to increase the area of the error ellipse.

Next, the error covariance matrix correction unit 35 again creates an error covariance matrix using the corrected long axis err ′ _a and short axis err ′ _b (S304). Then, the error covariance matrix correction unit 35 sends the generated corrected error covariance matrix together with the vehicle position and orientation information received from the position error calculation unit 34 to the map match processing unit 36, and the error covariance matrix The correction unit 35 ends the error covariance matrix correction processing shown in this flowchart.

  Here, for example, as shown in FIG. 11A, when a candidate point of the current position is calculated at the position of reference numeral 44 due to the influence of colored noise, if an error is estimated as white noise, for example, an error ellipse is The size is calculated as shown by reference numeral 43.

  Then, if the map match processing unit 36 calculates the likelihood for a link at least part of which is included in the error ellipse and determines the most probable link among them, even if the actual vehicle position is the link 41 Even when the link 41 is on the road, the link 41 is excluded from the likelihood calculation target. Therefore, the position of the car mark 42 may be calculated on the link 40 indicating a road that is not actually traveling.

  On the other hand, in the navigation apparatus 10 of the present embodiment, the error of each sensor is accumulated, and the error covariance matrix is corrected based on the accumulated error so that the area of the error ellipse becomes large. As a result, for example, as shown in FIG. 11B, a part of the link 41 is included in the corrected error ellipse, and the position of the car mark 42 is actually a link indicating the road on which the vehicle is running. 41 is more likely to be calculated.

  The embodiment of the present invention has been described above.

  As is clear from the above description, according to the navigation device 10 of the present embodiment, it is possible to calculate an error in consideration of colored noise and to reduce the consumption of computing resources.

  For example, in the navigation device in Patent Document 2, since colored noise components are added as determinant elements in the Kalman filter, the size (number of elements) of the matrix used for computation increases, and a large amount of computer resources are required. Become.

  On the other hand, in the navigation apparatus 10 of this embodiment, the error of each sensor is accumulated separately from the matrix calculation by the Kalman filter, and the error covariance matrix is corrected based on the accumulated error. As a result, it is not necessary to secure many resources necessary for computing a large matrix, and offset calculation does not require as many resources as matrix computation, so it is highly accurate in embedded devices with limited resources. Detection can be realized.

  In addition, this invention is not limited to above-described embodiment, Many deformation | transformation are possible within the range of the summary.

For example, in the above-described embodiment, the error covariance matrix correction unit 35 adds d _offset calculated by the offset calculation unit 33 to the major axis of the error ellipse, but the present invention is not limited to this, and the minor axis of the error ellipse. May be added. When d _offset is added to the short axis, the error covariance matrix correction unit 35 extends the long axis at the same ratio. Further, the error covariance matrix correction unit 35 enlarges the error ellipse by d _offset calculated by the offset calculation unit 33 in a direction other than the major axis or minor axis of the error ellipse, thereby increasing the area of the error ellipse. You may do it.

Further, the error covariance matrix correction unit 35 may add the d _offset calculated by the offset calculation unit 33 to the major axis and the minor axis of the error ellipse, respectively, and d _{offset to} either the major axis or the minor axis. When the number is added, the length of the other axis may not be changed.

  Moreover, although the above-mentioned embodiment demonstrated the navigation apparatus 10 mounted in a vehicle as an example, this invention is not restricted to this, If it is an apparatus which has the position detection function 30, it will be a general purpose computer, a portable information terminal, etc. There may be. Also, each sensor and GPS receiver is moved together with the moving body, information from these devices is transmitted to an external computer having the position detection function 30 by wireless communication, and the external computer performs the processing of the position detection function 30. It is also possible to create an image in which the car mark is superimposed on the map, and transmit the created image to the moving body by wireless communication so as to be displayed on the screen of the moving body.

  In addition, each component in the above-mentioned navigation apparatus 10 is classified according to the function according to main processing content in order to make an understanding of the structure of the navigation apparatus 10 which concerns on this embodiment easy. Therefore, the invention of the present application is not limited by the component classification method or its name. The configuration of the navigation device 10 according to the present embodiment can be divided into more components according to the processing contents, or can be divided so that one component performs more processing. . Each processing may be realized as processing by software, or may be realized by dedicated hardware such as ASIC (Application Specific Integrated Circuit).

10: navigation device, 100: arithmetic processing unit, 101: CPU, 102: RAM, 103: ROM, 104: I / F, 105: bus, 106: display, 107: storage device, 108: voice input / output device, 109 : Microphone, 110: speaker, 111: input device, 112: direction key, 113: dial switch, 114: touch panel, 115: ROM device, 116: vehicle speed sensor, 117: acceleration sensor, 118: gyro sensor, 119: GPS reception Device: 120: FM multiplex broadcast receiver, 121: beacon receiver, 20: link table, 21: mesh data, 22: mesh ID, 23: link data, 230: link ID, 231: start node coordinates, 232: end Node coordinates, 233: road type, 234: link length 235: travel time, 236: start connection link, 237: end connection link, 30: position detection function, 31: GPS position calculation unit, 32: sensor information acquisition unit, 33: offset calculation unit, 34: position error calculation unit, 35: error covariance matrix correction unit, 36: map match processing unit, 40: link, 41: link, 42: car mark, 43: error ellipse, 44: candidate point, 45: error ellipse after correction, 46: car mark

Claims (7)

A position detection device for detecting the position of a moving body,
A GPS position calculation unit that calculates a position of the moving body based on a GPS signal and outputs the position as a GPS position;
A sensor information acquisition unit for acquiring information output from each sensor provided in the moving body;
Information indicating the position and orientation of the moving body based on the GPS position information calculated by the GPS position calculation unit and the sensor information acquired by the sensor information acquisition unit, and error covariance matrices thereof A position error calculation unit for calculating
An offset calculation unit that calculates an error vector indicating an accumulation of errors and calculates information indicating the length of the calculated error vector as an offset;
Error covariance matrix correction for correcting the error covariance matrix so that the area of the error ellipse obtained from the error covariance matrix calculated by the position error calculation unit is increased based on the offset calculated by the offset calculation unit And
Using the information indicating the position and orientation of the moving body calculated by the position error calculation unit and the error covariance matrix corrected by the error covariance matrix correction unit, the road is included on the road included in the map data. A position detection apparatus comprising: a map match processing unit that calculates a probability that a moving object exists and estimates a position of the moving object on a road included in the map data based on the calculated probability. The position detection device according to claim 1,
The GPS position calculation unit calculates the GPS position every predetermined time,
The offset calculation unit
Using the GPS position calculated at the first timing as a reference position, the sensor information acquired until the second timing which is the timing after the predetermined time from the first timing is accumulated, Calculate the position and orientation of the moving body at the second timing,
From the GPS position calculated at the first timing to the direction of the first vector connecting from the GPS position calculated at the second timing to the GPS position calculated from the GPS position calculated at the second timing Calculating a first direction indicating an intermediate direction between the second vector connecting to the GPS position calculated at the third timing which is a timing after the predetermined time from the second timing. ,
The position of the mobile body at the second timing calculated based on the sensor information is matched with the GPS position calculated at the second timing, and the second calculated based on the sensor information Moving the trajectory of sensor information acquired between the first timing and the second timing so that the orientation of the moving body at the timing matches the first direction,
The position detecting device, wherein the offset is calculated based on a difference between the reference position after movement and the GPS position calculated at the first timing. The position detection device according to claim 2,
The offset calculation unit
A position detection apparatus that calculates a direction that is ½ of an angle formed by the first vector and the second vector as the first direction. The position detection device according to claim 2 or 3,
The offset calculation unit
Using the sensor information acquired every predetermined time and the calculated GPS position, a vector from the GPS position calculated at the first timing to the reference position is calculated, and every predetermined time A position detection device that calculates the length of a vector obtained by adding a predetermined number of vectors calculated consecutively as an offset. The position detection device according to any one of claims 1 to 4,
The error covariance matrix correction unit includes:
By adding the offset calculated by the offset calculator to the length of the major or minor axis of the error ellipse obtained from the error covariance matrix calculated by the position error calculator, the area of the error ellipse is increased. A position detecting device characterized by correcting so as to become. The position detection device according to claim 5,
The error covariance matrix correction unit includes:
Position detection characterized by extending the length of the other axis at the same ratio as the one axis when the offset is added to the length of either the major axis or the minor axis of the error ellipse apparatus. A program that causes a computer to function as a position detection device that detects the position of a moving object,
In the computer,
A GPS position calculation function for calculating the position of the moving body based on a GPS signal and outputting the position as a GPS position;
A sensor information acquisition function for acquiring information output from each sensor provided in the moving body;
Information indicating the position and orientation of the moving body based on the GPS position information calculated by the GPS position calculation function and the sensor information acquired by the sensor information acquisition function, and their error covariance matrix A position error calculation function for calculating
An offset calculation function for calculating an error vector indicating an accumulation of errors and calculating information indicating the length of the calculated error vector as an offset;
Based on the offset calculated by the offset calculation function, an error covariance matrix correction function that corrects the error ellipse area obtained from the error covariance matrix calculated by the position error calculation function,
Using the information indicating the position and orientation of the moving body calculated by the position error calculation function and the error covariance matrix corrected by the error covariance matrix correction function, the road is included on the road included in the map data. A program that calculates a probability that a moving object exists and implements a map match processing function that estimates a position of the moving object on a road included in the map data based on the calculated probability.

Images