JP6303890B2 - Position detection apparatus and position detection method - Google Patents

Description

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

  As a technique for calculating the current position of the moving object, a position detection device that calculates the position of the moving object on the map based on the relative position of the landmark to the moving object and the position of the landmark on the known map is known. ing. For example, the position detection device described in Patent Literature 1 images a landmark installed along a moving path of a moving body with an imaging unit. When at least three or more landmarks are recognized, the position detection device reads the position data of the three landmarks stored in advance in the storage unit. The position detection device calculates a relative angle θ between the landmarks as viewed from the moving body based on the image, and calculates the current position of the moving body based on the position data and the angle data.

JP 2001-142532 A

However, if the positions on the map are stored in advance and the number of landmarks whose relative positions can be measured is insufficient, the position of the moving object may not be calculated.
An object of the present invention is to provide a position detection device that reduces the possibility that the position of a moving object cannot be calculated when the position on the map is stored in advance and the number of landmarks whose relative position can be measured is insufficient. It is to provide a position detection method.

  A position detection apparatus according to an aspect of the present invention measures a relative position of a peripheral object with respect to a moving object, and stores a first landmark in which a position on a map is stored and a position on the map among these objects. An object to be measured that has not been detected is detected. The position detection device detects a stationary object to be measured as a second landmark, and based on the relative position of the first landmark, the position on the map, and the relative position of the second landmark, Calculate the position on the map. The position detection device calculates the position of the moving body on the map based on at least the relative position of the second landmark and the position on the map.

  ADVANTAGE OF THE INVENTION According to this invention, when the position on a map is memorize | stored previously and the number of the landmarks whose relative position can be measured is insufficient, a possibility that the position of a moving body cannot be calculated can be reduced.

It is a figure which shows the structural example of a vehicle provided with the position detection apparatus which concerns on embodiment of this invention. It is a figure which shows the function structure of the position detection apparatus which concerns on 1st Embodiment of this invention. It is explanatory drawing of the measurement of the position of the scanning point by a position measurement part. It is explanatory drawing of an example of the calculation method of the provisional absolute position and provisional front-back direction of a vehicle. It is explanatory drawing of an example of a detection process of a 1st landmark and a to-be-measured object. (A) is explanatory drawing of the measurement error of the relative position of an object, (b) is explanatory drawing of the object which has the surface of the horizontal cross-sectional shape containing the L-shaped part convex outside. It is explanatory drawing of an example of the detection process of a 2nd landmark. It is explanatory drawing of an example of the calculation process of the absolute position of a 2nd landmark. It is explanatory drawing of an example of the calculation process of the absolute position of a vehicle. It is explanatory drawing of an example of operation | movement of the position detection apparatus which concerns on 1st Embodiment of this invention. (A), (b) and (c) is explanatory drawing of an example of the landmark used for position calculation of a vehicle in 1st Embodiment. It is explanatory drawing of an example of operation | movement of the position detection apparatus which concerns on 2nd Embodiment of this invention. (A), (b) and (c) is explanatory drawing of an example of the landmark used for position calculation of a vehicle in 2nd Embodiment. It is a figure which shows the function structure of the landmark selection part of the position detection apparatus which concerns on 3rd Embodiment of this invention. It is a figure which shows an example of a known landmark. It is explanatory drawing of the relative angle and relative distance of a landmark pair. (A) is a figure showing an example of the 1st classification result by the 1st classification part, (b) is a figure showing an example of the 2nd classification result by the 2nd classification part, (c) It is a figure which shows an example of the 3rd classification result by a 1st classification | category part, (d) is a figure which shows an example of the 4th classification result by a 2nd classification | category part. It is explanatory drawing of an example of operation | movement of the landmark selection part shown in FIG.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
(First embodiment)
(Constitution)
Please refer to FIG. The position detection apparatus according to the embodiment of the present invention is mounted on, for example, the vehicle 1 and is used to detect the current position on the map of the vehicle 1. The current position of the vehicle 1 detected by the vehicle detection device can be used for, for example, a route calculation from the current position of the vehicle 1 to a desired destination in a navigation device mounted on the vehicle 1. Further, the current position of the vehicle 1 detected by the vehicle detection device is set, for example, as driving assistance for controlling the operation force so that the vehicle 1 continuously travels in the vicinity of the center of the travel lane and a travel route to a desired destination It can be used for automatic driving for vehicle control. In addition, the use application of the position detection apparatus which concerns on embodiment of this invention is not limited to this. The position detection apparatus according to the embodiment of the present invention can be widely used for detection of the current position of a moving body.

The vehicle 1 includes a wheel 2, a vehicle speed sensor 3, a steering wheel 4, a steering angle sensor 5, a position measurement sensor 6, a yaw rate sensor 7, a controller 10, and a map information storage unit 20.
The vehicle speed sensor 3 detects the wheel speed of the wheel 2 and calculates the speed of the vehicle 1 based on the wheel speed. The vehicle speed sensor 3 outputs the calculated vehicle speed information to the controller 10. The vehicle speed sensor 3 may include a pulse generator such as a rotary encoder that measures wheel speed pulses. The steering angle sensor 5 detects the steering angle of the steering wheel 4 and outputs the detected steering angle information to the controller 10. The steering angle sensor 5 is provided on a steering shaft or the like. The steering angle sensor 5 may detect the turning angle of the steered wheels as steering angle information.

The position measurement sensor 6 scans the surface of an object existing around the vehicle 1 to measure the distance from the vehicle 1 to the scanning point on the object surface, and acquires the direction to the scanning point. The position measurement sensor 6 may include, for example, a laser range finder or a radar mounted on the vehicle 1. The position measurement sensor 6 outputs information on the measured distance and direction to the scanning point to the controller 10. The yaw rate sensor 7 detects the yaw rate of the vehicle 1 (the changing speed of the rotation angle in the direction in which the vehicle body turns), and outputs the detected yaw rate information to the controller 10.
The map information storage unit 20 stores map information. The map information may include information on geographical landmarks that can be used as a reference when calculating the position of the vehicle 1 on the map. In the following description, a geographical landmark used as a reference when calculating the position of the vehicle 1 on the map may be referred to as “landmark”. The map information may include landmark identification information and position information on the map. The landmark may be, for example, a utility pole, a signal pole, a sign pole, an illumination pole, etc. installed on the side of the road. For example, the landmark may be a three-dimensional object such as a guide board or a building.

In the following description, a geographical landmark whose position is stored in the map information storage unit 20 may be referred to as a “first landmark”. The map information storage unit 20 corresponds to a position storage unit.
The position on the map stored in the map information storage unit 20 is expressed by the absolute position on the absolute coordinate system with the fixed point on the ground as the origin. The absolute coordinate system may be, for example, a world coordinate system or a geodetic coordinate system. In the following description, the position on the map stored in the map information storage unit 20, that is, the absolute position on the absolute coordinate system may be referred to as “absolute position”.
The map information stored in the map information storage unit 20 further includes road information regarding roads. The road information may include, for example, information on the absolute position of a point where the road passes, information on the curvature of each curved road included in the road, and information on the road width of the road.

