WO2020085062A1 - 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体 - Google Patents

計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体 Download PDF

Info

Publication number
WO2020085062A1
WO2020085062A1 PCT/JP2019/039479 JP2019039479W WO2020085062A1 WO 2020085062 A1 WO2020085062 A1 WO 2020085062A1 JP 2019039479 W JP2019039479 W JP 2019039479W WO 2020085062 A1 WO2020085062 A1 WO 2020085062A1
Authority
WO
WIPO (PCT)
Prior art keywords
feature
measurement
unit
information
accuracy
Prior art date
Application number
PCT/JP2019/039479
Other languages
English (en)
French (fr)
Inventor
加藤　正浩
Original Assignee
パイオニア株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by パイオニア株式会社 filed Critical パイオニア株式会社
Priority to JP2020553079A priority Critical patent/JP7155284B2/ja
Priority to EP19876435.9A priority patent/EP3872454A4/en
Priority to US17/272,920 priority patent/US20210278217A1/en
Publication of WO2020085062A1 publication Critical patent/WO2020085062A1/ja
Priority to JP2022160979A priority patent/JP2022188203A/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/14—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by recording the course traversed by the object
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle

Definitions

  • the present invention relates to a technique for calculating the accuracy of measurement by the measuring unit.
  • Patent Document 1 discloses a technique of estimating the self-position by collating the output of a measurement sensor with the position information of a feature registered on a map in advance.
  • Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.
  • the other feature is included in the measurement data together with the feature to be measured, and the accuracy of the measurement result (accurate Sex) may decrease.
  • the accuracy of the measurement result may decrease.
  • the present invention has been made to solve the above problems, and mainly provides a measurement accuracy calculation device and a self-position estimation device that can preferably calculate the measurement accuracy of a feature by a measurement unit. To aim.
  • the invention according to claim is a measurement accuracy calculation device, and a first acquisition unit that acquires a measurement result of a feature by a measurement unit, and a second acquisition unit that acquires feature information of the feature included in map data.
  • the invention according to the claims is a self-position estimating device, and acquires a first acquisition unit that acquires a measurement result of a feature by a measurement unit and the feature information of the feature included in map data.
  • the second acquisition unit, the third acquisition unit that acquires predicted position information indicating the predicted self position, and the difference between the measured size of the feature and the size of the feature indicated by the feature information.
  • a correction unit that corrects the predicted self-position.
  • the invention described in the claims is a control method executed by a measurement accuracy calculation device, comprising a first acquisition step of acquiring a measurement result of a feature by a measurement unit, and a feature of the feature included in map data. Accuracy information indicating the measurement accuracy of the feature by the measuring unit based on the second acquisition step of acquiring the feature information and the difference between the measured size of the feature and the size of the feature indicated by the feature information. And a calculation step of calculating.
  • the invention described in the claims is a program executed by a computer, and acquires a first acquisition unit that acquires a measurement result of a feature by a measurement unit, and acquires the feature information of the feature included in the map data. And a calculation unit that calculates accuracy information indicating the measurement accuracy of the feature by the measurement unit, based on the difference between the measured feature size and the feature size indicated by the feature information.
  • the computer is caused to function as.
  • the measurement accuracy calculation device includes a first acquisition unit that acquires the measurement result of the feature by the measurement unit and a first acquisition unit that acquires the feature information of the feature included in the map data. 2 acquisition unit, a calculation unit that calculates accuracy information indicating the measurement accuracy of the feature by the measurement unit, based on the difference between the measured feature size and the feature size indicated by the feature information, Have.
  • the measurement accuracy calculation device measures the measurement data of the other feature adjacent to the measurement target feature when occlusion occurs in the measurement target feature and some measurement data cannot be acquired. In any case where it is acquired as a part of the measurement data of the target feature, the accuracy information indicating the measurement accuracy of the feature by the measuring unit can be accurately calculated.
  • the calculation unit generates accuracy information with lower measurement accuracy as the difference is larger. By doing this, when occlusion occurs in the measurement target feature and some measurement data cannot be acquired, or the measurement data of the adjacent feature is acquired as part of the measurement data of the measurement target feature. In either case, it is possible to preferably generate accuracy information indicating that the measurement accuracy is low.
  • the calculation unit based on the difference, the moving direction of the moving body, and the normal direction of the feature indicated by the feature information, the moving direction of the moving body. Accuracy information indicating the measurement accuracy of the feature for each of the lateral directions is generated. Generally, depending on the direction of the feature with respect to the traveling direction of the moving body, there may be a direction in which a measurement error is unlikely to occur and a direction in which a measurement error is likely to occur. Therefore, according to this aspect, the measurement accuracy calculation device can suitably generate the accuracy information indicating the measurement accuracy of the feature for each of the traveling direction and the lateral direction of the moving body.
  • the calculation unit is such that, as the angular difference between the traveling direction and the normal line direction of the feature is larger, the degree of influence of the difference on the measurement accuracy in the lateral direction. And the smaller the angle difference, the lower the degree of influence of the difference on the measurement accuracy in the traveling direction.
  • the normal direction of the feature here refers to a direction within 90 degrees from the traveling direction of the moving body, of the front direction and the back direction of the feature that are perpendicular to the plane formed by the feature. To do. In this case, the larger the difference in angle between the traveling direction of the moving body and the normal direction of the feature, the more the feature is lateral to the moving body, and thus the measurement error in the lateral direction of the moving body is less likely to occur.
  • the measurement accuracy calculation device can generate the accuracy information that accurately represents the measurement accuracy of the feature of the moving body in each of the traveling direction and the lateral direction.
  • the measurement accuracy calculation device calculates the predicted self position based on the third acquisition unit that acquires predicted position information indicating the predicted self position and the accuracy information. And a correction unit that corrects.
  • the measurement accuracy calculation device can preferably execute the self-position estimation reflecting the generated accuracy information.
  • the correction unit is predicted based on a distance measured by the measurement unit from a moving body to the feature and position information of the feature included in the feature information. Further, the gain for the difference value when the predicted self-position is corrected by the difference value between the predicted distance from the moving body to the feature and the predicted value is determined based on the accuracy information. According to this aspect, the measurement accuracy calculation device prevents inaccurate correction by adjusting the gain based on the accuracy information when correcting the predicted self-position with the difference value between the measured distance and the predicted distance. It is possible to preferably improve the self-position estimation accuracy.
  • the self-position estimation device includes a first acquisition unit that acquires a measurement result of the feature by the measurement unit, and the feature information of the feature included in the map data. Based on a difference between the measured size of the feature and the size of the feature indicated by the feature information, a second acquisition unit that acquires the predicted position information, and a third acquisition unit that acquires the predicted position information indicating the predicted self-position. And a correction unit that corrects the predicted self-position.
  • the difference described above is obtained when occlusion occurs in the measurement target feature and some measurement data cannot be acquired, or when the measurement data of another feature adjacent to the measurement target feature is measured. It occurs in any case when acquired as a part of measurement data. Therefore, according to this aspect, the self-position estimation device can appropriately correct the predicted self-position in consideration of the above-mentioned difference to determine the final self-position.
  • a control method executed by the measurement accuracy calculation device comprising: a first acquisition step of acquiring a measurement result of a feature by the measurement unit; A second acquisition step of acquiring the feature information of the feature and a measurement accuracy of the feature by the measurement unit based on a difference between the measured size of the feature and the size of the feature indicated by the feature information. A calculation step of calculating accuracy information.
  • the measurement accuracy calculation device causes occlusion in the feature to be measured and some measurement data cannot be acquired, or another feature adjacent to the feature to be measured. In any case where the measurement data of 1 is acquired as a part of the measurement data of the feature to be measured, it is possible to accurately calculate the accuracy information indicating the measurement accuracy of the feature by the measurement unit.
  • a program executed by a computer the first acquisition unit acquiring a measurement result of the feature by the measurement unit, and the feature of the feature included in the map data.
  • Accuracy information indicating the measurement accuracy of the feature by the measurement unit is calculated based on a difference between the second acquisition unit that acquires information and the measured size of the feature and the size of the feature indicated by the feature information.
  • the computer is caused to function as a calculation unit for By executing this program, the computer executes occlusion on the feature to be measured and some measurement data cannot be acquired, or it measures the measurement data of other features adjacent to the feature to be measured. In any case where it is acquired as a part of the measurement data of the feature to be measured, it is possible to accurately calculate accuracy information indicating the measurement accuracy of the feature by the measurement unit.
  • the program is stored in a storage medium.
  • FIG. 1 is a schematic configuration of the driving support system according to the present embodiment.
  • the driving support system shown in FIG. 1 is mounted on a vehicle and performs on-vehicle device 1 control for driving support of the vehicle; And a vehicle speed sensor 4 and a GPS receiver 5.
  • the “map” is assumed to include ADAS (Advanced Driver Assistance System) and data used for automatic driving, in addition to the data referred to by the conventional vehicle-mounted device for route guidance.
  • ADAS Advanced Driver Assistance System
  • the vehicle-mounted device 1 is electrically connected to the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and based on these outputs, the position of the vehicle on which the vehicle-mounted device 1 is mounted (“own vehicle position”). Also called)) is estimated. Then, the in-vehicle device 1 performs automatic driving control of the vehicle so as to travel along the set route to the destination based on the estimation result of the vehicle position.
  • the vehicle-mounted device 1 stores a map database (DB: Data Base) 10 that stores road information and feature information that is information about a landmark provided near the road.
  • DB Data Base
  • the above-mentioned landmark features are, for example, kiloposts, 100m posts, delineators, transportation infrastructure equipment (eg, signs, direction signs, signals), telephone poles, streetlights, and the like that are periodically arranged beside the road. Then, the vehicle-mounted device 1 compares the output of the rider 2 and the like with the feature information to estimate the position of the own vehicle.
  • the vehicle-mounted device 1 is an example of the “measurement accuracy calculation device” and the “self-position estimation device” in the present invention.
  • the map DB 10 may be stored in an external storage device such as an external server instead of being stored in the vehicle-mounted device 1. In that case, the vehicle-mounted device 1 acquires at least a part of the map DB 10 from an external storage device via wireless communication or the like.
  • the lidar 2 discretely measures the distance to an object existing in the external world by emitting a pulse laser in a predetermined angle range in the horizontal direction and the vertical direction, and a three-dimensional point indicating the position of the object. Generate group information.
  • the lidar 2 includes an irradiation unit that emits laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) that the emitted laser light reflects from an object, and a light receiving signal that the light receiving unit outputs. And an output unit for outputting scan data based on the above.
  • the scan data is point cloud data, and is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the distance to the object in the irradiation direction, which is specified based on the above-described light reception signal.
  • the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 respectively supply output data to the vehicle-mounted device 1.
  • the rider 2 is an example of a “measuring unit”.
  • FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device 1.
  • the vehicle-mounted device 1 mainly includes an interface 11, a storage unit 12, an input unit 14, a control unit 15, and an information output unit 16. Each of these elements is connected to each other via a bus line.
  • the interface 11 acquires output data from sensors such as the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies the output data to the control unit 15.
  • the storage unit 12 stores a program executed by the control unit 15 and information necessary for the control unit 15 to execute a predetermined process.
  • the storage unit 12 stores the map DB 10 including the feature information.
  • FIG. 3 shows an example of the data structure of the map DB 10. As shown in FIG. 3, the map DB 10 includes facility information, road data, and feature information.
  • the feature information is information in which information about the feature is associated with each feature, and here, the feature ID corresponding to the feature index, position information, orientation information (normal line information), Including size information.
  • the position information indicates the absolute position of the feature represented by latitude and longitude (and altitude).
  • the orientation information and the size information are information provided for a feature having a planar shape such as a signboard, for example.
  • the orientation information is information indicating the orientation of the feature and indicates, for example, a normal vector to a surface formed on the feature.
  • the size information is information indicating the size of the feature, and in this embodiment, the size information includes the width and height of the measured surface formed on the feature.
  • the map DB 10 may be updated regularly.
  • the control unit 15 receives the partial map information regarding the area to which the own vehicle position belongs from the server device that manages the map information via the communication unit (not shown), and reflects it in the map DB 10.
  • the input unit 14 is a button for a user to operate, a touch panel, a remote controller, a voice input device, or the like.
  • the information output unit 16 is, for example, a display, a speaker, or the like that outputs under the control of the control unit 15.
  • the control unit 15 includes a CPU that executes a program and controls the entire vehicle-mounted device 1.
  • the control unit 15 has a vehicle position estimation unit 17 that estimates the vehicle position based on the output signals of the sensors supplied from the interface 11 and the map DB 10. Then, the control unit 15 performs control related to vehicle driving support including automatic driving control based on the estimation result of the vehicle position.
  • the control unit 15 is an example of the “first acquisition unit”, the “second acquisition unit”, the “third acquisition unit”, the “calculation unit”, the “correction unit”, and the “computer” that executes the program according to the present invention. is there.
  • the vehicle position estimation unit 17 uses the gyro based on the measured values of the distance and angle by the rider 2 with respect to the feature (also referred to as “landmark”) to be measured and the position information of the landmark extracted from the map DB 10.
  • the own vehicle position estimated from the output data of the sensor 3, the vehicle speed sensor 4, and / or the GPS receiver 5 is corrected.
  • the host vehicle position estimation unit 17 predicts the host vehicle position from the output data of the gyro sensor 3, the vehicle speed sensor 4, etc. based on the state estimation method based on Bayesian estimation, and the immediately preceding step.
  • the measurement update step of correcting the predicted value of the vehicle position calculated in the prediction step is alternately executed.
  • Various filters developed to perform Bayesian estimation can be used as the state estimation filter used in these steps, and examples thereof include an extended Kalman filter, an unscented Kalman filter, and a particle filter. As described above, various methods have been proposed for position estimation based on Bayesian estimation.
  • FIG. 4 is a diagram showing the state variable vector in two-dimensional Cartesian coordinates.
  • the vehicle position on the plane defined on the two-dimensional Cartesian coordinates of xy is represented by the coordinates "(x, y)" and the direction (yaw angle) " ⁇ " of the vehicle.
  • the yaw angle ⁇ is defined as the angle between the traveling direction of the vehicle and the x axis.
  • the coordinates (x, y) are, for example, absolute positions corresponding to a combination of latitude and longitude, or world coordinates indicating a position with a predetermined point as the origin.
  • FIG. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step.
  • FIG. 6 shows an example of functional blocks of the vehicle position estimation unit 17. As shown in FIG. 5, by repeating the prediction step and the measurement update step, the calculation and update of the estimated value of the state variable vector “X” indicating the vehicle position are sequentially executed. Further, as shown in FIG. 6, the vehicle position estimation unit 17 includes a position estimation unit 21 that executes a prediction step and a position estimation unit 22 that executes a measurement update step.
  • the position prediction unit 21 includes a dead reckoning block 23 and a position prediction block 24, and the position estimation unit 22 includes a landmark search / extraction block 25 and a position correction block 26.
  • FIG. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step.
  • FIG. 6 shows an example of functional blocks of the vehicle position estimation unit 17. As shown in FIG. 5, by repeating the prediction step and the measurement update step, the calculation and update of the estimated value of the state variable vector “X” indicating the vehicle
  • the provisional estimated value (predicted value) estimated in the prediction step is marked with " - " above the character representing the predicted value, and the estimated value with higher accuracy updated in the measurement update step. Is attached with " ⁇ " on the character representing the value.
  • the position prediction block 24 of the vehicle position estimation unit 17 adds the obtained moving distance and azimuth change to the state variable vector X ⁇ (t-1) at time t-1 calculated in the immediately preceding measurement update step.
  • a predicted value of the vehicle position at time t also referred to as “predicted vehicle position”
  • X ⁇ (t) is calculated.
  • the covariance matrix “P ⁇ (t)” corresponding to the error distribution of the predicted vehicle position X ⁇ (t) is set to the covariance at time t ⁇ 1 calculated in the immediately preceding measurement update step. It is calculated from the matrix "P ⁇ (t-1)".
  • the landmark search / extraction block 25 of the own vehicle position estimation unit 17 associates the position vector of the measurement target feature (landmark) registered in the map DB 10 with the scan data of the rider 2. To do. Then, the landmark search / extraction block 25 of the own vehicle position estimation unit 17, when this association is possible, the measured value “Z (t)” of the associated landmarks by the rider 2 and the predicted self.
  • the measured value of the landmark obtained by modeling the measurement process by the rider 2 using the vehicle position X ⁇ (t) and the position vector of the landmark registered in the map DB 10 (referred to as “measurement predicted value”) “Z. - (T) "are acquired.
  • the measurement value Z (t) is a vector value in the vehicle coordinate system in which the distance and scan angle of the landmark measured by the rider 2 at time t are converted into components having axes in the traveling direction and the lateral direction of the vehicle. Then, the position correction block 26 of the vehicle position estimation unit 17 calculates the difference value between the measured value Z (t) and the measured predicted value Z ⁇ (t).
  • the landmark has a predetermined size, and data corresponding to a plurality of measurement points on the irradiated surface irradiated with the laser light of the lidar 2 is acquired as point cloud data. Therefore, for example, the landmark search / extraction block 25 averages the measurement values at the measurement points at which the target landmark is measured to obtain the measurement value Z (t) corresponding to the center position of the target landmark. calculate.
  • the position correction block 26 of the vehicle position estimation unit 17 calculates the Kalman gain “K” as the difference value between the measured value Z (t) and the measured predicted value Z ⁇ (t) as shown in the following equation (1).
  • (T) ” is multiplied and this is added to the predicted vehicle position X ⁇ (t) to calculate the updated state variable vector (also referred to as“ estimated vehicle position ”) X ⁇ (t).
  • the position correction block 26 of the vehicle position estimation unit 17 has a covariance matrix P ⁇ (t) (simply, as in the prediction step, which corresponds to the error distribution of the estimated vehicle position X ⁇ (t). (Also referred to as P (t)) from the covariance matrix P ⁇ (t).
  • Parameters such as the Kalman gain K (t) can be calculated in the same manner as a known self-position estimation technique using an extended Kalman filter, for example.
  • the vehicle position estimation unit 17 uses the Kalman gain K (t) as a reliability which will be described later, which is an index indicating the reliability of the measured value Z (t) (that is, the measured distance to the center point of the landmark). Correct according to the degree information.
  • the Kalman gain K (t) is lowered to predict the predicted vehicle position X. - to reduce the correction amount for (t). In this case, inaccurate correction of the predicted vehicle position X ⁇ (t) is preferably suppressed, and the self-position estimation accuracy is improved.
  • the vehicle position estimation unit 17 selects one set of selected features.
  • the measurement updating step may be performed based on the measurement predicted value and the measured value, or the measurement updating step may be performed a plurality of times based on all the measured predicted values and the measured values that are associated with each other.
  • the own vehicle position estimation unit 17 considers that the rider measurement accuracy deteriorates as the feature is farther from the rider 2, and the rider 2 and the feature are considered. The longer the distance, the smaller the weighting for the feature.
  • the prediction step and the measurement update step are repeatedly performed, and the predicted vehicle position X ⁇ (t) and the estimated vehicle position X ⁇ (t) are sequentially calculated, so that the most probable vehicle position is obtained. Is calculated.
  • the measured value Z (t) is an example of the “measured distance” in the present invention
  • the measured predicted value Z ⁇ (t) is an example of the “predicted distance” in the present invention.
  • the vehicle position estimation unit 17 determines the size of the landmark indicated by the size information included in the feature information on the map and the range in which the measurement points of the landmark measured by the rider 2 are distributed (measurement range). ), The reliability information is calculated by normalizing the difference. As a result, the vehicle position estimation unit 17 calculates the reliability information that accurately represents the reliability of the measurement value Z (t).
  • the own vehicle position estimating unit 17 determines the horizontal width “W M ” and the vertical width (width in the height direction) “H M ” of the landmark included in the feature information on the map, and the rider 2 of the time t. Based on the lateral width “W L (t)” and the vertical width “H L (t)” of the landmark measurement range specified based on the point cloud data and the sensitivity coefficient “k”, the following formula (2) is obtained. As shown, the reliability information a (t) is determined.
  • the reliability information a (t) is normalized so as to be in the range of 0 to 1, and the measured horizontal width W L (t) and vertical width H L (t) are map.
  • the closer to the upper horizontal width W M and the vertical width H M the closer to the maximum value of 1.
  • the horizontal width W L (t) and the vertical width H L (t) become closer to 0 as the distance from the horizontal width W M and the vertical width H M increases.
  • the sensitivity coefficient k is the sensitivity to the reliability information a (t) with respect to the difference between the measured width W L (t) and height H L (t) and the width W M and height H M on the map.
  • the reliability information a (t) calculated becomes smaller as the sensitivity coefficient k becomes larger.
  • the sensitivity coefficient k is set to a predetermined value, for example.
  • FIG. 7A shows a horizontal width W L (t) and a formula (2) when a vertical width H L (t) is fixed to 60 cm with respect to a landmark having a horizontal width W M of 60 cm and a vertical width H M of 60 cm. 2) is a two-dimensional graph showing a relationship with the reliability information a (t) based on (4).
  • FIG. 7B shows a vertical width H L (t) and reliability information a (t) when the horizontal width W L (t) is fixed to 60 cm for the same landmark as in FIG. 7A. It is a two-dimensional graph which shows the relationship with. Further, FIG.
  • 7C shows the relationship between the horizontal width W L (t), the vertical width H L (t), and the reliability information a (t) for the same landmark as in FIGS. 7A and 7B. It is a three-dimensional graph shown. 7D is a graph showing the reciprocal “1 / a (t)” of the reliability information a (t) in FIG. 7A.
  • the reliability information a (t) is closer to 1 which is the maximum value.
  • the reliability information a (t) is the maximum value 1.
  • the reciprocal of the reliability information a (t) approaches 1 which is the minimum value as the width W L (t) approaches the width W M, and the width W L (t). Becomes larger as the distance from the width W M increases.
  • the reciprocal of the reliability information a (t) approaches 1 which is the minimum value as the vertical width H L (t) approaches the vertical width H M , and the vertical width H L (t) changes from the vertical width H M. The larger the distance, the larger the value.
  • the reciprocal of the reliability information a (t) is used when setting the Kalman gain according to the reliability information, as described later.
  • FIG. 8A is a diagram schematically showing a laser light irradiation range of the lidar 2 when occlusion occurs during measurement of the landmark 20 as a landmark by the lidar 2
  • FIG. [FIG. 3] is a plan view of a feature 20 that clearly shows measurement points on a surface to be illuminated on which laser light of the lidar 2 is illuminated.
  • a road sign having a high reflection intensity of laser light is used as an example of the feature 20.
  • the vehicle position estimation unit 17 recognizes that the feature 20 to be a landmark exists within the measurement range of the rider 2, and sets the window 21 indicating the range in which the feature 20 is predicted to exist. .
  • the own vehicle position estimation unit 17 identifies a feature 20 whose landmark information includes position information indicating a position within a predetermined distance from the predicted own vehicle position X ⁇ (t) as a landmark, and the feature 20 A rectangular area of a predetermined size centered on the position indicated by the position information is set as the window 21.
  • the vehicle position estimation unit 17 may determine the size of the window 21 by further referring to the size information of the feature 20. Then, the vehicle position estimation unit 17 regards the point cloud data of the measurement points indicating the position in the window 21 and the reflection intensity of which is equal to or higher than a predetermined threshold as the point cloud data of the feature 20.
  • the vehicle position estimation unit 17 sets the reliability information a (t) to a value smaller than 1 based on the equation (2) (see FIGS. 7A and 7C).
  • the vehicle position estimation unit 17 averages the measurement values at the measurement points in the window 21 to calculate the estimated vehicle position X ⁇ (t) (see Formula (1)) used for the feature 20.
  • the measured value Z (t) of is calculated.
  • the measured value Z (t) calculated by the averaging described above is a value that should be originally calculated (that is, the center position of the feature 20). The value deviates to the right from the value indicating.
  • the own vehicle position estimation unit 17 determines that the reliability information a (when the occlusion occurs in the landmark and the measured value Z (t) is deviated. t) can be set to a low value. As a result, as will be described later, the vehicle position estimation unit 17 reduces the correction amount for the predicted vehicle position X ⁇ (t) when the reliability of the measured value Z (t) is low to prevent inaccurate correction. Thus, the self-position estimation accuracy is preferably improved.
  • FIG. 9A is a diagram schematically showing the irradiation range of the laser light of the lidar 2 when another feature 25 exists adjacent to the feature 20 to be a landmark
  • FIG. 9A is a plan view of the feature 20 in which the measurement points on the irradiated surface irradiated with the laser light of the lidar 2 are clearly shown in FIG. 9A.
  • the own vehicle position estimation unit 17 sets the window 21 and then sets the point cloud data of the measurement points in the window 21 at which the reflection intensity is equal to or higher than a predetermined threshold to the point cloud data of the feature 20.
  • the window 21 also includes some measurement points of the feature 25 adjacent to the feature 20.
  • the vehicle position estimation unit 17 determines the reliability information a (t) based on the equation (2) because the difference between the width W L (t) and the width W M on the map becomes large. It is set to a value smaller than 1 (see FIGS. 7A and 7C).
  • the vehicle position estimation unit 17 averages the measurement values at the measurement points in the window 21 to calculate the estimated vehicle position X ⁇ (t) (see Formula (1)) used for the feature 20.
  • the measured value Z (t) of is calculated.
  • the point cloud data in the window 21 includes the point cloud data obtained by measuring the feature 25 adjacent to the feature 20 in addition to the point cloud data obtained by measuring the feature 20.
  • the measured value Z (t) calculated by averaging is a value that is displaced to the left from the value that should be calculated (that is, the value indicating the center position of the feature 20).
  • the vehicle position estimation unit 17 even when the measured value Z (t) is deviated due to the fact that another feature is adjacent to the feature to be measured, the vehicle position estimation unit 17, the reliability information a (t) can be set to a low value. As a result, as will be described later, the vehicle position estimation unit 17 reduces the correction amount for the predicted vehicle position X ⁇ (t) when the reliability of the measured value Z (t) is low to prevent inaccurate correction. Thus, the self-position estimation accuracy is preferably improved.
  • the vehicle position estimation unit 17 can appropriately set the reliability information a (t) indicating that the reliability of the measurement value is low, based on the equation (2). it can.
  • the vehicle position estimation unit 17 refers to the orientation information included in the feature information, and based on the relative orientation of the landmark with respect to the traveling direction of the vehicle, the traveling direction and the lateral direction of the vehicle.
  • the reliability information of each is calculated.
  • the reliability information in the traveling direction of the vehicle at time t is “a X (t)”
  • the reliability information in the lateral direction is “a Y (t)”.
  • the illuminated surface of the landmark is perpendicular to the traveling direction of the vehicle.
  • the center point of the measured value Z (t) may deviate.
  • a large error does not occur in the measured value L X (t) of the vehicle traveling direction component. Therefore, it is not necessary to make the value of the reliability information a X (t) too small.
  • an error occurs in the measured value L Y (t) of the lateral component of the vehicle, it is necessary to reduce the value of the reliability information a Y (t).
  • the vehicle position estimation unit 17 determines the sensitivity coefficients “k X ” and “k Y ” that are different when calculating the reliability information a X (t) and the reliability information a Y (t). Each of them is used and replaced with the sensitivity coefficient k of the equation (2) for calculation. Specifically, the vehicle position estimation unit 17 calculates the reliability information a X (t) by the following formula (3) using the sensitivity coefficient k X, and the following formula (3) using the sensitivity coefficient k Y. 4) calculates aY (t).
  • the vehicle position estimating unit 17 determines these sensitivity coefficients k X and k Y based on the relative orientation of the landmark with respect to the traveling direction of the vehicle. Specifically, the vehicle position estimation unit 17 decreases the sensitivity coefficient k X with respect to the reliability information a X (t) as the landmark direction and the traveling direction of the vehicle become closer to each other, and the reliability information a The sensitivity coefficient k Y for Y (t) is increased. On the other hand, the vehicle position estimation unit 17 increases the sensitivity coefficient k X with respect to the reliability information a X (t) as the landmark direction and the traveling direction of the vehicle become closer to vertical, and the reliability information a Y (t). to reduce the sensitivity coefficient k Y against).
  • FIG. 10 is a diagram showing a positional relationship between a vehicle and a landmark represented by a two-dimensional world coordinate system (absolute coordinate system) and a vehicle coordinate system.
  • the world coordinate system has a coordinate point “x w ” and a coordinate axis “y w ” which are perpendicular to each other with a predetermined point as the origin
  • the vehicle coordinate system has the center of the vehicle as the origin and follows the traveling direction of the vehicle. and having coordinate axes "x b" and coordinate "y b" along the side direction of the vehicle.
  • the yaw angle of the vehicle in the world coordinate system is “ ⁇ ⁇ ”
  • the position of the vehicle is [x ⁇ , y ⁇ ] T.
  • the vehicle position estimation unit 17 determines the sensitivity coefficients k X and k Y by the following formulas (5) and (6) using “M ⁇ ⁇ ⁇ ” and the constant “c”, respectively.
  • the sensitivity coefficient k X approaches the minimum value c and the sensitivity coefficient k Y becomes a large value as the landmark faces the front when viewed from the vehicle.
  • the sensitivity coefficient k X increases and the sensitivity coefficient k Y approaches the minimum value c as the landmark faces sideways when viewed from the vehicle.
  • FIG. 11A shows that when “M ⁇ ⁇ ⁇ ” is 10 °, the vertical width H L (t) is set to 60 cm for a landmark having a horizontal width W M of 60 cm and a vertical width H M of 60 cm.
  • 6 is a two-dimensional graph showing the relationship between the lateral width W L (t) when fixed and the reliability information a X (t) based on Expression (3).
  • FIG. 11B is a two-dimensional graph showing the relationship between the lateral width W L (t) and the reliability information a Y (t) based on the equation (4) under the same conditions as FIG. 11A. is there.
  • the constant c is set to 20.
  • the sensitivity coefficient k X is “1.015c” based on the equation (5).
  • the sensitivity coefficient k Y is “5.759c” based on the equation (6).
  • the reliability information a Y (t) shows the sensitivity to the deviation between the measured horizontal width W L (t) and vertical width H L (t) and the horizontal width W M and vertical width H M on the map.
  • FIG. 12A shows that when “M ⁇ ⁇ ⁇ ” is 60 °, the vertical width H L (t) is set to 60 cm with respect to the landmark having the horizontal width W M of 60 cm and the vertical width H M of 60 cm.
  • 6 is a two-dimensional graph showing the relationship between the lateral width W L (t) when fixed and the reliability information a X (t) based on Expression (3).
  • FIG. 12B is a two-dimensional graph showing the relationship between the lateral width W L (t) and the reliability information a Y (t) based on the equation (4) under the same conditions as FIG. 12A. is there.
  • the sensitivity coefficient k X is “2.0c” based on the equation (5).
  • the sensitivity coefficient k Y is “1.155c” based on the equation (6).
  • the reliability information a X (t) shows the sensitivity to the deviation between the measured horizontal width W L (t) and vertical width H L (t) and the horizontal width W M and vertical width H M on the map. It is relatively higher than the reliability information a Y (t).
  • Kalman gain according to reliability information Next, a method of setting the Kalman gain according to the reliability information will be described.
  • the vehicle position estimation unit 17 uses the reliability information a X (t) as the diagonal component of the observation noise matrix “R (t)” used when calculating the Kalman gain K (t) according to the following general formula (7). ), multiplied each the inverse of a Y (t).
  • observation noise matrix R (t) after the diagonal component is multiplied by the reciprocal of the reliability information a X (t) and a Y (t) (also referred to as “modified observation noise matrix R (t) ′”). .) Is represented by the following equation (8).
  • the reciprocal of the reliability information is also 1, so the modified observation noise matrix R (t) ′ is the original observation noise matrix R ( t) remains.
  • the reliability information is a value close to “0”
  • the reciprocal of the reliability information is larger than 1, and the diagonal component of the modified observation noise matrix R (t) ′ is multiplied.
  • the vehicle position estimation unit 17 calculates the adaptive Kalman gain “K (t) ′” from the following equation (9) using the modified observation noise matrix R (t) ′ shown in equation (7). To do.
  • the vehicle position estimating section 17 using the Kalman gain K (t) 'of the equation (9), the predicted vehicle based on the equation (1) Position X - (t) estimated by correcting the vehicle position X ⁇ Calculate (t). Therefore, when the reliability information is low, the Kalman gain K (t) ′ is small, and the correction amount for the predicted vehicle position X ⁇ (t) is small. In this case, since inaccurate correction can be prevented, the accuracy of the estimated own vehicle position X ⁇ (t) is suitably improved.
  • the vehicle position estimation unit 17 does not multiply the diagonal components of the observation noise matrix R (t) by the reciprocals of the reliability information a X (t) and a Y (t), but instead uses the Kalman gain K (t). ) May be multiplied by the reciprocal of reliability information a X (t) and a Y (t). In this case, the vehicle position estimation unit 17 calculates the Kalman gain K (t) 'based on the following formula (10).
  • FIG. 13 is a flowchart of the own vehicle position estimation processing performed by the own vehicle position estimation unit 17 of the vehicle-mounted device 1.
  • the vehicle-mounted device 1 repeatedly executes the process of the flowchart of FIG.
  • the reliability information a X (t) and a Y (t) for the traveling direction and the lateral direction of the vehicle will be described.
  • the vehicle position estimation unit 17 sets an initial value of the vehicle position based on the output of the GPS receiver 5 (step S101).
  • the vehicle position estimation unit 17 acquires the vehicle speed from the vehicle speed sensor 4 and the angular velocity in the yaw direction from the gyro sensor 3 (step S102).
  • the vehicle position estimation unit 17 calculates the movement distance of the vehicle and the change in the direction of the vehicle based on the acquisition result of step S102 (step S103).
  • the own vehicle position estimating unit 17 adds the moving distance and the heading change calculated in step S103 to the estimated own vehicle position X ⁇ (t-1) one hour before, and predicts the own vehicle position X ⁇ (t). Is calculated (step S104). Further, the own vehicle position estimation unit 17 refers to the feature information in the map DB 10 based on the predicted own vehicle position X ⁇ (t), and searches for a landmark within the measurement range of the rider 2 (step S105).
  • the own vehicle position estimation unit 17 calculates the measured predicted value Z ⁇ (t) from the predicted own vehicle position X ⁇ (t) and the position coordinates indicated by the feature information of the landmark searched in step S105 (step). S106). Further, in step S106, the vehicle position estimating unit 17 calculates the measurement value Z (t) from the measurement data of the rider 2 for the landmark searched in step S105.
  • the vehicle position estimating unit 17 determines the traveling direction of the vehicle corresponding to the azimuth (yaw angle) ⁇ ⁇ of the vehicle indicated by the predicted vehicle position X ⁇ (t) and the direction of the landmark recorded in the map DB 10. Based on the information, the sensitivity coefficient k X in the traveling direction of the vehicle and the sensitivity coefficient k Y in the lateral direction of the vehicle are determined (step S107). In this case, for example, the vehicle position estimation unit 17 calculates the sensitivity coefficient k X based on the equation (5) and the sensitivity coefficient k Y based on the equation (6).
  • the vehicle position estimating section 17 and the difference between the width of the map Landmarks W M and the longitudinal width H M and the measured width W L (t) and the vertical width H L (t), the sensitivity coefficient Based on k X and k Y , the reliability information a X (t) and a Y (t) are calculated with reference to the expressions (3) and (4) (step S108). Then, the vehicle position estimation unit 17 generates the Kalman gain K (t) ′ according to the reliability information a X (t) and a Y (t) (step S109).
  • the vehicle position estimation unit 17 calculates the modified observation noise matrix R (t) ′ based on the equation (8) using the reliability information a X (t) and a Y (t), and then the equation (9) The Kalman gain K (t) ′ is calculated based on After that, the own vehicle position estimation unit 17 corrects the predicted own vehicle position X ⁇ (t) by using the Kalman gain K (t) ′ instead of K (t) in the equation (1) to estimate the estimated own vehicle position.
  • X ⁇ (t) is calculated (step S110).
  • the vehicle position estimation unit 17 of the vehicle-mounted device 1 acquires the point cloud data, which is the measurement result of the landmark by the rider 2, and the location of the landmark included in the map DB 10. Get product information. Then, the vehicle position estimation unit 17 indicates the measurement accuracy of the landmark by the rider 2 based on the difference between the size of the landmark specified based on the acquired point cloud data and the size of the landmark indicated by the feature information. Calculate reliability information. As a result, the vehicle position estimation unit 17 corrects the correction amount for the predicted vehicle position X ⁇ (t) when calculating the estimated vehicle position X ⁇ (t) according to the accuracy of the measurement result of the rider 2. Can be accurately set.
  • the own vehicle position estimation unit 17 replaces the equation (2) with the absolute value of the difference between the measured width and height and the width and height on the map
  • may be calculated wherein on the basis of (11) reliability information a (t) follows with.
  • the reliability information a (t) based on the equation (11) is normalized to have a range of 0 to 1, and the measured width W The closer L (t) and the vertical width H L (t) are to the horizontal width W M and the vertical width H M on the map, the closer to 1 the maximum value, and the horizontal width W L (t) and the vertical width H L (t) are The distance from W M and the vertical width H M approaches 0.
  • FIG. 14A shows a formula with a horizontal width W L (t) when the vertical width H L (t) measured for a landmark having a horizontal width W M of 60 cm and a vertical width H M of 60 cm is fixed to 60 cm. It is a two-dimensional graph which shows the relationship with the reliability information a (t) based on (11).
  • FIG. 14B shows the equation (11) as the vertical width H L (t) when the horizontal width W L (t) measured for the same landmark as that of FIG. 14A is fixed at 60 cm. It is a two-dimensional graph which shows the relationship with the reliability information a (t) based on it. Further, FIG.
  • 14C shows the reliability information a (based on the lateral width W L (t), the vertical width H L (t), and the equation (11) for the same landmark as in FIGS. 14A and 14B. It is a three-dimensional graph which shows the relationship with t). 14D is a graph showing the reciprocal of the reliability information a (t) in FIG. 14A.
  • the lateral width W L (t) is closer to 60cm a lateral width W M reliability information a (t) approaches one.
  • the vertical width H L (t) approaches 1 as the reliability information a (t) approaches the vertical width H M of 60 cm.
  • the reciprocal of the reliability information a (t) the more the width W L (t) approaches the width W M approaches 1, the width W L (t) is the width W M The farther away from it, the greater the value.
  • the inverse of the reliability information a (t) is close enough to 1 vertical width H L (t) approaches the vertical width H M, the vertical width H L (t) is larger as the distance from the longitudinal width H M value Becomes
  • the vehicle position estimating unit 17 calculates the modified observation noise matrix R (t) ′ based on the equation (8) using the reciprocal of the reliability information a (t) based on the equation (11). It is possible to preferably determine the correction amount for the predicted vehicle position X ⁇ (t) when calculating the vehicle position X ⁇ (t).
  • the own vehicle position estimation unit 17 may calculate the reliability information a X (t) and the reliability information a Y (t) for each of the traveling direction and the lateral direction of the vehicle, as in the embodiment. In this case, the own vehicle position estimation unit 17 calculates the sensitivity coefficients k X and k Y based on the equations (5) and (6) to obtain the reliability information a X (t) and the reliability information a Y (t. ) Are calculated respectively.
  • the vehicle position estimating unit 17 may set the reliability information a (t) such that the higher the reliability is, the closer to the minimum value of 1, and the lower the reliability is, the larger the value is. Good.
  • the vehicle position estimation unit 17 calculates the reliability information a (t) based on the following formula (12).
  • the reliability information a (t) based on the equation (12) is normalized to have a range of 1 or more, and the measured width W L (t) and height H L (t) are the width W on the map. approaches one of the minimum value closer and M and the vertical width H M, becomes the width W L (t) and the vertical width H L (t) is a larger value as the distance from the transverse width W M and the longitudinal width H M.
  • FIG. 15A is a graph showing a horizontal width W L (t) when the vertical width H L (t) measured for a landmark having a horizontal width W M of 60 cm and a vertical width H M of 60 cm is fixed to 60 cm. It is a two-dimensional graph which shows the relationship with the reliability information a (t) based on (12).
  • FIG. 15B shows the vertical width H L (t) when the horizontal width W L (t) measured for the same landmark as that of FIG. It is a two-dimensional graph which shows the relationship with the reliability information a (t) based on it.
  • FIG. 15C shows the reliability information a (based on the lateral width W L (t) and the vertical width H L (t) and the equation (12) for the same landmark as in FIGS. 15A and 15B. It is a three-dimensional graph which shows the relationship with t).
  • the reliability information a (t) is close to one which is the minimum value to 60cm a lateral width W M lateral width W as L (t) approaches, the width The larger the width W L (t) from W M, the larger the value.
  • the reliability information a (t) has a minimum value as the height H L (t) approaches 60 cm, which is the height H M. As the value approaches 1 and the vertical width H L (t) becomes more distant from the vertical width H M, the value becomes larger.
  • the own vehicle position estimation unit 17 may calculate the reliability information a X (t) and the reliability information a Y (t) for each of the traveling direction and the lateral direction of the vehicle, as in the embodiment. In this case, the own vehicle position estimation unit 17 calculates the sensitivity coefficients k X and k Y based on the equations (5) and (6) to obtain the reliability information a X (t) and the reliability information a Y (t. ) Are calculated respectively.
  • the reliability information a (t) based on the expression (12) has a minimum value of 1 as in the reciprocal of the reliability information a (t) based on the expression (2) (see FIG. 7D), and, a large value width from the width W M W L (t) moves away or from the vertical width H M as vertical width H L (t) goes away. Therefore, when calculating the modified observation noise matrix R (t) ′, the vehicle position estimation unit 17 needs to multiply the diagonal component of the observation noise matrix R (t) by reciprocal the reliability information a (t). There is no. Specifically, when the vehicle position estimation unit 17 calculates the reliability information a X (t) and the reliability information a Y (t), respectively, the corrected observation noise is calculated based on the following equation (13). The matrix R (t) 'may be calculated.
  • the vehicle position estimating unit 17 can preferably determine the correction amount for the predicted vehicle position X ⁇ (t) when calculating the estimated vehicle position X ⁇ (t).
  • the configuration of the driving support system shown in FIG. 1 is an example, and the configuration of the driving support system to which the present invention is applicable is not limited to the configuration shown in FIG.
  • the electronic control device of the vehicle instead of having the vehicle-mounted device 1, the electronic control device of the vehicle may execute the process of the vehicle position estimation unit 17 of the vehicle-mounted device 1.
  • the map DB 10 is stored in, for example, a storage unit inside the vehicle, and the vehicle electronic control unit calculates the estimated vehicle position by executing the process of the flowchart of FIG. 13.
  • the reliability information is not limited to being used as a parameter for determining the correction amount of the predicted vehicle position X ⁇ (t) when the estimated vehicle position X ⁇ (t) is calculated.
  • the vehicle-mounted device 1 may use the calculated reliability information for other purposes such as obstacle detection based on the point cloud data output by the rider 2.

