JP5821275B2 - Moving body position detection device - Google Patents

Description

The present invention relates to a moving body position detection device.

In a navigation system using a GPS (Global Positioning System), as shown in Patent Document 1, in order to display a host vehicle on a road on a map in a display device in consideration of a GPS positioning error, etc. Map matching is performed by road shape.

By the way, recently, it has been considered that the navigation system and the traveling control of the moving body are associated with each other to improve driving support of the moving body. For this reason, map matching is performed on the position information of the moving body. However, the accuracy of the error number m is not sufficient, and more accurate position information is required.

Patent Document 2 proposes a method in which a detection unit detects a relative position of itself relative to an external fixed object whose absolute position is known, and corrects its position based on GPS detection based on the detection result. Has been.

Japanese Patent Laid-Open No. 6-102052 JP 2007-218848 A

By the way, the position information of the moving body is generally indicated by a position on a plan view of the moving body as viewed from above, that is, an orthogonal coordinate system. On the other hand, in a vehicle as a moving body, for example, the distance and azimuth to a fixed object existing around the road are detected by a radar or a camera, and based on the position information on the map data of the fixed object, It is possible to accurately detect the current position and direction.

However, if the traveling direction of the vehicle as the moving body is shifted in the left-right direction with respect to the road, that is, if the direction of the vehicle and the direction of the road are different, for example, the current position at the center position of the vehicle is Even if they are the same, the distance and direction to a fixed object (a specific position) around the road will be different. This causes an error in the current position of the vehicle obtained by map matching using map data in accordance with the difference in vehicle orientation. Therefore, in order to accurately detect the current position by map matching, map matching must be performed in consideration of errors in the traveling direction in addition to errors in the X-axis direction and the Y-axis direction in the orthogonal coordinate system. The calculation process is a heavy burden on the control system.

The present invention has been made in view of the above circumstances, and its purpose is to make it possible to easily and accurately determine the current position of a moving body by map matching while taking the traveling direction of the vehicle into account. The object is to provide a body position detecting device.

In order to achieve the above object, the following solution is adopted in the present invention. That is, as described in claim 1 in the claims,
Object detection means for detecting surrounding objects on the moving body is provided,
A moving body position detecting device that measures the current position of a moving body by matching three-dimensional map data including position information of a fixed object with detection data detected by the object detection means,
Prior to positioning of the current position of the moving body, the traveling direction of the moving body is corrected by matching the detection data of a predetermined fixed object around the road detected by the object detection means and map data,
Based on the traveling direction of the mobile object after correction, matching for positioning of the current position of the mobile object is performed.
It is like that. According to the above solution, since the moving direction of the moving body is corrected in advance, map matching processing for subsequent positioning of the current position can be performed easily and accurately.

A preferred mode based on the above solution is as described in claim 2 and the following claims. That is,
The correction of the moving direction of the moving body is performed on condition that a predetermined condition set in advance is satisfied (corresponding to claim 2). In this case, it is preferable to reduce the load on the control system by correcting the traveling direction unnecessarily as much as possible.

  The traveling direction of the moving body is corrected by comparing the line segment direction of the map data and the line segment direction of the detection data in a predetermined area (corresponding to claim 3). In this case, since the line segment part, that is, a part extending long, such as a guard rail, is compared, the correction of the traveling direction itself can be performed with high accuracy, and the current position can be determined with high accuracy.

  The predetermined area is a front area of the moving body (corresponding to claim 4). In this case, since it is common to detect objects (especially obstacles) in the front area of the moving body with a radar or a camera, the detection means for detecting the objects in the front area is effectively used. Thus, the traveling direction can be corrected.