The controller 10 is an electronic control unit including a CPU (Central Processing Unit) and CPU peripheral components such as a ROM (Read Only Memory) and a RAM (Random Access Memory). The controller 10 measures the relative position of an object around the vehicle 1 with respect to the vehicle 1 based on the distance and direction information to the scanning point output from the position measurement sensor 6. In the following description, the relative position with respect to the vehicle 1 may be simply referred to as “relative position”.
The controller 10 detects a first landmark whose absolute position is stored in the map information storage unit 20 from the measured objects. Furthermore, the controller 10 detects, as a second landmark, an object that is not stored in the map information storage unit 20 and is stationary, among the measured objects. The controller 10 calculates the absolute position of the second landmark based on the relative position and absolute position of the first landmark and the relative position of the second landmark.
The controller 10 selects a landmark to be used for calculating the absolute position of the vehicle 1 from the detected first landmark and second landmark. The controller 10 calculates the current absolute position of the vehicle 1 based on the relative position and absolute position of the selected landmark.

  Hereinafter, processing by the controller 10 will be described in detail. Please refer to FIG. The position detection device 30 includes the position measurement sensor 6 and the map information storage unit 20 described above. The position detection device 30 includes a position measurement unit 31, a movement amount calculation unit 32, a first position calculation unit 36, a second position calculation unit 33, a first detection unit 34, a second detection unit 35, a land A mark selection unit 37 and a second landmark storage unit 38 are provided. Processing described later by the position measurement unit 31, the movement amount calculation unit 32, the first position calculation unit 36, the second position calculation unit 33, the first detection unit 34, the second detection unit 35, and the landmark selection unit 37 is performed by a controller. 10 is executed.

  The position measurement unit 31 measures the relative position of the scanning point based on the distance and direction information to the scanning point on the object around the vehicle 1 output from the position measurement sensor 6. With reference to FIG. 3, the measurement of the position of the scanning point by the position measuring unit 31 will be described. In the accompanying drawings, the x-axis direction indicates the front-rear direction of the vehicle 1 and the y-axis direction indicates the lateral direction of the vehicle 1. The lateral direction of the vehicle 1 is a direction orthogonal to the moving direction of the vehicle 1. The front-rear direction of the vehicle 1 is a direction orthogonal to the lateral direction of the vehicle 1. Hereinafter, the lateral direction of the vehicle 1 is simply referred to as “lateral direction”. The front-rear direction of the vehicle 1 is simply referred to as “front-rear direction”.

  The position measuring unit 31 mounted on the vehicle 1 is determined as a relative position of the scanning point 60 on the surface of the object 50 by a distance r1 to the scanning point 60 and a direction θ1 in a relative coordinate system having the position of the vehicle 1 as an origin. The position may be measured. The direction θ1 may be a horizontal angle of the scanning point 60 with respect to the x-axis direction of the vehicle 1, for example. For example, the direction θ1 may be an azimuth angle of the scanning point 60 with the vehicle 1 as the origin and the x-axis direction as a reference. In the following description, information on the relative position measured by the position measurement unit 31 may be referred to as “measurement position information”. The position measurement unit 31 outputs measurement position information to the first detection unit 34.

  Please refer to FIG. The vehicle speed sensor 3 outputs vehicle speed information of the vehicle 1 to the movement amount calculation unit 32. The yaw rate sensor 7 outputs the yaw rate information of the vehicle 1 to the movement amount calculation unit 32. The movement amount calculation unit 32 calculates the movement amount (dx, dy, dφ) of the vehicle 1 from time t−1 to time t based on the received vehicle speed information and yaw rate information. The time t is the latest measurement time of the position measurement sensor 6, and the time t-1 is the measurement time of the position measurement sensor 6 before the time t. For example, the measurement interval of the position measuring sensor 6 may be Tm, and the time t may be a time delayed by a time Tm from the time t-1.

dx and dy indicate translational movement amounts in the front-rear direction and the lateral direction, respectively. dφ represents the amount of rotation in the direction in which the vehicle body turns. The movement amount calculation unit 32 outputs the calculated movement amounts (dx, dy, dφ) to the second position calculation unit 33.
Based on the absolute position (Xt-1, Yt-1) and the front-rear direction (Φt-1) of the vehicle 1 at the time t-1 that has already been calculated, the second position calculation unit 33 determines the vehicle 1 at the time t. The temporary absolute position (Xtt, Ytt) and the temporary front-rear direction (Φtt) are calculated. When calculation of the absolute position of the vehicle 1 and the direction in the front-rear direction at time t-1 has not yet been completed, the measurement values obtained by other positioning means may be used. Other positioning means may be, for example, GPS (Global Positioning System). In the following description, the front-rear direction of the vehicle 1 may be simply referred to as “the direction of the vehicle 1”.

  With reference to FIG. 4, an example of a method for calculating the temporary absolute position (Xtt, Ytt) and the temporary orientation (Φtt) of the vehicle 1 will be described. For example, the second position calculation unit 33 receives the movement amount (dx, dy, dφ) from the movement amount calculation unit 32. The second position calculation unit 33 corrects the absolute position (Xt−1, Yt−1) and the direction (Φt−1) at time t−1 with the movement amount (dx, dy, dφ), and the temporary position at time t. The absolute position (Xtt, Ytt) and the temporary orientation (Φtt) are calculated. The second position calculation unit 33 outputs the temporary absolute position (Xtt, Ytt) and the temporary orientation (Φtt) of the vehicle 1 to the first detection unit 34 and the second detection unit 35.

Please refer to FIG. The first detection unit 34 receives measurement position information from the position measurement unit 31. The first detector 34 receives the temporary absolute position (Xtt, Ytt) and the temporary orientation (Φtt) of the vehicle 1 from the second position calculator 33. The first detection unit 34 reads the identification information and absolute position of the first landmark from the map information storage unit 20.
The first detection unit 34 detects an object around the vehicle 1 based on the measurement position information received from the position measurement unit 31. For example, the first detection unit 34 performs segmentation processing on the measurement position information. At this time, the 1st detection part 34 connects the scanning points with a short separation distance. The first detection unit 34 divides the scan points received from the position measurement unit 31 into segments by detecting a set of linked scan points as a segment. The first detection unit 34 recognizes the detected segment as an object. The first detection unit 34 calculates the relative position of the object based on the measurement position information of the scanning points belonging to the segment. For example, the average coordinates of the measurement positions of the scanning points belonging to the segment may be calculated as the relative position of the object.

  The first detection unit 34 is based on the provisional absolute position and provisional direction of the vehicle 1 received from the second position calculation unit 33 and the absolute position read from the map information storage unit 20. A first landmark is detected from within the object. Further, the first detection unit 34 detects an object whose absolute position is not stored in the map information storage unit 20 from the detected objects around the vehicle 1. In the following description, among objects around the vehicle 1 detected by the first detection unit 34, an object whose absolute position is not stored in the map information storage unit 20 may be referred to as “measured object”.

With reference to FIG. 5, an example of detection processing of the first landmark and the object to be measured will be described. The objects 50 and 51 and the corners 70 of the other vehicle 80 are detected as objects around the vehicle 1, and the relative positions of the objects 50 and 51 and the corners 70 are pd1 (x1, y1), pd2 (x2, y2), and pd3, respectively. Assume that (x3, y3). Further, it is assumed that the absolute positions Pr1 (Xr1, Yr1) and Pr2 (Xr2, Yr2) of the two first landmarks are read from the map information storage unit 20.
The first detection unit 34 assumes that the absolute position and orientation of the vehicle 1 are the temporary absolute position (Xtt, Ytt) and the temporary orientation (φtt), and the relative positions pd1 of the objects 50 and 51 and the corner 70. Convert ~ pd3 to absolute position. The absolute coordinates of the objects 50 and 51 and the corner 70 are respectively expressed as Pd1 (Xd1, Yd1), Pd2 (Xd2, Yd2), and Pd3 (Xd3, Yd3).

  The first detector 34 compares the absolute coordinates Pd1 to Pd3 with the absolute positions Pr1 and Pr2 of the first landmark, and detects the objects 50 and 51 close to the absolute positions Pr1 and Pr2, respectively, as the first landmark. The first detector 34 detects a corner 70 that is not close to the absolute positions Pr1 and Pr2 of the first landmark as the object to be measured. For example, the first detection unit 34 may detect the object as the first landmark when the distance between the object and the first landmark is within a predetermined distance. The first detection unit 34 may detect the object as an object to be measured when the distance between the object and the first landmark exceeds a predetermined distance. The predetermined distance may be 0.5 m, for example.