Abstract

車載機１の自車位置推定部１７は、ライダ２によるランドマークの計測結果である点群データを取得すると共に、地図ＤＢ１０に含まれるランドマークの地物情報を取得する。そして、自車位置推定部１７は、取得した点群データに基づき特定したランドマークのサイズと地物情報が示すランドマークのサイズとの差分に基づいて、ライダ２によるランドマークの計測精度を示す信頼度情報を算出する。

Description

計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
　本発明は、計測部による計測の精度を算出する技術に関する。
　従来から、車両の進行先に設置される地物をレーダやカメラを用いて検出し、その検出結果に基づいて自車位置を校正する技術が知られている。例えば、特許文献１には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、特許文献２には、カルマンフィルタを用いた自車位置推定技術が開示されている。
特開２０１３－２５７７４２号公報 特開２０１７－７２４２２号公報
　自己位置推定処理において計測対象となる地物が他の地物と隣接している場合には、計測対象の地物と共に他の地物が計測データに含まれてしまい、計測結果の精度（正確性）が低下してしまうことがある。また、計測対象となる地物の一部にオクルージョンが発生した場合にも同様に、本来計測されるべき地物の一部が計測できなくなるため、計測結果の精度が低下してしまうことがある。
　本発明は、上記のような課題を解決するためになされたものであり、計測部による地物の計測精度を好適に算出可能な計測精度算出装置及び自己位置推定装置を提供することを主な目的とする。
　請求項に記載の発明は、計測精度算出装置であって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部と、を有する。
　また、請求項に記載の発明は、自己位置推定装置であって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、予測された自己位置を示す予測位置情報を取得する第３取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記予測された自己位置を補正する補正部と、を備える。
　また、請求項に記載の発明は、計測精度算出装置が実行する制御方法であって、計測部による地物の計測結果を取得する第１取得工程と、地図データに含まれる前記地物の地物情報を取得する第２取得工程と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出工程と、を有する。
　また、請求項に記載の発明は、コンピュータが実行するプログラムであって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部として前記コンピュータを機能させる。
運転支援システムの概略構成図である。 車載機の機能的構成を示すブロック図である。 地図データベースのデータ構造の一例を示す。 状態変数ベクトルを２次元直交座標で表した図である。 予測ステップと計測更新ステップとの概略的な関係を示す図である。 自車位置推定部の機能ブロックを示す。 信頼度情報のグラフを示す。 オクルージョンが発生した場合のライダのレーザ光の照射範囲を概略的に示した図である。 ランドマークとなる地物に隣接して他の地物が存在する場合のライダのレーザ光の照射範囲を概略的に示した図である。 ２次元のワールド座標系及び車両座標系により表された車両とランドマークとの位置関係を示した図である。 車両から見てランドマークがほぼ正面を向いているときの信頼度情報の２次元グラフである。 車両から見てランドマークがやや横向きに近いときの信頼度情報の２次元グラフである。 自車位置推定処理を示すフローチャートである。 変形例１に係る信頼度情報のグラフを示す。 変形例２に係る信頼度情報のグラフを示す。
　本発明の好適な実施形態によれば、計測精度算出装置は、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部と、を有する。この態様により、計測精度算出装置は、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測部による地物の計測精度を示す精度情報を的確に算出することができる。
　上記計測精度算出装置の一態様では、前記算出部は、前記差分が大きいほど、前記計測精度が低い精度情報を生成する。このようにすることで、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は隣接地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測精度が低いことを示す精度情報を好適に生成することができる。
　上記計測精度算出装置の一態様では、前記算出部は、前記差分と、移動体の進行方向と、前記地物情報が示す前記地物の法線方向とに基づき、前記移動体の進行方向と横方向のそれぞれに対する前記地物の計測精度を示す精度情報を生成する。一般に、移動体の進行方向に対する地物の向きによって、計測誤差が生じにくい方向と計測誤差が生じやすい方向とがそれぞれ生じる場合がある。従って、この態様によれば、計測精度算出装置は、移動体の進行方向と横方向のそれぞれに対する地物の計測精度を示す精度情報を好適に生成することができる。
　上記計測精度算出装置の他の一態様では、前記算出部は、前記進行方向と前記地物の法線方向との角度差が大きいほど、前記横方向に対する前記計測精度への前記差分の影響度合いを低くし、前記角度差が小さいほど、前記進行方向に対する前記計測精度への前記差分の影響度合いを低くする。ここでの地物の法線方向は、地物が形成する平面に対し垂直となる地物の正面方向及び背面方向のうち、移動体の進行方向と９０度差以内となる方向を指すものとする。この場合、移動体の進行方向と地物の法線方向との角度差が大きいほど、地物は移動体に対して横向きとなるため、移動体の横方向に対する計測誤差が生じにくくなる。一方、移動体の進行方向と地物の法線方向との角度差が小さいほど、地物は移動体に対して正面向きとなるため、移動体の進行方向に対する計測誤差が生じにくくなる。よって、計測精度算出装置は、この態様により、移動体の進行方向と横方向のそれぞれに対する地物の計測精度を的確に表す精度情報を生成することができる。
　上記計測精度算出装置の他の一態様では、計測精度算出装置は、予測された自己位置を示す予測位置情報を取得する第３取得部と、前記精度情報に基づき、前記予測された自己位置を補正する補正部と、をさらに備える。この態様により、計測精度算出装置は、生成した精度情報を反映した自己位置推定を好適に実行することができる。
　上記計測精度算出装置の他の一態様では、前記補正部は、移動体から前記地物までの前記計測部による計測距離と、前記地物情報に含まれる前記地物の位置情報に基づき予測された前記移動体から前記地物までの予測距離と、の差分値により前記予測された自己位置を補正する場合の前記差分値に対する利得を、前記精度情報に基づき決定する。この態様により、計測精度算出装置は、予測された自己位置を計測距離と予測距離との差分値により補正する場合に精度情報に基づきその利得を調整することで、不正確な補正を防止して自己位置推定精度を好適に向上させることができる。
　本発明の他の好適な実施形態によれば、自己位置推定装置であって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、予測された自己位置を示す予測位置情報を取得する第３取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記予測された自己位置を補正する補正部と、を備える。上述の差分は、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても発生する。従って、自己位置推定装置は、この態様により、予測された自己位置を上述の差分を考慮して好適に補正して最終的な自己位置を決定することができる。
　本発明の他の好適な実施形態によれば、計測精度算出装置が実行する制御方法であって、計測部による地物の計測結果を取得する第１取得工程と、地図データに含まれる前記地物の地物情報を取得する第２取得工程と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出工程と、を有する。計測精度算出装置は、この制御方法を実行することで、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測部による地物の計測精度を示す精度情報を的確に算出することができる。
　本発明の他の好適な実施形態によれば、コンピュータが実行するプログラムであって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測部による地物の計測精度を示す精度情報を的確に算出することができる。好適には、上記プログラムは、記憶媒体に記憶される。
　以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の記号の上に「＾」または「－」が付された文字を、本明細書では便宜上、「Ａ＾」または「Ａ－」（「Ａ」は任意の文字）と表す。
　［運転支援システムの概要］
　図１は、本実施例に係る運転支援システムの概略構成である。図１に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機１と、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ　Ｉｌｌｕｍｉｎａｔｅｄ　Ｄｅｔｅｃｔｉｏｎ　Ａｎｄ　Ｒａｎｇｉｎｇ）２と、ジャイロセンサ３と、車速センサ４と、ＧＰＳ受信機５とを有する。なお、以後において、「地図」とは、従来の経路案内用の車載機が参照するデータに加えて、ＡＤＡＳ（Ａｄｖａｎｃｅｄ　Ｄｒｉｖｅｒ　Ａｓｓｉｓｔａｎｃｅ　Ｓｙｓｔｅｍ）や自動運転に用いられるデータも含むものとする。
　車載機１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５と電気的に接続し、これらの出力に基づき、車載機１が搭載される車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機１は、道路データ及び道路付近に設けられた目印となる地物に関する情報である地物情報を記憶した地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。上述の目印となる地物は、例えば、道路脇に周期的に並んでいるキロポスト、１００ｍポスト、デリニエータ、交通インフラ設備（例えば標識、方面看板、信号）、電柱、街灯などの地物である。そして、車載機１は、この地物情報に基づき、ライダ２等の出力と照合させて自車位置の推定を行う。車載機１は、本発明における「計測精度算出装置」及び「自己位置推定装置」の一例である。尚、地図ＤＢ１０は車載装置１に記憶される代わりに、外部サーバ等の外部の記憶装置に記憶されてもよい。その場合、車載装置１は無線通信等を介して外部の記憶装置から地図ＤＢ１０の少なくとも一部を取得する。
　ライダ２は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群情報を生成する。この場合、ライダ２は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光が物体で反射した反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、点群データであり、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される、その照射方向での物体までの距離とに基づき生成される。ライダ２、ジャイロセンサ３、車速センサ４、ＧＰＳ受信機５は、それぞれ、出力データを車載機１へ供給する。ライダ２は、「計測部」の一例である。
　図２は、車載機１の機能的構成を示すブロック図である。車載機１は、主に、インターフェース１１と、記憶部１２と、入力部１４と、制御部１５と、情報出力部１６と、を有する。これらの各要素は、バスラインを介して相互に接続されている。
　インターフェース１１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５などのセンサから出力データを取得し、制御部１５へ供給する。
　記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、地物情報を含む地図ＤＢ１０を記憶する。図３は、地図ＤＢ１０のデータ構造の一例を示す。図３に示すように、地図ＤＢ１０は、施設情報、道路データ、及び地物情報を含む。
　地物情報は、地物ごとに当該地物に関する情報が関連付けられた情報であり、ここでは、地物のインデックスに相当する地物ＩＤと、位置情報と、向き情報（法線情報）と、サイズ情報とを含む。位置情報は、緯度及び経度（及び標高）等により表わされた地物の絶対的な位置を示す。向き情報及びサイズ情報は、例えば、看板などの平面的な形状を有している地物に対して設けられる情報である。向き情報は、地物の向きを表す情報であり、例えば地物に形成された面に対する法線ベクトル等を示す。サイズ情報は、地物の大きさを表す情報であり、本実施例では、地物に形成された被計測面の横幅及び縦幅の情報を含んでいる。
　なお、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、図示しない通信部を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。
　入力部１４は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等である。情報出力部１６は、例えば、制御部１５の制御に基づき出力を行うディスプレイやスピーカ等である。
　制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。本実施例では、制御部１５は、インターフェース１１から供給される各センサの出力信号及び地図ＤＢ１０に基づき、自車位置の推定を行う自車位置推定部１７を有する。そして、制御部１５は、自車位置の推定結果に基づいて自動運転制御を含む車両の運転支援に関する制御などを行う。制御部１５は、本発明における「第１取得部」、「第２取得部」、「第３取得部」、「算出部」、「補正部」、及びプログラムを実行する「コンピュータ」の一例である。
　［自車位置推定処理の概要］
　まず、自車位置推定部１７による自車位置の推定処理の概要について説明する。
　自車位置推定部１７は、計測対象となる地物（「ランドマーク」とも呼ぶ。）に対するライダ２による距離及び角度の計測値と、地図ＤＢ１０から抽出したランドマークの位置情報とに基づき、ジャイロセンサ３、車速センサ４、及び／又はＧＰＳ受信機５の出力データから推定した自車位置を補正する。本実施例では、一例として、自車位置推定部１７は、ベイズ推定に基づく状態推定手法に基づき、ジャイロセンサ３、車速センサ４等の出力データから自車位置を予測する予測ステップと、直前の予測ステップで算出した自車位置の予測値を補正する計測更新ステップとを交互に実行する。これらのステップで用いる状態推定フィルタは、ベイズ推定を行うように開発された様々のフィルタが利用可能であり、例えば、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、パーティクルフィルタなどが該当する。このように、ベイズ推定に基づく位置推定は、種々の方法が提案されている。
　以下では、拡張カルマンフィルタを用いた自車位置推定について簡略的に説明する。
　図４は、状態変数ベクトルを２次元直交座標で表した図である。図４に示すように、ｘｙの２次元直交座標上で定義された平面での自車位置は、座標「（ｘ、ｙ）」、自車の方位（ヨー角）「ψ」により表される。ここでは、ヨー角ψは、車の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ）は、例えば緯度及び経度の組合せに相当する絶対位置、あるいは所定地点を原点とした位置を示すワールド座標である。
　図５は、予測ステップと計測更新ステップとの概略的な関係を示す図である。また、図６は、自車位置推定部１７の機能ブロックの一例を示す。図５に示すように、予測ステップと計測更新ステップとを繰り返すことで、自車位置を示す状態変数ベクトル「Ｘ」の推定値の算出及び更新を逐次的に実行する。また、図６に示すように、自車位置推定部１７は、予測ステップを実行する位置予測部２１と、計測更新ステップを実行する位置推定部２２とを有する。位置予測部２１は、デッドレコニングブロック２３及び位置予測ブロック２４を含み、位置推定部２２は、ランドマーク探索・抽出ブロック２５及び位置補正ブロック２６を含む。なお、図５では、計算対象となる基準時刻（即ち現在時刻）「ｔ」の状態変数ベクトルを、「Ｘ－（ｔ）」または「Ｘ＾（ｔ）」と表記している（「状態変数ベクトルＸ（ｔ）＝（ｘ（ｔ）、ｙ（ｔ）、ψ（ｔ））Ｔ」と表記する）。ここで、予測ステップで推定された暫定的な推定値（予測値）には当該予測値を表す文字の上に「－」を付し、計測更新ステップで更新された，より精度の高い推定値には当該値を表す文字の上に「＾」を付す。
　予測ステップでは、自車位置推定部１７のデッドレコニングブロック２３は、車両の移動速度「ｖ」と角速度「ω」（これらをまとめて「制御値ｕ（ｔ）＝（ｖ（ｔ）、ω（ｔ））Ｔ」と表記する。）を用い、前回時刻からの移動距離と方位変化を求める。自車位置推定部１７の位置予測ブロック２４は、直前の計測更新ステップで算出された時刻ｔ－１の状態変数ベクトルＸ＾（ｔ－１）に対し、求めた移動距離と方位変化を加えて、時刻ｔの自車位置の予測値（「予測自車位置」とも呼ぶ。）Ｘ－（ｔ）を算出する。また、これと同時に、予測自車位置Ｘ－（ｔ）の誤差分布に相当する共分散行列「Ｐ－（ｔ）」を、直前の計測更新ステップで算出された時刻ｔ－１での共分散行列「Ｐ＾（ｔ－１）」から算出する。
　計測更新ステップでは、自車位置推定部１７のランドマーク探索・抽出ブロック２５は、地図ＤＢ１０に登録された計測対象の地物（ランドマーク）の位置ベクトルとライダ２のスキャンデータとの対応付けを行う。そして、自車位置推定部１７のランドマーク探索・抽出ブロック２５は、この対応付けができた場合に、対応付けができたランドマークのライダ２による計測値「Ｚ（ｔ）」と、予測自車位置Ｘ－（ｔ）及び地図ＤＢ１０に登録されたランドマークの位置ベクトルを用いてライダ２による計測処理をモデル化して求めたランドマークの計測値（「計測予測値」と呼ぶ。）「Ｚ－（ｔ）」と、をそれぞれ取得する。計測値Ｚ（ｔ）は、時刻ｔにライダ２が計測したランドマークの距離及びスキャン角度から、車両の進行方向と横方向を軸とした成分に変換した車両座標系におけるベクトル値である。そして、自車位置推定部１７の位置補正ブロック２６は、計測値Ｚ（ｔ）と計測予測値Ｚ－（ｔ）との差分値を算出する。
　なお、実際には、ランドマークは所定の大きさを有し、ライダ２のレーザ光が照射された被照射面上の複数の計測点に対応するデータが点群データとして取得される。よって、例えば、ランドマーク探索・抽出ブロック２５は、対象のランドマークを計測した各計測点における計測値を平均化することで、対象のランドマークの中心位置に相当する計測値Ｚ（ｔ）を算出する。
　そして、自車位置推定部１７の位置補正ブロック２６は、以下の式（１）に示すように、計測値Ｚ（ｔ）と計測予測値Ｚ－（ｔ）との差分値にカルマンゲイン「Ｋ（ｔ）」を乗算し、これを予測自車位置Ｘ－（ｔ）に加えることで、更新された状態変数ベクトル（「推定自車位置」とも呼ぶ。）Ｘ＾（ｔ）を算出する。
Figure JPOXMLDOC01-appb-M000001
 　また、計測更新ステップでは、自車位置推定部１７の位置補正ブロック２６は、予測ステップと同様、推定自車位置Ｘ＾（ｔ）の誤差分布に相当する共分散行列Ｐ＾（ｔ）（単にＰ（ｔ）とも表記する）を共分散行列Ｐ－（ｔ）から求める。カルマンゲインＫ（ｔ）等のパラメータについては、例えば拡張カルマンフィルタを用いた公知の自己位置推定技術と同様に算出することが可能である。
