WO2018212302A1 - 自己位置推定装置、制御方法、プログラム及び記憶媒体 - Google Patents

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

Info

Publication number
WO2018212302A1
WO2018212302A1 PCT/JP2018/019176 JP2018019176W WO2018212302A1 WO 2018212302 A1 WO2018212302 A1 WO 2018212302A1 JP 2018019176 W JP2018019176 W JP 2018019176W WO 2018212302 A1 WO2018212302 A1 WO 2018212302A1
Authority
WO
WIPO (PCT)
Prior art keywords
predicted
self
position estimation
vehicle position
unit
Prior art date
Application number
PCT/JP2018/019176
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 パイオニア株式会社
Publication of WO2018212302A1 publication Critical patent/WO2018212302A1/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/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
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/09—Arrangements for giving variable traffic instructions
    • G08G1/0962—Arrangements for giving variable traffic instructions having an indicator mounted inside the vehicle, e.g. giving voice messages
    • G08G1/0968—Systems involving transmission of navigation instructions to the vehicle
    • G08G1/0969—Systems involving transmission of navigation instructions to the vehicle having a display in the form of a map

Definitions

  • the present invention relates to a self-position estimation technique.
  • Patent Document 1 discloses a technique for estimating a self-position by collating the output of a measurement sensor with the position information of a feature registered in advance on a map.
  • Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.
  • the present invention has been made in order to solve the above-described problems, and it is possible to suitably improve the position estimation accuracy in self-position estimation in which an object existing continuously or intermittently is a measurement object.
  • the main object is to provide a self-position estimation apparatus.
  • the invention according to claim 1 is a self-position estimation device, and obtains predicted position information indicating a predicted self-position, and measurement from a moving object to an object existing continuously or intermittently.
  • a correction unit that corrects the predicted self-position based on the measurement distance by the unit and the predicted distance from the moving body to the target predicted based on the position information of the target, The correction unit corrects the predicted self-position so that a correction amount in a first direction intersecting the longitudinal direction is larger than a correction amount in the longitudinal direction of the object.
  • the invention according to claim 9 is a control method executed by the self-position estimation device, and is present continuously or intermittently from the acquisition step of obtaining predicted position information indicating the predicted self-position and the moving body.
  • a correction step of correcting the predicted self-position based on a measurement distance by the measurement unit to the target and a predicted distance from the moving body to the target predicted based on position information of the target The correction step corrects the predicted self-position so that the correction amount in the first direction intersecting the longitudinal direction is larger than the correction amount in the longitudinal direction of the object.
  • the invention according to claim 10 is a program executed by a computer, from an acquisition unit that acquires predicted position information indicating a predicted self-position, to a target that exists continuously or intermittently from a moving object.
  • the computer as a correction unit that corrects the predicted self-position based on the measurement distance by the measurement unit and the predicted distance from the moving body to the target predicted based on the position information of the target.
  • the correction unit corrects the predicted self-position so that a correction amount in a first direction intersecting the longitudinal direction is larger than a correction amount in the longitudinal direction of the object.
  • the functional block of the own vehicle position estimation part is shown.
  • working the lane divided by the lane marking is shown.
  • a self-position estimation device from an acquisition unit that acquires predicted position information indicating a predicted self-position, and a moving object to an object that exists continuously or intermittently.
  • a correction unit that corrects the predicted self-position based on a measurement distance by the measurement unit and a predicted distance from the moving body to the target predicted based on position information of the target.
  • the correction unit corrects the predicted self-position so that a correction amount in a first direction intersecting the longitudinal direction is larger than a correction amount in the longitudinal direction of the object.
  • the self-position estimation apparatus can appropriately improve the self-position by appropriately correcting the self-position with a target that is continuously or intermittently present as a measurement target, and can suitably improve the position estimation accuracy.
  • the correction unit may set the correction amount in the longitudinal direction of the object to a value sufficiently small to be negligible with respect to 0 or the correction amount in the first direction.
  • the correction unit when the correction unit uses a structure extending along a road as the target object, the correction unit has a lateral direction of the moving body rather than a correction amount of the moving direction of the moving body.
  • the predicted self-position is corrected so that the correction amount in the vehicle is large and the structure extending in the width direction of the road is the object, the movement is greater than the lateral correction amount of the moving body.
  • the predicted self-position is corrected so that the correction amount in the body traveling direction is increased.
  • the self-position estimation device can be used for measuring any of vertically long structures such as lane markings, curbs, guardrails, or horizontally long structures such as stop lines, pedestrian bridges, and overpasses.
  • the self-position can be appropriately corrected, and the position estimation accuracy can be preferably improved.
  • the coordinates of discrete points of the object are stored in map information, and the correction unit is separated from the predicted self-position by a predetermined distance from the self-position.
  • the distance from the coordinates of the point of the object in the prediction window set at the position is calculated as the prediction distance, and the distance to the object measured in the prediction window is calculated as the measurement distance.
  • the self-position estimation apparatus can preferably calculate the predicted distance and the measurement distance, respectively, even when performing self-position estimation using an object that exists continuously or intermittently as a measurement target.
  • the correction unit corrects the predicted self-position by a value obtained by multiplying the difference value by a predetermined gain, and the correction unit includes a length of the object.
  • the correction coefficient for the gain is determined so that the correction amount in the first direction intersecting the longitudinal direction is larger than the correction amount in the direction.
  • the self-position estimation apparatus can suitably correct the predicted self-position so that the correction amount in the first direction is larger than the correction amount in the longitudinal direction of the object.
  • the correction unit corrects the predicted self-position by a value obtained by multiplying the difference value by a predetermined gain, and the correction unit includes a length of the object.
  • the correction coefficient for the difference value in the longitudinal direction of the object is determined so that the correction amount in the first direction intersecting with the longitudinal direction is larger than the correction amount in the direction.
  • the self-position estimation apparatus can suitably correct the predicted self-position so that the correction amount in the first direction is larger than the correction amount in the longitudinal direction of the object.
  • the gain is a Kalman gain.
  • the correction unit is either when the object is measured by the measurement unit or when a feature other than the object is measured by the measurement unit. Also, the correction coefficient is provided when the predicted self-position is corrected based on the Kalman gain and the object is measured by the measuring unit. According to this aspect, the self-position estimation apparatus can suitably perform the self-position estimation process based on the same method, regardless of whether the object or another feature is a measurement target.
  • the correction unit is either when the object is measured by the measurement unit or when a feature other than the object is measured by the measurement unit.
  • the covariance matrix is updated based on the Kalman gain.
  • the self-position estimation apparatus can preferably update the covariance matrix corresponding to the predicted error distribution of the self-position, regardless of whether the object or other features are to be measured.
  • a control method executed by the self-position estimation device executed by the self-position estimation device, the obtaining step of obtaining predicted position information indicating the predicted self-position, and continuous or intermittent from the moving body The predicted self-position based on the measured distance by the measurement unit to the existing object and the predicted distance from the moving object to the object predicted based on the position information of the object.
  • a correction step for correcting wherein the correction step determines the predicted self-position so that a correction amount in a first direction intersecting the longitudinal direction is larger than a correction amount in the longitudinal direction of the object. to correct.
  • the self-position estimation apparatus can appropriately correct the self-position with a target that is continuously or intermittently present as a measurement target, and can suitably improve the position estimation accuracy.
  • the program is executed by a computer and is present continuously or intermittently from an acquisition unit that acquires predicted position information indicating a predicted self-position and a moving object.
  • a correction unit that corrects the predicted self-position based on a measurement distance by the measurement unit to the target and a predicted distance from the moving body to the target predicted based on position information of the target
  • the correction unit corrects the predicted self-position so that the correction amount in the first direction intersecting the longitudinal direction is larger than the correction amount in the longitudinal direction of the object.
  • the computer can appropriately correct its own position with a target that is continuously or intermittently present as a measurement target, and can suitably improve the position estimation accuracy.
  • the program is stored in a storage medium.
  • FIG. 1 is a schematic configuration diagram of a driving support system according to the present embodiment.
  • the driving support system shown in FIG. 1 is mounted on a vehicle and has an in-vehicle device 1 that performs control related to driving support of the vehicle, a lidar (Lidar: Light Detection and Ranging, or Laser Illuminated Detection And Ranging) 2, and a gyro sensor 3. And a vehicle speed sensor 4 and a GPS receiver 5.
  • a lidar Light Detection and Ranging, or Laser Illuminated Detection And Ranging
  • the in-vehicle 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 in-vehicle device 1 is mounted ("own vehicle position"). Also called.) And the vehicle equipment 1 performs automatic driving
  • the in-vehicle device 1 stores a map database (DB: DataBase) 10 in which information on road data and landmarks and lane markings provided near the road is registered.
  • DB DataBase
  • the features that serve as the above-mentioned landmarks are, for example, features such as kilometer posts, 100 m posts, delineators, traffic infrastructure facilities (for example, signs, direction signs, signals), utility poles, street lamps, and the like that are periodically arranged on the side of the road.
  • the vehicle equipment 1 estimates the own vehicle position by collating with the output of the lidar 2 etc. based on this map DB10.
  • the in-vehicle device 1 is an example of the “self-position estimation device” in the present invention.
  • the lidar 2 emits a pulse laser in a predetermined angle range in the horizontal direction and the vertical direction, thereby discretely measuring the distance to an object existing in the outside world, 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) of the irradiated laser light, and scan data based on a light reception signal output by the light receiving unit.
  • Output unit the laser irradiation range emitted from the lidar 2 includes at least the road surface of the road.
  • the scan data is generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the response delay time of the laser beam specified based on the above-described received light signal.
  • the accuracy of the lidar distance measurement value is higher as the distance to the object is shorter, and the accuracy is lower as the distance is longer.
  • the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the in-vehicle device 1.
  • the lidar 2 is an example of the “measurement unit” in the present invention.
  • FIG. 2 is a block diagram showing a functional configuration of the in-vehicle device 1.
  • the in-vehicle 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 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 a map DB 10 including lane marking information and feature information.
  • the lane marking information is information regarding the lane marking (white line) provided on each road, and each lane marking includes at least coordinate information indicating a discrete position of the lane marking.
  • the lane marking information may be information incorporated in road data for each road.
  • the feature information is information about features other than the lane markings, and here, for each feature, the feature ID corresponding to the feature index, the latitude and longitude (and elevation), etc. At least the position information indicating the absolute position of the object is associated.
  • the input unit 14 is a button for operation by the user, a touch panel, a remote controller, a voice input device, or the like.
  • the information output unit 16 is, for example, a display or a speaker that outputs based on the control of the control unit 15.
  • the own vehicle position estimation unit 17 is based on the measured values of the distance and angle by the lidar 2 with respect to the landmark and the position information of the landmark extracted from the map DB 10, and the gyro sensor 3, the vehicle speed sensor 4, and / or the GPS receiver.
  • the vehicle position estimated from the output data of 5 is corrected.
  • the vehicle position estimation unit 17 predicts the vehicle position from output data of the gyro sensor 3, the vehicle speed sensor 4 and the like based on a state estimation method based on Bayesian estimation,
  • the measurement update step for correcting the predicted value of the vehicle position calculated in the prediction step is executed alternately.
  • 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.
  • the own vehicle position estimating unit 17 uses the extended Kalman filter to uniformly estimate the own vehicle position when performing vehicle position estimation for either a feature or a lane marking. Process.
  • FIG. 3 is a diagram showing the state variable vector x in two-dimensional orthogonal coordinates.
  • the vehicle position on the plane defined on the two-dimensional orthogonal coordinates of xy is represented by coordinates “(x, y)” and the direction “ ⁇ ” of the vehicle.
  • the direction ⁇ is defined as an angle formed by the traveling direction of the vehicle and the x axis.
  • the coordinates (x, y) indicate an absolute position corresponding to a combination of latitude and longitude, for example.
  • FIG. 4 is a diagram illustrating a schematic relationship between the prediction step and the measurement update step.
  • FIG. 5 shows an example of functional blocks of the vehicle position estimation unit 17. As shown in FIG. 4, by repeating the prediction step and the measurement update step, calculation and update of the estimated value of the state variable vector “X” indicating the vehicle position are sequentially executed. Moreover, as shown in FIG. 5, the own vehicle position estimation part 17 has the position estimation part 21 which performs a prediction step, and the position estimation part 22 which performs 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.
  • the provisional estimated value (predicted value) estimated in the predicting step is appended with “ - ” on the character representing the predicted value, and the estimated value with higher accuracy updated in the measurement updating step. Is appended with “ ⁇ ” on the character representing the value.
  • the covariance matrix “P ⁇ (t)” corresponding to the error distribution of the predicted vehicle position X ⁇ (t) is converted into 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 vehicle position estimation unit 17 associates the landmark position vector registered in the map DB 10 with the scan data of the lidar 2. Then, the landmark search / extraction block 25 of the vehicle position estimation unit 17 calls the measurement value (“landmark measurement value”) by the lidar 2 of the landmark that can be associated when the association is made. ) Landmark measurement estimated value obtained by modeling the measurement process by the rider 2 using “Z (t)”, the predicted vehicle position X ⁇ (t) and the landmark position vector registered in the map DB 10. (Referred to as “landmark prediction value”) “Z ⁇ (t)” is acquired.
  • the landmark measurement value Z (t) is a two-dimensional vehicle body coordinate system in which the landmark distance and scan angle measured by the lidar 2 at time t are converted into components with the vehicle traveling direction and the lateral direction as axes. Is a vector. Then, the position correction block 26 of the vehicle position estimation unit 17 calculates a difference value between the landmark measurement value Z (t) and the landmark prediction value Z ⁇ (t) as shown in the following equation (1). To do.
  • the position correction block 26 of the vehicle position estimation unit 17 calculates the Kalman to the difference value between the landmark measurement value Z (t) and the landmark prediction value Z ⁇ (t) as shown in the following equation (2).
  • the gain “K (t)” By multiplying the gain “K (t)” and adding this to the predicted own vehicle position X ⁇ (t), the updated state variable vector (also referred to as “estimated own vehicle position”) X ⁇ (t) calculate.
  • the position correction block 26 of the vehicle position estimating section 17 similarly to the prediction step, the covariance matrix corresponding to the error distribution of the estimated vehicle position X ⁇ (t) P ⁇ ( t) ( simply P (t)) is obtained 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 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 likely vehicle position Is calculated.
  • the vehicle position estimation unit 17 uses a landmark measurement value and a landmark in a coordinate system (also referred to as a “body coordinate system”) with the traveling direction of the vehicle as an axis. The predicted value is converted, and the Kalman gain in the traveling direction of the vehicle is set to zero. Thereby, the own vehicle position estimation part 17 performs highly accurate own vehicle position estimation even when it is a case where the lane marking which exists continuously or intermittently in the advancing direction is made into a measuring object.
  • a coordinate system also referred to as a “body coordinate system”
  • the vehicle position estimation unit 17 first predicts points Pp2 and Pp5 closest to a position away from the vehicle by a predetermined distance (for example, 5 m) in the left front, left rear, right front, and right rear directions of the vehicle. , Pp12 and Pp15 are extracted. Then, the vehicle position estimation unit 17 sets the rectangular areas centered on the extracted prediction points Pp2, Pp5, Pp12, and Pp15 as prediction windows “Wp1” to “Wp4” that define ranges for detecting the lane markings, respectively. To do.
  • a predetermined distance for example, 5 m
  • the vehicle position estimation unit 17 extracts from the measurement data output from the lidar 2 high-reflectance point cloud data that has a reflectance equal to or higher than a predetermined threshold value on the road surface within the prediction windows Wp1 to Wp4. . Then, for each of the prediction windows Wp1 to Wp4, the vehicle position estimation unit 17 uses the center of gravity position (that is, the average of the coordinates) of each position indicated by the extracted point cloud data as a reference for calculating the landmark measurement value. Calculated as points “Ptag1” to “Ptag4”. In the example of FIG.
  • the vehicle position estimation unit 17 calculates the measurement reference point Ptag1 based on the point cloud data obtained by measuring the partial region 41 of the continuous line 30 in the prediction window Wp1, and the continuous line 30 in the prediction window Wp1.
  • a measurement reference point Ptag2 is calculated based on the point cloud data obtained by measuring the partial region 42 of the first.
  • the vehicle position estimation unit 17 calculates the measurement reference point Ptag3 based on the point cloud data obtained by measuring the partial area 43 of the broken line 31 in the prediction window Wp3, and measures the partial area 44 of the broken line 31 in the prediction window Wp4. Based on the obtained point cloud data, a measurement reference point Ptag4 is calculated.
  • the host vehicle position estimation unit 17 determines the distance from the estimated host vehicle position to the measurement reference points Ptag1 to Ptag4 as a distance measurement value “L” to the continuous line 30 and the broken line 31 that are lane markings, and estimates The distances from the vehicle position to the prediction points Pp2, Pp5, Pp12, and Pp15 that are the centers of the prediction windows Wp1 to Wp4 are determined as the distance prediction value “L ⁇ ” to the continuous line 30 and the broken line 31. Further, the own vehicle position estimating unit 17 calculates an angle formed by a line connecting the measurement reference points Ptag1 to Ptag4 from the estimated own vehicle position as an angle measurement value “ ⁇ ”, and calculates the predicted windows Wp1 to Wp4 from the estimated own vehicle position.
  • the measurement reference points Ptag1 to Ptag4 are different from the discrete points P2, P5, P12, and P15 corresponding to the prediction points Pp2, Pp5, Pp12, and Pp15 that are the centers of the prediction windows Wp1 to Wp4, and the distance measurement values
  • the L and angle measurement values ⁇ are different from the values that should be measured. This is because the continuous line 30 and the broken line 31 extend in the traveling direction of the vehicle, and the position in the traveling direction of the vehicle cannot be specified.
  • the coordinate information of the broken line itself is not registered in the lane line information of the map DB 10 even if there is identification information indicating whether the lane line is a solid line or a broken line. Therefore, when the lane marking is a broken line, the high reflectance part included in the prediction window changes depending on the relative positional relationship between the broken line and the window, and the distance measurement value L and the angle measurement value ⁇ change depending on the relative positional relationship described above. Resulting in.
  • the vehicle position estimation unit 17 converts the landmark measurement value and the landmark prediction value into the body coordinate system, and the traveling direction of the vehicle, which is the direction in which the lane marking extends. Set the Kalman gain to zero.
  • the own vehicle position estimation part 17 suppresses suitably the fall of the own vehicle position estimation precision resulting from not being able to pinpoint the position of the lane marking in the advancing direction of a vehicle.
  • the lane markings are examples of the “object” and “structure extending along the road” in the present invention.
  • FIG. 7 is a diagram showing an outline of the own vehicle position estimation process using a lane marking as a measurement target.
  • FIG. 7 shows an example in which the estimated host vehicle position is calculated based on the measurement data of the partial region 41 of the continuous line 30 in the prediction window Wp1.
  • the x-coordinate and y-coordinate of the discrete point P2 represented by the map coordinate system having the xy coordinate axes are “m x ” and “m y ”, respectively, and measurement is performed in the body coordinate system having the XY coordinate axes.
  • the X coordinate and Y coordinate of the reference point Ptag1 are “L X ” and “L Y ”, respectively.
  • the coordinates are converted into the coordinates (L X , L Y ) of the body coordinate system taken.
  • the X coordinate L X indicating the position in the traveling direction of the vehicle is different from the coordinate to be originally measured due to the continuity of the continuous line 30, but the Y coordinate L Y is not affected by the continuity of the continuous line 30.
  • the broken line 31 is a measurement target, the value of the X coordinate L X differs depending on the relative relationship between the prediction window and the broken line 31, but the Y coordinate L Y is not affected.
  • the vehicle position estimation unit 17 also uses the body coordinate system coordinates (L ⁇ X , L ⁇ Y ) for the distance predicted value L ⁇ and the angle predicted value ⁇ ⁇ so that the measured value and the predicted value are converted into the body. Unify into coordinate system.
  • the landmark predicted value Z ⁇ (t) is expressed by the following equation (3).
  • the vehicle position estimation unit 17 multiplies the first column of the Kalman gain matrix by a correction coefficient 0 as shown in the following equation (4) so that the Kalman gain in the X direction of the body coordinate system becomes zero. .
  • the own vehicle position estimation part 17 shows to the following formula
  • K (t) the estimated own vehicle position X ⁇ (t) is calculated.
  • the X-coordinates L X and L ⁇ X are not used for the calculation of the estimated own vehicle position X ⁇ (t), and the difference value “L ⁇ Y ⁇ L Y ” in the Y coordinate is used.
  • the predicted vehicle position is corrected based on the above. Thereby, even if it is a case where a lane marking is made into a measuring object, a position estimation precision can be improved suitably.
  • the coordinates (L X, L Y) is an example of the "measurement distance" of the present invention
  • the coordinates (L - X, L - Y ) is an example of the "predicted distance" in the present invention It is.
  • the own vehicle position estimation unit 17 also calculates the estimated own vehicle position by converting the landmark measurement value and the landmark prediction value into the body coordinate system even in the case of performing the vehicle position estimation with the feature as the measurement target. .
  • FIG. 8 is a diagram showing an outline of the vehicle position estimation process for measuring a feature.
  • x-coordinate indicated by the feature information of the feature 32 that are registered in the map DB 10 respectively y coordinates m x, and m y, the measurement position by the rider second feature 32 of the body coordinate system
  • the X coordinate and Y coordinate shown are L X and L Y , respectively.
  • the vehicle position estimation unit 17 calculates the landmark measurement value based on the measurement data obtained by the lidar 2 of the feature 32, and calculates the landmark prediction value based on the above-described equation (3).
  • the host vehicle position estimation unit 17 uses a Kalman gain matrix represented by the following equation (6) that is not corrected by the correction coefficient as the Kalman gain used in equation (2).
  • the own vehicle position estimation part 17 calculates the estimated own vehicle position X ⁇ (t) using the landmark measurement value and landmark prediction value of the body coordinate system, as shown in the following formula (7).
  • the vehicle position estimation unit 17 performs the vehicle position estimation process uniformly using the extended Kalman filter even when the vehicle position is estimated for any of the features or the lane markings.
  • the own vehicle position estimation unit 17 determines whether or not the vehicle body speed and the angular velocity in the yaw direction of the vehicle have been detected (step S101). For example, the host vehicle position estimation unit 17 detects the vehicle body speed based on the output of the vehicle speed sensor 4 and detects the angular speed in the yaw direction based on the output of the gyro sensor 3. When the vehicle position estimation unit 17 detects the vehicle body speed and the angular velocity in the yaw direction of the vehicle (step S101; Yes), the estimated vehicle position X ⁇ one time before is detected using the detected vehicle body speed and angular velocity. A predicted host vehicle position X ⁇ (t) is calculated from (t ⁇ 1) (step S102). Further, in step S102, the vehicle position estimation unit 17 calculates a covariance matrix at the current time from the covariance matrix one hour before.
  • the vehicle position estimation unit 17 determines whether or not a feature used for vehicle position estimation has been detected (step S103). For example, the host vehicle position estimation unit 17 determines whether there is a feature in which the position vector of the feature registered in the map DB 10 can be associated with the scan data of the rider 2. And the own vehicle position estimation part 17 calculates the landmark measurement value Z (t) based on the point cloud data which the lidar 2 outputs, when the feature used for own vehicle position estimation is detected (step S103; Yes). At the same time, the landmark predicted value Z ⁇ (t) is calculated based on the position coordinates indicated by the feature information of the target feature (step S104). At this time, preferably, the vehicle position estimation unit 17 converts the landmark measurement value Z (t) and the landmark prediction value Z ⁇ (t) into the body coordinate system as described with reference to FIG. Good.
  • the vehicle position estimation unit 17 calculates the Kalman gain based on the following equation (8) using the covariance matrix and the observation noise matrix “R (t)” calculated in step S102 (step S105). .
  • step S105 the host vehicle position estimation unit 17 uses the calculated Kalman gain K, the landmark measurement value Z (t), and the landmark prediction value Z ⁇ (t) to calculate the equation (2) or ( Based on 7), the estimated host vehicle position X ⁇ (t) is calculated. Further, in step S105, the host vehicle position estimation unit 17 updates the covariance matrix based on the following equation (9) using the Kalman gain K.
  • the own vehicle position estimation unit 17 can select any one selected ground when the position vector registered in the map DB 10 and the scan data of the lidar 2 can be associated with a plurality of features. Steps S104 and S105 may be executed for an object, and steps S104 and S105 may be repeatedly executed for all the features that can be associated. In the latter case, the vehicle position estimation unit 17 considers that the lidar measurement accuracy deteriorates as the feature farther from the rider 2, and the longer the distance between the rider 2 and the feature, the more relevant the feature is. It is better to reduce the weight.
  • the host vehicle position estimation unit 17 determines whether or not a lane line used for host vehicle position estimation has been detected (step S106). For example, the vehicle position estimation unit 17 sets prediction windows Wp1 to Wp4 as shown in FIG. 6, and sets the measurement reference points corresponding to the discrete points of the lane markings on the map DB 10 in the prediction window. It is determined whether or not the calculation is possible based on the scan data.
  • the vehicle position estimation unit 17 detects a lane line used for vehicle position estimation (step S106; Yes)
  • the landmark measurement value Z (t) and the landmark prediction value Z ⁇ converted to the body coordinate system. (T) is calculated (step S107).
  • the own vehicle position estimation part 17 calculates a Kalman gain based on above-mentioned Formula (8) using a covariance matrix (step S108). Further, in step S108, the own vehicle position estimating unit 17 uses the Kalman gain K (t) ′ (see the expression (4)) in which the first column is set to 0, and the estimated own vehicle position X based on the expression (5). ⁇ (T) is calculated. Further, in step S108, the vehicle position estimation unit 17 updates the covariance matrix based on Expression (9) using the Kalman gain K (t) ′ instead of K (t).
  • the vehicle position estimation unit 17 can handle the covariance matrix indicating the accuracy of the estimated position in a unified manner by incorporating both the feature detection and the lane marking detection into the vehicle position estimation process based on the Kalman filter. Can do. That is, the own vehicle position estimation unit 17 performs the own vehicle position estimation process by lane marking measurement using the covariance matrix updated by the position estimation by the feature measurement, and also updates the covariance matrix there. The updated covariance matrix is also used in the vehicle position estimation process at the next time. As described above, according to the flowchart of FIG. 9, since the covariance matrix is constantly updated, a suitable value for the Kalman gain is always calculated, and as a result, the vehicle position estimation accuracy is improved.
  • FIG. 10 is a flowchart showing the procedure of the vehicle position estimation process in the comparative example.
  • the comparative example shown in FIG. 10 performs the vehicle position estimation process based on different methods at the time of feature detection and at the time of lane marking detection.
  • the own vehicle position estimating unit 17 performs the estimation of the own vehicle position of the Kalman filter with the feature as the measurement target in steps S201 to S205, as in steps S101 to S105 of the embodiment shown in FIG. Do.
  • the vehicle position estimation unit 17 detects a lane line used for vehicle position estimation (step S206; Yes)
  • the point cloud data is matched using a scan matching method such as ICP (Iterative Closest Point) (step S207).
  • ICP Intelligent Closest Point
  • FIG. 11 (A) is a graph showing the transition of the position in the x direction and the y direction when the vehicle is running while executing the vehicle position estimation process shown in FIG. 9 in a certain driving test course.
  • the vehicle position also referred to as “reference position”
  • the estimated vehicle position calculated by the vehicle position estimation process shown in FIG. are drawn to the extent that they cannot be distinguished visually.
  • FIG. 11 (B) is an enlarged view of the data in the traveling section within the frame 70 of FIG. 11 (A).
  • the positions of the features that are the measurement targets are plotted with circles.
  • FIGS. 12A to 12C show the experimental results in the traveling section of the frame 70 when the lane marking is not added to the position estimation measurement target (that is, only the feature is the measurement target).
  • FIGS. 13A to 13C show the experimental results in the traveling section of the frame 70 when the lane marking is added to the position estimation measurement target.
  • FIG. 12 (A) and FIG. 13 (A) show the difference in the traveling direction between the estimated vehicle position and the reference position
  • FIGS. 12 (B) and 13 (B) show the estimated vehicle position and the reference vehicle position. The difference in the lateral direction from the reference position is shown
  • FIGS. 12C and 13B show the azimuth difference between the estimated vehicle position and the reference position.
  • the difference in the lateral direction between the estimated vehicle position and the reference position is significantly reduced by adding the lane markings to the position estimation measurement target. Further, the difference in the traveling direction between the estimated vehicle position and the reference position does not increase even when the lane marking is added to the position estimation measurement target. Thus, it can be seen that when the lane marking is added to the position estimation measurement target, the lateral position estimation accuracy is clearly improved, and the traveling direction position estimation accuracy is not deteriorated.
  • the host vehicle position estimation unit 17 of the vehicle-mounted device 1 calculates the predicted host vehicle position X ⁇ (t).
  • the vehicle position estimation unit 17 is based on the coordinates (L X , L Y ) of the body coordinate system corresponding to the measurement distance by the rider 2 from the vehicle to the lane marking, and the lane marking position information indicated by the lane marking information.
  • the estimated own vehicle position X obtained by correcting the predicted own vehicle position X ⁇ (t) ⁇ (T) is calculated.
  • the vehicle position estimation unit 17 sets the Kalman gain in the traveling direction to 0 so that the correction amount in the lateral direction intersecting the traveling direction is larger than the correction amount in the traveling direction that is the longitudinal direction of the lane marking.
  • the predicted host vehicle position X ⁇ (t) is corrected.
  • the own vehicle position estimation part 17 can improve the own vehicle position estimation precision suitably by making a marking line into a measuring object.
  • the vehicle position estimation unit 17 multiplies the difference value dx shown in the equation (1) by a correction coefficient of 0, as shown in the following equation (10).
  • the estimated host vehicle position X ⁇ (t) may be calculated.
  • the Kalman gain K (t) is generated by the equation (8) and is not updated based on the equation (4), the covariance matrix P (t) represented by the equation (9) The update does not reflect that the direction information is not available. That is, the position estimation does not use the information on the traveling direction, while the covariance matrix uses the information on the traveling direction, and thus processing that considers the difference is necessary.
  • the host vehicle position estimation unit 17 includes an element indicating the accuracy of the traveling direction of the observation noise matrix R (t) used when calculating the Kalman gain K (t) within a range that does not overflow in the program.
  • a large numerical value “D” (for example, 10000) is set as shown in the following formula (11).
  • the own vehicle position estimating unit 17 can suitably reduce the correction amount of the predicted own vehicle position X ⁇ (t) in the traveling direction of the vehicle.
  • step S106 to step S108 in FIG. 9 the vehicle position estimation unit 17 performs a vehicle position estimation process on a structure other than a lane line existing continuously or intermittently in the traveling direction (for example, curbstone or guardrail). You may go.
  • the map DB 10 stores discrete position information and the like of the above-described structure to be measured, and the vehicle position estimation unit 17 is described above in addition to or instead of the lane marking.
  • the presence / absence of detection of the structure is determined in step S106.
  • the vehicle position estimation unit 17 executes step S107 and step S108, thereby predicting the vehicle position X ⁇ (t) where the Kalman gain in the traveling direction is zero. Correction, update of the covariance matrix, and the like. Even in this case, the vehicle position estimation unit 17 can preferably improve the vehicle position estimation accuracy.
  • the above-described structures are examples of the “object” and “structure extending along the road” in the present invention.
  • the vehicle position estimation unit 17 is not limited to a structure that exists continuously or intermittently in the traveling direction of the vehicle, such as a lane marking, but a structure that exists continuously or intermittently in the lateral direction of the vehicle (“The vehicle position estimation process may be performed with “horizontal structure” as a measurement target. Examples of the horizontally long structure include a stop line, a footbridge, and an overpass.
  • the map DB 10 stores discrete position information and the like of the horizontally long structure to be measured, and the vehicle position estimation unit 17 sets, for example, a lane marking after execution of step S108 or at step S106. If it is determined not to be detected, whether or not the horizontally long structure is detected is determined.
  • the feature information of the horizontally long structure includes flag information indicating that it is a horizontally long structure, and the vehicle position estimation unit 17 refers to the flag information described above, so that the horizontally long structure Whether or not an object is detected may be determined.
  • the vehicle position estimating unit 17 corrects the predicted vehicle position X ⁇ (t) in which the Kalman gain in the lateral direction of the vehicle is set to 0, updates the covariance matrix, etc. I do. Specifically, in this case, the vehicle position estimation unit 17 multiplies the second column of the Kalman gain matrix by a coefficient of 0, as shown in the following equation (12).
  • the host vehicle position estimation unit 17 calculates an estimated host vehicle position X ⁇ (t) as shown in the following equation (13).
  • the vehicle position estimation unit 17 can appropriately improve the position estimation accuracy in the traveling direction by performing the vehicle position estimation using the horizontally long structure as a measurement target.
  • the horizontally long structure is an example of the “object” and “structure extending in the width direction of the road” in the present invention.
  • 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 in-vehicle device 1, the electronic control device of the vehicle may execute the processing of the vehicle position estimation unit 17 of the in-vehicle device 1.
  • map DB10 is memorize

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 (11)

  1. 　予測された自己位置を示す予測位置情報を取得する取得部と、
    　移動体から、連続又は断続して存在する対象物までの計測部による計測距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの予測距離と、に基づいて、前記予測された自己位置を補正する補正部と、を備え、
    　前記補正部は、前記対象物の長手方向における補正量よりも、前記長手方向と交わる第１方向における補正量が大きくなるように前記予測された自己位置を補正する自己位置推定装置。
  2. 　前記補正部は、
    　道路に沿って延在する構造物を前記対象物とする場合、前記移動体の進行方向の補正量よりも、前記移動体の横方向における補正量が大きくなるように前記予測された自己位置を補正し、
    　道路の幅方向に延在する構造物を前記対象物とする場合、前記移動体の横方向の補正量よりも、前記移動体の進行方向における補正量が大きくなるように前記予測された自己位置を補正する請求項１に記載の自己位置推定装置。
  3. 　前記対象物の離散的な点の座標が地図情報に記憶され、
    　前記補正部は、前記予測された自己位置と、当該自己位置から所定距離離れた位置に設定された予測ウィンドウ内の前記対象物の点の座標との距離を前記予測距離として算出し、かつ、前記予測ウィンドウ内において計測された前記対象物までの距離を前記計測距離として算出する請求項１または２に記載の自己位置推定装置。
  4. 　前記補正部は、前記差分値に所定の利得を乗じた値により、前記予測された自己位置を補正し、
    　前記補正部は、前記対象物の長手方向における補正量よりも、前記長手方向と交わる第１方向における補正量が大きくなるように、前記利得に対する補正係数を決定する請求項１～３のいずれか一項に記載の自己位置推定装置。
  5. 　前記補正部は、前記差分値に所定の利得を乗じた値により、前記予測された自己位置を補正し、
    　前記補正部は、前記対象物の長手方向における補正量よりも、前記長手方向と交わる第１方向における補正量が大きくなるように、前記対象物の長手方向における前記差分値に対する補正係数を決定する請求項１～４のいずれか一項に記載の自己位置推定装置。
  6. 　前記利得は、カルマンゲインである請求項４または５に記載の自己位置推定装置。
  7. 　前記補正部は、前記対象物を前記計測部により計測した場合と、前記対象物以外の地物を前記計測部により計測した場合とのいずれの場合も、前記カルマンゲインに基づき前記予測された自己位置を補正し、かつ、前記対象物を前記計測部により計測した場合に前記補正係数を設ける請求項６に記載の自己位置推定装置。
  8. 　前記補正部は、前記対象物を前記計測部により計測した場合と、前記対象物以外の地物を前記計測部により計測した場合とのいずれの場合も、前記カルマンゲインに基づき共分散行列を更新する請求項７に記載の自己位置推定装置。
  9. 　自己位置推定装置が実行する制御方法であって、
    　予測された自己位置を示す予測位置情報を取得する取得工程と、
    　移動体から、連続又は断続して存在する対象物までの計測部による計測距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの予測距離と、に基づいて、前記予測された自己位置を補正する補正工程と、を有し、
    　前記補正工程は、前記対象物の長手方向における補正量よりも、前記長手方向と交わる第１方向における補正量が大きくなるように前記予測された自己位置を補正する制御方法。
  10. 　コンピュータが実行するプログラムであって、
    　予測された自己位置を示す予測位置情報を取得する取得部と、
    　移動体から、連続又は断続して存在する対象物までの計測部による計測距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの予測距離と、に基づいて、前記予測された自己位置を補正する補正部
    として前記コンピュータを機能させ、
    　前記補正部は、前記対象物の長手方向における補正量よりも、前記長手方向と交わる第１方向における補正量が大きくなるように前記予測された自己位置を補正するプログラム。
  11. 　請求項１０に記載のプログラムを記憶した記憶媒体。
PCT/JP2018/019176 2017-05-19 2018-05-17 自己位置推定装置、制御方法、プログラム及び記憶媒体 WO2018212302A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2017100239 2017-05-19
JP2017-100239 2017-05-19

Publications (1)

Publication Number Publication Date
WO2018212302A1 true WO2018212302A1 (ja) 2018-11-22

Family

ID=64273930

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2018/019176 WO2018212302A1 (ja) 2017-05-19 2018-05-17 自己位置推定装置、制御方法、プログラム及び記憶媒体

Country Status (1)

Country Link
WO (1) WO2018212302A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110031646A (zh) * 2019-05-10 2019-07-19 北京工业大学 一种利用行车方向振动加速度修正行车速度方法
US20220214173A1 (en) * 2019-05-15 2022-07-07 Nissan Motor Co., Ltd. Self-Position Correction Method and Self-Position Correction Device
CN114964172A (zh) * 2021-02-26 2022-08-30 广东博智林机器人有限公司 一种定位装置、划线系统、划线方法及划线装置

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004045227A (ja) * 2002-07-12 2004-02-12 Alpine Electronics Inc 車両位置測定装置および方法
JP2017072422A (ja) * 2015-10-05 2017-04-13 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004045227A (ja) * 2002-07-12 2004-02-12 Alpine Electronics Inc 車両位置測定装置および方法
JP2017072422A (ja) * 2015-10-05 2017-04-13 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110031646A (zh) * 2019-05-10 2019-07-19 北京工业大学 一种利用行车方向振动加速度修正行车速度方法
US20220214173A1 (en) * 2019-05-15 2022-07-07 Nissan Motor Co., Ltd. Self-Position Correction Method and Self-Position Correction Device
US11754403B2 (en) * 2019-05-15 2023-09-12 Nissan Motor Co., Ltd. Self-position correction method and self-position correction device
CN114964172A (zh) * 2021-02-26 2022-08-30 广东博智林机器人有限公司 一种定位装置、划线系统、划线方法及划线装置

Similar Documents

Publication Publication Date Title
WO2018181974A1 (ja) 判定装置、判定方法、及び、プログラム
JP7155284B2 (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
WO2017060947A1 (ja) 推定装置、制御方法、プログラム及び記憶媒体
JP6980010B2 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP6806891B2 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP2022176322A (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP2021113816A (ja) 出力装置、制御方法、プログラム及び記憶媒体
WO2018212302A1 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP2023164553A (ja) 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
JP2023078138A (ja) 出力装置、制御方法、プログラム及び記憶媒体
JP2022136145A (ja) データ構造、情報処理装置、及び地図データ生成装置
WO2018212290A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP6923750B2 (ja) 自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体
US20240053440A1 (en) Self-position estimation device, self-position estimation method, program, and recording 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: 18801899

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 18801899

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: JP