The first detection unit 34 detects, as an object to be measured, an object having a horizontal cross-sectional surface including an outwardly convex L-shaped portion among objects whose absolute positions are not stored in the map information storage unit 20. You can do it. The reason will be described with reference to FIGS. 6A and 6B. Reference numeral 1a indicates the position of the vehicle 1 at a certain time t0, and reference numeral 1b indicates the position of the vehicle 1 at time t1 after the time t0.
When the object 71 whose absolute position is not stored in the map information storage unit 20 is used as the second landmark for calculating the position of the vehicle 1, the outer shape of the object 71 cannot be predicted in advance. For this reason, even if the scanning point on the object 71 scanned by the position measurement unit 31 changes from the point 60 to the point 61 with the relative movement between the vehicle 1 and the object 71, it detects that the scanning point has changed. It ’s difficult. As a result, in the measurement result of the relative position of the object 71, an error e in a specific direction occurs depending on the direction of relative movement and the shape of the object 71.

  Reference is made to FIG. The object 70 has a surface having a horizontal cross-sectional shape including an outwardly convex L-shaped portion. The corner of the other vehicle 80 is an example of such an object 70. The outwardly convex L-shaped portion includes a first straight line 62 and a second straight line 63. The first detection unit 34 calculates an approximate straight line of the first straight line 62 based on the plurality of scanning points 64 and 65 on the first straight line 62, and the second based on the plurality of scanning points 66 and 67 on the second straight line 63. An approximate straight line of the straight line 63 is calculated. The first detector 34 can detect the apex 68 of the L-shaped portion based on the approximate straight line of the first straight line 62 and the approximate straight line of the second straight line 63.

By detecting such a vertex 68 as the relative position of the object, it is possible to reduce an error in the measurement result even if the vehicle 1 and the object 71 are relatively moved. It is desirable that the angle α formed by the first straight line 62 and the second straight line 63 is a larger angle so that the range of the vehicle 1 that can scan the two straight lines 62 and 63 of the L-shaped portion is widened. For example, the angle α may be 90 degrees, or the angle α may be 90 degrees or more and less than 180 degrees.
Please refer to FIG. The first detector 34 outputs the detected identification information of the first landmark and its relative position to the first position calculator 36, the second position calculator 33, and the landmark selector 37. The first detector 34 outputs the detected relative position of the first landmark and the detected relative position of the measured object to the second detector 35.

The second detector 35 receives the relative position of the first landmark and the relative position of the measured object from the first detector 34. The second detection unit 35 receives the temporary absolute position (Xtt, Ytt) and the temporary orientation (Φtt) of the vehicle 1 from the second position calculation unit 33.
The second detector 35 detects a stationary object to be measured as a second landmark. For example, the second detection unit 35 may determine whether or not the measured object is stationary according to a change in the relative position of the measured object. For example, the second detection unit 35 determines whether the measured object is stationary according to a comparison result between the change in the relative position of the measured object and the change in the relative position of the first landmark. Good.

An example of the second landmark detection process will be described with reference to FIG. Reference numerals 50a and 70a indicate relative positions of the first landmark 50 and the measured object 70 at a certain time t0, respectively. Reference numerals 50b and 70b respectively indicate the relative positions of the first landmark 50 and the measured object 70 at time t1 after time t0.
The second detector 35 calculates changes d1 and d2 in the relative positions of the first landmark 50 and the object 70 to be measured that occurred between time t0 and time t1. The second detection unit 35 compares the changes d1 and d2, and determines that the object to be measured is stationary when the difference between the changes d1 and d2 is within a predetermined value range, and the difference between the changes d1 and d2. Is outside the predetermined value range, it is determined that the object to be measured is not stationary.

  Please refer to FIG. The second landmark storage unit 38 is used to store the identification information and the absolute position of the second landmark that has already been detected by the second detection unit 35. When the second detection unit 35 detects a second landmark that is not stored in the second landmark storage unit 38, the second detection unit 35 generates identification information for the second landmark, and determines the identification information for the second landmark and the relative position. The data is output to the 1-position calculator 36. The first position calculation unit 36 calculates the absolute position of the second landmark from the relative position of the second landmark. The first position calculation unit 36 stores the identification information and the absolute position of the second landmark in the second landmark storage unit 38.

  For example, when the second detection unit 35 detects the second landmark, whether or not the absolute position of the second landmark has already been calculated by the first position calculation unit 36 and stored in the second landmark storage unit 38. Determine whether. The second detection unit 35 reads the absolute position and identification information of the second landmark from the second landmark storage unit 38. The second detector 35 detects the second landmark detected based on the temporary absolute position and temporary orientation of the vehicle 1 received from the second position calculator 33 and the absolute position read from the second landmark storage 38. It is determined whether or not the absolute position is stored in the second landmark storage unit 38.

  For example, the second detection unit 35 assumes that the current absolute position and orientation of the vehicle 1 are the temporary absolute position and temporary orientation of the received vehicle 1 and determines the relative position of the detected second landmark. Convert to absolute position. When the distance between the absolute position of the detected second landmark and the absolute position read from the second landmark storage unit 38 is within a predetermined distance, the second detection unit 35 determines that the detected second landmark is the second It is determined that it is stored in the landmark storage unit 38. When the distance between the absolute position of the detected second landmark and the absolute position read from the second landmark storage unit 38 exceeds a predetermined distance, the second detection unit 35 determines that the detected second landmark is the second It is determined that it is not stored in the landmark storage unit 38.

When the detected second landmark is not stored in the second landmark storage unit 38, the second detection unit 35 generates identification information of the detected second landmark. The second detection unit 35 outputs the identification information of the second landmark and its relative position to the first position calculation unit 36.
When the detected second landmark is stored in the second landmark storage unit 38, the second detection unit 35 displays the identification information of the second landmark read from the second landmark storage unit 38 as a landmark selection unit. 37 and the second position calculation unit 33. When the detected absolute position of the second landmark is stored in the second landmark storage unit 38, the second detection unit 35 determines the relative position of the second landmark as a landmark selection unit 37 and a second position calculation unit. To 33.

The first position calculation unit 36 receives the identification information of the first landmark and the relative position from the first detection unit 34. The first position calculation unit 36 receives the identification information and the relative position of the second landmark from the second detection unit 35. The first position calculation unit 36 reads the absolute position of the first landmark detected by the first detection unit 34 from the map information storage unit 20 based on the identification information of the first landmark. The first position calculator 36 calculates the absolute position of the second landmark based on the relative position and absolute position of the first landmark and the relative position of the second landmark.
At this time, for example, the first position calculation unit 36 determines whether or not the first detection unit 34 has detected two or more first landmarks from the measurement position information obtained from the measurement result at the measurement time t. You can do it. Hereinafter, the measurement position information obtained from the measurement result at the measurement time t may be simply referred to as “measurement result at the time t”.
When the first detection unit 34 detects two or more first landmarks, the first position calculation unit 36 sets the relative positions and absolute positions of the two first landmarks and the relative positions of the second landmarks. Based on this, the absolute position of the second landmark is calculated.

  An example of the calculation process of the absolute position of the second landmark will be described with reference to FIG. It is assumed that the first landmarks 50 and 51 are detected and their absolute positions are Pr1 (Xr1, Yr1) and Pr2 (Xr2, Yr2), respectively. The first position calculation unit 36 calculates a distance r1 between the first landmark 50 and the second landmark 70 and a distance r2 between the first landmark 51 and the second landmark 70. The first position calculation unit 36 uses the second landmark 70 among the intersections Pr3 and Pr4 of a circle 90 having a radius r1 centered on the first landmark 50 and a circle 91 having a radius r2 centered on the first landmark 51. Is calculated as the absolute position of the second landmark 70. The first position calculation unit 36 may calculate the absolute position of the second landmark based on the relative position and the absolute position of the three first landmarks.

  Please refer to FIG. The first position calculation unit 36 stores the identification information of the second landmark and the absolute position thereof in the second landmark storage unit 38. The landmark selection unit 37 receives the identification information of the first landmark and its relative position from the first detection unit 34. The landmark selection unit 37 receives the identification information of the second landmark and its relative position from the second detection unit 35. As described above, the identification information received from the second detection unit 35 and the relative position thereof are the identification information and relative position of the second landmark whose absolute position is stored in the second landmark storage unit 38.

  In the following description, the first landmark and the second landmark whose absolute position is stored in the second landmark storage unit 38 may be collectively referred to as “known landmark”. The second landmark whose absolute position is stored in the second landmark storage unit 38 may be referred to as a “known second landmark” (corresponding to a third landmark). A second landmark that has been detected by the second detector 35 and whose absolute position has not yet been stored in the second landmark storage unit 38 may be referred to as an “unstored second landmark” (the fourth landmark). Equivalent). The landmark selection unit 37 selects a landmark to be used for calculating the absolute position of the vehicle 1 from known landmarks.

