JP3693383B2 - Current position calculation system and current position calculation method - Google Patents

Description

[0001]
[Industrial application fields]
The present invention relates to a current position calculation system that is mounted on a moving body such as a vehicle, measures a traveling distance, a traveling direction, and the like of the moving body, and thereby calculates a current position of the moving body. Specifically, the present invention relates to a current position calculation system that calculates the current position of a moving object more accurately without increasing the processing time when it is determined that the moving object exists on a road.
[0002]
[Prior art]
Conventionally, in a current position calculation system for calculating the current position of a vehicle traveling on a road, the current position of the vehicle is measured by a vehicle traveling direction measured by an orientation sensor such as a gyro and a vehicle speed sensor or a distance sensor. It is calculated based on the travel distance of the vehicle.
[0003]
Further, the travel distance of the vehicle is generally obtained by measuring the output shaft of the transmission or the rotation speed of the tire and multiplying the rotation speed by a distance coefficient that is the distance the vehicle travels per rotation of the tire. It is demanded by.
[0004]
Further, as described in JP-A-61-56910, based on the current position obtained from the traveling direction and traveling distance of the vehicle, a predetermined line segment included in the map data stored in a CD-ROM or the like is obtained. A so-called map matching technique is known in which a perpendicular line is drawn from the current position obtained and the intersection point between the perpendicular line and the line segment is to be displayed on the display device. On the display device, a symbol indicating the current position of the vehicle matching the predetermined road can be represented.
[0005]
[Problems to be solved by the invention]
However, in the conventional map matching technique described above, the position corresponding to the vertical foot between the current position and the line segment obtained is estimated as the position of the vehicle to be displayed. An error occurs between the position of the vehicle after the vehicle travels and the position of the vehicle to be displayed, and this error is accumulated as the vehicle travels, and as a result, the position where the vehicle is actually traveling There is a problem that an error of about several hundred meters may occur between the calculated current position of the vehicle.
[0006]
It is an object of the present invention to provide a current position calculation system that calculates a current position of a vehicle more accurately with less processing time.
[0007]
[Means for solving the problems]
An object of the present invention is a current position calculation system that is mounted on a vehicle and calculates the current position of the vehicle, the direction detection means for detecting the traveling direction of the vehicle, and the distance calculation means for calculating the travel distance of the vehicle, Based on the relative displacement obtained based on the traveling direction and the travel distance and a candidate point indicating a state where the vehicle is located on any road or a state where the vehicle is not located on the road. A virtual current position generating means for generating a virtual current position expected to be a position, and a road on which the candidate point is located or a road connected thereto in relation to a candidate point indicating a state of being located on the road And a position calculating means for calculating a position advanced from the candidate point by a travel distance of the vehicle along the road searched by the road searching means, and the position calculating means It is achieved by the current position calculating system which is configured to the point corresponding to more obtained position as a new candidate point.
[0008]
In a preferred embodiment of the present invention, the road search means is a road on which the candidate point is located or a road connected thereto, and the azimuth difference between the direction and the traveling direction is within a predetermined range. Is configured to select a road.
[0009]
In a further preferred aspect of the present invention, the road search means is a road on which the candidate point is located or a road connected thereto, and a road whose distance from the virtual current position is within a predetermined range. Configured to select.
[0010]
In a further preferred embodiment of the present invention, a second road search means for searching for a road existing within a predetermined range from the candidate point in relation to the candidate point indicating a state not located on the road. A second position calculating means for calculating an intersection of a perpendicular to the road searched by the second road searching means from a virtual current position related to a candidate point indicating a state that does not exist on the road, The point corresponding to the position obtained by the second calculating means is configured as another new candidate point.
[0011]
In a further preferred embodiment of the present invention, further, a reliability calculation means for calculating reliability indicating the credibility that the new candidate point is a current position of a vehicle, and a reliability associated with the new candidate point. And determining means for determining the position of the candidate point having the highest reliability value as the current position of the vehicle.
[0012]
In a further preferred embodiment of the present invention, the road search means calculates the distance from the point where the candidate point is located to the end portion of the line segment where the vehicle is located in the traveling direction of the vehicle. And a connecting line segment search means for searching for a line segment connected to the end when the calculated distance is smaller than the traveling distance of the vehicle.
[0013]
[Action]
According to the present invention, a road where a candidate point is located and a road connected thereto are searched, and a new candidate point is generated on the searched road. The current position can be obtained.
[0014]
Further, according to the present invention, since the point corresponding to the position advanced from the candidate point by the travel distance of the vehicle is set as a new candidate point, the calculated position of the new candidate point and the actual position of the vehicle It is possible to reduce the error between and.
[0015]
According to a preferred embodiment of the present embodiment, a road whose azimuth difference between the azimuth and the traveling azimuth of the vehicle is within a predetermined range is selected, and a new candidate point is associated with the selected road. Therefore, it is possible to calculate the accurate current position of the vehicle without going through unnecessary processing.
[0016]
According to a preferred embodiment of the present embodiment, a road whose distance from the virtual current position is within a predetermined range is selected, and a new candidate point is obtained for the selected road. Without this, it is possible to calculate the accurate current position of the vehicle.
[0017]
【Example】
Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings.
[0018]
FIG. 1 is a block diagram showing a configuration of a current position calculation apparatus according to an embodiment of the present invention. As shown in FIG. 1, the current position calculation device 10 includes an angular velocity sensor 11 that detects a change in traveling azimuth by detecting a yaw rate of the vehicle, and a geomagnetic sensor 12 that detects a traveling azimuth of the vehicle by detecting geomagnetism. And a vehicle speed sensor 13 that outputs pulses at time intervals proportional to the rotation of the output shaft of the vehicle transmission.
[0019]
In addition, a display 17 that displays a map around the current position, a mark indicating the current position, and the like, a switch 14 that receives a command to switch the scale of the map displayed on the display 17 from the driver, and stores digital map data. A CD-ROM 15 is provided, and a driver 16 for reading map data from the CD-ROM 15 is provided. Moreover, the controller 18 which controls operation | movement of each peripheral apparatus shown above is provided. In the present embodiment, the digital map data described above includes road data composed of coordinates indicating the end portions of a plurality of line segments, road width data indicating the road width of the road, and roads that are expressways or general roads. An expressway flag indicating whether or not there is included.
[0020]
The controller 18 includes an A / D converter 19 that converts a signal (analog) of the angular velocity sensor 11 into a digital signal, an A / D converter 20 that converts a signal (analog) of the geomagnetic sensor 12 into a digital signal, and a vehicle speed sensor. A counter 26 that counts the number of pulses output every 13 seconds, a parallel I / O 21 that inputs whether or not the switch 14 is pressed, and a DMA that transfers map data read from the CD-ROM 15 ( (Direct Memory Access) controller 22 and display processor 23 for displaying a map image on display 17.
[0021]
The controller 18 further includes a microprocessor 24 and a memory 25. The microprocessor 24 outputs a signal from the angular velocity sensor 11 obtained through the A / D converter 19, a signal from the geomagnetic sensor 12 obtained through the A / D converter 20, and an output pulse from the vehicle speed sensor 13 counted by the counter 26. Number, whether or not the switch 14 is input via the parallel I / O 21, the map data from the CD-ROM 15 obtained via the DMA controller 22 is received, and processing based on these signals is performed. The current position is calculated and displayed on the display 17 via the display processor 23. As shown in FIG. 2, the vehicle position is displayed by superimposing an arrow mark or the like on a map already displayed on the display 17. Thereby, the user can know the current position of the vehicle on the map. The memory 25 includes a ROM that stores a program that defines the contents of processing (to be described later) for realizing such an operation, and a RAM that is used as a work area when the microprocessor 24 performs processing. Yes.
[0022]
Hereinafter, the operation of the current position calculation device 10 configured as described above will be described.
[0023]
The operation of the apparatus 10 generally includes processing for calculating the traveling direction and distance of the vehicle, processing for determining the current position of the vehicle from the calculated traveling direction and distance, and displaying the obtained vehicle position and direction. Since these can be divided into three processes, i.e., the process to be performed, these will be described in turn.
[0024]
FIG. 3 illustrates a flow of processing for calculating the traveling direction and traveling distance of the vehicle.
[0025]
This process is a routine of the microprocessor 24 that is activated and executed at a constant cycle, for example, every 100 ms.
[0026]
In this routine, first, the output value of the angular velocity sensor 11 is read from the A / D converter 19 (step 301). Since an azimuth change is output as the output value of the angular velocity sensor 11, only a relative value in the traveling direction of the vehicle can be detected. For this reason, next, the output value of the geomagnetic sensor 12 is read from the A / D converter 20 (step 302), the absolute azimuth calculated from the output value of the geomagnetic sensor 12 and the azimuth change (angular velocity) output from the gyro 11 Output) is used to determine the estimated direction of the vehicle (step 303).
[0027]
For example, when the vehicle speed is low for a long time, the angular velocity sensor has a large error. Therefore, when the vehicle speed is low for a certain time or more, the direction is determined by using only the geomagnetic sensor direction.
[0028]
Next, the number of pulses output from the vehicle speed sensor 13 is counted by the counter 26 every 0.1 second, and the counted value is read (step 304). By multiplying this read value by a distance coefficient, a distance advanced in 0.1 seconds is obtained (step 305).
[0029]
Next, the travel distance value per 0.1 second thus determined is added to the previously obtained value to check whether the travel distance of the vehicle has reached 20 m (step 306). If not satisfied (No in step 306), the current process is terminated and a new process is started.
[0030]
As a result of the travel distance calculation process, when the accumulated travel distance becomes a certain distance, for example, 20 m (Yes in Step 306), the travel direction and the travel distance R (20 m in this embodiment) at that time Are output (step 307). In step 307, the accumulated distance is further initialized, and the accumulated travel distance is newly started.
[0031]
Next, a process of calculating a virtual current position of the vehicle based on the calculated traveling direction and traveling distance and obtaining a candidate point of the vehicle based on the calculated virtual current position will be described.
[0032]
FIG. 4 shows the flow of this process.
[0033]
This process is a routine of the microprocessor 24 that is activated and executed in response to the output of the traveling direction and traveling distance from FIG. That is, in this embodiment, this process is started every time the vehicle travels 20 meters.
[0034]
In this process, first, the travel direction and travel distance output in step 407 are read (step 401). Next, based on these values, the movement amount of the vehicle is obtained separately in the latitude and longitude directions. Furthermore, the movement amount in each direction is added to the position of the candidate point of the vehicle obtained in the process for obtaining the candidate point of the previous vehicle, and the virtual current position (the position where the current vehicle is estimated to exist) A) is obtained (step 402).
[0035]
If there is no candidate point obtained by the process of obtaining the previous candidate point of the vehicle, such as immediately after the start of the device, the virtual current position is set using the position set separately as the position of the previously obtained candidate point. The position (A) is obtained.
[0036]
Here, explanation is added for each candidate point. In an initial state such as immediately after the start of the device, the virtual current position (A) is uniquely determined by the user (driver) inputting predetermined information using the switch 14 and the like. Is located on the line corresponding to the road. However, after the vehicle travels, the virtual current position (A) may not exist on the line segment corresponding to the road due to an error of a direction sensor such as a gyro. As a result, for example, as shown in FIG. 5, when the road is branched, that is, when two line segments 64 and 65 appear from the node or end 68 of the line segment 61 corresponding to the road, In many cases, it cannot be clarified whether a vehicle exists on the road corresponding to the line segment.
[0037]
Therefore, in such a case, in this embodiment, a point existing on a line segment (two line segments in the example shown in FIG. 5) obtained by the processing described later is selected as a candidate point under a predetermined condition. The current position, error cost, accumulated error cost, which will be described later, and the like are each stored in a predetermined area of the RAM of the memory 25. For ease of explanation, in the following explanation, one or more new candidate points are generated from a single candidate point unless it is clearly indicated that there are a plurality of candidate points.
[0038]
Next, the first road search process for matching with the road is executed only for the candidate points in the matching state obtained by the process for obtaining the candidate points of the previous vehicle (step 403). In this embodiment, the matching state means that a distance from the virtual current position or an azimuth difference between the direction and the traveling direction of the vehicle is within a predetermined range in relation to a certain virtual current position. This means a state in which a candidate point is obtained at a predetermined position on the road based on the virtual current position. On the other hand, in relation to a certain virtual current position, there may be a case where there is no road where the above-described distance and direction difference are within a predetermined range. In this embodiment, in such a case, the point itself corresponding to the virtual current position A is a candidate point, and the candidate point obtained in this way is referred to as a free state candidate point.
[0039]
6 and 7 are flowcharts showing in detail the first road search process according to the present embodiment.
[0040]
As shown in FIG. 6, in this road search process, first, predetermined road data is read from the CD-ROM 15 via the driver 16 and the DMA controller 23 (step 601). In this embodiment, as road data, as shown in FIG. 8, a plurality of line segments 51 to 55 connecting two points are approximated, and those line segments are expressed by coordinates of the start point and the end point. Used. For example, the line segment 53 is represented by its start point (x3, y3) and end point (x4, y4). Therefore, in this step 601, the line segment where the candidate point of the matching state obtained by the previous process is located is selected, and data relating to this is read out.
[0041]
Next, based on the coordinates of the line segment selected in step 601, it is determined whether or not the length from the position of the candidate point to the end of the line segment in the traveling direction is equal to or less than the length corresponding to 20 m. (Step 602). If it is determined No in step 602, the line segment is temporarily stored as a primary candidate line segment (step 603). On the other hand, when it is determined Yes in step 602, it is determined whether or not there is a line segment connected to the end portion of the vehicle in the traveling direction of the line segment (step 604). ).
[0042]
If another line segment is connected to the end of the line segment, the connected line segment is temporarily stored as a primary candidate line segment (step 605). Here, when a plurality of line segments are connected to the end portions of the line segments, each of the plurality of line segments is temporarily stored as a primary candidate line segment. On the other hand, if it is determined No in step 604, the process proceeds to step 606.
[0043]
Next, an angle comparison process is performed in which the angle between the traveling direction of the vehicle and the direction of the line segment is compared (step 607). FIG. 9 is a flowchart illustrating the angle comparison process according to the present embodiment. As shown in FIG. 9, in this process, an azimuth difference θd between the traveling azimuth of the vehicle and the azimuth of the line segment is calculated (step 901). Is also larger (step 902). In this embodiment, the threshold value θth is set to 30 °. When it is determined NO in step 902, the process is terminated. On the other hand, when it is determined YES, it is determined that the road is not a primary candidate line segment. The temporarily stored data corresponding to the primary candidate line segment is deleted (step 903).
[0044]
When the angle comparison process shown in step 607 of FIG. 6 is completed, if a plurality of primary candidate line segments are stored in step 608, the angle comparison process is also performed for the other primary candidate line segments (steps 607 and 608). ) When the angle comparison process is executed for all the primary candidate line segments, the process proceeds to step 609. In this way, the azimuth difference between the azimuth and the vehicle azimuth among the line segment where the candidate point obtained in the previous process is located or connected to this is less than the threshold θth. Are stored as primary candidate line segments.
[0045]
Step 609 determines whether there is at least one primary candidate line segment temporarily stored. If it is determined NO in step 609, the process proceeds to step 606. On the other hand, if it is determined as Yes in step 609, the distance R traveled by the vehicle along the line segment 1 or 2 from the candidate point obtained in the previous process, in this embodiment, It is determined whether or not the position advanced by a length corresponding to 20 m is not on the primary candidate line segment (step 610). For example, regarding the primary candidate line segment obtained as a result of being determined as No (No) at Step 602 described above, it is determined as No (No) at Step 610. In addition, even if it is a primary candidate line segment obtained when it is determined to be Yes in Step 602 described above, the line segment where this point is located is determined from the position of the candidate point obtained in the previous process. If the sum of the length to the end and the length of the obtained primary candidate line segment is greater than the distance R, that is, the length corresponding to 20 m in this embodiment, in this step 610, no ( No). In other words, if it is determined yes in step 610, the length from the position of the candidate point obtained in the previous process to the end of the line segment where this point is located is obtained. The sum of the lengths of the primary candidate line segments is limited to a case where the sum is not less than the distance R, that is, the length corresponding to 20 m in this embodiment.
[0046]
If it is determined NO in step 610, a perpendicular distance comparison process is executed (step 611). FIG. 10 is a flowchart showing the perpendicular distance comparison processing according to the present embodiment. As shown in FIG. 10, in this process, a perpendicular line is drawn from the virtual current position toward the primary candidate line segment, and the length L of this perpendicular line is obtained (step 1001). Next, it is determined whether or not the length L of the perpendicular is equal to or less than a predetermined threshold value Lth (step 1002). That is, it is determined whether or not the primary candidate line segment exists within the range of the threshold value Lth from the virtual current position. If it is determined as YES in step 1002, the primary candidate line segment is stored as a secondary candidate line segment, and from a candidate point obtained in the previous process, along a predetermined line segment, Coordinate data of positions separated by the distance R is stored as position data corresponding to the secondary candidate line segment.
[0047]
On the other hand, if it is determined NO in step 1002, the primary candidate line segment is deleted, and as a result, no secondary candidate line segment is generated (step 1004).
[0048]
Thus, when the perpendicular distance comparison process 611 is completed, it is determined whether or not the process has been completed for all primary candidate line segments (step 612).
[0049]
On the other hand, if it is determined as Yes in step 610 described above, it is further determined whether or not there is a line segment connected to the end portion of the vehicle in the traveling direction of the primary candidate line segment. (Step 613). If it is determined yes in this step, that is, if it is determined that there is no connected line segment, this primary candidate line segment is deleted, and as a result, a secondary candidate line segment is generated. Not (step 614). Proceed to step 612. On the other hand, if it is determined NO in step 613, the connected line segment is temporarily stored as a primary candidate line segment (step 615), and angle comparison processing is further executed. (Step 616).
[0050]
By repeating the processing from step 610 to step 616, a predetermined segment is obtained from the line segment where the candidate point obtained by the previous process is located along the line segment or a line segment connected thereto in the traveling direction of the vehicle. There is a point separated by a distance R, the azimuth difference between the azimuth and the traveling azimuth of the vehicle is equal to or smaller than a predetermined threshold value θth, and the distance from the virtual current position is a predetermined value. A secondary candidate line segment that is equal to or less than the threshold value Lth can be obtained. Furthermore, as position data corresponding to the obtained secondary line segment, coordinate data of a position separated by a distance R along a predetermined line segment can be obtained from the candidate point obtained in the previous process. This coordinate data corresponds to data indicating candidate points to be described later.
[0051]
When the processing from step 610 to step 616 is completed for the primary candidate line segment, it is determined whether or not the secondary candidate line segment is obtained as a result of these steps (step 617). In step 617, if it is determined no (No), the process proceeds to step 606. On the other hand, if it is determined yes (Yes), the process ends.
[0052]
For example, in FIG. 5, it is assumed that the virtual current position A is represented at a position indicated by a point 63 with respect to a certain candidate point 62 existing on the line segment 61. In such a case, first, the line segment 61 where the candidate point 62 is located is selected (step 601), and the distance from the candidate point 62 on this line segment to the end portion 68 in the traveling direction of the vehicle is determined ( Step 602). In the case of the example of FIG. 5, since there is a road connected to this line segment, each of the line segments 64 and 65 is temporarily stored as a primary candidate line segment, and an angle comparison is performed in relation to each. Processing (step 607) is executed. Here, in the example of FIG. 5, assuming that | θ (1) −θcar | <θth and | θ (2) −θcar | <θth, two processing loops consisting of steps 610, 611 and 612 are obtained. The perpendicular distance comparison process (step 611) is executed in association with each of the line segments 64 and 65.
[0053]
Furthermore, in the example of FIG. 5, assuming that L (1) <Lth and L (2) <Lth, the line segments 64 and 65 are stored as secondary candidate line segments. Points advanced by a length corresponding to the travel distance R (20 m in this embodiment) along the line segments 61 and 64 or the line segments 61 and 65 are set as candidate points 66 and 67. Coordinate data, data corresponding to the distances L (1) and L (2) from the virtual current position to the line segment, and data corresponding to the angles θ (1) and θ (2) of the line segment are stored.
[0054]
Further, as shown in FIG. 11, a new virtual current position A is represented at the position indicated by a point 71 with respect to the candidate point 66 on the line segment 64, while the candidate point 67 on the line segment 65. On the other hand, it is assumed that the new current position A ′ is represented at the position indicated by the point 72. Also in this case, the line segments 73 and 74 are temporarily stored as the primary candidate line segments in relation to the candidate point 66, and the line segment 75 is related to the candidate point 67 in the primary candidate line segment. As temporarily stored. Further, in relation to the primary candidate line segment, an angle comparison process, and if a certain condition is satisfied, a perpendicular distance comparison process is further executed. In the example shown in FIG. 11, if the line segments 73 to 75 can be secondary candidate line segments, as shown in FIG. 12, from the candidate point 66, the line segments 64 and 73, or the line segment 64. , 74, or from the candidate point 67, along the line segments 65 and 75, a point advanced by a length corresponding to the travel distance R of the vehicle (20 m in this embodiment) is a candidate point 81, 82 and 83, the coordinate data are stored, and the distances L1 (1) and L1 (2) from the virtual current position A to the line segments 73 and 74 corresponding to these candidate points are set. , Data corresponding to the distance L2 (1) from the virtual current position A ′ to the line segment 75, and data corresponding to the angles θ1 (1) and θ1 (2) of the line segments 73 and 74 , And the angle θ2 of the line segment 75 ( Data corresponding to 1) is stored.
[0055]
Again, the process returns to the first road search process of FIGS. 6 and 7. Here, when it is determined as No (No) in Step 604, that is, there is no line segment connected to the end of the vehicle in the traveling direction of the line segment where the candidate point obtained in the previous process is located. If no primary candidate line segment is obtained in steps 607 to 608 (No in step 609), or if no secondary candidate line segment is obtained in steps 610 to 616 (step If no (No) at 617, the coordinate data indicating the virtual current position becomes data indicating the candidate point.
[0056]
When the first road search process in step 403 is completed in this way, the second road search process for matching with the road for the free candidate point obtained by the process for obtaining the previous candidate point of the vehicle. Is executed (step 404). FIG. 13 is a flowchart illustrating the second road search process according to the present embodiment.
[0057]
As shown in FIG. 13, this process (steps 1301 to 1304) is similar to the first road search process (step 403 in FIG. 4) for the candidate points in the matching state shown in FIG. The difference between these two processes is as follows. That is, in the first road search process, the line segment where the candidate point obtained by the process for obtaining the candidate point of the previous vehicle is located or the line segment connected thereto is temporarily used as the primary candidate line segment. From these primary candidate line segments, further, the direction difference between the direction and the vehicle direction is equal to or smaller than a predetermined threshold value θth, and the length corresponding to the distance from the virtual current position is a predetermined threshold. A line segment within the value Lth is selected as a secondary candidate line segment. On the other hand, in the second road search process, all the line segments within the preset distance D centered on the virtual current position (A) are extracted (step 1301), and the direction is determined from these line segments. A line segment whose azimuth difference between the vehicle direction and the vehicle direction is equal to or smaller than a predetermined threshold value θth is selected (step 1302). A line segment whose length is within a predetermined threshold Lth is selected (step 1303). In other words, in the first road selection process, the line segment where the candidate point is located, or some line segments extending from the branch point of the line segment are selected, but step 1302 in FIG. In this process, a line segment to be extracted is determined from road data in the map corresponding to the read map data. Therefore, the processing time for executing steps 1301 to 1303 depends on the density of the road corresponding to the road data, and particularly when the map corresponding to the read map data is an urban area. In the case where the density is high, this processing time is longer than the time required to execute the corresponding processing in the first road search processing.
[0058]
In this second road search process, when step 1303 ends, the line segment extracted in steps 1302 and 1303, that is, the azimuth difference between the azimuth and the vehicle azimuth is equal to or less than the threshold value θth. In addition, a line segment whose length corresponding to the distance from the virtual current position is equal to or less than the threshold value Lth is selected as the secondary candidate line segment, and the vertical leg extending from the virtual current position to the line segment, that is, The intersection between the perpendicular and the line segment is set as a candidate point, and the coordinate data is stored (step 1304). Corresponding to the set candidate points, data indicating the distance from the virtual current position to the line segment and data indicating the azimuth difference between the line segment direction and the vehicle direction are stored. Note that, in the second road search process, the intersection of the perpendicular line and the line segment taken down from the virtual current position is a candidate point. This is because the candidate point obtained by the previous process is in a free state. This is because there is no line segment to follow.
[0059]
In the second road search process, when there is no line segment within the predetermined range D from the virtual current position, there is a line segment whose azimuth difference between the direction and the traveling direction of the vehicle is equal to or smaller than the threshold value θth. Otherwise, or when there is no line segment whose length corresponding to the distance from the virtual current position is equal to or less than the threshold value Lth, the point corresponding to the virtual current position is a candidate point.
[0060]
As described above, when the second road search process in step 404 is finished, the length L (i of the perpendicular line related to the n candidate points obtained by the first road search process and the second road search process is obtained. ) (1 ≦ i ≦ n) and an orientation difference θd (i) between the orientation of the secondary candidate line segment where the candidate point is located and the orientation of the vehicle, the error cost ec defined by the following equation: (I) is calculated (step 404).
[0061]
ec (i) = α × | θd (i) | + β × L (i)
Here, α and β are weighting factors. The values of these weighting factors should be used to select a road between a deviation between the traveling direction of the vehicle and the direction of the line segment, or a deviation between the virtual current position and the line segment. Therefore, it is determined. For example, when it is important to select a traveling direction and a road having a similar direction and direction, α is increased.
[0062]
Next, the accumulated error cost in the current process defined by the following equation according to the calculated error cost ec (i) and the accumulated error cost es related to the candidate point obtained in the previous process. es is calculated (step 405).
[0063]
es (i) = (1-k) × es + k × ec (i)
Here, k is a weight coefficient larger than 0 and smaller than 1. This accumulated error cost es (i) represents how much the error cost calculated in the previous process is reflected in the error cost calculated in the current process.
[0064]
Further, based on the calculated accumulated error cost es (i), the reliability trst (i) defined in the following equation is calculated (step 406).
[0065]
trst (i) = 100 / (1 + es (i))
As is clear from the above equation, as the accumulated error cost ec (i) increases, the reliability trst (i) decreases and approaches 0 (zero). On the other hand, as it decreases, the reliability trst (i) increases and its value approaches 100.
[0066]
By performing such processing, the reliability trst (i) related to the n secondary candidate line segments obtained corresponding to a certain candidate point is obtained. When there are a plurality of candidate points, the reliability trst related to the plurality of secondary candidate line segments obtained corresponding to each candidate point may be calculated.
[0067]
In relation to the candidate point in the free state, the error cost ec (n) to be calculated in step 405 is given a constant value larger than the error cost value obtained in the matching state.
[0068]
Next, the new candidate points are sorted according to the value of the reliability trst corresponding to each of the candidate points thus obtained (step 408), and the candidate point C having the highest reliability value is displayed as a display candidate. The point CD, that is, as a candidate point to be displayed on the display 17, its position, accumulated error cost, reliability, status flag indicating whether it is in a matching state or a free state, and the like are stored in the RAM of the memory 25. In addition to storing in a predetermined area, the positions of candidate points other than the display candidate points, accumulated error cost, reliability, status flag, and the like are also stored in a predetermined area of the RAM (step 409). In this embodiment, data related to seven candidate points can be stored. Accordingly, when eight or more candidate points are calculated as a result of executing the processing of steps 401 to 408 in FIG. 4, among these, they are related to the seven candidate points in descending order of the reliability trst value. Various data are stored in a predetermined area of the RAM of the memory 25.
[0069]
Finally, data related to the display candidate point is output (step 410), and the process ends.
[0070]
The display candidate points obtained by executing steps 401 to 410 in FIG. 4 are displayed on the screen of the display 17 by processing based on the flowchart shown in FIG.
[0071]
This process is a routine of the microprocessor 24 that is activated and executed every second.
[0072]
First, it is determined by checking the contents of the parallel I / O 21 whether the switch 14 is instructed to change the scale of the map by pressing (step 1401). If it is pressed (Yes in Step 1401), a predetermined scale flag is set correspondingly (Step 1402).
[0073]
Next, data indicating the position and orientation of the display candidate point obtained by executing the processing of FIG. 4 is read from a predetermined area of the RAM of the memory 25 (step 1403), and the scale flag switched in step 1402 A map of a scale corresponding to the contents of is displayed on the display 17 in a state as shown in FIG. 2, for example (step 1404).
[0074]
Then, the position of the display candidate point and its orientation are displayed using the arrow symbol “↑” as shown in FIG. 2, for example, as shown in FIG. 2 (step 1405). Finally, a north mark indicating north and a distance mark corresponding to the scale are displayed as shown in FIG. 2 so as to be superimposed on these (step 1406).
[0075]
In the present embodiment, the vehicle position and direction are indicated by using the arrow symbols as described above. However, in the display form of the vehicle position and direction, the display state is clearly shown for the position and the traveling direction. If it exists, the form may be arbitrary. The same applies to the north mark and the like.
[0076]
According to this embodiment, in order to search for a line segment where the candidate point is located or connected to the virtual current position obtained from the matching candidate point, the virtual current position is searched. As compared with the case of searching for all roads existing in the vicinity, the desired line segment can be searched in a shorter time.
[0077]
Further, according to the present embodiment, only the length corresponding to the traveling distance of the vehicle along the line segment where the candidate point in the matching state is located and / or the line segment connected to the line segment where the candidate point is located. Since the position advanced from the candidate point is set as the candidate point, the error between the calculated current position of the vehicle and the actual current position of the vehicle can be further reduced.
[0078]
The present invention is not limited to the above-described embodiments, and various modifications are possible within the scope of the invention described in the claims, and these are also included in the scope of the present invention. Needless to say.
[0079]
For example, in the above-described embodiment, in the first road search process, the line segment where the candidate point in the matching state is located or the line segment connected thereto is temporarily stored as the primary candidate line segment, and the angle comparison is performed on this. If the primary candidate line segment whose azimuth difference is within the predetermined range is less than the predetermined length by the angle comparison process, the line segment connected to this is further searched. Yes. This is because, in the present embodiment, the first road search process is executed every 20 m of the vehicle travel distance, and therefore on the road corresponding to the line segment where the candidate point is located and the line segment connected thereto. In addition, the credibility that the vehicle is actually located is high.
[0080]
However, the present invention is not limited to such an algorithm. That is, from the line segment where the candidate point is located, first, all line segments that include points advanced along the line segment by the distance traveled by the vehicle are searched, and the angle comparison process is performed for all the searched line segments. It may be configured to execute.
[0081]
In addition, the parameters used in this embodiment are merely given for illustration, and therefore these parameters are the processing speed of the device, the type of road on which the vehicle is traveling (for example, general road, highway) It should be understood that the change can be made according to the above.
[0082]
Furthermore, in this specification, the means does not necessarily mean a physical means, but includes cases where the functions of the means are realized by software. Further, the function of one means may be realized by two or more physical means, or the functions of two or more means may be realized by one physical means.
[0083]
【The invention's effect】
According to the present invention, it is possible to provide a current position calculation system that calculates a current position of a vehicle more accurately with less processing time.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of a current position calculation apparatus according to an embodiment of the present invention.
FIG. 2 is a diagram illustrating a display example of a map and a current position according to the present embodiment.
FIG. 3 is a flowchart illustrating a process of calculating a traveling direction and a traveling distance of the vehicle according to the present embodiment.
FIG. 4 is a flowchart illustrating a process for calculating candidate points for a vehicle according to the embodiment.
FIG. 5 is a diagram for explaining a line segment corresponding to a road, a virtual current position, and candidate points;
FIG. 6 is a flowchart illustrating a first road search process according to the embodiment.
FIG. 7 is a flowchart illustrating a first road search process according to the embodiment.
FIG. 8 is a diagram for explaining an example of road data according to the embodiment.
FIG. 9 is a flowchart illustrating an angle comparison process according to the present embodiment.
FIG. 10 is a flowchart illustrating normal distance comparison processing according to the embodiment.
FIG. 11 is a diagram for explaining a line segment corresponding to a road, a virtual current position, and candidate points;
FIG. 12 is a diagram for explaining a line segment corresponding to a road, a virtual current position, and candidate points;
FIG. 13 is a flowchart of a second road search process according to the embodiment.
FIG. 14 is a flowchart illustrating a current position display process according to the embodiment.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 10 Present position calculation apparatus 11 Angular velocity sensor 12 Direction sensor 13 Vehicle speed sensor 14 Switch 15 CD-ROM
16 CD-ROM reading driver 17 Display 18 Controller

Claims (5)

A current position calculation system that is mounted on a vehicle and calculates a current position of the vehicle,
Storage means for storing a road map comprising a plurality of line segments;
Direction detecting means for detecting the traveling direction of the vehicle;
Distance measuring means for measuring the travel distance of the vehicle;
A current position calculating means for calculating the current position of the vehicle,
The current position calculating means includes
For each candidate point calculated last time, the virtual current position of the vehicle is calculated based on the candidate point and the relative displacement after the candidate point obtained from the traveling direction and the travel distance is calculated. Current position calculating means;
For each of the previously calculated candidate points, the line segment where the candidate point is located is read from the storage means, and from the previously calculated candidate point to the end of the read line segment on the traveling direction side of the vehicle If the remaining distance is less than the travel distance that is the travel distance of the vehicle measured by the distance measuring means since the candidate point calculated last time is calculated, the line segment connected to the read line segment is Road selection means for searching from the storage means and selecting the searched line segment as a candidate line segment;
For each of the previously calculated candidate points, when the remaining distance is less than the travel distance, along the line segment where the previously calculated candidate point is located and the candidate line segment selected by the road selection means, A position advanced by the traveling distance from the previously calculated candidate point is calculated as a current candidate point, and when the remaining distance is equal to or longer than the traveling distance, along the line segment where the previously calculated candidate point is located A candidate point calculating means for calculating a position advanced by the travel distance from the previously calculated candidate point as a current candidate point;
The distance from the virtual current position of the vehicle calculated by the virtual current position calculating means to the line segment where the current candidate point calculated by the candidate point calculating means is located, and the traveling direction of the vehicle and the current candidate A reliability calculation means for calculating the reliability of the candidate point of this time, based on an azimuth difference with the azimuth of the line segment where the point is located;
A current position calculation system comprising: current position determination means for determining the current position of the vehicle from the current candidate points based on the reliability calculated by the reliability calculation means. The road selection means is
Among the searched line segments, select a line segment whose azimuth difference with the traveling direction of the vehicle at the virtual current position is within a predetermined range as the candidate line segment,
The candidate point calculation means includes:
When the remaining distance is less than the travel distance, the travel distance from the previously calculated candidate point along the line segment where the previously calculated candidate point is located and the candidate line segment selected by the road selection unit Calculated as the candidate point for this time,
When the remaining distance is equal to or longer than the traveling distance, and the difference between the azimuth of the line segment where the previously calculated candidate point is located and the traveling azimuth of the virtual current position is within a predetermined range The method according to claim 1, wherein a position advanced by the travel distance from the previously calculated candidate point along the line segment where the previously calculated candidate point is located is calculated as the current candidate point. The current position calculation system described. The road selection means is
Among the searched line segments, select a line segment within a predetermined distance from the virtual current position as the candidate line segment,
The candidate point calculation means includes:
When the remaining distance is less than the travel distance, the travel distance from the previously calculated candidate point along the line segment where the previously calculated candidate point is located and the candidate line segment selected by the road selection unit Calculated as the candidate point for this time,
If the remaining distance is greater than or equal to the travel distance, and the line segment where the previously calculated candidate point is located is within a predetermined distance from the virtual current position, the previously calculated candidate point is 3. The current position calculation system according to claim 1, wherein a position advanced by the travel distance from the previously calculated candidate point along the line segment is calculated as a current candidate point. 4. The candidate point calculation means includes:
If the remaining distance is less than the travel distance and a line segment connected to the line segment where the previously calculated candidate point is located does not exist in the storage unit, the virtual current position is determined as the current candidate. Further calculate as free candidate points,
When the previously calculated candidate point is the free candidate point, a line segment existing within a predetermined distance from the virtual current position calculated based on the free candidate point is searched, and the free candidate The current position calculation system according to any one of claims 1 to 3, wherein an intersection of a perpendicular calculated from a virtual current position to the searched line segment is calculated as a candidate point for this time. A current position calculation method in a current position calculation system that is mounted on a vehicle and calculates a current position of the vehicle,
The current position calculation system
A storage step of storing a road map comprising a plurality of line segments in the storage means;
A direction detecting step for detecting a traveling direction of the vehicle;
A distance measuring step for measuring a travel distance of the vehicle;
Performing a current position calculating step for calculating a current position of the vehicle;
The current position calculating step includes:
For each candidate point calculated last time, the virtual current position of the vehicle is calculated based on the candidate point and the relative displacement after the candidate point obtained from the traveling direction and the travel distance is calculated. A current position calculating step;
For each of the previously calculated candidate points, the line segment where the candidate point is located is read from the storage means, and from the previously calculated candidate point to the end of the read line segment on the traveling direction side of the vehicle If the remaining distance is less than the travel distance that is the travel distance of the vehicle measured in the distance measurement step after the previously calculated candidate point is calculated, the line segment connected to the read line segment is A road selection step of searching from the storage means and selecting the searched line segment as a candidate line segment;
For each of the previously calculated candidate points, when the remaining distance is less than the travel distance, along the line segment where the previously calculated candidate point is located and the candidate line segment selected in the road selection step, A position advanced by the travel distance from the previously calculated candidate point is calculated as the current candidate point, and when the remaining distance is greater than or equal to the travel distance, along the line segment where the previously calculated candidate point is located A candidate point calculation step of calculating a position advanced by the travel distance from the previously calculated candidate point as a current candidate point;
Before Symbol distance from the virtual current position of the vehicle calculated in the virtual current position calculation step to the segment candidate points of the current calculated it is positioned at said candidate point calculating step, and traveling direction and the current candidate points of the vehicle A reliability calculation step of calculating the reliability of the candidate point of this time, based on the azimuth difference with the direction of the line segment where
And a current position determining step of determining a current position of the vehicle from the current candidate points based on the reliability calculated in the reliability calculating step.

Images