　本実施例では、自車位置推定部１７は、カルマンゲインＫ（ｔ）を、計測値Ｚ（ｔ）（即ちランドマークの中心点までの計測距離）の信頼度を表す指標である後述の信頼度情報に応じて補正する。これにより、オクルージョンの発生やランドマークに隣接する地物の存在等に起因して計測値Ｚ（ｔ）の信頼度が低い場合にはカルマンゲインＫ（ｔ）を低くし、予測自車位置Ｘ－（ｔ）に対する補正量を少なくする。この場合、予測自車位置Ｘ－（ｔ）に対する不正確な補正が好適に抑制され、自己位置推定精度が向上する。
　なお、自車位置推定部１７は、複数の地物に対し、地図ＤＢ１０に登録された地物の位置ベクトルとライダ２のスキャンデータとの対応付けができた場合、選定した任意の一組の計測予測値及び計測値等に基づき計測更新ステップを行ってもよく、対応付けができた全ての計測予測値及び計測値等に基づき計測更新ステップを複数回行ってもよい。なお、複数の計測予測値及び計測値等を用いる場合には、自車位置推定部１７は、ライダ２から遠い地物ほどライダ計測精度が悪化することを勘案し、ライダ２と地物との距離が長いほど、当該地物に関する重み付けを小さくするとよい。
　このように、予測ステップと計測更新ステップが繰り返し実施され、予測自車位置Ｘ－（ｔ）と推定自車位置Ｘ＾（ｔ）が逐次的に計算されることにより、もっとも確からしい自車位置が計算される。
　なお、上記の説明において、計測値Ｚ（ｔ）は本発明の「計測距離」の一例であり、計測予測値Ｚ－（ｔ）は本発明の「予測距離」の一例である。
　［信頼度情報の算出］
　次に、計測値Ｚ（ｔ）の信頼度を表す指標である信頼度情報の算出方法について説明する。概略的には、自車位置推定部１７は、地物情報に含まれるサイズ情報が示すランドマークの地図上のサイズと、ライダ２により計測されたランドマークの計測点が分布する範囲（計測範囲）のサイズとの差分を算出し、当該差分を正規化することで、信頼度情報を算出する。これにより、自車位置推定部１７は、計測値Ｚ（ｔ）の信頼度を的確に表した信頼度情報を算出する。
　（１）方向によらない信頼度情報の算出
　まず、計測値Ｚ（ｔ）に対して方向によらない信頼度情報を生成する場合について説明する。
　この場合、自車位置推定部１７は、地物情報に含まれるランドマークの地図上の横幅「ＷＭ」及び縦幅（高さ方向の幅）「ＨＭ」と、時刻ｔにおけるライダ２の点群データに基づき特定されたランドマークの計測範囲の横幅「ＷＬ（ｔ）」及び縦幅「ＨＬ（ｔ）」と、感度係数「ｋ」とに基づき、以下の式（２）に示すように信頼度情報ａ（ｔ）を定める。
Figure JPOXMLDOC01-appb-M000002
 　上述の式（２）では、信頼度情報ａ（ｔ）は、０から１までの値域となるように正規化され、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）が地図上の横幅ＷＭ及び縦幅ＨＭと近いほど最大値の１に近づき、横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）が横幅ＷＭ及び縦幅ＨＭから離れるほど０に近づく。なお、信頼度情報ａ（ｔ）は、１に近いほど計測値Ｚ（ｔ）に対する信頼度が高い（即ち計測値Ｚ（ｔ）が正確）であることを示す。また、感度係数ｋは、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）と地図上の横幅ＷＭ及び縦幅ＨＭとの差分に対する信頼度情報ａ（ｔ）への感度を調整する係数であり、感度係数ｋが大きいほど算出される信頼度情報ａ（ｔ）が小さくなる。なお、感度係数ｋは、例えば予め定められた値に設定される。