For example, when two or more known landmarks are detected from the measurement result at time t, the landmark selection unit 37 selects two known landmarks. For example, when two or more first landmarks are detected from the measurement result at time t, the landmark selection unit 37 detects the two first landmarks. For example, when one first landmark and one or more known second landmarks are detected from the measurement result at time t, the landmark selection unit 37 includes one first landmark and one landmark. A known second landmark is selected.
When two or more known second landmarks are detected from the measurement result at time t, the landmark selection unit 37 may select two known second landmarks. However, if no first landmark is detected from the measurement result at time t, the landmark selection unit 37 may not select a landmark used for calculating the absolute position of the vehicle 1. In this case, the second position calculation unit 33 does not use the landmark for calculating the absolute position of the vehicle 1. For example, the second position calculation unit 33 may calculate the absolute position of the vehicle 1 based on the movement amount (dx, dy, dφ) received from the movement amount calculation unit 32.

Please refer to FIG. The landmark selection unit 37 outputs the identification information of the selected landmark to the second position calculation unit 33. The second position calculation unit 33 receives the identification information and the relative position of the first landmark from the first detection unit 34. The second position calculator 33 receives the identification information and relative position of the second landmark from the second detector 35. The second position calculation unit 33 receives landmark identification information from the landmark selection unit 37.
Based on the identification information received from the landmark selection unit 37, the second position calculation unit 33 calculates the absolute position of the landmark selected by the landmark selection unit 37 based on the map information storage unit 20 or the second landmark storage unit 38. Read from. The second position calculation unit 33 receives from the landmark selection unit 37 the relative position of the first landmark received from the first detection unit 34 and the relative position of the second landmark received from the second detection unit 35. The relative position of the landmark of the identification information is selected. The second position calculation unit 33 calculates the absolute position of the vehicle 1 and the direction of the vehicle 1 at time t based on the read absolute position of the landmark and the relative position of the selected landmark.

With reference to FIG. 9, an example of the calculation process of the absolute position of the vehicle 1 will be described. Let Pm1 and Pm2 be the absolute positions of the landmarks. Further, the coordinates of Pm1 are (Xm1, Ym1), and the coordinates of Pm2 are (Xm2, Ym2). The second position calculation unit 33 assumes that the absolute position and orientation of the vehicle 1 are the temporary absolute position (Xtt, Ytt) and the temporary orientation (φtt), and converts the relative position of the landmark to the absolute position. . The absolute positions of the converted landmarks are P1 and P2, respectively. The coordinates of P1 are (X1, Y1), and the coordinates of P2 are (X2, Y2). Further, an error between the absolute positions Pm1 and P1 is D1, and an error between the absolute positions Pm2 and P2 is D2.
The second position calculation unit 33 calculates an error E between the absolute positions Pm1 and Pm2 read from the map information storage unit 20 and the absolute positions P1 and P2 obtained by conversion from the relative position based on the following equation (1). To calculate.

In the above formula (1), N indicates the total number of landmarks selected by the landmark selection unit 37. In the example of FIG. 9, N = 2. The landmark selection unit 37 may select three or more landmarks. When the landmark selection unit 37 selects three or more landmarks, N ≧ 3.
The second position calculation unit 33 calculates the provisional absolute position (Xtt, Ytt) and provisional orientation (φtt) of the vehicle 1 that minimizes the error E as the absolute position and orientation of the vehicle 1 at time t. . For example, the second position calculation unit 33 may calculate the absolute position and orientation of the vehicle 1 at time t by optimization calculation that minimizes the error E. Instead of minimizing the error E, the second position calculation unit 33 calculates the temporary absolute position (Xtt, Ytt) and the temporary orientation (φtt) of the vehicle 1 such that the error E is less than a predetermined value. May be.

(Operation)
Next, the operation of the position detection device according to the first embodiment will be described. Please refer to FIG. Steps S10 to S21 described in FIG. 10 are repeatedly executed until an ignition switch (not shown) of the vehicle 1 is switched off.
In step S <b> 10, the position measurement unit 31 acquires the output of the position measurement sensor 6. In step S <b> 11, the position measurement unit 31 measures the relative position of an object existing around the vehicle 1 based on the output of the position measurement sensor 6. In step S12, the second position calculation unit 33 determines the vehicle 1 at time t based on the calculated absolute position (Xt-1, Yt-1) and direction (Φt-1) of the vehicle 1 at time t-1. A temporary absolute position (Xtt, Ytt) and a temporary direction (Φtt) are calculated. In step S <b> 13, the first detection unit 34 reads the absolute position of the landmark from the map information storage unit 20.

In step S14, the first detector 34 determines the first landmark and the object to be detected from the object measured in step S11 based on the temporary absolute position and orientation of the vehicle 1 calculated in step S12 and the absolute position read in step S13. Detect the measurement object. In step S <b> 15, the second detection unit 35 detects the stationary object to be measured as the second landmark.
In step S <b> 16, the first position calculation unit 36 and the landmark selection unit 37 determine whether two or more first landmarks have been detected from the measurement result at time t. When two or more first landmarks are detected (step S16: Y), the operation proceeds to step S17. If two or more first landmarks are not detected (step S16: N), the operation proceeds to step S19.

In step S17, the first position calculator 36 calculates the absolute position of the unstored second landmark based on the relative position and absolute position of the two first landmarks and the relative position of the unstored second landmark. . The first position calculation unit 36 stores the absolute position of the unstored second landmark in the second landmark storage unit 38. In step S18, the landmark selection unit 37 selects any two of the first landmarks. The second position calculation unit 33 calculates the absolute position of the vehicle 1 and the direction of the vehicle 1 at time t based on the relative position and the absolute position of the landmark selected by the landmark selection unit 37. Thereafter, the operation proceeds to step S21.
In step S19, the landmark selection unit 37 determines whether or not two or more known landmarks are detected from the measurement result at time t. When two or more known landmarks are detected (step S19: Y), the operation proceeds to step S20. If two or more known landmarks are not detected (step S19: N), the operation proceeds to step S21.

In step S20, the landmark selection unit 37 selects any two known landmarks. The second position calculation unit 33 calculates the absolute position of the vehicle 1 and the direction of the vehicle 1 at time t based on the relative position and the absolute position of the landmark selected by the landmark selection unit 37. Thereafter, the operation proceeds to step S21.
In step S21, the position detection device 30 determines whether or not the ignition switch (IGN) of the vehicle 1 is off. If the ignition switch is not off (step S21: N), the operation returns to step S10. When the ignition switch is off (step S21: Y), the operation of the position detection device 30 ends.

  Hereinafter, referring to FIGS. 11A, 11B, and 11C, examples of landmarks used by the first embodiment when the vehicle 1 avoids the parked vehicle 80 are described. Will be explained. In the state shown in FIG. 11A, the first detector 34 can detect the first landmarks 50 and 51. Therefore, since the determination in step S16 shown in FIG. 10 is affirmative (Y), the absolute position of the second landmark 70 is calculated using the first landmarks 50 and 51 in step S17. In step S18, the absolute position of the vehicle 1 is calculated using the first landmarks 50 and 51.

In the state shown in FIG. 11B, the parked vehicle 80 blocks the first landmarks 51 and 53 from being visible. However, the first detector 34 can detect the first landmark 50, and the second landmark can detect the known second landmark 70 (corresponding to the third landmark). Therefore, the determination in step S16 shown in FIG. 10 is negative (N), and the determination in step S19 is affirmative (Y). Accordingly, in step S20, the absolute position of the vehicle 1 is calculated using the first landmark 50 and the known second landmark 70.
In the state shown in FIG. 11C, the first detection unit 34 can detect the first landmarks 50 and 53. Therefore, the determination in step S16 shown in FIG. 10 becomes affirmative (Y) again, and the absolute position of the vehicle 1 is calculated using the first landmarks 50 and 53 in step S18.

(Effect of 1st Embodiment)
(1) The position detection device 30 according to the first embodiment detects the measurement object as the second landmark when the measurement object whose absolute position is not stored in the map information storage unit 20 is stationary. The position detection device 30 calculates the absolute position of the second landmark based on the relative position and absolute position of the first landmark and the relative position of the second landmark. The position detection device 30 calculates the absolute position of the vehicle 1 based on at least the relative position and the absolute position of the second landmark. For this reason, the position detection device 30 can calculate the absolute position of the vehicle 1 without measuring the relative positions of a sufficient number of first landmarks.