The predetermined area is an area of map data used for positioning the current position last time,
Matching is performed to correct the traveling direction of the moving object using the data matched for the previous current position measurement.
(Corresponding to claim 5). In this case, it is possible to accurately correct the traveling direction while minimizing the amount of data used.

  The predetermined fixed object for correcting the traveling direction of the moving body is a parallel object parallel to the road lane on which the moving body is moving (corresponding to claim 6). In this case, the traveling direction can be accurately corrected, and there are many fixed objects parallel to the road, which is preferable in securing as many opportunities for correcting the traveling direction as possible.

  Among the parallel objects, those within the range in which the distance and direction to the parallel object can be detected by the object detection means are used for correcting the traveling direction (corresponding to claim 7). In this case, since the distance and direction are detected in a state limited to a certain length range among the parallel objects, that is, data such as infinite extension of the parallel object is not used, so the process for correcting the traveling direction is not performed. It becomes simple, and it is possible to correct the traveling direction using a parallel object even when moving along a curve.

  Matching for positioning of the current position of the moving object is performed using the parallel object (corresponding to claim 8). In this case, since there are many fixed objects along the road, it is preferable to secure as many opportunities as possible to determine the current position.

  Using the same detection data, positioning of the current position of the moving body and correction of the traveling direction of the moving body are performed (corresponding to claim 9). In this case, the current position is measured using the data used for the correction of the traveling direction while reducing the burden on the control system by reducing the amount of data to be used as much as possible. This is also preferable.

  The fixed object is at least one of a guard rail, a lane line that divides a lane, and a building (corresponding to claim 10). In this case, it is preferable to effectively use the lane lines, guardrails, and buildings existing along the road to increase the opportunity to correct the traveling direction and subsequently determine the current position.

  The mobile body is an automobile (corresponding to claim 11). In this case, it is preferable for various driving support and the like to accurately measure the current position of an automobile that has extremely high GPS utilization and requires safety such as collision avoidance.

ADVANTAGE OF THE INVENTION According to this invention, positioning of the present position of the moving body by map matching can be performed simply and accurately.

1 is a simplified plan view of a vehicle to which the present invention is applied. The figure which shows the example of a control system of this invention in a block diagram. The top view which shows the example of the fixed object in the periphery of the road where the vehicle is drive | working. The figure which shows an example of the detection data detected by the object detection sensor which mounted the fixed object corresponding to FIG. The figure which shows the state on the map data of the fixed object corresponding to FIG. An error is given to map matching due to a deviation in the direction of travel of the vehicle. The figure which shows the state by which the shift | offset | difference of the advancing direction of a vehicle is correct | amended and map matching will be performed accurately. The top view for demonstrating the deviation correction of the advancing direction of a vehicle using the fixed object extended in parallel along a road. The flowchart which shows the example of control of this invention. The flowchart which shows the example of control of this invention. The figure which shows an example of map matching.

In FIG. 1, V is a vehicle (automobile in the embodiment) as a moving body. The vehicle V is equipped with object detection sensors 1 composed of at least one of a radar (for example, a laser radar) or a camera at each of the four left and right corners. The two front object detection sensors 1 detect the azimuth and relative distance of surrounding objects from the front side of the vehicle to the front, and the two rear object detection sensors 1 from the rear side of the vehicle. This is to detect the azimuth and distance of the object across the back. The vehicle V is equipped with a wheel speed sensor 2 that detects the speeds of front, rear, left and right wheels. The average value of the wheel speeds detected by each wheel speed sensor 2 is the vehicle speed. The vehicle V has a navigation device, the GPS (GPS sensor) of the navigation device is indicated by reference numeral 3, the map database is indicated by reference numeral 4, and the display screen is indicated by reference numeral 5. Under the control of the controller U, the current position of the vehicle V detected by the GPS 3 is displayed on the map displayed on the display screen 5. The vehicle V further includes a gyro 6 and a steering angle sensor 7.

The vehicle V has a controller (control unit) U configured using a microcomputer. As will be described later, the controller U performs a function of accurately performing map matching between the current position and the traveling direction of the vehicle V and control such as driving support for collision avoidance. Therefore, as shown in FIG. 2, the controller U receives signals from the above-described various sensors 1 to 7 and controls the display screen 5, the alarm device 8, and the vehicle control device 9. ing. The controller U performs control to display the current position of the vehicle V on the display screen 5. When it is determined that there is a possibility that an object detected by the object detection sensor 1 may come into contact with the vehicle V, the alarm device 8 is activated to alert the driver by voice, for example. Further, the controller U activates the vehicle control device 9 when the object detection sensor 1 detects an object at a closer distance than the alarm device 8 is activated and determines that the risk of collision is extremely high. Thus, for example, control for avoiding a collision is performed by performing automatic steering or automatic braking.

Further, the controller U notifies the other vehicle existing near the front of the current position and the traveling direction of the vehicle V through inter-vehicle communication, thereby preventing a collision with the other vehicle. Further, when the vehicle V departs from the lane and is dangerous, the controller U performs automatic steering to prevent the lane departure. In addition, the controller U performs appropriate control on the vehicle V in accordance with, for example, the distance between the vehicle V and the stop line position, the intersection position, and the guard rail in the blind spot area. Although various types of control by the controller U according to the position of the vehicle V are assumed, since it is not directly related to the position detection itself, further explanation is omitted.

