[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

WO2018020589A1 - 自己位置推定方法及び自己位置推定装置 - Google Patents

自己位置推定方法及び自己位置推定装置 Download PDF

Info

Publication number
WO2018020589A1
WO2018020589A1 PCT/JP2016/071922 JP2016071922W WO2018020589A1 WO 2018020589 A1 WO2018020589 A1 WO 2018020589A1 JP 2016071922 W JP2016071922 W JP 2016071922W WO 2018020589 A1 WO2018020589 A1 WO 2018020589A1
Authority
WO
WIPO (PCT)
Prior art keywords
target
gradient
section
position data
vehicle
Prior art date
Application number
PCT/JP2016/071922
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 PCT/JP2016/071922 priority Critical patent/WO2018020589A1/ja
Priority to RU2019105128A priority patent/RU2722356C1/ru
Priority to JP2018530245A priority patent/JP6575686B2/ja
Priority to MX2019001092A priority patent/MX2019001092A/es
Priority to BR112019001441-1A priority patent/BR112019001441B1/pt
Priority to KR1020197004534A priority patent/KR20190028528A/ko
Priority to US16/320,307 priority patent/US11243080B2/en
Priority to CA3032068A priority patent/CA3032068C/en
Priority to EP16910492.4A priority patent/EP3492871B1/en
Priority to CN201680087942.5A priority patent/CN109564098B/zh
Publication of WO2018020589A1 publication Critical patent/WO2018020589A1/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/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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • 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/34—Route searching; Route guidance
    • G01C21/36—Input/output arrangements for on-board computers
    • G01C21/3602—Input other than that of destination using image analysis, e.g. detection of road signs, lanes, buildings, real preceding vehicles using a camera
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D1/00—Measuring arrangements giving results other than momentary value of variable, of general application
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser
    • 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
    • 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
    • G06V20/588—Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems

Definitions

  • the present invention relates to a self-position estimation method and a self-position estimation apparatus.
  • Patent Document 1 As a technique for detecting the relative position between a known target and a moving body and estimating the position of the moving body, a technique described in Patent Document 1 is known.
  • the robot described in Patent Document 1 is based on the amount of positional deviation between a point environment map that shows a movable area as point set data and ambient environment information that shows a detection result of a laser range sensor mounted on the robot as point set data.
  • the robot self-position estimation result is corrected.
  • An object of this invention is to suppress the fall of the estimation precision of the position on a two-dimensional map resulting from the difference between the diagonal distance and horizontal distance in a gradient area.
  • the relative position of the target existing around the moving body is detected, the relative position is corrected based on the moving amount of the moving body, and the target position data is accumulated.
  • the current position of the moving object is checked by comparing the target position data of the target in the section where the gradient amount is less than the threshold with map information indicating the position of the target on the two-dimensional map. Estimate the position.
  • the vehicle 1 is equipped with a self-position estimation device 2 and a driving support system 3.
  • the self-position estimation device 2 includes an imaging device 10, a distance measurement device 11, a wheel speed sensor 12, a steering angle sensor 13, a gyro sensor 14, an acceleration sensor 15, and a self-position estimation circuit 16.
  • the imaging device 10 is attached to the interior of the vehicle 1 or the like, and images the front area of the vehicle 1, for example.
  • the imaging device 10 may be a wide angle camera, for example.
  • the imaging device 10 outputs a captured image of the front area of the vehicle 1 to the self-position estimation circuit 16.
  • the distance measuring device 11 is attached outside the passenger compartment of the vehicle 1 and irradiates the front area of the vehicle 1 with electromagnetic waves and detects the reflected wave.
  • the distance measuring device 11 may be a laser range finder, for example.
  • the attachment position of the distance measuring device 11 may be, for example, around the hood, bumper, license plate, headlight, or side mirror of the vehicle 1.
  • the distance measuring device 11 outputs the measurement result to the self-position estimation circuit 16.
  • the wheel speed sensor 12 generates a predetermined number of wheel speed pulses each time the wheel of the vehicle 1 makes one revolution.
  • the wheel speed sensor 12 outputs a wheel speed pulse to the self-position estimation circuit 16.
  • the steering angle sensor 13 is provided, for example, in a steering column that rotatably supports the steering wheel of the vehicle 1.
  • the steering angle sensor 13 detects a current steering angle that is a current rotation angle (a steering operation amount) of a steering wheel that is a steering operator.
  • the steering angle sensor 13 outputs the detected current steering angle to the self-position estimation circuit 16.
  • the gyro sensor 14 detects the yaw rate generated in the vehicle 1, the displacement amount in the pitch direction, and the displacement amount in the roll direction.
  • the gyro sensor 14 outputs the detected yaw rate, the displacement amount in the pitch direction, and the displacement amount in the roll direction to the self-position estimation circuit 16.
  • the acceleration sensor 15 detects the lateral G that is the acceleration / deceleration in the vehicle width direction generated in the vehicle 1 and the acceleration / deceleration in the front-rear direction.
  • the acceleration sensor 15 outputs the detected lateral G and longitudinal acceleration / deceleration to the self-position estimation circuit 16.
  • the self-position estimation circuit 16 is an electronic circuit device including a processor such as a CPU (Central Processing Unit), a storage device, and peripheral components.
  • the self-position estimation circuit 16 is a signal 2 received from the imaging device 10, the distance measurement device 11, the wheel speed sensor 12, the steering angle sensor 13, and the gyro sensor 14, and 2 indicating the position of a known target on the two-dimensional map. Based on the dimensional map information, the current position on the map of the vehicle 1 is estimated. Hereinafter, the current position on the map of the vehicle 1 may be referred to as “self position”.
  • the self-position estimation circuit 16 outputs a self-position signal indicating the self-position to the driving support system 3.
  • the driving support system 3 uses the self-position indicated by the self-position signal received from the self-position estimation circuit 16 to perform driving assistance for driving the vehicle 1 by the driver.
  • An example of driving assistance may be provision of information such as an alarm for the driver, for example.
  • the driving support system 3 may control at least one of the type and intensity of the alarm presented to the driver according to the self-position of the vehicle 1.
  • An example of driving assistance may be control of the running state of the vehicle 1 including at least one of braking control, acceleration control, and steering control of the vehicle 1.
  • the driving support system 3 may determine which of the braking force and the driving force is generated in the vehicle 1 according to the self position of the vehicle 1.
  • the self-position estimation circuit 16 includes a target position detection unit 20, a movement amount estimation unit 21, a gradient detection unit 22, a target position storage unit 23, a storage device 24, a selection unit 25, and a position estimation unit 26. And the map information acquisition part 27 is provided.
  • the processor included in the self-position estimation circuit 16 executes a computer program stored in the storage device 24, thereby causing a target position detection unit 20, a movement amount estimation unit 21, a gradient detection unit 22, a target position storage unit 23, The function of the selection part 25, the position estimation part 26, and the map information acquisition part 27 is implement
  • the target position detection unit 20 receives a captured image of the front area of the vehicle 1 generated by the imaging device 10. Further, the target position detection unit 20 receives the measurement result of the distance measuring device 11. The target position detection unit 20 detects a target existing around the vehicle 1 based on the captured image of the front area of the vehicle 1 and the measurement result of the distance measuring device 11. For example, the target position detection unit 20 detects a target existing in front of the vehicle 1. Furthermore, the target position detection unit 20 detects the relative position of the target with respect to the vehicle 1. The target position detection unit 20 outputs a relative position signal indicating the detected relative position to the target position storage unit 23.
  • the target may be, for example, a line on the road surface on which the vehicle 1 travels (lane line, etc.), a curb on the road shoulder, a guardrail, or the like.
  • the movement amount estimation unit 21 receives the wheel speed pulse, the current steering angle, and the yaw rate from the wheel speed sensor 12, the steering angle sensor 13, and the gyro sensor 14, respectively. Based on these signals received from the wheel speed sensor 12, the steering angle sensor 13, and the gyro sensor 14, the movement amount estimation unit 21 estimates the self-position of the vehicle 1 in the previous processing cycle and continues until the present vehicle 1. Is estimated by odometry. The movement amount estimation unit 21 outputs a movement amount signal indicating the estimated movement amount ⁇ P to the target position accumulation unit 23.
  • the gradient detector 22 receives the displacement amount in the pitch direction from the gyro sensor 14.
  • the gradient detection unit 22 detects the gradient amount of the traveling path of the vehicle 1, that is, the inclination of the traveling direction of the vehicle 1 based on the displacement amount in the pitch direction received from the gyro sensor 14.
  • the gradient detection unit 22 may receive a captured image of the front area of the vehicle 1 generated by the imaging device 10.
  • the gradient detection unit 22 may detect the gradient amount of the travel path of the vehicle 1 based on the flow of the 3D point group by analyzing the captured image.
  • the gradient detection unit 22 determines whether or not the travel path of the vehicle 1 is a gradient section.
  • the gradient detection unit 22 may determine that the travel path is a gradient section when the gradient amount of the travel path of the vehicle 1 is equal to or greater than a predetermined threshold.
  • the gradient detection unit 22 outputs a determination result signal indicating the determination result to the selection unit 25.
  • the target position accumulation unit 23 receives a relative position signal from the target position detection unit 20 and receives a movement amount signal from the movement amount estimation unit 21.
  • the target position storage unit 23 stores the relative position of the target around the vehicle 1 indicated by the relative position signal in the storage device 24.
  • the target position accumulation unit 23 corrects the relative position of the target accumulated in the past to a relative position with respect to the current position of the vehicle 1 using the elapsed time up to the present and the movement amount ⁇ P indicated by the movement amount signal. . That is, the target position accumulating unit 23 moves the relative position in the direction opposite to the moving direction of the vehicle 1 by the movement amount ⁇ P that the vehicle has moved during the elapsed time up to the present time.
  • the target position accumulating unit 23 accumulates data of a target position that is a corrected relative position (hereinafter may be referred to as “target position data”) in the storage device 24. If the target position data has already been accumulated in the storage device 24, the target position accumulation unit 23 updates the accumulated target position data using the movement amount ⁇ P indicated by the movement amount signal. That is, the target position accumulation unit 23 moves the relative position of the accumulated target position data in the direction opposite to the movement direction of the vehicle 1 by the movement amount ⁇ P. Thereafter, the target position accumulating unit 23 overwrites the accumulated target position data with the relative position moved relative to the movement amount ⁇ P.
  • the selection unit 25 selects target position data used for estimating the self position of the vehicle 1 from the target position data accumulated in the storage device 24.
  • the target position data selected for use in the estimation of the self position is hereinafter referred to as “selected target position data”. The process in which the selection unit 25 selects the selected target position data will be described later.
  • the position estimation unit 26 estimates the self-position of the vehicle 1 by comparing the selected target position data with the two-dimensional map information acquired by the map information acquisition unit 27.
  • the map information acquisition unit 27 acquires map data and two-dimensional map information indicating the position of the target existing on the map data on the two-dimensional map.
  • the map information acquisition unit 27 is a car navigation system or a map database.
  • the map information acquisition unit 27 may acquire the two-dimensional map information from the outside via a communication system such as wireless communication (road-to-vehicle communication or vehicle-to-vehicle communication is also possible). In this case, the map information acquisition unit 27 may periodically acquire the latest 2D map information and update the stored 2D map information.
  • the map information acquisition part 27 may accumulate
  • the position estimation unit 26 may estimate the self position of the vehicle 1 by collating the selected target position data with the two-dimensional map information, for example, by the following data collation process. Please refer to FIG. Reference sign S i indicates selected target position data.
  • the index i is an integer from 1 to N, where N is the number of selected target position data.
  • the position estimation unit 26 determines the temporary position of the vehicle 1 by correcting the self-position estimated in the previous processing cycle with the movement amount ⁇ P.
  • the position estimation unit 26 assumes that the position of the vehicle 1 on the two-dimensional map is a temporary position, and converts the relative position of the target indicated by the selected target position data S i into an absolute position on the two-dimensional map. To do.
  • the position estimation unit 26 selects the position information M j of the target in the two-dimensional map information that is closest to the absolute position of the selected target position data S i .
  • nearest closest, Selected target object position location information M z position information M y is the Selected target object position data S 2 position information M x is the Selected target object position data S 1 closest to the data S 3.
  • the position estimation unit 26 calculates the distance D ij between the selected target position data S i and the position information M j closest to this data, and calculates the average S of the distances D ij using the following equation (1). To do.
  • the position estimation unit 26 calculates the position and orientation of the vehicle 1 that minimizes the average S by numerical analysis, and determines the estimated value of the self position of the vehicle 1.
  • the position estimation unit 26 outputs the estimated value of the self position to the driving support system 3.
  • selection method of selected target position data Next, a process in which the selection unit 25 selects selected target position data will be described. As described above, there is a difference between the oblique distance and the horizontal distance in the gradient section. For this reason, the distance between the target indicated by the target position data of the target detected before passing through the gradient section and the vehicle 1 may be longer than the actual horizontal distance. The reason will be described with reference to FIG.
  • the upper stage is a schematic diagram of the traveling path of the vehicle 1 including the gradient section Ss and the target on the traveling path.
  • Square plots T1 to T7 indicate targets on the road.
  • the position of the vehicle 1 in FIG. 4 indicates the position at the time after passing through the gradient section.
  • the middle row is a schematic diagram of the distance between the target and the vehicle 1 indicated by the target position data stored in the storage device 24 at the time after passing through the gradient section.
  • Circular plots S1 to S7 correspond to the target position data of the targets T1 to T7.
  • the lower part is a schematic diagram of the distance between the targets T1 to T7 and the vehicle 1 on the two-dimensional map.
  • Triangular plots M1 to M7 indicate the positions of the targets T1 to T7 on the map.
  • the oblique distance is longer than the horizontal distance. Therefore, using the movement amount ⁇ P including the movement amount in the gradient section Ss, the target positions of the targets T1 to T7 accumulated in the past at the position of the vehicle 1 after passing through the gradient section in FIG. Since the data is corrected, the distance between the target indicated by the target position data S3 to S7 of the target T3 to T7 in the section before passing through the gradient section Ss and the vehicle 1 is the distance on the two-dimensional map (that is, Longer than (horizontal distance).
  • the difference between the distance between the target T3 and T4 indicated by the target position data S3 and S4 and the vehicle 1 and the distance on the two-dimensional map is e3 and e4, and e4 is longer than e3.
  • the relative positions of the targets T5 to T7 indicated by the target position data S5 to S7 are the same as the oblique distance and the horizontal distance of the gradient section Ss.
  • the difference e5 is shifted backward.
  • the relative positions of the target position data S5 to S7 do not change.
  • the target position data S1 to S2 of the targets T1 to T2 in the flat section after passing through the gradient section Ss are not corrected using the movement amount ⁇ P estimated in the gradient section Ss. Therefore, there is no difference between the distance between the targets T1 and T2 indicated by the target position data S1 and S2 and the vehicle 1 and the distance on the two-dimensional map. Further, the relative position between the target position data S1 and S2 does not change.
  • the target position data S1 to S7 When such target position data S1 to S7 are collated with the two-dimensional map information, the target position data S1 to S2 of the flat section after passing through the gradient section where the relative position between the targets does not change, and before the gradient section is entered.
  • the target position data S5 to S7 in the flat section agree well with the position information on the map. For this reason, if the self-position is estimated so that the average S of the distance D ij between the target position data and the position information on the map is minimized, the target position data S5 to S7 before entering the gradient section and the position information on the map As a result, the estimation error may not be reduced.
  • the target position data before entering the gradient section Ss is larger than the target position data after passing through the gradient section Ss. For this reason, the target position data before entering the gradient section Ss acts dominantly, and the estimation error may increase. As a result, when the vehicle passes through the gradient section Ss and enters the intersection, an estimation error becomes large in self-position estimation using a target around the intersection such as a stop line. Further, the estimated position calculated with the dominant action of the target position data S1 to S2 after passing through the gradient section and the target position data S5 to S7 before the approach of the gradient section with the dominant action were calculated. Unlike the estimated position, the error of the estimated position of the self position may fluctuate and become unstable.
  • Reference numeral P1 indicates an estimated position calculated by the target position data S1 to S2 after passing through the gradient section dominantly acting.
  • Reference numeral P2 indicates that the target position data S5 to S7 before entering the gradient section is The estimated position calculated by acting dominantly is shown.
  • the calculation result may be unstablely oscillated between P1 and P2.
  • the selection unit 25 determines whether or not the vehicle 1 has passed the gradient section Ss based on the determination result signal from the gradient detection unit 22.
  • the selection unit 25 selects the target position data S1 to S2 after passing through the gradient section (that is, the target position of the target from the section through the gradient section to the current position). Data) is selected as the selected target position data. That is, the selection unit 25 excludes the target position data S3 to S7 before passing through the gradient section from the selected target position data.
  • the position estimation unit 26 estimates the self position of the vehicle 1 by comparing the target position data S1 to S2 after passing through the selected gradient section with the positions M1 to M2 on the map of the targets T1 to T2. To do. This is shown in FIG.
  • the distance between the target indicated by the target position data S3 to S7 of the target detected before the vehicle 1 passes through the gradient section Ss and the vehicle 1 when the vehicle 1 travels in the gradient section Ss is the actual horizontal level. Even if the distance is longer than the distance, the target position data S3 to S7 can be excluded from the position estimation. As a result, it is possible to suppress a decrease in position estimation accuracy on the two-dimensional map due to the difference between the oblique distance and the horizontal distance in the gradient section.
  • the selection unit 25 may delete target position data other than the selected target position data (that is, target position data before passing through the gradient section) from the storage device 24.
  • the selection unit 25 may delete the target position data S3 to S4 in the gradient section and the target position data S5 to S7 before entering the gradient section from the storage device 24.
  • the position estimation unit 26 may estimate the current position of the vehicle 1 by collating the target position data remaining in the storage device 24 with map information indicating the position of the target on the map. By deleting the target position data before passing through the gradient section from the storage device 24, the storage area of the storage device 24 can be used effectively.
  • the selection unit 25 preferentially uses the target position data of the target that is detected by the target position detection unit 20 after passing through the gradient section and has a shorter elapsed time since detection as the selected target position data. You may choose.
  • the selection unit 25 may select target position data of targets around the current position of the vehicle 1 after passing through the gradient section. For example, target position data of a target within about 20 m from the current position of the vehicle 1 is selected.
  • the target position data of the target around the current position of the vehicle 1 tends to have high position accuracy because there is little error accumulation due to correction using the movement amount ⁇ P.
  • the position data of lanes and curbs that are road boundaries have high lateral position accuracy in the runway.
  • step S ⁇ b> 1 the imaging device 10, the distance measurement device 11, and the target position detection unit 20 detect the relative positions of the targets existing around the vehicle 1 with respect to the vehicle 1.
  • the target position detection unit 20 outputs a relative position signal indicating the detected relative position to the target position storage unit 23.
  • step S ⁇ b> 2 the movement amount estimation unit 21 estimates the movement amount ⁇ P of the vehicle 1 from the estimation of the self-position of the vehicle 1 in the previous processing cycle to the present.
  • step S ⁇ b> 3 the target position accumulation unit 23 accumulates the relative position of the target around the vehicle 1 indicated by the relative position signal in the storage device 24.
  • the target position accumulating unit 23 corrects the relative position of the target accumulated in the past to a relative position with respect to the current position of the vehicle 1 using the elapsed time up to the present and the movement amount ⁇ P indicated by the movement amount signal.
  • the data is stored in the storage device 24 as standard position data.
  • step S ⁇ b> 4 the imaging device 10, the gyro sensor 14, and the gradient detection unit 22 detect the gradient amount of the travel path of the vehicle 1.
  • step S5 the gradient detection unit 22 and the selection unit 25 determine whether the vehicle 1 is in the gradient section, before entering the gradient section, or after passing through the gradient section, by the gradient section passage determination process.
  • step S6 the selection unit 25 determines whether or not the vehicle 1 is determined to be in the gradient section in the gradient section passage determination process.
  • step S6: Y the process proceeds to step S9. If the vehicle 1 is not in the gradient section (step S6: N), the process proceeds to step S7.
  • step S ⁇ b> 7 the selection unit 25 determines whether or not the vehicle 1 has been determined to have passed through the gradient section in the gradient section passage determination process. When the vehicle 1 passes through the gradient section (step S7: Y), the process proceeds to step S8.
  • step S7: N When the vehicle 1 has not passed through the gradient section (step S7: N), that is, when the vehicle 1 is before entering the gradient section, the process proceeds to step S9.
  • step S8 the selection unit 25 deletes the target position data S3 to S7 before passing through the gradient section from the storage device 24. That is, the selection unit 25 selects the target position data S1 to S2 after passing through the gradient section, and leaves them in the storage device 24 as selected target position data.
  • step S ⁇ b> 9 the position estimation unit 26 collates the selected target position data with the map information and estimates the self position of the vehicle 1. That is, the current position of the vehicle 1 is estimated by comparing the target position data remaining in the storage device 24 with the map information.
  • step S ⁇ b> 10 the driving support system 3 uses the self-position of the vehicle 1 estimated by the position estimation unit 26 to perform driving support for driving the vehicle 1 by the driver.
  • step S20 the selection unit 25 determines whether or not the vehicle 1 is determined to be in the gradient section in the previous gradient section passage determination process. When it is determined that the vehicle 1 is in the gradient section (step S20: Y), the process proceeds to step S24. If it is not determined that the vehicle 1 is in the gradient section (step S20: N), the process proceeds to step S21.
  • step S ⁇ b> 21 the gradient detection unit 22 determines whether the gradient amount of the travel path of the vehicle 1 is equal to or greater than a threshold value.
  • This threshold value may be set according to whether or not the difference between the oblique distance and the horizontal distance due to the gradient is within an allowable range. The threshold may be 2 degrees, for example. If the gradient amount is greater than or equal to the threshold (step S21: Y), the process proceeds to step S23. If the gradient amount is less than the threshold value (step S21: N), the process proceeds to step S22. In step S22, the selection unit 25 determines that the vehicle 1 has not yet entered the gradient section. Thereafter, the process ends.
  • step S23 the gradient detector 22 determines that the vehicle 1 is in the gradient section. Thereafter, the process ends.
  • step S24 the gradient detection unit 22 determines whether the gradient amount of the travel path of the vehicle 1 is equal to or greater than a threshold value. If the gradient amount is greater than or equal to the threshold (step S24: Y), the process proceeds to step S23. If the gradient amount is less than the threshold value (step S24: N), the process proceeds to step S25. In step S25, the selection unit 25 determines that the vehicle 1 has still passed the gradient section. Thereafter, the process ends.
  • the imaging device 10 and the distance measuring device 11 as the target detection sensor and the target position detection unit 20 detect the relative position of the target existing around the vehicle 1 with respect to the vehicle 1.
  • the movement amount estimation unit 21 estimates the movement amount of the vehicle 1.
  • the target position accumulation unit 23 corrects the relative position based on the movement amount of the vehicle 1 and accumulates it as target position data.
  • the imaging device 10 and the gyro sensor 14 as the gradient detection sensor and the gradient detection unit 22 detect the gradient of the travel path of the vehicle 1.
  • the selection unit 25 selects the target position data of the target in the section from the accumulated target position data to the current position after passing through the gradient section.
  • the position estimation unit 26 estimates the current position of the vehicle 1 by collating the selected target position data with map information indicating the position of the target on the map.
  • the target position data of the target detected before passing through the gradient section can be excluded from the position estimation.
  • the self-position can be estimated based on an accurate target position without a distance difference around the intersection, so that the estimation accuracy is improved.
  • the selection unit 25 selects target position data of a target around the current position of the vehicle 1, and the position estimation unit 26 collates the selected target position data with map information.
  • the target position data of the target around the current position of the vehicle 1 tends to have high position accuracy because there is little error accumulation due to correction using the movement amount ⁇ P.
  • the selection unit 25 preferentially selects one of the targets after passing through the gradient section as the selected target position data and selects the remaining target position. It may be excluded from the data. For example, the selection unit 25 may preferentially select the target that has a larger angle between the straight line connecting the vehicle 1 and the target and the traveling direction of the vehicle 1 as the selected target position data. Further, for example, the selection unit 25 may exclude a target whose distance from the vehicle 1 is longer than a predetermined upper limit from the selected target position data. Here, the longer the distance between the target and the vehicle 1, the easier it is to enter a gradient section between the target and the vehicle 1, and the estimation error of the movement amount ⁇ P is likely to increase. Therefore, the upper limit of the distance between the target and the vehicle 1 may be adjusted according to the allowable range of the position estimation error caused by the estimation error of the movement amount ⁇ P.
  • the self-position estimation apparatus 2 of 2nd Embodiment is demonstrated. While traveling in a section where the amount of slope is continuously below the threshold, self-estimation is performed using the target position data of the target in this section, so that the difference between the oblique distance and the horizontal distance is obtained. The estimated accuracy of the position on the two-dimensional map can be suppressed. Therefore, in the first section having a slope amount less than the threshold that the vehicle 1 traveled before entering the slope section, and in the second section having a slope amount less than the threshold that the vehicle 1 travels after passing through the slope section, the vehicle 1 The self-position can be detected with high accuracy. For this reason, the relative position between the self position of the vehicle 1 estimated in the first section and the self position of the vehicle 1 estimated in the second section can be calculated with high accuracy.
  • the self-position estimation circuit 16 uses the relative position between the self-position estimated in the first section and the self-position estimated in the second section to obtain target position data of the target in the first section. to correct.
  • the self-position estimation circuit 16 includes a correction unit 28.
  • the processor included in the self-position estimation circuit 16 implements the function of the correction unit 28 by executing a computer program stored in the storage device 24.
  • the position estimation unit 26 compares the target position data of the target in the first section with the map information in the first section having a gradient amount less than the threshold traveled by the vehicle 1 before entering the gradient section. The first position of the vehicle 1 before entering is estimated.
  • the position estimation unit 26 outputs the first position to the driving support system 3 and the correction unit 28.
  • the correction unit 28 adds the information on the first position of the vehicle 1 estimated in the first section to the target position data of the target in the first section and stores it in the storage device 24.
  • the position estimation unit 26 passes the gradient section by comparing the target position data of the target in the second section and the map information in the second section having a gradient amount less than the threshold value at which the vehicle 1 travels after passing the gradient section. A second position of the subsequent vehicle 1 is estimated. The position estimation unit 26 outputs the second position to the correction unit 28. The correction unit 28 corrects the target position data of the target in the first section based on the relative position between the first position and the second position.
  • the position estimation unit 26 collates the corrected target position data and the target position data of the target in the second section with map information. To estimate the second position of the vehicle 1 after passing through the gradient section.
  • the object position estimation unit 26 outputs the second position estimated after the correction of the target position data to the driving support system 3.
  • the target position estimation unit 26 adds the information on the second position estimated after the correction of the target position data to the target position data of the target in the second section, and stores it in the storage device 24.
  • step S35 the selection unit 25 determines whether or not the vehicle 1 is determined to be in the gradient section in the gradient section passage determination process.
  • step S35: Y the process proceeds to step S43. If the vehicle 1 is not in the gradient section (step 35: N), the process proceeds to step S36.
  • step S ⁇ b> 36 the selection unit 25 determines whether or not the vehicle 1 has been determined to have passed through the gradient section in the gradient section passage determination process.
  • step S36: Y the process proceeds to step S37.
  • step S36: N the vehicle 1 has not passed through the gradient section
  • step S43 the process proceeds to step S43.
  • step S ⁇ b> 36 the selection unit 25 deletes the target position data of the target in the gradient section from the storage device 24. That is, the selection unit 25 selects a target other than the gradient section (that is, the target position data before the gradient section and the target position after passing through the gradient section) as the selected target position data.
  • the selection unit 25 is not limited to the target position data of the target after passing through the gradient section, but selects the target position data of the target in the section where the gradient amount other than the gradient section is less than the threshold value. Select as position data.
  • it is not necessary to select all the targets other than the gradient section as the selected target position data and it is necessary for collating the map information acquired by the map information acquisition unit 27 to estimate the self position of the vehicle 1. Only target position data may be selected.
  • step S38 the selection unit 25 selects target position data of the target in the second section after passing through the gradient section.
  • step S39 the position estimation unit 26 collates the target position data selected in step S38 with the two-dimensional map information, and estimates the second position of the vehicle 1.
  • step S ⁇ b> 40 the correction unit 28 stores the information on the first position of the vehicle 1 estimated in the first section stored in addition to the target position data of the target in the first section before entering the gradient section. Read from. The correction unit 28 corrects the target position data of the target in the first section based on the relative position between the first position and the second position.
  • step S41 the position estimation unit 26 collates the target position data remaining in the storage device 24 (that is, the target position data corrected in step S40 and the target position data of the target in the second section) with map information.
  • the correction unit 28 adds the information on the second position of the vehicle 1 estimated in step S41 to the target position data of the target in the second section and stores the information in the storage device 24.
  • the process of step S42 is the same as the process of step S10 of FIG.
  • the process in step S43 is the same as the process in step S9 in FIG. After step S43, the process proceeds to step S42.
  • the position estimation unit 26 collates the target position data of the target of the first section having a gradient amount less than the threshold before entering the gradient section and the map information to check the vehicle 1 before entering the gradient section. A first position is estimated. Further, after passing through the gradient section, the second position of the vehicle 1 after passing through the gradient section is estimated by collating the target position data of the target of the second section having a gradient amount less than the threshold and map information. To do.
  • the correction unit 28 corrects the target position data of the target in the first section based on the relative position between the first position and the second position.
  • the position estimation unit 26 estimates the self-position of the vehicle 1 by collating the corrected target position data and the target position data of the target in the second section with map information.
  • the position estimation unit 26 collates the target position data of the target in the section whose gradient amount is less than the threshold and the map information, not only the target position data of the target in the second section after passing through the gradient section. By doing so, the self-position of the vehicle 1 is estimated. Thereby, the fall of the estimation precision of the position on a two-dimensional map resulting from the difference of the diagonal distance between the targets of a gradient area and a horizontal distance can be suppressed. Moreover, since the target position data of the target before entering the gradient section can be used again, the accuracy of self-position estimation can be improved.
  • the target position data of the target in the undulation need not be excluded from the selected target position data.
  • the selection unit 25 excludes the target position data of the target of the gradient section such as the entrance to the bridge or the highway from the selected target position data, for example, when the gradient amount is 1 to 2 degrees and the transit time May not be excluded from the selected target position data.
  • the selection unit 25 may select target position data of the target in this section as the selected target position data.
  • the selection unit 25 may exclude the target position data of the target in this section from the selected target position data. That is, the target position data of a target in a section having a gradient less than the threshold other than this section is selected as the selected target position data. The same applies to the first embodiment.
  • target position data for the target in this section is selected, so the target position of the gradient section that affects the self-position estimation accuracy is selected. Data can be excluded appropriately.
  • the predetermined length may be set based on, for example, the travel time for traveling in a section having a gradient equal to or greater than a threshold value. For example, the predetermined length may be set to a length of 3 seconds or more. The predetermined length may be set based on the distance of a section having a gradient equal to or greater than the threshold. For example, the predetermined length may be set to a length of 30 seconds or more. The predetermined length may be set dynamically such that the larger the gradient amount of the traveling path of the vehicle 1 is, the shorter the length becomes. Thereby, the measurement error resulting from the difference between the oblique distance and the horizontal distance can be suppressed within a desired allowable range regardless of the magnitude of the gradient amount.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Optics & Photonics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

自己位置推定方法では、移動体（１）の周囲に存在する物標の前記移動体に対する相対位置を検出し（Ｓ１）、移動体の移動量を推定し（Ｓ２）、移動体の移動量に基づき相対位置を補正して物標位置データとして蓄積し（Ｓ３）、移動体の走行路の勾配量を検出し（Ｓ４）、蓄積した物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データを選択し（Ｓ８）、選択した物標位置データと２次元地図上の物標の位置を示す地図情報とを照合することにより移動体の現在位置を推定する（Ｓ９）。

Description

自己位置推定方法及び自己位置推定装置
　本発明は、自己位置推定方法及び自己位置推定装置に関する。
　既知の物標と移動体との間の相対位置を検出して移動体の位置を推定する技術として、特許文献１に記載の技術が知られている。
　特許文献１に記載のロボットは、移動可能な領域を点集合データで示す点環境地図と、ロボットに搭載したレーザーレンジセンサの検出結果を点集合データで示す周囲環境情報との位置ずれ量に基づき、ロボットの自己位置の推定結果を補正する。
特開２００８‐２５０９０６号公報
　勾配区間では斜距離と水平距離との間に差があるため、２次元地図上の移動体の位置の推定精度が低下するおそれがある。
　本発明は、勾配区間における斜距離と水平距離との間の差に起因する２次元地図上の位置の推定精度の低下を抑制することを目的とする。
　本発明の一態様に係る自己位置推定方法では、移動体の周囲に存在する物標の相対位置を検出し、移動体の移動量に基づき相対位置を補正して物標位置データとして蓄積する。蓄積した物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データと２次元地図上の物標の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する。
　本発明の一態様によれば、勾配区間における斜距離と水平距離との間の差に起因する２次元地図上の位置の推定精度の低下を抑制できる。
　本発明の目的及び利点は、特許請求の範囲に示した要素及びその組合せを用いて具現化され達成される。前述の一般的な記述及び以下の詳細な記述の両方は、単なる例示及び説明であり、特許請求の範囲のように本発明を限定するものでないと解するべきである。
実施形態の自己位置推定装置を搭載した車両の概略構成の一例のブロック図である。 自己位置推定回路の概略構成の一例のブロック図である。 物標位置データと地図情報との照合による自己位置の推定方法の一例の説明図である。 勾配に起因する物標位置データの誤差の説明図である。 勾配に起因する車両の位置推定誤差の説明図である。 選択済物標位置データの説明図である。 第１実施形態の自己位置推定方法の一例を示すフローチャートである。 勾配区間通過判定処理の一例を示すフローチャートである。 第２実施形態の自己位置推定回路の概略構成の一例のブロック図である。 第２実施形態の自己位置推定方法の一例を示すフローチャートである。
　以下、本発明の実施形態について、図面を参照しつつ説明する。
　（第１実施形態）
　（構成）
　図１を参照する。以下、移動体の一例として車両の現在位置を推定する場合について説明するが、本発明は車両に限らず様々な移動体の現在位置の推定に広く適用することができる。
　車両１には、自己位置推定装置２と運転支援システム３が搭載される。自己位置推定装置２は、撮像装置１０と、距離測定装置１１と、車輪速センサ１２と、操舵角センサ１３と、ジャイロセンサ１４と、加速度センサ１５と、自己位置推定回路１６を備える。
　撮像装置１０は、車両１の車室内などに取り付けられ、例えば車両１の前方領域を撮像する。撮像装置１０は、例えば広角カメラであってよい。撮像装置１０は、車両１の前方領域の撮像画像を自己位置推定回路１６へ出力する。
　距離測定装置１１は、車両１の車室外などに取り付けられ、車両１の前方領域に電磁波を照射しその反射波を検出する。距離測定装置１１は、例えばレーザレンジファインダであってよい。また、距離測定装置１１の取り付け位置は、例えば車両１のボンネット、バンパー、ナンバープレート、ヘッドライト、又はサイドミラーの周辺であってよい。距離測定装置１１は、測定結果を自己位置推定回路１６へ出力する。
　車輪速センサ１２は、車両１の車輪が１回転する毎に予め設定した数の車輪速パルスを発生させる。車輪速センサ１２は、車輪速パルスを自己位置推定回路１６へ出力する。
　操舵角センサ１３は、例えば、車両１のステアリングホイールを回転可能に支持するステアリングコラムに設けられる。操舵角センサ１３は、操舵操作子であるステアリングホイールの現在の回転角度（操舵操作量）である現在操舵角を検出する。操舵角センサ１３は、検出した現在操舵角を自己位置推定回路１６へ出力する。
　ジャイロセンサ１４は、車両１に発生するヨーレート、ピッチ方向の変位量、及びロール方向の変位量を検出する。ジャイロセンサ１４は、検出したヨーレート、ピッチ方向の変位量、及びロール方向の変位量を自己位置推定回路１６へ出力する。
　加速度センサ１５は、車両１に発生する車幅方向の加減速度である横Ｇ、及び前後方向の加減速度を検出する。加速度センサ１５は、検出した横Ｇ及び前後方向の加減速度を自己位置推定回路１６へ出力する。
　自己位置推定回路１６は、ＣＰＵ（Central Processing Unit）等のプロセッサ、記憶装置及び周辺部品を含む電子回路装置である。
　自己位置推定回路１６は、撮像装置１０、距離測定装置１１、車輪速センサ１２、操舵角センサ１３、及びジャイロセンサ１４から受信した信号と、既知の物標の２次元地図上の位置を示す２次元地図情報に基づいて、車両１の地図上の現在位置を推定する。以下、車両１の地図上の現在位置を「自己位置」と表記することがある。自己位置推定回路１６は、自己位置を示す自己位置信号を運転支援システム３へ出力する。
　運転支援システム３は、自己位置推定回路１６から受信した自己位置信号が示す自己位置を用いて、運転者による車両１の運転に対する運転支援を行う。
　運転支援の一例は、例えば運転者に対する警報等の情報提供であってよい。運転支援システム３は、車両１の自己位置に応じて運転者に提示する警報の種類及び強度の少なくとも一方を制御してよい。
　運転支援の一例は、車両１の制動制御、加速制御及び操舵制御の少なくとも１つを含む車両１の走行状態の制御であってもよい。例えば運転支援システム３は、車両１の自己位置に応じて車両１に制動力及び駆動力のいずれを発生させるのかを決定してよい。
　次に、自己位置推定回路１６の構成を説明する。図２を参照する。自己位置推定回路１６は、物標位置検出部２０と、移動量推定部２１と、勾配検出部２２と、物標位置蓄積部２３と、記憶装置２４と、選択部２５と、位置推定部２６と、地図情報取得部２７を備える。
　自己位置推定回路１６が備えるプロセッサは、記憶装置２４に格納されたコンピュータプログラムを実行することにより、物標位置検出部２０、移動量推定部２１、勾配検出部２２、物標位置蓄積部２３、選択部２５、位置推定部２６、及び地図情報取得部２７の機能を実現する。
　物標位置検出部２０は、撮像装置１０が生成した車両１の前方領域の撮像画像を受信する。また、物標位置検出部２０は、距離測定装置１１の測定結果を受信する。
　物標位置検出部２０は、車両１の前方領域の撮像画像と距離測定装置１１の測定結果に基づいて、車両１の周囲に存在する物標を検出する。例えば物標位置検出部２０は、車両１の前方に存在する物標を検出する。
　さらに、物標位置検出部２０は、車両１に対する物標の相対位置を検出する。物標位置検出部２０は、検出した相対位置を示す相対位置信号を物標位置蓄積部２３へ出力する。
　ここで物標とは、例えば車両１が走行する走行路面上の線（車線区分線等）や、路肩の縁石、ガードレール等であってよい。
　移動量推定部２１は、車輪速センサ１２、操舵角センサ１３及びジャイロセンサ１４から、それぞれ車輪速パルス、現在操舵角及びヨーレートを受信する。移動量推定部２１は、車輪速センサ１２、操舵角センサ１３及びジャイロセンサ１４から受信したこれらの信号に基づいて、前回の処理周期で車両１の自己位置を推定してから現在までの車両１の移動量ΔＰをオドメトリにより推定する。移動量推定部２１は、推定した移動量ΔＰを示す移動量信号を物標位置蓄積部２３へ出力する。
　勾配検出部２２は、ジャイロセンサ１４からピッチ方向の変位量を受信する。勾配検出部２２は、ジャイロセンサ１４から受信したピッチ方向の変位量に基づいて車両１の走行路の勾配量、すなわち車両１の走行方向の傾斜を検出する。
　また、勾配検出部２２は、撮像装置１０が生成した車両１の前方領域の撮像画像を受信してもよい。勾配検出部２２は、撮像画像を解析することにより３Ｄ点群のフローに基づいて車両１の走行路の勾配量を検出してもよい。
　勾配検出部２２は、車両１の走行路が勾配区間であるか否かを判定する。例えば勾配検出部２２は、車両１の走行路の勾配量が所定の閾値以上である場合に走行路が勾配区間であると判定してよい。勾配検出部２２は、判定結果を示す判定結果信号を選択部２５に出力する。
　物標位置蓄積部２３は、物標位置検出部２０から相対位置信号を受信し、移動量推定部２１から移動量信号を受信する。
　物標位置蓄積部２３は、相対位置信号が示す車両１の周囲の物標の相対位置を記憶装置２４に蓄積する。
　また、物標位置蓄積部２３は、過去に蓄積した物標の相対位置を、現在までの経過時間と移動量信号が示す移動量ΔＰを用いて、車両１の現在位置に対する相対位置へ補正する。すなわち、物標位置蓄積部２３は、現在までの経過時間に車両が移動した移動量ΔＰだけ車両１の移動方向と逆方向に相対位置を移動させる。
　物標位置蓄積部２３は、補正した相対位置である物標位置のデータ（以下「物標位置データ」と記載する場合がある）を記憶装置２４に蓄積する。
　既に物標位置データが記憶装置２４に蓄積されている場合には、物標位置蓄積部２３は、移動量信号が示す移動量ΔＰを用いて蓄積された物標位置データを更新する。すなわち物標位置蓄積部２３は、蓄積された物標位置データの相対位置を、移動量ΔＰ分だけ車両１の移動方向と逆方向に移動させる。その後、物標位置蓄積部２３は、移動量ΔＰ分だけ相対移動させた相対位置を、蓄積していた物標位置データに上書きする。
　選択部２５は、記憶装置２４に蓄積されている物標位置データから、車両１の自己位置の推定に用いる物標位置データを選択する。自己位置の推定に用いるために選択される物標位置データを、以下「選択済物標位置データ」と記載する場合がある。
　選択部２５が選択済物標位置データを選択する処理については後述する。
　位置推定部２６は、選択済物標位置データを、地図情報取得部２７が取得した２次元地図情報と照合することにより、車両１の自己位置を推定する。
　地図情報取得部２７は、地図データと、地図データ上に存在する物標の２次元地図上の位置を示す２次元地図情報を取得する。例えば、地図情報取得部２７は、カーナビゲーションシステムや地図データベース等である。なお、地図情報取得部２７は、無線通信（路車間通信、または、車車間通信でも可）等の通信システムを介して外部から２次元地図情報を取得してもよい。この場合、地図情報取得部２７は、定期的に最新の２次元地図情報を入手して、保有する２次元地図情報を更新してもよい。また、地図情報取得部２７は、車両１が実際に走行した走路で検出した物標の位置情報を、２次元地図情報として蓄積してもよい。
　位置推定部２６は、例えば以下のようなデータ照合処理によって、選択済物標位置データと２次元地図情報とを照合して車両１の自己位置を推定してよい。
　図３を参照する。参照符号Ｓｉは選択済物標位置データを示す。インデックスｉは１～Ｎの整数であり、Ｎは選択済物標位置データの個数である。
　位置推定部２６は、前回の処理周期で推定した自己位置を移動量ΔＰで補正して車両１の仮位置を決定する。
　位置推定部２６は、車両１の２次元地図上の位置が仮位置であると仮定して、選択済物標位置データＳｉが示す物標の相対位置を２次元地図上の絶対位置に変換する。位置推定部２６は、選択済物標位置データＳｉの絶対位置に最も近い２次元地図情報中の物標の位置情報Ｍｊを選択する。図３の例では、位置情報Ｍｘが選択済物標位置データＳ１に最も近く、位置情報Ｍｙが選択済物標位置データＳ２に最も近く、位置情報Ｍｚが選択済物標位置データＳ３に最も近い。
　位置推定部２６は、選択済物標位置データＳｉとこのデータに最も近い位置情報Ｍｊとの距離Ｄｉｊを算出し、以下の式（１）を用いて距離Ｄｉｊの平均Ｓを算出する。
Figure JPOXMLDOC01-appb-M000001
　位置推定部２６は、数値解析により、平均Ｓが最小となる車両１の位置及び姿勢を算出して、車両１の自己位置の推定値として決定する。位置推定部２６は、自己位置の推定値を運転支援システム３へ出力する。
　（選択済物標位置データの選択方法）
　次に、選択部２５が選択済物標位置データを選択する処理を説明する。
　上述の通り、勾配区間では斜距離と水平距離との間に差がある。このため、勾配区間通過前に検出した物標の物標位置データが示す物標と車両１との間の距離が、実際の水平距離より伸びることがある。図４を参照してその理由を説明する。
　上段は、勾配区間Ｓｓを含む車両１の走行路と走行路上の物標との模式図である。四角形のプロットＴ１～Ｔ７は走行路上の物標を示す。図４中の車両１の位置は、勾配区間通過後の時点の位置を示している。
　中段は、勾配区間通過後の時点で記憶装置２４に蓄積されている物標位置データが示す物標と車両１との間の距離の模式図である。円形プロットＳ１～Ｓ７は、物標Ｔ１～Ｔ７の物標位置データに対応する。
　下段は、２次元地図上での物標Ｔ１～Ｔ７と車両１との間の距離の模式図である。三角形のプロットＭ１～Ｍ７は、物標Ｔ１～Ｔ７の地図上の位置を示す。
　勾配区間Ｓｓでは斜距離が水平距離より長くなる。このため、勾配区間Ｓｓでの移動量を含んだ移動量ΔＰを用いて図４中の勾配区間通過後の時点の車両１の位置で、過去蓄積されている物標Ｔ１～Ｔ７の物標位置データが補正されるので、勾配区間Ｓｓを通過する前の区間の物標Ｔ３～Ｔ７の物標位置データＳ３～Ｓ７が示す物標と車両１との距離が、２次元地図上の距離（すなわち水平距離）よりも長くなる。
　例えば、勾配区間Ｓｓ内の物標Ｔ３及びＴ４について、物標位置データＳ３及びＳ４が示す物標Ｔ３及びＴ４と車両１との距離と、２次元地図上の距離と、の差はそれぞれｅ３及びｅ４であり、ｅ４はｅ３より長い。
　勾配区間Ｓｓに進入する前の平坦区間の物標Ｔ５～Ｔ７について、物標位置データＳ５～Ｓ７が示す物標Ｔ５～Ｔ７の相対位置は、皆同様に、勾配区間Ｓｓの斜距離と水平距離の差ｅ５だけ後方にずれる。また、物標位置データＳ５～Ｓ７同士の相対位置は変化しない。
　一方で、勾配区間Ｓｓを通過した後の平坦区間の物標Ｔ１～Ｔ２の物標位置データＳ１～Ｓ２は、勾配区間Ｓｓで推定した移動量ΔＰを用いて補正されていない。このため、物標位置データＳ１～Ｓ２が示す物標Ｔ１及びＴ２と車両１との距離と、２次元地図上の距離と、の間に差は生じない。また、物標位置データＳ１～Ｓ２同士の相対位置も変化しない。
　このような物標位置データＳ１～Ｓ７を２次元地図情報と照合すると、物標間の相対位置が変化しない勾配区間通過後の平坦区間の物標位置データＳ１～Ｓ２と、勾配区間進入前の平坦区間の物標位置データＳ５～Ｓ７が、地図上の位置情報とよく一致する。
　このため、物標位置データと地図上の位置情報との距離Ｄｉｊの平均Ｓが最小となるよう自己位置を推定すると、勾配区間進入前の物標位置データＳ５～Ｓ７と地図上の位置情報との距離も小さくする作用が働くため、推定誤差が小さくならない場合がある。
　また、勾配区間Ｓｓ通過後の物標位置データが蓄積されるまで暫くの間は、勾配区間Ｓｓ通過後の物標位置データよりも勾配区間Ｓｓ進入前の物標位置データの方が多い。このため、勾配区間Ｓｓ進入前の物標位置データが支配的に作用することになり推定誤差が大きくなってしまうことがある。この結果、勾配区間Ｓｓを通過して交差点に進入した場合、停止線などの交差点周辺の物標を用いた自己位置推定で推定誤差が大きくなってしまう。
　さらに、勾配区間通過後の物標位置データＳ１～Ｓ２が支配的に作用して算出された推定位置と、勾配区間進入前の物標位置データＳ５～Ｓ７が支配的に作用して算出された推定位置とが異なり、自己位置の推定位置の誤差が変動して不安定になるおそれがある。
　この様子を図５に示す。参照符号Ｐ１は、勾配区間通過後の物標位置データＳ１～Ｓ２が支配的に作用して算出された推定位置を示し、参照符号Ｐ２は、勾配区間進入前の物標位置データＳ５～Ｓ７が支配的に作用して算出された推定位置を示す。勾配区間通過後の物標位置データと勾配区間進入前の物標位置データのどちらが支配的に作用するかで、算出結果がＰ１とＰ２の間で不安定に振動するおそれがある。
　そこで選択部２５は、勾配検出部２２からの判定結果信号に基づいて、車両１が勾配区間Ｓｓを通過したか否かを判定する。
　車両１が勾配区間Ｓｓを通過した場合、選択部２５は、勾配区間通過後の物標位置データＳ１～Ｓ２（すなわち、勾配区間を通過してから現在位置までの区間の物標の物標位置データ）を選択済物標位置データとして選択する。すなわち選択部２５は、勾配区間通過前の物標位置データＳ３～Ｓ７を選択済物標位置データから除外する。
　そして、位置推定部２６は、選択した勾配区間通過後の物標位置データＳ１～Ｓ２を、物標Ｔ１～Ｔ２の地図上の位置Ｍ１～Ｍ２と照合することにより、車両１の自己位置を推定する。この様子を図６に示す。
　このため、車両１が勾配区間Ｓｓを走行することにより勾配区間Ｓｓの通過前に検出した物標の物標位置データＳ３～Ｓ７が示す物標と車両１との間の距離が、実際の水平距離より長くなっても、物標位置データＳ３～Ｓ７を位置推定から除外できる。この結果、勾配区間における斜距離と水平距離との間の差に起因する２次元地図上の位置推定精度の低下を抑制できる。
　なお、勾配区間通過後の物標位置データを、選択済物標位置データとしてすべて選択する必要はなく、地図情報取得部２７が取得した地図情報と照合して、車両１の自己位置を推定できるために必要な物標位置データのみ選択するようにしてもよい。
　また、選択部２５は、選択済物標位置データ以外の物標位置データ（すなわち勾配区間通過前の物標位置データ）を記憶装置２４から削除してもよい。例えば、選択部２５は、勾配区間内の物標位置データＳ３～Ｓ４と勾配区間進入前の物標位置データＳ５～Ｓ７を記憶装置２４から削除してもよい。位置推定部２６は、記憶装置２４に残っている物標位置データと物標の地図上の位置を示す地図情報とを照合することで車両１の現在位置を推定してよい。
　勾配区間通過前の物標位置データを記憶装置２４から削除することにより、記憶装置２４の記憶領域を有効に活用することができる。
　また、選択部２５は、勾配区間通過後に物標位置検出部２０に検出され、検出してからの経過時間がより短い物標の物標位置データを、優先して選択済物標位置データとして選択してよい。例えば選択部２５は、勾配区間通過後の車両１の現在位置の周囲の物標の物標位置データを選択してよい。例えば、車両１の現在位置から約２０ｍ以内の物標の物標位置データを選択する。車両１の現在位置の周囲の物標の物標位置データは、移動量ΔＰを用いた補正による誤差の蓄積が少ないので位置精度が高い傾向にある。例えば、道路境界であるレーンや縁石の位置データは走路内の横位置の精度が高い。
　（動作）
　次に、第１実施形態に係る自己位置推定装置２の動作について説明する。図７を参照する。
　ステップＳ１において撮像装置１０、距離測定装置１１、及び物標位置検出部２０は、車両１の周囲に存在する物標の車両１に対する相対位置を検出する。物標位置検出部２０は、検出した相対位置を示す相対位置信号を物標位置蓄積部２３へ出力する。
　ステップＳ２において移動量推定部２１は、前回の処理周期で車両１の自己位置を推定してから現在までの車両１の移動量ΔＰを推定する。
　ステップＳ３において物標位置蓄積部２３は、相対位置信号が示す車両１の周囲の物標の相対位置を記憶装置２４に蓄積する。また物標位置蓄積部２３は、過去に蓄積した物標の相対位置を、現在までの経過時間と移動量信号が示す移動量ΔＰを用いて車両１の現在位置に対する相対位置へ補正し、物標位置データとして記憶装置２４に蓄積する。
　ステップＳ４において撮像装置１０、ジャイロセンサ１４及び勾配検出部２２は、車両１の走行路の勾配量を検出する。
　ステップＳ５において勾配検出部２２及び選択部２５は、勾配区間通過判定処理により、車両１が勾配区間内にいるのか、勾配区間進入前であるのか、勾配区間通過後であるのかを判定する。
　ステップＳ６において選択部２５は、勾配区間通過判定処理にて車両１が勾配区間内にいると判定されたか否かを判断する。車両１が勾配区間内にいる場合（ステップＳ６：Ｙ）に処理はステップＳ９へ進む。車両１が勾配区間内にいない場合（ステップＳ６：Ｎ）に処理はステップＳ７へ進む。
　ステップＳ７において選択部２５は、勾配区間通過判定処理にて車両１が勾配区間通過後であると判定されたか否かを判断する。車両１が勾配区間を通過した場合（ステップＳ７：Ｙ）に処理はステップＳ８へ進む。
　車両１が勾配区間を通過していない場合（ステップＳ７：Ｎ）、すなわち車両１が勾配区間への進入前である場合、処理はステップＳ９へ進む。
　ステップＳ８において選択部２５は、勾配区間通過前の物標位置データＳ３～Ｓ７を記憶装置２４から削除する。すなわち選択部２５は、勾配区間通過後の物標位置データＳ１～Ｓ２を選択して、選択済物標位置データとして記憶装置２４に残す。
　ステップＳ９において位置推定部２６は、選択済物標位置データと地図情報とを照合して車両１の自己位置を推定する。すなわち記憶装置２４に残っている物標位置データと地図情報とを照合することで車両１の現在位置を推定する。
　ステップＳ１０において運転支援システム３は、位置推定部２６が推定した車両１の自己位置を用いて、運転者による車両１の運転に対する運転支援を実施する。
　図８を参照して、図７のステップＳ５で行われる勾配区間通過判定処理を説明する。ステップＳ２０において選択部２５は、前回の勾配区間通過判定処理で、車両１が勾配区間内だと判定されたか否かを判断する。車両１が勾配区間内だと判定された場合（ステップＳ２０：Ｙ）に処理はステップＳ２４へ進む。車両１が勾配区間内だと判定されなかった場合（ステップＳ２０：Ｎ）に処理はステップＳ２１へ進む。
　ステップＳ２１において勾配検出部２２は、車両１の走行路の勾配量が閾値以上であるか否かを判断する。なお、この閾値は勾配による斜距離と水平距離との差が許容範囲内であるか否かに応じて設定してよい。閾値は例えば２度であってよい。勾配量が閾値以上である場合（ステップＳ２１：Ｙ）に処理はステップＳ２３へ進む。勾配量が閾値未満である場合（ステップＳ２１：Ｎ）に処理はステップＳ２２へ進む。
　ステップＳ２２において選択部２５は、車両１がまだ勾配区間に進入していないと判断する。その後処理は終了する。
　ステップＳ２３において勾配検出部２２は、車両１が勾配区間内にいると判断する。その後処理は終了する。
　一方で、ステップＳ２４において勾配検出部２２は、車両１の走行路の勾配量が閾値以上であるか否かを判断する。勾配量が閾値以上である場合（ステップＳ２４：Ｙ）に処理はステップＳ２３へ進む。勾配量が閾値未満である場合（ステップＳ２４：Ｎ）に処理はステップＳ２５へ進む。
　ステップＳ２５において選択部２５は、車両１がまだ勾配区間を通過したと判断する。その後処理は終了する。
　（第１実施形態の効果）
　（１）物標検出センサとしての撮像装置１０及び距離測定装置１１と物標位置検出部２０とは、車両１の周囲に存在する物標の車両１に対する相対位置を検出する。移動量推定部２１は、車両１の移動量を推定する。物標位置蓄積部２３は、車両１の移動量に基づき相対位置を補正して物標位置データとして蓄積する。勾配検出センサとしての撮像装置１０及びジャイロセンサ１４と勾配検出部２２とは、車両１の走行路の勾配を検出する。選択部２５は、蓄積した物標位置データのうち、勾配区間を通過してから現在位置までの区間の物標の物標位置データを選択する。位置推定部２６は、選択した物標位置データと物標の地図上の位置を示す地図情報とを照合することで車両１の現在位置を推定する。
　このため、車両１が勾配区間を走行することにより勾配区間通過前に検出した物標の物標位置データが示す物標と車両１との間の距離が、実際の水平距離より長くなっても、勾配区間通過前に検出した物標の物標位置データを位置推定から除外できる。この結果、勾配区間における斜距離と水平距離との間の差に起因する２次元地図上の位置の推定精度の低下を抑制できる。
　例えば、勾配区間を通過して交差点に進入するような場合に、交差点周辺の距離差の無い正確な物標位置に基づき自己位置を推定できるため、推定精度が向上する。
　（２）選択部２５は、車両１の現在位置の周囲の物標の物標位置データを選択し、位置推定部２６は、選択した物標位置データと地図情報と照合する。車両１の現在位置の周囲の物標の物標位置データは、移動量ΔＰを用いた補正による誤差の蓄積が少ないので位置精度が高い傾向にある。車両１の現在位置の周囲の物標の物標位置データを選択して車両１の位置推定に用いることにより、車両１の位置推定精度を向上することができる。
　（変形例）
　推定位置の精度向上や処理時間の短縮のため、選択部２５は、勾配区間通過後の物標のうちいずれかを優先的に選択済物標位置データとして選択し、残りを選択済物標位置データから除外してもよい。例えば選択部２５は、車両１と物標とを結ぶ直線と車両１の進行方向とのなす角が大きくなる物標ほど、優先して選択済物標位置データとして選択してもよい。
　また例えば選択部２５は、車両１からの距離が所定の上限より長い物標を選択済物標位置データから除外してもよい。ここで、物標と車両１との間の距離が長いほど、物標と車両１との間に勾配区間が入りやすくなり、移動量ΔＰの推定誤差が増加しやすくなる。したがって、物標と車両１との間の距離の上限は、移動量ΔＰの推定誤差に起因する位置推定誤差の許容範囲に応じて調整してもよい。
　（第２実施形態）
　続いて、第２実施形態の自己位置推定装置２を説明する。
　勾配量が継続して閾値未満である区間を走行している間は、この区間の物標の物標位置データを用いて自己推定を行うことで、斜距離と水平距離との間の差に起因する２次元地図上の位置の推定精度を抑制することができる。
　したがって、勾配区間進入前に車両１が走行した閾値未満の勾配量を有する第１区間と、勾配区間通過後に車両１が走行する閾値未満の勾配量を有する第２区間とでは、それぞれ車両１の自己位置を高い精度で検出することができる。このため、第１区間で推定した車両１の自己位置と第２区間で推定した車両１の自己位置との間の相対位置を高い精度で算出することができる。
　したがって、第１区間の物標の物標位置データが示す物標と車両１との距離が、勾配区間走行中に推定した移動量ΔＰで物標位置データを補正することで実際の水平距離より長くなっても、第１及び第２区間で推定した自己位置間の相対位置を用いて補正できる。
　第３実施形態の自己位置推定回路１６は、第１区間で推定した自己位置と第２区間で推定した自己位置との間の相対位置を用いて第１区間の物標の物標位置データを補正する。
　図９を参照する。自己位置推定回路１６は、補正部２８を備える。自己位置推定回路１６が備えるプロセッサは、記憶装置２４に格納されたコンピュータプログラムを実行することにより、補正部２８の機能を実現する。
　位置推定部２６は、勾配区間進入前に車両１が走行した閾値未満の勾配量を有する第１区間において、第１区間の物標の物標位置データと地図情報とを照合することにより勾配区間へ進入する前の車両１の第１位置を推定する。位置推定部２６は、第１位置を運転支援システム３及び補正部２８に出力する。
　補正部２８は、第１区間で推定した車両１の第１位置の情報を、第１区間の物標の物標位置データに付加して記憶装置２４に記憶する。
　位置推定部２６は、勾配区間通過後に車両１が走行する閾値未満の勾配量を有する第２区間において、第２区間の物標の物標位置データと地図情報とを照合することにより勾配区間通過後の車両１の第２位置を推定する。位置推定部２６は、第２位置を補正部２８に出力する。
　補正部２８は、第１位置と第２位置との相対位置に基づいて、第１区間の物標の物標位置データを補正する。
　第１区間の物標の物標位置データが補正された後、位置推定部２６は、補正された物標位置データ及び第２区間の物標の物標位置データと地図情報とを照合することにより勾配区間通過後の車両１の第２位置を推定する。
　物位置推定部２６は、物標位置データの補正後に推定した第２位置を運転支援システム３に出力する。物位置推定部２６は、物標位置データの補正後に推定した第２位置の情報を、第２区間の物標の物標位置データに付加して記憶装置２４に記憶する。
　図１０を参照する。ステップＳ３０～Ｓ３４の処理は、図７のステップＳ１～Ｓ５と同様である。
　ステップＳ３５において選択部２５は、勾配区間通過判定処理にて車両１が勾配区間内にいると判定されたか否かを判断する。車両１が勾配区間内にいる場合（ステップＳ３５：Ｙ）に処理はステップＳ４３へ進む。車両１が勾配区間内にいない場合（ステップ３５：Ｎ）に処理はステップＳ３６へ進む。
　ステップＳ３６において選択部２５は、勾配区間通過判定処理にて車両１が勾配区間通過後であると判定されたか否かを判断する。車両１が勾配区間を通過した場合（ステップＳ３６：Ｙ）に処理はステップＳ３７へ進む。
　車両１が勾配区間を通過していない場合（ステップＳ３６：Ｎ）、すなわち車両１が勾配区間への進入前である場合、処理はステップＳ４３へ進む。
　ステップＳ３６において選択部２５は、勾配区間の物標の物標位置データを記憶装置２４から削除する。
　すなわち選択部２５は、勾配区間以外の物標（すなわち、勾配区間進入前の物標と勾配区間通過後の物標の物標位置データ）を選択済物標位置データとして選択する。
　言い換えれば、選択部２５は、勾配区間通過後の物標の物標位置データに限定せず、勾配区間以外の勾配量が閾値未満である区間の物標の物標位置データを選択済物標位置データとして選択する。なお、勾配区間以外の物標を、選択済物標位置データとしてすべて選択する必要はなく、地図情報取得部２７が取得した地図情報と照合して、車両１の自己位置を推定できるために必要な物標位置データのみ選択するようにしてもよい。
　ステップＳ３８において選択部２５は、勾配区間通過後の第２区間の物標の物標位置データを選択する。
　ステップＳ３９において位置推定部２６は、ステップＳ３８で選択した物標位置データと２次元地図情報とを照合して車両１の第２位置を推定する。
　ステップＳ４０において補正部２８は、勾配区間進入前の第１区間の物標の物標位置データに付加して記憶された、第１区間で推定した車両１の第１位置の情報を記憶装置２４から読み出す。補正部２８は、第１位置と第２位置との相対位置に基づいて、第１区間の物標の物標位置データを補正する。
　ステップＳ４１において位置推定部２６は、記憶装置２４に残っている物標位置データ（すなわちステップＳ４０で補正した物標位置データ及び第２区間の物標の物標位置データ）と地図情報とを照合することで勾配区間通過後の車両１の第２位置を推定する。補正部２８は、ステップＳ４１において推定した車両１の第２位置の情報を、第２区間の物標の物標位置データに付加して記憶装置２４に記憶する。
　ステップＳ４２の処理は、図７のステップＳ１０の処理と同様である。
　ステップＳ４３の処理は、図７のステップＳ９の処理と同様である。ステップＳ４３の後、処理はステップＳ４２へ進む。
　（第２実施形態の効果）
　位置推定部２６は、勾配区間へ進入する前に閾値未満の勾配量を有する第１区間の物標の物標位置データと地図情報とを照合することにより勾配区間へ進入する前の車両１の第１位置を推定する。また、勾配区間を通過した後に閾値未満の勾配量を有する第２区間の物標の物標位置データと地図情報とを照合することにより勾配区間を通過した後の車両１の第２位置を推定する。補正部２８は、第１位置と第２位置との相対位置に基づいて、第１区間の物標の前記物標位置データを補正する。
　位置推定部２６は、補正した物標位置データ及び第２区間の物標の物標位置データと地図情報とを照合することにより車両１の自己位置を推定する。
　すなわち、位置推定部２６は、勾配区間通過後の第２区間の物標の物標位置データに限らず、勾配量が閾値未満である区間の物標の物標位置データと地図情報とを照合することにより車両１の自己位置を推定する。
　これにより、勾配区間の物標同士の間の斜距離と水平距離との差に起因する２次元地図上の位置の推定精度の低下を抑制できる。
　また、勾配区間進入前の物標の物標位置データを再度利用することができるので、自己位置推定の精度を向上できる。
　（変形例）
　どこにでもあるような多少の起伏を通過した場合には、起伏にある物標の物標位置データを選択済物標位置データから除外しなくてもよい。
　例えば、選択部２５は、橋梁や高速道路への進入口等の勾配区間の物標の物標位置データを選択済物標位置データから除外し、例えば、勾配量が１～２度で通過時間が２～３秒程度の起伏にある物標の物標位置データを選択済物標位置データから除外しなくてもよい。
　例えば、選択部２５は、閾値以上の勾配を有する区間が所定長以上継続しない場合には、この区間の物標の物標位置データも、選択済物標位置データとして選択してよい。
　一方で、選択部２５は、閾値以上の勾配を有する区間が所定長以上継続した場合には、この区間の物標の物標位置データを選択済物標位置データから除外してよい。すなわち、この区間以外の、閾値未満の勾配を有する区間の物標の物標位置データを選択済物標位置データとして選択する。第１実施形態でも同様である。
　このように、閾値以上の勾配を有する区間が所定長以上継続した場合に、この区間の物標の物標位置データを選択するので、自己位置の推定精度に影響を与える勾配区間の物標位置データを適切に除外できる。
　所定長は、例えば閾値以上の勾配を有する区間を走行する走行時間に基づいて設定してよい。例えば所定長は３秒以上の長さに設定してよい。また所定長は、閾値以上の勾配を有する区間の距離に基づいて設定してよい。例えば所定長は３０秒以上の長さに設定してよい。
　車両１の走行路の勾配量が大きいほど短くなるように所定長を動的に設定してもよい。これにより、勾配量の大小に関わらず斜距離と水平距離との差に起因する測定誤差を所望の許容範囲内に抑えることができる。
　ここに記載されている全ての例及び条件的な用語は、読者が、本発明と技術の進展のために発明者により与えられる概念とを理解する際の助けとなるように、教育的な目的を意図したものであり、具体的に記載されている上記の例及び条件、並びに本発明の優位性及び劣等性を示すことに関する本明細書における例の構成に限定されることなく解釈されるべきものである。本発明の実施例は詳細に説明されているが、本発明の精神及び範囲から外れることなく、様々な変更、置換及び修正をこれに加えることが可能であると解すべきである。
　１…車両，２…自己位置推定装置，３…運転支援システム，１０…撮像装置，１１…距離測定装置，１２…車輪速センサ，１３…操舵角センサ，１４…ジャイロセンサ，１５…加速度センサ，１６…自己位置推定回路，２０…物標位置検出部，２１…移動量推定部，２２…勾配検出部，２３…物標位置蓄積部，２４…記憶装置，２５…選択部，２６…位置推定部，２７…地図情報取得部，２８…補正部

Claims (7)

  1. 　移動体の周囲に存在する物標の前記移動体に対する相対位置を検出し、
    　前記移動体の移動量を推定し、
    　前記移動体の移動量に基づき前記相対位置を補正して物標位置データとして蓄積し、
    　前記移動体の走行路の勾配量を検出し、
    　蓄積した前記物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データを選択し、
    　選択した前記物標位置データと２次元地図上の前記物標の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する、
    　ことを特徴とする自己位置推定方法。
  2. 　前記閾値以上の勾配量を有する勾配区間を通過してから現在位置までの区間の物標の物標位置データを選択して、前記地図情報と照合することを特徴とする請求項１に記載の自己位置推定方法。
  3. 　前記勾配区間へ進入する前に前記閾値未満の勾配量を有する第１区間の物標の物標位置データと前記地図情報とを照合することにより前記勾配区間へ進入する前の前記移動体の第１位置を推定し、
    　前記勾配区間を通過した後に前記閾値未満の勾配量を有する第２区間の物標の物標位置データと前記地図情報とを照合することにより前記勾配区間を通過した後の前記移動体の第２位置を推定し、
    　前記第１位置と前記第２位置との相対位置に基づいて、前記第１区間の物標の前記物標位置データを補正する、
    　ことを特徴とする請求項２に記載の自己位置推定方法。
  4. 　前記閾値以上の勾配量を有する区間が所定長以上継続した場合に、前記閾値未満の勾配量を有する区間の物標の物標位置データを選択して前記地図情報と照合し、
    　前記閾値以上の勾配量を有する区間が前記所定長以上継続しない場合に、前記閾値以上の勾配量を有する区間の物標の物標位置データを、前記地図情報と照合する物標位置データに含める、
    　ことを特徴とする請求項１～３の何れか一項に記載の自己位置推定方法。
  5. 　前記移動体の走行路の勾配量が大きいほど前記所定長を短く設定することを特徴とする請求項４に記載の自己位置推定方法。
  6. 　前記移動体の現在位置の周囲の物標の物標位置データを選択して前記地図情報と照合することを特徴とする請求項１～５のいずれか一項に記載の自己位置推定方法。
  7. 　移動体の周囲に存在する物標の前記移動体に対する相対位置を検出する物標検出センサと、
    　前記移動体の車輪速を検出する車輪速センサと、
    　前記移動体の走行路の勾配量を検出する勾配検出センサと、
    　少なくとも前記車輪速センサの検出結果に応じて前記移動体の移動量を推定し、前記物標検出センサが検出した前記相対位置を前記移動量に基づき補正して物標位置データとして記憶装置に蓄積し、蓄積した前記物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データを選択し、選択した前記物標位置データと２次元地図上の前記物標の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する自己位置推定回路と、
    　を備えることを特徴とする自己位置推定装置。
PCT/JP2016/071922 2016-07-26 2016-07-26 自己位置推定方法及び自己位置推定装置 WO2018020589A1 (ja)

Priority Applications (10)

Application Number Priority Date Filing Date Title
PCT/JP2016/071922 WO2018020589A1 (ja) 2016-07-26 2016-07-26 自己位置推定方法及び自己位置推定装置
RU2019105128A RU2722356C1 (ru) 2016-07-26 2016-07-26 Способ оценки собственного положения и устройство оценки собственного положения
JP2018530245A JP6575686B2 (ja) 2016-07-26 2016-07-26 自己位置推定方法及び自己位置推定装置
MX2019001092A MX2019001092A (es) 2016-07-26 2016-07-26 Metodo de estimacion de la posicion propia y dispositivo de estimacion de la posicion propia.
BR112019001441-1A BR112019001441B1 (pt) 2016-07-26 2016-07-26 Método de estimativa de autoposição e dispositivo de estimativa de autoposição
KR1020197004534A KR20190028528A (ko) 2016-07-26 2016-07-26 자기 위치 추정 방법 및 자기 위치 추정 장치
US16/320,307 US11243080B2 (en) 2016-07-26 2016-07-26 Self-position estimation method and self-position estimation device
CA3032068A CA3032068C (en) 2016-07-26 2016-07-26 Self-position estimation method and self-position estimation device
EP16910492.4A EP3492871B1 (en) 2016-07-26 2016-07-26 Self-position estimation method and self-position estimation apparatus
CN201680087942.5A CN109564098B (zh) 2016-07-26 2016-07-26 自身位置推定方法及自身位置推定装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2016/071922 WO2018020589A1 (ja) 2016-07-26 2016-07-26 自己位置推定方法及び自己位置推定装置

Publications (1)

Publication Number Publication Date
WO2018020589A1 true WO2018020589A1 (ja) 2018-02-01

Family

ID=61016515

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2016/071922 WO2018020589A1 (ja) 2016-07-26 2016-07-26 自己位置推定方法及び自己位置推定装置

Country Status (10)

Country Link
US (1) US11243080B2 (ja)
EP (1) EP3492871B1 (ja)
JP (1) JP6575686B2 (ja)
KR (1) KR20190028528A (ja)
CN (1) CN109564098B (ja)
BR (1) BR112019001441B1 (ja)
CA (1) CA3032068C (ja)
MX (1) MX2019001092A (ja)
RU (1) RU2722356C1 (ja)
WO (1) WO2018020589A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019230098A1 (ja) * 2018-05-30 2019-12-05 クラリオン株式会社 情報処理装置
JP2019219204A (ja) * 2018-06-18 2019-12-26 日産自動車株式会社 走行軌跡推定方法及び走行軌跡推定装置
JP7358108B2 (ja) 2019-07-31 2023-10-10 キヤノン株式会社 情報処理装置、情報処理方法及びプログラム

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3492870B1 (en) * 2016-07-26 2020-07-15 Nissan Motor Co., Ltd. Self-position estimation method and self-position estimation device
EP3842886A4 (en) * 2018-08-23 2022-05-11 Nsk Ltd. SELF-PROPELLED DEVICE, AND TRAVEL CONTROL METHOD AND TRAVEL CONTROL PROGRAM FOR SELF-PROPELLED DEVICE
CN109215136B (zh) 2018-09-07 2020-03-20 百度在线网络技术（北京）有限公司 一种真实数据增强方法、装置以及终端
CN109146898B (zh) * 2018-09-07 2020-07-24 百度在线网络技术（北京）有限公司 一种仿真数据量增强方法、装置以及终端
CN110103823B (zh) * 2019-05-21 2021-06-11 东南大学 一种基于增强型数字地图的车辆侧翻事前预警方法
CN110473417A (zh) * 2019-06-03 2019-11-19 浙江吉利控股集团有限公司 一种高架桥限速值提示方法、装置及存储介质
JP7332403B2 (ja) * 2019-09-11 2023-08-23 株式会社東芝 位置推定装置、移動体制御システム、位置推定方法およびプログラム
CN113450407B (zh) * 2021-05-14 2023-10-13 东莞市李群自动化技术有限公司 定位方法及作业方法、电子设备、轨道设备和存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003252149A (ja) * 2002-03-01 2003-09-10 Mitsubishi Electric Corp 車線認識画像処理装置及びその処理を実行させるためのプログラム
JP2008250906A (ja) * 2007-03-30 2008-10-16 Sogo Keibi Hosho Co Ltd 移動ロボット、自己位置補正方法および自己位置補正プログラム
JP2012008706A (ja) * 2010-06-23 2012-01-12 Denso Corp 道路形状検出装置
JP2012103858A (ja) * 2010-11-09 2012-05-31 Toyota Motor Corp 障害物認識装置
JP2016017758A (ja) * 2014-07-04 2016-02-01 日産自動車株式会社 走行支援装置及び走行支援方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5367458A (en) * 1993-08-10 1994-11-22 Caterpillar Industrial Inc. Apparatus and method for identifying scanned reflective anonymous targets
JP4092308B2 (ja) * 2004-06-02 2008-05-28 トヨタ自動車株式会社 境界線検出装置
AU2009211435A1 (en) * 2008-02-04 2009-08-13 Tele Atlas B.V. Method for map matching with sensor detected objects
JP2010146202A (ja) * 2008-12-17 2010-07-01 Toyota Central R&D Labs Inc 移動体と移動体の位置推定方法
JP5441549B2 (ja) * 2009-07-29 2014-03-12 日立オートモティブシステムズ株式会社 道路形状認識装置
JP5278378B2 (ja) * 2009-07-30 2013-09-04 日産自動車株式会社 車両運転支援装置及び車両運転支援方法
JP5321497B2 (ja) * 2010-02-22 2013-10-23 株式会社デンソー 白線認識装置
KR20110134633A (ko) 2010-06-09 2011-12-15 엠텍비젼 주식회사 속도 측정 장치 및 측정된 속도 보정 방법
JP5951266B2 (ja) * 2012-01-27 2016-07-13 三菱重工業株式会社 勾配情報取得方法、勾配情報記憶済記憶媒体を作成する方法、勾配情報取得装置およびプログラム
WO2014076844A1 (ja) * 2012-11-19 2014-05-22 株式会社日立製作所 自律移動システムおよび管制装置
US9740942B2 (en) * 2012-12-12 2017-08-22 Nissan Motor Co., Ltd. Moving object location/attitude angle estimation device and moving object location/attitude angle estimation method
WO2014115319A1 (ja) * 2013-01-25 2014-07-31 トヨタ自動車株式会社 道路環境認識システム
RU2621823C1 (ru) * 2014-02-24 2017-06-07 Ниссан Мотор Ко., Лтд. Устройство вычисления собственного местоположения и способ вычисления собственного местоположения
WO2017056484A1 (ja) * 2015-09-28 2017-04-06 京セラ株式会社 画像処理装置、ステレオカメラ装置、車両及び画像処理方法
JP6406226B2 (ja) * 2015-11-27 2018-10-17 株式会社デンソー 車両制御装置
US10184799B2 (en) * 2016-06-13 2019-01-22 The Boeing Company Systems and methods for targeting objects of interest in denied GPS environments
GB2552021B (en) * 2016-07-08 2019-08-28 Jaguar Land Rover Ltd Improvements in vehicle speed control

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003252149A (ja) * 2002-03-01 2003-09-10 Mitsubishi Electric Corp 車線認識画像処理装置及びその処理を実行させるためのプログラム
JP2008250906A (ja) * 2007-03-30 2008-10-16 Sogo Keibi Hosho Co Ltd 移動ロボット、自己位置補正方法および自己位置補正プログラム
JP2012008706A (ja) * 2010-06-23 2012-01-12 Denso Corp 道路形状検出装置
JP2012103858A (ja) * 2010-11-09 2012-05-31 Toyota Motor Corp 障害物認識装置
JP2016017758A (ja) * 2014-07-04 2016-02-01 日産自動車株式会社 走行支援装置及び走行支援方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP3492871A4 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019230098A1 (ja) * 2018-05-30 2019-12-05 クラリオン株式会社 情報処理装置
JP2019207214A (ja) * 2018-05-30 2019-12-05 クラリオン株式会社 情報処理装置
CN112204348A (zh) * 2018-05-30 2021-01-08 歌乐株式会社 信息处理装置
US20210206391A1 (en) * 2018-05-30 2021-07-08 Clarion Co., Ltd. Information processing apparatus
JP7137359B2 (ja) 2018-05-30 2022-09-14 フォルシアクラリオン・エレクトロニクス株式会社 情報処理装置
US11560160B2 (en) 2018-05-30 2023-01-24 Faurecia Clarion Electronics Co., Ltd. Information processing apparatus
CN112204348B (zh) * 2018-05-30 2024-08-27 歌乐株式会社 信息处理装置
JP2019219204A (ja) * 2018-06-18 2019-12-26 日産自動車株式会社 走行軌跡推定方法及び走行軌跡推定装置
JP7084792B2 (ja) 2018-06-18 2022-06-15 日産自動車株式会社 走行軌跡推定方法及び走行軌跡推定装置
JP7358108B2 (ja) 2019-07-31 2023-10-10 キヤノン株式会社 情報処理装置、情報処理方法及びプログラム

Also Published As

Publication number Publication date
CN109564098B (zh) 2020-07-14
US20190265040A1 (en) 2019-08-29
US11243080B2 (en) 2022-02-08
EP3492871A1 (en) 2019-06-05
EP3492871A4 (en) 2019-09-04
BR112019001441B1 (pt) 2023-02-07
MX2019001092A (es) 2019-07-04
CA3032068A1 (en) 2018-02-01
RU2722356C1 (ru) 2020-05-29
JPWO2018020589A1 (ja) 2019-03-28
CN109564098A (zh) 2019-04-02
EP3492871B1 (en) 2020-05-06
CA3032068C (en) 2020-01-14
KR20190028528A (ko) 2019-03-18
BR112019001441A2 (pt) 2019-05-07
JP6575686B2 (ja) 2019-09-18

Similar Documents

Publication Publication Date Title
JP6575686B2 (ja) 自己位置推定方法及び自己位置推定装置
KR101880013B1 (ko) 자기 위치 추정 장치 및 자기 위치 추정 방법
JP6575685B2 (ja) 自己位置推定方法及び自己位置推定装置
JP4724043B2 (ja) 対象物認識装置
US10289120B2 (en) Self-position estimation device and self-position estimation method
RU2735567C1 (ru) Способ хранения предысторий движения, способ для выработки модели пути движения, способ для оценки локальной позиции и устройство хранения предысторий движения
KR102134841B1 (ko) 자율주행차의 위치추정시스템 및 그의 위치추정방법
CN110914639B (zh) 产生和更新至少一个建筑物的至少一个空间的拓扑地图的数据生成方法
JP6747157B2 (ja) 自己位置推定方法及び自己位置推定装置
KR20190005189A (ko) 차간 거리 추정 방법 및 차간 거리 추정 장치
JP2021030980A (ja) 走行支援方法及び走行支援装置

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2018530245

Country of ref document: JP

Kind code of ref document: A

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

Ref document number: 16910492

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 3032068

Country of ref document: CA

NENP Non-entry into the national phase

Ref country code: DE

REG Reference to national code

Ref country code: BR

Ref legal event code: B01A

Ref document number: 112019001441

Country of ref document: BR

ENP Entry into the national phase

Ref document number: 20197004534

Country of ref document: KR

Kind code of ref document: A

ENP Entry into the national phase

Ref document number: 2016910492

Country of ref document: EP

Effective date: 20190226

ENP Entry into the national phase

Ref document number: 112019001441

Country of ref document: BR

Kind code of ref document: A2

Effective date: 20190124