(2) The position detection device 30 according to the first embodiment determines whether the measured object is stationary according to a change in the relative position of the measured object with respect to the vehicle 1. For this reason, the position detection device 30 can accurately detect a stationary object to be measured as the second landmark.
(3) The position detection device 30 according to the first embodiment determines whether the position of the object to be measured relative to the vehicle 1 is changed and the change in the relative position of the first landmark relative to the vehicle 1 is compared. It is determined whether or not the measurement object is stationary. For this reason, the position detection device 30 can accurately detect a stationary object to be measured as the second landmark.

(4) The position detection device 30 according to the first embodiment is a vehicle based on the relative position and absolute position of at least one first landmark and the relative position and absolute position of at least one second landmark. The absolute position of 1 is calculated. For this reason, the position detection device 30 can calculate the absolute position of the vehicle 1 even with one first landmark that can measure the relative position.
(5) The position detection device 30 according to the first embodiment, when two or more second landmarks are detected, the vehicle 1 based on the relative position and the absolute position of at least two second landmarks. The absolute position of is calculated. Therefore, the position detection device 30 can calculate the absolute position of the vehicle 1 even when there is no first landmark that can measure the relative position.

(6) The position detection device 30 according to the first embodiment is based on the relative position of the first landmark, the relative position of the second landmark, and the absolute position of the first landmark measured at the first time point. Calculate the absolute position of the two landmarks. The position detection device 30 calculates the absolute position of the vehicle 1 based on the absolute position of the second landmark and at least the relative position of the second landmark measured at the second time point after the first time point. Therefore, even if the relative position of the first landmark cannot be measured at the second time point after the relative position of the first landmark is measured at the first time point, the absolute position of the vehicle 1 is calculated at the second time point. Can do.
(7) The position detection device 30 according to the first embodiment detects an object to be measured having a horizontal cross-sectional surface including an outwardly convex L-shaped portion as a second landmark. For this reason, the error which arises in the measurement of the relative position of a 2nd landmark can be reduced, and the absolute position of the vehicle 1 can be calculated accurately.

(Modification)
The second position calculator 33 may calculate the average coordinates of the relative positions of the landmarks selected by the landmark selector 37 when calculating the absolute position of the vehicle 1. Further, the second position calculation unit 33 may calculate the average coordinates of the absolute position of the landmark selected by the landmark selection unit 37. The second position calculation unit 33 may calculate the absolute position of the vehicle 1 by subtracting the X coordinate value and the Y coordinate value of the average coordinate of the relative position from the X coordinate value and the Y coordinate value of the average coordinate of the absolute position. Good. According to such a calculation method, the absolute position of the vehicle 1 can be calculated more easily.

The movement amount calculation unit 32 may calculate the yaw rate based on the speed of the vehicle 1 and the steering angle of the steering wheel 4 detected by the steering angle sensor 5. The movement amount calculation unit 32 may calculate the rotation amount dφ of the vehicle 1 based on the calculated yaw rate. The yaw rate sensor 7 can be omitted by calculating the yaw rate based on the speed and the steering angle.
The second detection unit 35 determines whether the measured object is stationary according to a comparison result between the translational movement amount (dx, dy) calculated by the movement amount calculating unit 32 and a change in the relative position of the measured object. It may be judged. For example, the second detection unit 35 determines that the measured object is stationary when the difference between the translational movement amount and the change in the relative position is within a predetermined value range, and determines the translational movement amount and the change in the relative position. If the difference is outside the predetermined value range, it may be determined that the object to be measured is not stationary.

(Second Embodiment)
(Constitution)
Next, the position detection device 30 according to the second embodiment of the present invention will be described. The functional configuration of the position detection device 30 according to the second embodiment of the present invention is the same as the functional configuration shown in FIG. The second detector 35 detects a known second landmark (corresponding to a third landmark) from the measured object received from the first detector 34. For example, the second detection unit 35 reads the absolute position of the known second landmark from the second landmark storage unit 38. Based on the temporary absolute position and temporary orientation of the vehicle 1 received from the second position calculation unit 33 and the absolute position read from the second landmark storage unit 38, the second detection unit 35 knows the second object to be measured. It is determined whether or not it is a landmark.

The second detector 35 determines whether or not an object to be measured that is not a known second landmark is stationary. The second detector 35 detects a stationary object to be measured as an unstored second landmark (corresponding to a fourth landmark). The second detection unit 35 generates an unstored second landmark identification signal. The second detection unit 35 outputs the identification information of the unstored second landmark and the relative position thereof to the first position calculation unit 36.
The first position calculation unit 36 determines whether two or more known landmarks are detected from the measurement result at time t. That is, the first position calculator 36 determines the number of first landmarks detected by the first detector 34 from the measurement result at time t and the known second landmark detected by the second detector 35 from the measurement result at time t. It is determined whether or not the sum of the numbers is 2 or more. When two or more known landmarks are detected, the first position calculation unit 36 stores the second unstored second based on the relative position and absolute position of the known landmark and the relative position of the unstored second landmark. Calculate the absolute position of the landmark. The first position calculation unit 36 stores the absolute position and identification information of the unstored second landmark in the second landmark storage unit 38. For example, the first position calculation unit 36 calculates the absolute position of the unstored second landmark based on the relative position and absolute position of the two known landmarks and the relative position of the unstored second landmark. It's okay.

  The first position calculation unit 36 may calculate the absolute position of the unstored second landmark based on the relative position and absolute position of the known second landmark and the relative position of the unstored second landmark. . That is, the known landmark used for calculating the absolute position of the unstored second landmark may include the known second landmark. For example, the first position calculation unit 36 is based on the relative position and absolute position of one or more first landmarks and one or more known second landmarks, and the relative position of unstored second landmarks, The absolute position of the unstored second landmark may be calculated.

Further, the first position calculation unit 36 does not use the first landmark, and based on the relative position and absolute position of the known second landmark and the relative position of the unstored second landmark, The absolute position of the two landmarks may be calculated. For example, the first position calculation unit 36 calculates the absolute position of the unstored second landmark based on the relative position and absolute position of the two known second landmarks and the relative position of the unstored second landmark. It may be calculated.
The landmark selection unit 37 determines whether two or more known landmarks have been detected from the measurement result at time t. When two or more known landmarks are detected, the landmark selection unit 37 selects a known landmark. For example, the landmark selection unit 37 selects two known landmarks. The second position calculation unit 33 calculates the absolute position of the vehicle 1 and the direction of the vehicle 1 at time t based on the relative position and absolute position of the selected landmark.

When the number of known landmarks detected from the measurement result at time t is less than two, the landmark selection unit 37 does not have to select a landmark used for calculating the absolute position of the vehicle 1. In this case, the second position calculation unit 33 does not use the landmark for calculating the absolute position of the vehicle 1. For example, the second position calculation unit 33 may calculate the absolute position of the vehicle 1 based on the movement amount (dx, dy, dφ) received from the movement amount calculation unit 32.
The known landmark selected by the landmark selection unit 37 may include only the first landmark, may include both the first landmark and the known second landmark, or only the known second landmark. May be included. For example, the known landmark selected by the landmark selection unit 37 may include two first landmarks, or may include one first landmark and one known second landmark. Alternatively, it may include two known second landmarks.

(Operation)
Next, the operation of the position detection device according to the second embodiment will be described. Please refer to FIG. Steps S30 to S39 described in FIG. 12 are repeatedly executed until an ignition switch (not shown) of the vehicle 1 is switched off. The operations in steps S30 to S33 are the same as the operations in steps S10 to S13 of the position detection device 30 of the first embodiment shown in FIG.
In step S34, the first detector 34 determines the first landmark and the object to be detected from the object measured in step S31 based on the temporary absolute position and orientation of the vehicle 1 calculated in step S32 and the absolute position read in step S33. Detect the measurement object. The second detector 35 detects a known second landmark from the measured object.