In order to accurately detect the current position and the traveling direction of the vehicle V, the following control is performed. First, the current position of the vehicle V is indicated on a plan view when the vehicle V is viewed from above, that is, for example, in an orthogonal coordinate system having the front-rear direction as the X axis and the left-right direction as the Y axis. Further, the traveling direction of the vehicle V is the left-right direction (yaw direction) of the vehicle V, and can be represented as, for example, the slope of a linear line in an orthogonal coordinate system.

The map database 4 stores voxel data as 3D map data. The map displayed on the display screen 5 is a plan view of the actual terrain viewed from above, and each position on the map is accurately set according to the actual terrain. The voxel data is obtained by dividing a three-dimensional space into, for example, a cube having a side of 50 cm, and the divided voxels include the position of the center position of the voxel and the object (fixed ground) included in the voxel. Object), for example, accurate position information such as utility poles, guardrails, buildings, power supply boxes, road lanes, etc. is included as position data. In the following description, the three-dimensional map data is simply referred to as map data. As the position information in the map data used for determining the position of the vehicle V, data at the same height position as the sensing plane of the object detection sensor 1 is used.

FIG. 3 shows an example of an actual fixed object around the vehicle V. In FIG. 3, 11 is a guardrail, 12 is a building, 13 is a fence, and 14 is a building. Of the lanes on the road on which the vehicle V is traveling, the central lane is indicated by reference numeral 21, and the left and right side lanes are indicated by reference numerals 22 and 23. In addition, a travel lane between the central lane 21 and the side lane 22 is denoted by reference numeral 24, and a travel lane between the central lane 21 and the side lane 23 is denoted by reference numeral 25. A state in which the vehicle V is traveling on the traveling lane 24 is shown. The object detection sensor 1 can also detect the lanes 21, 22, and 23 and the traveling lanes 24 and 25. The lanes 21 to 23 are detected by the object detection sensor 1 in the same manner as the guardrail 11 and the like. It is considered as a fixed object (feature). In the following description, reference numerals 11 to 14 or 21 to 23 may be collectively referred to as fixed objects.

FIG. 4 shows detection data when the object detection sensor 1 detects the fixed objects 11 to 14 in the state of FIG. In this detection data, each fixed object 11-14 is grasped | ascertained as the shape of the part which faced the road. In FIG. 4, the data portion corresponding to the fixed objects 11 to 14 is shown with the symbol “A”. That is, for example, data corresponding to the guard rail 11 is indicated by reference numeral 11A.

FIG. 5 shows map data having the same height as the sensing plane in a predetermined area corresponding to FIG. In FIG. 5, each square cell is a voxel. In FIG. 5, data portions corresponding to the fixed objects 11 to 14 are shown with a symbol “B”. That is, for example, data corresponding to the guard rail 11 is indicated by reference numeral 11B.

When the direction of the orthogonal coordinate system in FIG. 4 and the direction of the orthogonal coordinate system in FIG. 5 coincide with each other, as shown in FIG. 7, the fixed objects 11A to 14A on the detection data in FIG. It is possible to completely match the fixed objects 11B to 14B on the map data. That is, the data of all the fixed objects can be matched such that 11A and 11B match.

On the other hand, when the direction of the orthogonal coordinate system in FIG. 4 and the direction of the orthogonal coordinate system in FIG. 5 do not match, as shown in FIG. 6, the data of some fixed objects can be matched. However, there are fixed objects in which the data are not matched. In particular, the fixed objects 14A and 14B far from the vehicle V do not match. For this reason, conventionally, a deviation in the X-axis direction, a deviation in the Y-axis direction, and a deviation in the direction of the orthogonal coordinate system between the detection data and the map data (that is, a deviation in the traveling direction of the vehicle V). )) In consideration of the three shift elements, and the amount of calculation is enormous, which places a heavy burden on the control system.

Next, with reference to FIG. 8, the point of deviation correction in the traveling direction of the vehicle V will be described. First, in FIG. 8, reference numerals 21 to 25 denote lanes or traveling lanes similar to those shown in FIG. 3. The center line of the travel lane 24 is denoted by reference numeral 24a, and the center line of the travel lane 25 is denoted by reference numeral 25a. Further, guardrails 31 and 32 and a building 33 are shown as fixed objects that are located around the road and parallel to the road. Of the guardrails 31 and 32 and the building 33, the side facing the road and parallel to the road is hatched.