　図７（Ａ）は、横幅ＷＭが６０ｃｍ、縦幅ＨＭが６０ｃｍとなるランドマークに対して縦幅ＨＬ（ｔ）を６０ｃｍに固定した場合の横幅ＷＬ（ｔ）と式（２）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。また、図７（Ｂ）は、図７（Ａ）と同一のランドマークに対して横幅ＷＬ（ｔ）を６０ｃｍに固定した場合の縦幅ＨＬ（ｔ）と信頼度情報ａ（ｔ）との関係を示す２次元グラフである。さらに、図７（Ｃ）は、図７（Ａ）、（Ｂ）と同一のランドマークに対する横幅ＷＬ（ｔ）と縦幅ＨＬ（ｔ）と信頼度情報ａ（ｔ）との関係を示す３次元グラフである。また、図７（Ｄ）は、図７（Ａ）における信頼度情報ａ（ｔ）の逆数「１／ａ（ｔ）」を示したグラフである。
　図７（Ａ）及び図７（Ｃ）に示すように、横幅ＷＬ（ｔ）が横幅ＷＭである６０ｃｍに近づくほど、信頼度情報ａ（ｔ）は最大値である１に近づく。一方、図７（Ｂ）及び図７（Ｃ）に示すように、縦幅ＨＬ（ｔ）が縦幅ＨＭである６０ｃｍに近づくほど、信頼度情報ａ（ｔ）は最大値である１に近づく。なお、図７（Ｄ）に示すように、信頼度情報ａ（ｔ）の逆数は、横幅ＷＬ（ｔ）が横幅ＷＭに近づくほど最小値である１に近づき、横幅ＷＬ（ｔ）が横幅ＷＭから遠ざかるほど大きい値となる。同様に、信頼度情報ａ（ｔ）の逆数は、縦幅ＨＬ（ｔ）が縦幅ＨＭに近づくほど最小値である１に近づき、縦幅ＨＬ（ｔ）が縦幅ＨＭから遠ざかるほど大きい値となる。信頼度情報ａ（ｔ）の逆数は、後述するように、信頼度情報に応じてカルマンゲインを設定する際に用いられる。
　ここで、式（２）の妥当性について、図８及び図９に示す具体例を用いて補足説明する。
　図８（Ａ）は、ランドマークとなる地物２０のライダ２による計測時においてオクルージョンが発生した場合のライダ２のレーザ光の照射範囲を概略的に示した図であり、図８（Ｂ）は、ライダ２のレーザ光が照射された被照射面上の計測点を明示した地物２０の平面図である。なお、ここではレーザ光の反射強度が高い道路標識を地物２０の例としている。
　自車位置推定部１７は、ライダ２の計測範囲内にランドマークとなる地物２０が存在することを認識し、当該地物２０が存在することが予測される範囲を示すウィンドウ２１を設定する。例えば、自車位置推定部１７は、予測自車位置Ｘ－（ｔ）から所定距離以内の位置を示す位置情報が地物情報に含まれる地物２０をランドマークとして特定し、当該地物２０の位置情報が示す位置を中心とする所定サイズの矩形領域をウィンドウ２１として設定する。なお、この場合、自車位置推定部１７は、地物２０のサイズ情報をさらに参照することで、ウィンドウ２１の大きさを決定してもよい。そして、自車位置推定部１７は、ウィンドウ２１内の位置を示す計測点であって、反射強度が所定の閾値以上となる計測点の点群データを、地物２０の点群データとみなす。
　この場合、図８（Ｂ）に示すように、ウィンドウ２１内のライダ２の点群データに基づき特定する地物２０の横幅ＷＬ（ｔ）は、地図上の地物２０の横幅ＷＭより短くなる。よって、この場合、自車位置推定部１７は、式（２）に基づき、信頼度情報ａ（ｔ）を１より小さい値に設定する（図７（Ａ）及び図７（Ｃ）参照）。
　一方、自車位置推定部１７は、ウィンドウ２１内の各計測点の計測値を平均化することで、推定自車位置Ｘ＾（ｔ）の算出（式（１）参照）に用いる地物２０の計測値Ｚ（ｔ）を算出する。ここで、地物２０の左側の一部にオクルージョンが発生しているため、上述の平均化により算出される計測値Ｚ（ｔ）は、本来算出されるべき値（即ち地物２０の中心位置を示す値）よりも右側にずれた値となる。
　このように、式（２）によれば、自車位置推定部１７は、ランドマークにオクルージョンが発生し、計測値Ｚ（ｔ）にずれが生じると推測される場合に、信頼度情報ａ（ｔ）を低い値に設定することができる。これにより、後述するように、自車位置推定部１７は、計測値Ｚ（ｔ）の信頼度が低いときの予測自車位置Ｘ－（ｔ）に対する補正量を少なくし、不正確な補正防止によって自己位置推定精度を好適に向上させる。
　図９（Ａ）は、ランドマークとなる地物２０に隣接して他の地物２５が存在する場合のライダ２のレーザ光の照射範囲を概略的に示した図であり、図９（Ｂ）は、図９（Ａ）においてライダ２のレーザ光が照射された被照射面上の計測点を明示した地物２０の平面図である。
　図９（Ａ）の例では、自車位置推定部１７は、ウィンドウ２１を設定後、ウィンドウ２１内において所定閾値以上の反射強度となる計測点の点群データを、地物２０の点群データとみなす。ここで、ウィンドウ２１には、地物２０の計測点に加えて、地物２０に隣接する地物２５の一部の計測点も含まれている。
　この場合、図９（Ｂ）に示すように、ウィンドウ２１内のライダ２の点群データに基づき特定される横幅ＷＬ（ｔ）は、地図上の地物２０の横幅ＷＭより長くなる。よって、この場合、自車位置推定部１７は、横幅ＷＬ（ｔ）と地図上の横幅ＷＭとの差異が大きくなることから、式（２）に基づき、信頼度情報ａ（ｔ）を１より小さい値に設定する（図７（Ａ）及び図７（Ｃ）参照）。
　一方、自車位置推定部１７は、ウィンドウ２１内の各計測点の計測値を平均化することで、推定自車位置Ｘ＾（ｔ）の算出（式（１）参照）に用いる地物２０の計測値Ｚ（ｔ）を算出する。ここで、ウィンドウ２１内の点群データには、地物２０を計測した点群データに加えて、地物２０に隣接する地物２５を計測した点群データも含まれているため、上述の平均化により算出される計測値Ｚ（ｔ）は、本来算出されるべき値（即ち地物２０の中心位置を示す値）よりも左側にずれた値となる。
　このように、式（２）によれば、計測対象の地物に他の地物が隣接することに起因して計測値Ｚ（ｔ）にずれが生じた場合においても、自車位置推定部１７は、信頼度情報ａ（ｔ）を低い値に設定することができる。これにより、後述するように、自車位置推定部１７は、計測値Ｚ（ｔ）の信頼度が低いときの予測自車位置Ｘ－（ｔ）に対する補正量を少なくし、不正確な補正防止によって自己位置推定精度を好適に向上させる。
　なお、ランドマークとなる地物は、再帰性反射材が使用されているものが多く、ライダ２に戻ってくる反射光の強度が高いため、反射強度に対して閾値を用いたフィルタリングにより、点群データを抽出しやすい対象と言える。一方、走行環境において、視線誘導標や他車両の反射板等の反射強度の高い物体が多数存在し、それらの点群データをランドマークの点群データと誤って検出してしまう可能性がある。このような場合、横幅ＷＬ（ｔ）が横幅ＷＭよりも長くなる、又は／及び、縦幅ＨＬ（ｔ）が縦幅ＨＭよりも長くなる。このような場合において、本実施例では、自車位置推定部１７は、式（２）に基づき、計測値の信頼度が低いことを示す信頼度情報ａ（ｔ）を適切に設定することができる。
　（２）方向毎の信頼度情報の算出
　次に、車両の進行方向と横方向のそれぞれに対して信頼度情報を算出する例について説明する。この場合、概略的には、自車位置推定部１７は、地物情報に含まれる向き情報を参照し、車両の進行方向に対するランドマークの相対的な向きに基づき、車両の進行方向及び横方向の信頼度情報をそれぞれ算出する。以後では、時刻ｔにおける車両の進行方向の信頼度情報を「ａＸ（ｔ）」、横方向の信頼度情報を「ａＹ（ｔ）」とする。
　ランドマークが車両の進行方向と同一の方向を向いている場合、ランドマークの被照射面が車両の進行方向に対して垂直となる。この場合、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）と地図上の横幅ＷＭ及び縦幅ＨＭとの差異が大きいと、計測値Ｚ（ｔ）の中心点はずれるものの、車両の進行方向成分の計測値ＬＸ（ｔ）には大きな誤差は生じない。よって、信頼度情報ａＸ（ｔ）の値をあまり小さくする必要はない。一方、車両の横方向成分の計測値ＬＹ（ｔ）には誤差が生じることから、信頼度情報ａＹ（ｔ）の値を小さくする必要がある。
　以上を勘案し、自車位置推定部１７は、信頼度情報ａＸ（ｔ）と信頼度情報ａＹ（ｔ）とを算出する場合とで異なる感度係数「ｋＸ」、「ｋＹ」をそれぞれ用い、式（２）の感度係数ｋと置き換えて計算を行う。具体的には、自車位置推定部１７は、感度係数ｋＸを用いた以下の式（３）により信頼度情報ａＸ（ｔ）を算出し、感度係数ｋＹを用いた以下の式（４）によりａＹ（ｔ）を算出する。
Figure JPOXMLDOC01-appb-M000003
Figure JPOXMLDOC01-appb-M000004
 　そして、自車位置推定部１７は、これらの感度係数ｋＸ、ｋＹを、車両の進行方向に対するランドマークの相対的な向きに基づいて決定する。具体的には、自車位置推定部１７は、ランドマークの向きと車両の進行方向とが平行に近づくほど、信頼度情報ａＸ（ｔ）に対する感度係数ｋＸを小さくし、信頼度情報ａＹ（ｔ）に対する感度係数ｋＹを大きくする。一方、自車位置推定部１７は、ランドマークの向きと車両の進行方向とが垂直に近づくほど、信頼度情報ａＸ（ｔ）に対する感度係数ｋＸを大きくし、信頼度情報ａＹ（ｔ）に対する感度係数ｋＹを小さくする。