In step S35, the first position calculation unit 36 and the landmark selection unit 37 determine whether two or more known landmarks have been detected from the measurement result at time t. When two or more known landmarks are detected (step S35: Y), the operation proceeds to step S36. If two or more known landmarks are not detected (step S35: N), the operation proceeds to step S39.
In step S <b> 36, the second detection unit 35 detects a stationary measurement object as a non-stored second landmark from measurement objects that are not known second landmarks. In step S37, the first position calculation unit 36 calculates the absolute position of the unstored second landmark based on the relative position and absolute position of the two known landmarks and the relative position of the unstored second landmark. The first position calculation unit 36 stores the absolute position of the unstored second landmark in the second landmark storage unit 38.

  In step S38, the landmark selection unit 37 selects any two of the known landmarks. The second position calculation unit 33 calculates the absolute position of the vehicle 1 and the direction of the vehicle 1 at time t based on the relative position and the absolute position of the landmark selected by the landmark selection unit 37. In step S39, the position detection device 30 determines whether the ignition switch of the vehicle 1 is off. If the ignition switch is not off (step S39: N), the operation returns to step S30. When the ignition switch is off (step S39: Y), the operation of the position detection device 30 ends.

Hereinafter, referring to FIGS. 13A, 13B, and 13C, examples of landmarks used by the second embodiment when the vehicle 1 avoids the parked vehicle 80 are described. Will be explained. In the state shown in FIG. 13A, the first detector 34 can detect the first landmarks 50 and 51. Therefore, in step S37 shown in FIG. 12, the absolute position of the unstored second landmark 70a is calculated using the first landmarks 50 and 51. In step S38, the absolute position of the vehicle 1 is calculated using the first landmarks 50 and 51.
In the state shown in FIG. 13B, the parked vehicle 80 blocks the line of sight of the first landmark 51. Therefore, in step S37 shown in FIG. 12, the absolute position of the unstored second landmark 70b is calculated using the first landmark 50 and the known second landmark 70a. In step S38, the absolute position of the vehicle 1 is calculated using the first landmark 50 and the known second landmark 70a.

In the state shown in FIG. 13C, the parked vehicle 80 blocks the line of sight of the first landmark 51. Further, since the distance between the first landmark 50 and the vehicle 1 is too long, the position measuring unit 31 cannot measure the position of the first landmark 50. Therefore, in step S37 shown in FIG. 12, the absolute position of the unstored second landmark 70c is calculated using the known second landmarks 70a and 70b. In step S38, the absolute position of the vehicle 1 is calculated using the known second landmarks 70a and 70b.
The known second landmark 70a in FIG. 13B or the known second landmark 70a or 70b in FIG. 13C corresponds to the third landmark. The unstored second landmark 70b in FIG. 13B or the unstored second landmark 70c in FIG. 13C corresponds to the fourth landmark.

(Effect of 2nd Embodiment)
The position detection device 30 according to the first embodiment stores the calculated absolute position of the second landmark in the second landmark storage unit 38. The position detection device 30 includes a relative position of the known second landmark (corresponding to the third landmark), an absolute position of the known second landmark stored in the second landmark storage unit 38, and an unstored second land. Based on the relative position of the mark (corresponding to the fourth landmark), the absolute position of the unstored second landmark is calculated. The position detection device 30 calculates the position of the vehicle 1 on the map based on the relative position of the second landmark and the position on the map calculated using the relative position and absolute position of the known second landmark. For this reason, the 1st landmark which can measure a relative position is lost, and even if the period which can detect only the 2nd landmark continues, the absolute position of vehicles 1 can be calculated continuously.

(Third embodiment)
(Constitution)
Subsequently, a position detection device 30 according to a third embodiment of the present invention will be described. The functional configuration of the position detection device 30 according to the third embodiment of the present invention is the same as the functional configuration shown in FIG. Refer to FIG. The landmark selection unit 37 according to the third embodiment is used, for example, in the second embodiment to select a landmark used for calculating the absolute position of the vehicle 1 from three or more known landmarks. It's okay.
The landmark selection unit 37 includes a first selection unit 100, an angle calculation unit 101, a distance calculation unit 102, and a second selection unit 103. Processing described later by the first selection unit 100, the angle calculation unit 101, the distance calculation unit 102, and the second selection unit 103 is executed by the controller 10. The first selection unit 100 receives identification information of known landmarks and their relative positions from the first detection unit 34 and the second detection unit 35. The first selection unit 100 selects a plurality of landmark pairs from the received set of known landmarks.

Refer to FIG. Hereinafter, the case where the first landmarks 50a, 50d, 50e, and 50f are detected by the first detector 34 and the second known landmarks 70b and 70c are detected by the second detector 35 will be described. The first selection unit 100 selects the landmark pair 50a and 70b, the landmark pair 50a and 70c, the landmark pair 50a and 50d, the landmark pair 50a and 50e, and the landmark pair 50a and 50f.
The first selection unit 100 selects the landmark pair 70b and 70c, the landmark pair 70b and 50d, the landmark pair 70b and 50e, the landmark pair 70b and 50f, and the landmark pair 70c and 50d. The first selection unit 100 selects the landmark pair 70c and 50e, the landmark pair 70c and 50f, the landmark pair 50d and 50e, the landmark pair 50d and 50f, and the landmark pair 50e and 50f.

In the following description, the landmark pairs 50a and 70b, the landmark pairs 50a and 70c, the landmark pairs 50a and 50d, and the landmark pairs 50a and 50e may be referred to as 50a70b, 50a70c, 50a50d, and 50a50e, respectively. The landmark pair 50a and 50f, the landmark pair 70b and 70c, the landmark pair 70b and 50d, and the landmark pair 70b and 50e may be referred to as 50a50f, 70b70c, 70b50d, and 70b50e, respectively.
The landmark pairs 70b and 50f, the landmark pairs 70c and 50d, and the landmark pairs 70c and 50e may be referred to as 70b50f, 70c50d, and 70c50e, respectively. The landmark pairs 70c and 50f, the landmark pairs 50d and 50e, the landmark pairs 50d and 50f, and the landmark pairs 50e and 50f may be referred to as 70c50f, 50d50e, 50d50f, and 50e50f, respectively. The first selection unit 100 outputs the identification information and the relative position of these landmark pairs to the angle calculation unit 101 and the distance calculation unit 102. The first selection unit 100 outputs the landmark pair identification information to the second selection unit 103.

  The angle calculation unit 101 calculates an angle θ formed by a straight line connecting one landmark included in the landmark pair and the vehicle 1 and a straight line connecting the other landmark included in the landmark pair and the vehicle 1. To do. Hereinafter, an angle formed by a straight line connecting one landmark included in the landmark pair and the vehicle 1 and a straight line connecting the other landmark included in the landmark pair and the vehicle 1 is referred to as a “relative angle”. To do. Further, the distance calculation unit 102 calculates a separation distance L in the moving direction of the vehicle 1 between two landmarks included in the landmark pair. Hereinafter, the separation distance in the moving direction of the vehicle 1 between two landmarks included in the landmark pair is referred to as “relative distance”. FIG. 16 shows the relative angle θ and the relative distance L of the landmark pair 70b50d.

The angle calculation unit 101 outputs the calculated relative angle θ to the second selection unit 103. The distance calculation unit 102 outputs the calculated relative distance L to the second selection unit 103. Based on the relative angle θ and the relative distance L, the second selection unit 103 selects one of the plurality of landmark pairs as a landmark used for calculating the absolute position of the vehicle 1.
The calculation accuracy of the position of the vehicle 1 increases as the relative angle θ increases. However, when the relative distance L is short, there is a possibility that the variation of the relative angle θ accompanying the movement of the vehicle 1 becomes large. For example, when the vehicle 1 is sandwiched between a pair of landmarks having a relatively short relative distance L, the relative angle θ may rapidly decrease as the vehicle 1 moves, and the calculation accuracy may rapidly decrease. For this reason, it is preferable that the relative distance L is relatively long.