In FIG. 8, the vehicle V is in a state shifted by an angle δ in the right direction with respect to the center line 24 a of the travel lane 24. Here, the fact that the guardrails 31 and 32 and the building 33 are parallel to the road is, for example, a position between the side lane 22 and the guardrails 31 and 32 or the building 33 by referring to the map data or from the detection data. You can find out by looking at the relationship. Therefore, the angle δ formed by the vehicle V with the center line 24a of the traveling lane 24 and the distance L between the vehicle V and the parallel fixed object are known from the detection data obtained when the object detection sensor 1 detects the parallel fixed object. Can do.

Since the positions and orientations of the parallel fixed objects 31 to 33 are accurately set by the map data, the distance L from the parallel fixed objects 31 to 33 of the vehicle V and the angle δ formed with respect to the center line 24a, The traveling direction (absolute direction) of the vehicle V can be known with high accuracy. In this way, correction of the traveling direction of the vehicle V (determination of the absolute direction of the traveling direction) is performed.

Here, a description will be given of an example of a technique for calculating the amount of deviation when both positional deviation and azimuth deviation are included. First, the position of the fixed object on the map data is (xi, yi), the position of the fixed object on the detection data is (ui, vi), the position deviation is qx, qy, and the direction deviation is θ. Then, Formula 1 is materialized. When n pieces of data are used, Equation 2 is established.

The equation group shown in Equation 2 can be placed as a matrix of Equation 3.

Here, when A is a transposed matrix of A, Equation 4 is solved to obtain qx, qy, and θ. Then, when the obtained qx, qy, and θ are equal to or smaller than a predetermined value (when the deviation is small), the accuracy is sufficient. When “AtA” and “AtY” in Expression 4 are expanded, Expression 5 is obtained.

As described above, the amount of calculation increases in determining the amount of deviation in consideration of the azimuth deviation θ, which is a heavy burden on the control system. However, as described above, if the azimuth deviation is corrected in advance and determined to be accurate, the azimuth deviation θ may be regarded as a known constant value in the above-described calculation, and the positional deviation is corrected. That is, positioning of the current position of the vehicle V by map matching can be performed easily and accurately.

Further, when the cartesian coordinate system of the map data and the cartesian coordinate system of the detection data are matched in the state after the azimuth deviation is corrected, the following method can be used. That is, in FIG. 11, a black portion is a fixed object (portion) to be compared between map data and detection data, and a rectangular frame is a mesh (mesh) corresponding to the required position accuracy (for example, 10cm square). In FIG. 11, it is preferable that the extracted feature portion (region) includes a fixed object (for example, a building or a guardrail) that extends in a straight line along the road (for example, 5 m to 10 m). If it is a straight line as a line segment, it may be a part of a building or a guardrail.

For example, while fixing the feature portions on the map data, the feature portions on the detection data are moved one mesh at a time in the X-axis direction and the Y-axis direction. The degree of coincidence is determined. That is, each time the extracted feature part on the map data is moved by one mesh in the X-axis direction and the Y-axis direction with respect to the extracted feature part on the map data, the degree of coincidence is finally seen. A high state is a positional deviation amount in the orthogonal coordinate system between the map data and the detection data. By such a method, it is possible to easily know the amount of positional deviation between the map data and the detection data. By correcting the position of the vehicle V on the map data based on the amount of positional deviation, the absolute position of the vehicle V in the Cartesian coordinate system is accurately measured.

Next, detailed control contents of the controller U will be described with reference to the flowcharts of FIGS. In the following description, Q indicates a step. First, in Q1 of FIG. 9, signals from various sensors shown in FIG. 2 are input. Thereafter, in Q 2, the provisional current position of the vehicle V is determined based on the detection result in the GPS 3, and the road information of the provisional current position is read from the map data 4.

After Q2, in Q3, a predetermined area around the vehicle V (for example, 25 m square centered on the vehicle V) is set within the detection range of the object detection sensor 1. Thereafter, in Q4, based on the detection result of the object detection sensor 1, detection data (sensor map data as shown in FIG. 4) in the peripheral area is created.