　図１０は、２次元のワールド座標系（絶対座標系）及び車両座標系により表された車両とランドマークとの位置関係を示した図である。ここで、ワールド座標系は、所定地点を原点とし、互いに垂直な座標軸「ｘｗ」及び座標軸「ｙｗ」を有し、車両座標系は、車両の中心を原点とし、車両の進行方向に沿った座標軸「ｘｂ」と車両の側面方向に沿った座標軸「ｙｂ」を有する。そして、図１０では、ワールド座標系における車両のヨー角を「ψ－」、車両の位置を［ｘ－、ｙ－］Ｔとしている。また、ワールド座標系におけるランドマークのヨー角を「Ｍψ」、位置を［Ｍｘ、Ｍｙ］Ｔとし、車両座標系におけるランドマークのヨー角を「Ｌψ」、位置を［Ｌｘ、Ｌｙ］Ｔとしている。
　この場合、車両から見たランドマークの向きは、車両座標系におけるランドマークのヨー角Ｌψに等しく、ヨー角Ｌψは、図１０に示すように、「Ｍψ－ψ－」に等しい。従って、自車位置推定部１７は、感度係数ｋＸ、ｋＹを、「Ｍψ－ψ－」と定数「ｃ」とを用いてそれぞれ以下の式（５）及び式（６）により定める。
Figure JPOXMLDOC01-appb-M000005
Figure JPOXMLDOC01-appb-M000006
 　上記の式（５）及び式（６）によれば、車両から見てランドマークが正面を向いているほど、感度係数ｋＸは最小値ｃに近づき、感度係数ｋＹは大きな値となる。言い換えると、車両から見てランドマークが横を向いているほど、感度係数ｋＸは大きくなり、感度係数ｋＹは最小値ｃに近づく。