For example, the second selection unit 103 may select a landmark pair having a relatively large relative angle θ and a relatively long relative distance L among the landmark pairs selected by the first selection unit 100. For example, the second selection unit 103 may select a landmark pair according to the following procedure. First, the second selection unit 103 defines a first set of landmark pairs so as to include the landmark pairs selected by the first selection unit 100. Next, the second selection unit 103 selects a second set including only landmark pairs having a relatively large relative angle θ and a relatively long relative distance L from the first set, and selects the second set as the second set. Repeat the redefinition of the first set to include only included landmark pairs. The second selection unit 103 selects a landmark pair included in the second set further selected from the redefined first set as a landmark used for calculating the absolute position of the vehicle 1.
For example, the relatively large relative angle θ may be equal to or greater than a threshold angle determined within the numerical range of the relative angle θ of the landmark pairs included in the first set. For example, the relatively long relative distance L may be equal to or greater than a threshold distance determined within a numerical range of the relative distance L of the landmark pairs included in the first set.

Refer to FIG. For example, the second selection unit 103 may include a restriction unit 104, a first classification unit 105, a second classification unit 106, and a third selection unit 107. Processing described later by the restriction unit 104, the first classification unit 105, the second classification unit 106, and the third selection unit 107 is executed by the controller 10.
The restriction unit 104 receives the landmark pair identification information from the first selection unit 100. The restriction unit 104 receives the relative angle θ and the relative distance L of each landmark pair from the angle calculation unit 101 and the distance calculation unit 102. The restriction unit 104 restricts landmark pair candidates used for calculating the position of the vehicle 1 to landmark pairs having a relative distance L equal to or greater than a set value. For this reason, the set value may be 20 m, for example.

  Hereinafter, the case where the relative distance L between the landmark pairs 50a50d, 70b50e, and 70c50f is less than the set value will be described. The restriction unit 104 restricts the landmark pairs used for calculating the position of the vehicle 1 by excluding these landmark pairs 50a50d, 70b50e, and 70c50f. The restriction unit 104 outputs the restricted landmark pair to the first classification unit 105 as a landmark pair candidate used for calculating the position of the vehicle 1. The landmark pair to be output corresponds to the first set.

  Refer to FIG. The first classification unit 105 classifies the received landmark pairs into a first group having a high priority and a second group having a low priority. For example, the first classification unit 105 classifies landmark pairs having a relative angle θ greater than or equal to the threshold angle θt into the first group, and classifies landmark pairs having a relative angle θ less than the threshold angle θt into the second group. It's okay. Also, for example, the first classification unit 105 receives the signal so that the difference in the number of landmark pairs belonging to the first group having a relatively large relative angle θ and the second group having a relatively small relative angle θ is minimized. The landmark pairs may be classified into first and second groups.

  FIG. 17A is a diagram illustrating an example of a first classification result by the first classification unit 105. For example, the first classification unit 105 may use an intermediate value or an average value of the received relative angles θ of the landmark pairs as the threshold angle θt. The first classifying unit 105 classifies the landmark pairs 70b50d, 70c50d, 50d50e, 50d50f, 50a70b, 50a70c, 50a50e, and 50a50f having a relative angle θ equal to or greater than the threshold angle 33 degrees into the first group. The first classification unit 105 classifies the landmark pairs 50e50f, 70b70c, 70c50e, and 70b50f having a relative angle θ less than the threshold angle 33 degrees into the second group.

  Refer to FIG. The first classification unit 105 outputs the identification information of landmark pairs belonging to the first group and the identification information of landmark pairs belonging to the second group to the second classification unit 106. The second classifying unit 106 classifies the landmark pairs belonging to the first group into a third group having a high priority and a fourth group having a low priority. The second classification unit 106 classifies landmark pairs belonging to the second group into a fifth group having a high priority and a sixth group having a low priority. Therefore, the priority of the third group, the fourth group, the fifth group, and the sixth group is higher in the order of the third group, the fourth group, the fifth group, and the sixth group.

For example, the second classifying unit 106 classifies the landmark pairs having the relative distance L equal to or greater than the first threshold distance Lt1 into the third group, and sets the landmark pairs having the relative distance L less than the first threshold distance Lt1 to the fourth group. You may classify into groups. The second classifying unit 106 classifies landmark pairs having a relative distance L equal to or greater than the second threshold distance Lt2 into the fifth group, and sets landmark pairs having a relative distance L less than the second threshold distance Lt2 as the sixth group. May be classified.
In addition, for example, the second classification unit 106 may reduce the difference in the number of landmark pairs belonging to the third group having a relatively long relative distance L and the fourth group having a relatively short relative distance L. The landmark pairs in one group may be classified into third and fourth groups. The second classification unit 106 is configured to reduce the difference in the number of landmark pairs belonging to the fifth group having a relatively long relative distance L and the sixth group having a relatively short relative distance L. The landmark pairs may be classified into fifth and sixth groups. The third group corresponds to the second set.

FIG. 17B is a diagram illustrating an example of a second classification result by the second classification unit 106. For example, the second classification unit 106 may use an intermediate value or an average value of the relative distances L of the landmark pairs belonging to the first group as the first threshold distance Lt1. For example, the second classification unit 106 may use an intermediate value or an average value of the relative distances L of the landmark pairs belonging to the second group as the second threshold distance Lt2.
The second classifying unit 106 classifies the landmark pairs 50a70c, 50a50f, 70c50d, and 50d50f having a relative distance L equal to or greater than the first threshold distance 45m into the third group. The second classification unit 106 classifies the landmark pairs 50a70b, 50a50e, 50d50e, and 70b50d having the relative distance L less than the first threshold distance 45m into the fourth group. The second classifying unit 106 classifies the landmark pairs 50e50f, 70b70c, and 70b50f having a relative distance L equal to or greater than the second threshold distance 29m into the fifth group. The second classifying unit 106 classifies the landmark pair 70c50e having the relative distance L less than the second threshold distance 29m into the sixth group.

Refer to FIG. The second classification unit 106 outputs identification information of landmark pairs belonging to the third group, the fourth group, the fifth group, and the sixth group to the third selection unit 107, respectively. The third selection unit 107 determines whether or not all landmark pairs belong to different groups, that is, whether or not the number of landmark pairs belonging to each of the third to sixth groups is one or less.
When the landmark pair does not belong to different groups, the third selection unit 107 excludes the landmark pair belonging to the fourth to sixth groups from the candidates used for position calculation. The third selection unit 107 selects a landmark pair belonging to the third group having the highest priority and outputs it to the first classification unit 105. The output third group corresponds to the redefined first set.

The first classification unit 105 classifies the landmark pairs received from the third selection unit 107 into a first group and a second group. FIG. 17C is a diagram illustrating an example of a third classification result by the first classification unit 105. The first classification unit 105 classifies the landmark pairs 50a50f and 50a70c having a relative angle θ equal to or greater than the threshold angle of 76 degrees into the first group. The first classification unit 105 classifies the landmark pairs 50d50f and 70c50d having a relative angle θ less than the threshold angle of 76 degrees into the second group.
The second classification unit 106 classifies the landmark pairs belonging to the first group into a third group and a fourth group. The second classification unit 106 classifies the landmark pairs belonging to the second group into a fifth group and a sixth group. FIG. 17D is a diagram illustrating an example of a fourth classification result by the second classification unit 106. The second classification unit 106 classifies the landmark pairs 50a50f having the relative distance L of the first threshold distance 61.5m or more into the third group.

The second classification unit 106 classifies the landmark pairs 50a70c having the relative distance L less than the first threshold distance 61.5m into the fourth group. The second classifying unit 106 classifies the landmark pair 50d50f having the relative distance L equal to or greater than the second threshold distance 58.5 m into the fifth group. The second classification unit 106 classifies the landmark pair 70c50d having the relative distance L less than the second threshold distance 58.5m into the sixth group. The first classification unit 105 and the second classification unit 106 repeat the classification until all landmark pairs belong to different groups.
When all landmark pairs belong to different groups, the third selection unit 107 selects any one of the landmark pairs belonging to the third to sixth groups. For example, the third selection unit 107 may select the landmark pair 50a50f belonging to the group having the highest priority among the non-empty groups. The third selection unit 107 outputs the selected landmark pair to the second position calculation unit 33 as a landmark pair used for calculating the position of the vehicle 1.