After Q4, in Q5, it is determined whether or not it is necessary to determine the direction, that is, whether or not it is necessary to correct the traveling direction of the vehicle V. In the embodiment, the predetermined condition when the direction needs to be determined is set as follows. That is, (1) when GPS data is acquired after engine startup, (2) when traveling a predetermined distance (when speed x time reaches a predetermined value), (3) when the steering angle is equal to or greater than a predetermined angle, (4 ) When the difference between the left and right wheel speed is greater than or equal to the predetermined value, (5) When the blinker is activated, (6) When the predetermined feature point stored in the map data (when there is a fixed object parallel to the road) When it is located.

When the determination in Q5 is YES, a direction determination is executed in Q6, as will be described later. After Q6 or when NO is determined in Q5, the current position of the vehicle V is calculated based on the result of the direction determination in Q7 (with the traveling direction corrected). After Q7, it is determined in Q8 whether the current position can be calculated. If the determination in Q8 is YES, in Q9, the current position calculated in Q7 is determined as the final current position. If NO in Q8, the process returns without passing through Q9. If NO in Q8, the temporary current position in Q2 is used as it is as the current position, or the current position is estimated based on the previously determined current position, vehicle speed, steering angle, and yaw rate. Is done.

Details of Q6 in FIG. 6 are shown in FIG. That is, first, in Q21, a fixed object parallel to the road lane is extracted. Thereafter, in Q22, an angle and a distance with respect to the extracted parallel fixed object are calculated. Thereafter, in Q23, the direction of the detection data is corrected using the angle and distance calculated in Q22. After all, the process of Q23 is to change the orientation unmatching in FIG. 6 into the orientation matching state in FIG.

Although the embodiment has been described above, the present invention is not limited to the embodiment, and can be appropriately changed within the scope described in the scope of claims. For example, the invention includes the following cases. . Examples of the moving object include vehicles such as automobiles and two-wheeled vehicles, and pedestrians that move arbitrarily on the road. The predetermined area cut out from the detection data is used as the map data area used for positioning the previous current position, and matching for correcting the moving direction of the moving object is performed using data matched for positioning the previous current position. This is preferable for reducing the amount of data to be used. A fixed object behind the vehicle V may be used as detection data. Of course, the object of the present invention is not limited to what is explicitly stated, but also implicitly includes providing what is substantially preferred or expressed as an advantage.

According to the present invention, it is preferable to easily and accurately determine the current position of a vehicle or the like.

V: Vehicle U: Controller 1: Object detection sensor 2: Wheel speed sensor 3: GPS
4: Map data 5: Display screen 6: Gyro 7: Rudder angle sensor

Claims (11)

Object detection means for detecting surrounding objects on the moving body is provided,
A moving body position detecting device that measures the current position of a moving body by matching three-dimensional map data including position information of a fixed object with detection data detected by the object detection means,
Prior to positioning of the current position of the moving body, the traveling direction of the moving body is corrected by matching the detection data of a predetermined fixed object around the road detected by the object detection means and map data,
Based on the traveling direction of the mobile object after correction, matching for positioning of the current position of the mobile object is performed.
A moving body position detecting device characterized by the above. In claim 1,
The moving body position detecting apparatus according to claim 1, wherein correction of the moving direction of the moving body is performed on condition that a predetermined condition set in advance is satisfied. In claim 1,
The moving body position detecting device, wherein the moving direction of the moving body is corrected by comparing a line segment direction of map data and a line segment direction of detection data in a predetermined area. In claim 3,
The moving body position detecting device, wherein the predetermined area is a front area of the moving body. In claim 3,
The predetermined area is an area of map data used for positioning the current position last time,
Matching is performed to correct the traveling direction of the moving object using the data matched for the previous current position measurement.
A moving body position detecting device characterized by the above. In claim 1,
The moving body position detecting device, wherein the predetermined fixed object for correcting the traveling direction of the moving body is a parallel object parallel to a road lane on which the moving body is moving. In claim 6,
Among the parallel objects, a moving object position detecting device in which a distance and a direction to the parallel object can be detected by the object detection means is used for correcting the traveling direction. In claim 6,
A moving object position detecting device, wherein matching for positioning of the current position of the moving object is performed using the parallel object. In claim 1,
A moving body position detecting apparatus characterized in that positioning of the current position of the moving body and correction of the traveling direction of the moving body are performed using the same detection data. In any one of Claims 1 thru | or 9,
The moving object position detecting device, wherein the fixed object is at least one of a guard rail, a lane line that divides a lane, and a building. In any one of Claims 1 thru | or 10,
The moving body position detecting device, wherein the moving body is an automobile.

Images