　図１１（Ａ）は、「Ｍψ－ψ－」が１０°の場合において、横幅ＷＭが６０ｃｍ、縦幅ＨＭが６０ｃｍとなるランドマークに対して縦幅ＨＬ（ｔ）を６０ｃｍに固定したときの横幅ＷＬ（ｔ）と式（３）に基づく信頼度情報ａＸ（ｔ）との関係を示す２次元グラフである。また、図１１（Ｂ）は、図１１（Ａ）と同一条件下での横幅ＷＬ（ｔ）と式（４）に基づく信頼度情報ａＹ（ｔ）との関係を示す２次元グラフである。なお、図１１及び後述の図１２の各グラフでは、定数ｃは２０に設定されている。
　図１１（Ａ）の例では、「Ｍψ－ψ－」が１０°であることから、感度係数ｋＸは、式（５）に基づき「１．０１５ｃ」となる。一方、図１１（Ｂ）の例では、感度係数ｋＹは、式（６）に基づき「５．７５９ｃ」となる。このように、車両から見てランドマークがほぼ正面を向いているときには、感度係数ｋＸはｃに近い値となり、感度係数ｋＹは感度係数ｋＸよりも遥かに大きい値となる。そして、この場合、信頼度情報ａＹ（ｔ）は、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）と地図上の横幅ＷＭ及び縦幅ＨＭとのずれに対する感度が高くなる。
　図１２（Ａ）は、「Ｍψ－ψ－」が６０°の場合において、横幅ＷＭが６０ｃｍ、縦幅ＨＭが６０ｃｍとなるランドマークに対して縦幅ＨＬ（ｔ）を６０ｃｍに固定したときの横幅ＷＬ（ｔ）と式（３）に基づく信頼度情報ａＸ（ｔ）との関係を示す２次元グラフである。また、図１２（Ｂ）は、図１２（Ａ）と同一条件下での横幅ＷＬ（ｔ）と式（４）に基づく信頼度情報ａＹ（ｔ）との関係を示す２次元グラフである。
　図１２（Ａ）の例では、「Ｍψ－ψ－」が６０°であることから、感度係数ｋＸは、式（５）に基づき「２．０ｃ」となる。一方、図１２（Ｂ）の例では、感度係数ｋＹは、式（６）に基づき「１．１５５ｃ」となる。このように、車両から見てランドマークがやや横向きに近いときには、感度係数ｋＸは感度係数ｋＹよりも大きい値となる。そして、この場合、信頼度情報ａＸ（ｔ）は、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）と地図上の横幅ＷＭ及び縦幅ＨＭとのずれに対する感度が信頼度情報ａＹ（ｔ）と比べて相対的に高くなる。
　［信頼度情報に応じたカルマンゲイン］
　次に、信頼度情報に応じてカルマンゲインを設定する方法について説明する。
　自車位置推定部１７は、以下の一般式（７）によりカルマンゲインＫ（ｔ）を算出する際に用いる観測雑音行列「Ｒ（ｔ）」の対角成分に、信頼度情報ａＸ（ｔ）、ａＹ（ｔ）の逆数をそれぞれ乗じる。
Figure JPOXMLDOC01-appb-M000007
 　ここで、対角成分に信頼度情報ａＸ（ｔ）、ａＹ（ｔ）の逆数が乗じられた後の観測雑音行列Ｒ（ｔ）（「修正観測雑音行列Ｒ（ｔ）’」とも呼ぶ。）は、以下の式（８）により表される。