(Operation)
FIG. 18 is an explanatory diagram of an example of the operation of the landmark selection unit 37 shown in FIG. In step S40, the first selection unit 100 selects a plurality of landmark pairs from a set including at least one of known landmarks. In step S41, the angle calculation unit 101 and the distance calculation unit 102 calculate the relative angle θ and the relative distance L of each landmark pair. In step S <b> 42, the restriction unit 104 restricts landmark pair candidates used for calculating the position of the vehicle 1 to landmark pairs having a relative distance L equal to or greater than a set value. The restriction unit 104 outputs the restricted landmark pair to the first classification unit 105.
In step S43, the first classification unit 105 classifies the received landmark pairs into a first group having a high priority and a second group having a low priority according to the relative angle θ. In step S44, the second classification unit 106 classifies the landmark pairs belonging to the first group into a third group having a high priority and a fourth group having a low priority according to the relative distance L. The second classifying unit 106 classifies the landmark pairs belonging to the second group into a fifth group having a high priority and a sixth group having a low priority according to the relative distance L.

In step S45, the third selection unit 107 determines whether or not all landmark pairs belong to different groups. When all the landmark pairs do not belong to different groups (step S45: N), the operation proceeds to step S46. If all landmark pairs do not belong to different groups (step S45: Y), the operation proceeds to step S47.
In step S46, the third selection unit 107 excludes the landmark pairs belonging to the fourth to sixth groups from the candidates used for position calculation. The third selection unit 107 selects landmark pairs belonging to the third group and outputs them to the first classification unit 105. Thereafter, the operation returns to step S43. In step S <b> 47, the third selection unit 107 selects any one of the landmark pairs belonging to the third to sixth groups, and outputs the selected landmark pair to the second position calculation unit 33.

(Effect of the third embodiment)
The position detection device 30 according to the first embodiment selects a plurality of landmark pairs from a set including at least one of the first landmark and the second landmark. The position detection device 30 includes a relative angle θ between straight lines connecting the two landmarks included in each of the plurality of landmark pairs and the vehicle 1, and two lands included in each of the plurality of landmark pairs. The relative distance L in the movement direction of the vehicle 1 at the mark interval is calculated. The position detection device 30 selects any one of the plurality of landmark pairs based on the relative angle θ and the relative distance L, and uses it for calculating the absolute position of the vehicle 1. For this reason, it is possible to calculate the absolute position of the vehicle 1 using a landmark pair that increases the accuracy of calculating the position of the vehicle 1.

(Modification)
The landmark selection unit 37 according to the third embodiment is used, for example, to select a landmark to be used for calculating the absolute position of the vehicle 1 from three or more first landmarks in the first embodiment described above. You can do it. Further, the landmark selection unit 37 according to the third embodiment, for example, in the first embodiment described above, in order to select a landmark used for calculating the absolute position of the vehicle 1 from three or more known landmarks. May be used.
When there are three or more landmark pairs belonging to the first group, two or more landmark pairs belong to either the third or fourth group, so that the landmark pairs belonging to the fourth to sixth groups It can be determined that it is excluded from the candidates used for position calculation. Therefore, when there are three or more landmark pairs belonging to the first group, the second classification unit 106 may omit the process of classifying the landmark pairs belonging to the second group into the fifth group and the sixth group. Good.

In step S45, the third selection unit 107 may determine whether or not there is one landmark pair belonging to the group having the highest priority among the non-empty groups among the third to sixth groups. When there is one landmark pair belonging to the group having the highest priority, the third selection unit 107 belongs to the group having the highest priority even if two or more landmark pairs belong to other groups. A landmark pair is selected and output to the second position calculator 33.
Although the present invention has been described with reference to a limited number of embodiments, the scope of rights is not limited thereto, and modifications of the embodiments based on the above disclosure are obvious to those skilled in the art.

DESCRIPTION OF SYMBOLS 3 Vehicle speed sensor 5 Steering angle sensor 6 Position measurement sensor 7 Yaw rate sensor 10 Controller 20 Map information storage part 30 Position detection apparatus 31 Position measurement part 32 Movement amount calculation part 33 2nd position calculation part 34 1st detection part 35 2nd detection part 36 first position calculation unit 37 landmark selection unit 100 first selection unit 101 angle calculation unit 102 distance calculation unit 103 second selection unit 104 restriction unit 105 first classification unit 106 second classification unit 107 third selection unit

Claims (10)

A position measuring unit for measuring a relative position of an object around the moving body with respect to the moving body;
A location storage unit that stores locations of landmarks on the map;
Among the objects measured by the position measurement unit, a first landmark whose position on the map is stored in the position storage unit, and an object to be measured whose position on the map is not stored in the position storage unit A first detector for detecting
A second detection unit that detects the measured object that is detected by the first detection unit and is stationary as a second landmark;
A first position calculation unit that calculates a position of the second landmark on the map based on the relative position of the first landmark and the position on the map and the relative position of the second landmark;
A second position calculating unit that calculates a position of the moving body on the map based on at least the relative position of the second landmark and the position on the map;
A position detection device comprising:   The position detection device according to claim 1, wherein the second detection unit determines whether or not the object to be measured is stationary according to a change in the relative position of the object to be measured.   The second detection unit determines whether the object to be measured is stationary according to a comparison result between the change in the relative position of the object to be measured and the change in the relative position of the first landmark. The position detection apparatus according to claim 2, wherein:   When at least one of the first landmarks and at least one of the second landmarks are detected, the second position calculation unit determines that the relative position of the at least one of the first landmarks and the map The position of the moving body on the map is calculated based on the position of the moving object, the relative position of the at least one second landmark, and the position on the map. The position detection device according to one item. The first position calculation unit is based on the relative position of the first landmark and the relative position of the second landmark measured at a first time point, and the position of the first landmark on the map. Calculate the location of the second landmark on the map,
When the first landmark is not detected at the second time point after the first time point and two or more second landmarks are detected, the second position calculating unit is configured to output at least two of the second landmarks. 5. The position detection device according to claim 1, wherein the position of the moving body on the map is calculated based on the relative position of the two landmarks and the position on the map. The first position calculation unit is based on the relative position of the first landmark and the relative position of the second landmark measured at a first time point, and the position of the first landmark on the map. Calculate the location of the second landmark on the map,
The second position calculation unit is configured to determine the moving body based on at least the relative position of the second landmark and the position of the second landmark on the map measured at a second time point after the first time point. The position detection device according to claim 1, wherein the position on the map is calculated. A second landmark storage unit for storing a position of the second landmark on the map calculated by the first position calculation unit;
The second landmark includes a third landmark and a fourth landmark,
The first position calculation unit includes the relative position of the third landmark, the position of the third landmark on the map stored in the second landmark storage unit, and the relative of the fourth landmark. And calculating the position of the fourth landmark on the map based on the position;
The said 2nd position calculation part calculates the position on the map of the said mobile body based on the said relative position of the said 4th landmark and the position on a map at least. The position detection device according to claim 1. A first selection unit that selects a plurality of landmark pairs from a set including at least one of the first landmark and the second landmark;
An angle calculating unit that calculates an angle between straight lines connecting two landmarks included in each of the plurality of landmark pairs and the moving body;
A distance calculating unit that calculates a distance in the moving direction of the moving body at an interval between two landmarks included in each of the plurality of landmark pairs;
A second selection unit that selects any one of the plurality of landmark pairs based on the angle and the distance; and
The second position calculation unit calculates a position on the map of the moving body based on the relative position of the landmark pair selected by the second selection unit and the position on the map. The position detection device according to any one of claims 1 to 7.   2. The second detection unit determines whether or not the object to be measured having a surface including an L-shaped portion having an outwardly convex horizontal cross-sectional shape is the second landmark. The position detection apparatus as described in any one of -8. Measuring the relative position of an object around the moving body relative to the moving body;
Among the measured objects, a first landmark whose position on a map is stored in advance in a storage device and a measured object whose position on the map is not stored in the storage device are detected,
Detecting the object under measurement as a second landmark,
Based on the relative position of the first landmark and the position on the map and the relative position of the second landmark, the position of the second landmark on the map is calculated,
A position detection method comprising: calculating a position of the moving body on a map based on at least the relative position of the second landmark and a position on the map.

Images