Figure JPOXMLDOC01-appb-M000008
 　このように、式（３）及び式（４）に基づき信頼度情報ａＸ（ｔ）、ａＹ（ｔ）をそれぞれ生成した場合、観測雑音行列の各成分に乗じる係数は個別となる。なお、自車位置推定部１７は、式（２）に基づき信頼度情報ａ（ｔ）を算出した場合には、式（８）において「ａＸ（ｔ）＝ａＹ（ｔ）＝ａ（ｔ）」とみなして修正観測雑音行列Ｒ（ｔ）’を算出すればよい。
　式（７）によれば、信頼度情報が最高値の「１」であった場合には、その逆数も１であるため、修正観測雑音行列Ｒ（ｔ）’が元の観測雑音行列Ｒ（ｔ）のままとなる。一方、信頼度情報が「０」に近い値であった場合には、その逆数は１より大きい値となり、修正観測雑音行列Ｒ（ｔ）’の対角成分は増倍されることになる。
　そして、自車位置推定部１７は、式（７）に示す修正観測雑音行列Ｒ（ｔ）’を用いて、以下の式（９）から適応的なカルマンゲイン「Ｋ（ｔ）’」を算出する。
Figure JPOXMLDOC01-appb-M000009
 　そして、自車位置推定部１７は、式（９）のカルマンゲインＫ（ｔ）’を用いて、式（１）に基づき予測自車位置Ｘ－（ｔ）を補正した推定自車位置Ｘ＾（ｔ）を算出する。従って、信頼度情報が低い場合は、カルマンゲインＫ（ｔ）’が小さくなるため、予測自車位置Ｘ－（ｔ）に対する補正量が少なくなる。この場合、不正確な補正を防止できるため、推定自車位置Ｘ＾（ｔ）の精度が好適に向上する。
　なお、自車位置推定部１７は、観測雑音行列Ｒ（ｔ）の対角成分に、信頼度情報ａＸ（ｔ）、ａＹ（ｔ）の逆数をそれぞれ乗じる代わりに、カルマンゲインＫ（ｔ）に対して信頼度情報ａＸ（ｔ）、ａＹ（ｔ）の逆数をそれぞれ乗じてもよい。この場合、自車位置推定部１７は、カルマンゲインＫ（ｔ）’を以下の式（１０）に基づき算出する。
Figure JPOXMLDOC01-appb-M000010
 　この式（１０）を用いた場合であっても、信頼度情報が低い場合は、カルマンゲインＫ（ｔ）’が小さくなるため、予測自車位置Ｘ－（ｔ）に対する補正量が少なくなり、不正確な補正を好適に防止することができる。
　［処理フロー］
　図１３は、車載機１の自車位置推定部１７により行われる自車位置推定処理のフローチャートである。車載機１は、図１３のフローチャートの処理を繰り返し実行する。ここでは、一例として、車両の進行方向及び横方向のそれぞれに対する信頼度情報ａＸ（ｔ）、ａＹ（ｔ）を算出する例について説明する。
　まず、自車位置推定部１７は、ＧＰＳ受信機５等の出力に基づき、自車位置の初期値を設定する（ステップＳ１０１）。次に、自車位置推定部１７は、車速センサ４から車体速度を取得すると共に、ジャイロセンサ３からヨー方向の角速度を取得する（ステップＳ１０２）。そして、自車位置推定部１７は、ステップＳ１０２の取得結果に基づき、車両の移動距離と車両の方位変化を計算する（ステップＳ１０３）。
　その後、自車位置推定部１７は、１時刻前の推定自車位置Ｘ＾（ｔ－１）に、ステップＳ１０３で計算した移動距離と方位変化を加算し、予測自車位置Ｘ－（ｔ）を算出する（ステップＳ１０４）。さらに、自車位置推定部１７は、予測自車位置Ｘ－（ｔ）に基づき、地図ＤＢ１０の地物情報を参照し、ライダ２の計測範囲内となるランドマークを探索する（ステップＳ１０５）。
　そして、自車位置推定部１７は、予測自車位置Ｘ－（ｔ）及びステップＳ１０５で探索したランドマークの地物情報が示す位置座標から、計測予測値Ｚ－（ｔ）を算出する（ステップＳ１０６）。さらに、ステップＳ１０６では、自車位置推定部１７は、ステップＳ１０５で探索したランドマークに対するライダ２の計測データから計測値Ｚ（ｔ）を算出する。
　そして、自車位置推定部１７は、予測自車位置Ｘ－（ｔ）が示す自車の方位（ヨー角）ψ－に相当する車両の進行方向と、地図ＤＢ１０に記録されたランドマークの向き情報とに基づき、車両の進行方向に対する感度係数ｋＸと、車両の横方向に対する感度係数ｋＹとをそれぞれ決定する（ステップＳ１０７）。この場合、例えば、自車位置推定部１７は、式（５）に基づき感度係数ｋＸを算出し、式（６）に基づき感度係数ｋＹを算出する。
　次に、自車位置推定部１７は、ランドマークの地図上の横幅ＷＭ及び縦幅ＨＭと計測した横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）との各差分と、感度係数ｋＸ、ｋＹとに基づき、式（３）及び式（４）を参照して、信頼度情報ａＸ（ｔ）、ａＹ（ｔ）をそれぞれ算出する（ステップＳ１０８）。そして、自車位置推定部１７は、信頼度情報ａＸ（ｔ）、ａＹ（ｔ）に応じてカルマンゲインＫ（ｔ）’を生成する（ステップＳ１０９）。例えば、自車位置推定部１７は、信頼度情報ａＸ（ｔ）、ａＹ（ｔ）を用いて式（８）に基づき修正観測雑音行列Ｒ（ｔ）’を算出後、式（９）に基づきカルマンゲインＫ（ｔ）’を算出する。その後、自車位置推定部１７は、カルマンゲインＫ（ｔ）´を式（１）のＫ（ｔ）の代わりに用いることで予測自車位置Ｘ－（ｔ）を補正し、推定自車位置Ｘ＾（ｔ）を算出する（ステップＳ１１０）。
　以上説明したように、本実施例に係る車載機１の自車位置推定部１７は、ライダ２によるランドマークの計測結果である点群データを取得すると共に、地図ＤＢ１０に含まれるランドマークの地物情報を取得する。そして、自車位置推定部１７は、取得した点群データに基づき特定したランドマークのサイズと地物情報が示すランドマークのサイズとの差分に基づいて、ライダ２によるランドマークの計測精度を示す信頼度情報を算出する。これにより、自車位置推定部１７は、推定自車位置Ｘ＾（ｔ）を算出する際の予測自車位置Ｘ－（ｔ）に対する補正量を、ライダ２の計測結果の正確さに応じて的確に定めることができる。
　［変形例］
　以下、実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。
　（変形例１）
　自車位置推定部１７は、式（２）に代えて、計測された横幅及び縦幅と地図上の横幅及び縦幅とのそれぞれの差の絶対値｜ＷＬ（ｔ）－ＷＭ｜、｜ＨＬ（ｔ）－ＨＭ｜を用いた以下の式（１１）に基づき信頼度情報ａ（ｔ）を算出してもよい。
Figure JPOXMLDOC01-appb-M000011
 　式（１１）に基づく信頼度情報ａ（ｔ）は、式（２）に基づく信頼度情報ａ（ｔ）と同様、０から１までの値域となるように正規化され、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）が地図上の横幅ＷＭ及び縦幅ＨＭと近いほど最大値の１に近づき、横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）が横幅ＷＭ及び縦幅ＨＭから離れるほど０に近づく。
　図１４（Ａ）は、横幅ＷＭが６０ｃｍ、縦幅ＨＭが６０ｃｍとなるランドマークに対して計測した縦幅ＨＬ（ｔ）を６０ｃｍに固定した場合の横幅ＷＬ（ｔ）と式（１１）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。また、図１４（Ｂ）は、図１４（Ａ）と同一のランドマークに対して計測した横幅ＷＬ（ｔ）を６０ｃｍに固定した場合の縦幅ＨＬ（ｔ）と式（１１）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。さらに、図１４（Ｃ）は、図１４（Ａ）、（Ｂ）と同一のランドマークに対する横幅ＷＬ（ｔ）と縦幅ＨＬ（ｔ）と式（１１）に基づく信頼度情報ａ（ｔ）との関係を示す３次元グラフである。また、図１４（Ｄ）は、図１４（Ａ）における信頼度情報ａ（ｔ）の逆数を示したグラフである。
　図１４（Ａ）及び図１４（Ｃ）に示すように、横幅ＷＬ（ｔ）は、横幅ＷＭである６０ｃｍに近づくほど信頼度情報ａ（ｔ）が１に近づく。一方、図１４（Ｂ）及び図１４（Ｃ）に示すように、縦幅ＨＬ（ｔ）は、縦幅ＨＭである６０ｃｍに近づくほど信頼度情報ａ（ｔ）が１に近づく。また、図１４（Ｄ）に示すように、信頼度情報ａ（ｔ）の逆数は、横幅ＷＬ（ｔ）が横幅ＷＭに近づくほど１に近づき、横幅ＷＬ（ｔ）が横幅ＷＭから遠ざかるほど大きい値となる。同様に、信頼度情報ａ（ｔ）の逆数は、縦幅ＨＬ（ｔ）が縦幅ＨＭに近づくほど１に近づき、縦幅ＨＬ（ｔ）が縦幅ＨＭから遠ざかるほど大きい値となる。
　このように、式（１１）に基づく信頼度情報ａ（ｔ）は、式（２）に基づく信頼度情報ａ（ｔ）と同様、最大値が１となり、計測された横幅及び縦幅と地図上の横幅及び縦幅とのそれぞれの差が大きいほど小さい値となる。よって、自車位置推定部１７は、式（１１）に基づく信頼度情報ａ（ｔ）の逆数を用いた式（８）に基づき修正観測雑音行列Ｒ（ｔ）’を算出することで、推定自車位置Ｘ＾（ｔ）を算出する際の予測自車位置Ｘ－（ｔ）に対する補正量を好適に定めることができる。
　なお、自車位置推定部１７は、実施例と同様、車両の進行方向及び横方向のそれぞれに対する信頼度情報ａＸ（ｔ）及び信頼度情報ａＹ（ｔ）を算出してもよい。この場合、自車位置推定部１７は、感度係数ｋＸ、ｋＹを式（５）、（６）に基づき算出することで、信頼度情報ａＸ（ｔ）及び信頼度情報ａＹ（ｔ）をそれぞれ算出する。
　（変形例２）
　自車位置推定部１７は、式（２）に代えて、信頼度が高いほど最小値の１に近づき、信頼度が低いほど大きな値になるように信頼度情報ａ（ｔ）を定めてもよい。
　例えば、自車位置推定部１７は、以下の式（１２）に基づき、信頼度情報ａ（ｔ）を算出する。
Figure JPOXMLDOC01-appb-M000012
 　式（１２）に基づく信頼度情報ａ（ｔ）は、１以上の値域となるように正規化され、計測された横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）が地図上の横幅ＷＭ及び縦幅ＨＭと近いほど最小値の１に近づき、横幅ＷＬ（ｔ）及び縦幅ＨＬ（ｔ）が横幅ＷＭ及び縦幅ＨＭから離れるほど大きい値となる。
　図１５（Ａ）は、横幅ＷＭが６０ｃｍ、縦幅ＨＭが６０ｃｍとなるランドマークに対して計測した縦幅ＨＬ（ｔ）を６０ｃｍに固定した場合の横幅ＷＬ（ｔ）と式（１２）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。また、図１５（Ｂ）は、図１５（Ａ）と同一のランドマークに対して計測した横幅ＷＬ（ｔ）を６０ｃｍに固定した場合の縦幅ＨＬ（ｔ）と式（１２）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。さらに、図１５（Ｃ）は、図１５（Ａ）、（Ｂ）と同一のランドマークに対する横幅ＷＬ（ｔ）と縦幅ＨＬ（ｔ）と式（１２）に基づく信頼度情報ａ（ｔ）との関係を示す３次元グラフである。
　図１５（Ａ）及び図１５（Ｃ）に示すように、信頼度情報ａ（ｔ）は、横幅ＷＭである６０ｃｍに横幅ＷＬ（ｔ）が近づくほど最小値である１に近づき、横幅ＷＭから横幅ＷＬ（ｔ）が遠ざかるほど大きい値となる。同様に、図１５（Ｂ）及び図１５（Ｃ）に示すように、信頼度情報ａ（ｔ）は、縦幅ＨＭである６０ｃｍに縦幅ＨＬ（ｔ）が近づくほど最小値である１に近づき、縦幅ＨＭから縦幅ＨＬ（ｔ）が遠ざかるほど大きい値となる。
　なお、自車位置推定部１７は、実施例と同様、車両の進行方向及び横方向のそれぞれに対する信頼度情報ａＸ（ｔ）及び信頼度情報ａＹ（ｔ）を算出してもよい。この場合、自車位置推定部１７は、感度係数ｋＸ、ｋＹを式（５）、（６）に基づき算出することで、信頼度情報ａＸ（ｔ）及び信頼度情報ａＹ（ｔ）をそれぞれ算出する。
　また、式（１２）に基づく信頼度情報ａ（ｔ）は、式（２）に基づく信頼度情報ａ（ｔ）の逆数（図７（Ｄ）参照）と同様に、最小値が１となり、かつ、横幅ＷＭから横幅ＷＬ（ｔ）が遠ざかる又は縦幅ＨＭから縦幅ＨＬ（ｔ）が遠ざかるほど大きい値となる。よって、自車位置推定部１７は、修正観測雑音行列Ｒ（ｔ）’を算出する際に、信頼度情報ａ（ｔ）を逆数にして観測雑音行列Ｒ（ｔ）の対角成分に乗じる必要がない。具体的には、自車位置推定部１７は、信頼度情報ａＸ（ｔ）及び信頼度情報ａＹ（ｔ）をそれぞれ算出した場合には、以下の式（１３）に基づき、修正観測雑音行列Ｒ（ｔ）’を算出すればよい。
Figure JPOXMLDOC01-appb-M000013
 　これにより、実施例と同様、自車位置推定部１７は、推定自車位置Ｘ＾（ｔ）を算出する際の予測自車位置Ｘ－（ｔ）に対する補正量を好適に定めることができる。
　（変形例３）
　図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が車載機１の自車位置推定部１７の処理を実行してもよい。この場合、地図ＤＢ１０は、例えば車両内の記憶部に記憶され、車両の電子制御装置は、図１３のフローチャートの処理を実行することで推定自車位置を算出する。
　（変形例４）
　信頼度情報は、推定自車位置Ｘ＾（ｔ）を算出する際の予測自車位置Ｘ－（ｔ）の補正量を決定するパラメータとして用いられることに限定されない。例えば、車載機１は、算出した信頼度情報を、ライダ２が出力する点群データに基づく障害物検知などの他の用途に用いてもよい。
　１　車載機
　２　ライダ
　３　ジャイロセンサ
　４　車速センサ
　５　ＧＰＳ受信機
　１０　地図ＤＢ

Claims (10)

  1. 　計測部による地物の計測結果を取得する第１取得部と、
    　地図データに含まれる前記地物の地物情報を取得する第２取得部と、
    　計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部と、を有する計測精度算出装置。
  2. 　前記算出部は、前記差分が大きいほど、前記計測精度が低い精度情報を生成する請求項１に記載の計測精度算出装置。
  3. 　前記算出部は、前記差分と、移動体の進行方向と、前記地物情報が示す前記地物の法線方向とに基づき、前記移動体の進行方向と横方向のそれぞれに対する前記地物の計測精度を示す精度情報を生成する請求項１または２に記載の計測精度算出装置。
  4. 　前記算出部は、前記進行方向と前記地物の法線方向との角度差が大きいほど、前記横方向に対する前記計測精度への前記差分の影響度合いを低くし、前記角度差が小さいほど、前記進行方向に対する前記計測精度への前記差分の影響度合いを低くする請求項３に記載の計測精度算出装置。
  5. 　予測された自己位置を示す予測位置情報を取得する第３取得部と、
    　前記精度情報に基づき、前記予測された自己位置を補正する補正部と、
    をさらに備える請求項１～４のいずれか一項に記載の計測精度算出装置。
  6. 　前記補正部は、移動体から前記地物までの前記計測部による計測距離と、前記地物情報に含まれる前記地物の位置情報に基づき予測された前記移動体から前記地物までの予測距離と、の差分値により前記予測された自己位置を補正する場合の前記差分値に対する利得を、前記精度情報に基づき決定する請求項５に記載の計測精度算出装置。
  7. 　計測部による地物の計測結果を取得する第１取得部と、
    　地図データに含まれる前記地物の地物情報を取得する第２取得部と、
    　予測された自己位置を示す予測位置情報を取得する第３取得部と、
    　計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記予測された自己位置を補正する補正部と、
    を備える自己位置推定装置。
  8. 　計測精度算出装置が実行する制御方法であって、
    　計測部による地物の計測結果を取得する第１取得工程と、
    　地図データに含まれる前記地物の地物情報を取得する第２取得工程と、
    　計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出工程と、
    を有する制御方法。
  9. 　コンピュータが実行するプログラムであって、
    　計測部による地物の計測結果を取得する第１取得部と、
    　地図データに含まれる前記地物の地物情報を取得する第２取得部と、
    　計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部
    として前記コンピュータを機能させるプログラム。
  10. 　請求項９に記載のプログラムを記憶した記憶媒体。
PCT/JP2019/039479 2018-10-24 2019-10-07 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体 WO2020085062A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2020553079A JP7155284B2 (ja) 2018-10-24 2019-10-07 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
EP19876435.9A EP3872454A4 (en) 2018-10-24 2019-10-07 MEASUREMENT ACCURACY CALCULATION DEVICE, HOST POSITION ESTIMATING DEVICE, CONTROL METHOD, PROGRAM AND STORAGE MEDIUM
US17/272,920 US20210278217A1 (en) 2018-10-24 2019-10-07 Measurement accuracy calculation device, self-position estimation device, control method, program and storage medium
JP2022160979A JP2022188203A (ja) 2018-10-24 2022-10-05 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018200067 2018-10-24
JP2018-200067 2018-10-24

Publications (1)

Publication Number Publication Date
WO2020085062A1 true WO2020085062A1 (ja) 2020-04-30

Family

ID=70331005

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/039479 WO2020085062A1 (ja) 2018-10-24 2019-10-07 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体

Country Status (4)

Country Link
US (1) US20210278217A1 (ja)
EP (1) EP3872454A4 (ja)
JP (2) JP7155284B2 (ja)
WO (1) WO2020085062A1 (ja)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20220059754A (ko) * 2020-11-03 2022-05-10 울산대학교 산학협력단 센서 퓨전에 의한 차량의 위치 추정 방법
WO2022124115A1 (ja) * 2020-12-07 2022-06-16 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体
KR20220160881A (ko) * 2021-05-28 2022-12-06 울산대학교 산학협력단 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법
JP7459732B2 (ja) 2020-09-08 2024-04-02 株式会社豊田自動織機 自己位置推定システム

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPWO2021095879A1 (ja) * 2019-11-13 2021-05-20

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013257742A (ja) 2012-06-13 2013-12-26 Sumitomo Heavy Ind Ltd 移動体位置推定方法及び移動体
JP2017072422A (ja) 2015-10-05 2017-04-13 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体
JP2018036067A (ja) * 2016-08-29 2018-03-08 株式会社Ｓｏｋｅｎ 自車位置認識装置
JP2018059744A (ja) * 2016-10-03 2018-04-12 株式会社Ｓｏｋｅｎ 自車位置認識装置
JP2018116014A (ja) * 2017-01-20 2018-07-26 パイオニア株式会社 自車位置推定装置、制御方法、プログラム及び記憶媒体

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011511281A (ja) * 2008-02-04 2011-04-07 テレ アトラス ノース アメリカ インコーポレイテッド センサにより検出されたオブジェクトとマップマッチングする方法
ES2650607T3 (es) * 2011-04-21 2018-01-19 Konecranes Global Corporation Técnicas para posicionar un vehículo
EP3018448B1 (en) * 2014-11-04 2021-01-06 Volvo Car Corporation Methods and systems for enabling improved positioning of a vehicle
CN105718860B (zh) * 2016-01-15 2019-09-10 武汉光庭科技有限公司 基于驾驶安全地图及双目交通标志识别的定位方法及系统
WO2017147792A1 (en) * 2016-03-01 2017-09-08 SZ DJI Technology Co., Ltd. Methods and systems for target tracking
US10370102B2 (en) * 2016-05-09 2019-08-06 Coban Technologies, Inc. Systems, apparatuses and methods for unmanned aerial vehicle
EP3493177B1 (en) * 2016-07-29 2022-09-07 Pioneer Corporation Measurement device, measurement method, and program
JP6776707B2 (ja) * 2016-08-02 2020-10-28 トヨタ自動車株式会社 自車位置推定装置
JP6828714B2 (ja) * 2018-05-30 2021-02-10 株式会社Ｓｏｋｅｎ 自己位置推定装置
JP7132037B2 (ja) * 2018-08-29 2022-09-06 フォルシアクラリオン・エレクトロニクス株式会社 車載処理装置
JP7067536B2 (ja) * 2018-08-31 2022-05-16 株式会社デンソー 車両制御装置、方法および記憶媒体

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013257742A (ja) 2012-06-13 2013-12-26 Sumitomo Heavy Ind Ltd 移動体位置推定方法及び移動体
JP2017072422A (ja) 2015-10-05 2017-04-13 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体
JP2018036067A (ja) * 2016-08-29 2018-03-08 株式会社Ｓｏｋｅｎ 自車位置認識装置
JP2018059744A (ja) * 2016-10-03 2018-04-12 株式会社Ｓｏｋｅｎ 自車位置認識装置
JP2018116014A (ja) * 2017-01-20 2018-07-26 パイオニア株式会社 自車位置推定装置、制御方法、プログラム及び記憶媒体

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7459732B2 (ja) 2020-09-08 2024-04-02 株式会社豊田自動織機 自己位置推定システム
KR20220059754A (ko) * 2020-11-03 2022-05-10 울산대학교 산학협력단 센서 퓨전에 의한 차량의 위치 추정 방법
KR102460842B1 (ko) * 2020-11-03 2022-10-31 울산대학교 산학협력단 센서 퓨전에 의한 차량의 위치 추정 방법
WO2022124115A1 (ja) * 2020-12-07 2022-06-16 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体
KR20220160881A (ko) * 2021-05-28 2022-12-06 울산대학교 산학협력단 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법
KR102489865B1 (ko) * 2021-05-28 2023-01-18 울산대학교 산학협력단 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법

Also Published As

Publication number Publication date
US20210278217A1 (en) 2021-09-09
JP7155284B2 (ja) 2022-10-18
EP3872454A1 (en) 2021-09-01
EP3872454A4 (en) 2022-08-10
JPWO2020085062A1 (ja) 2021-09-02
JP2022188203A (ja) 2022-12-20

Similar Documents

Publication Publication Date Title
WO2020085062A1 (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
WO2018181974A1 (ja) 判定装置、判定方法、及び、プログラム
JP6980010B2 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP6806891B2 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP6968877B2 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
WO2021112074A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP2021113816A (ja) 出力装置、制御方法、プログラム及び記憶媒体
JP2023164553A (ja) 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
WO2018212302A1 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP2019174191A (ja) データ構造、情報送信装置、制御方法、プログラム及び記憶媒体
WO2019188886A1 (ja) 端末装置、情報処理方法、プログラム、及び、記憶媒体
WO2018212290A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2019188874A1 (ja) データ構造、情報処理装置、及び地図データ生成装置
JP6923750B2 (ja) 自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体
US20240053440A1 (en) Self-position estimation device, self-position estimation method, program, and recording medium
JP2019174675A (ja) データ構造、地図データ生成装置、制御方法、プログラム及び記憶媒体
US20220413150A1 (en) Information processing device, control method, program and storage medium

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19876435

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2020553079

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2019876435

Country of ref document: EP

Effective date: 20210525