WO2023037570A1 - 車載処理装置、車両制御装置、及び自己位置推定方法 - Google Patents

車載処理装置、車両制御装置、及び自己位置推定方法 Download PDF

Info

Publication number
WO2023037570A1
WO2023037570A1 PCT/JP2022/002692 JP2022002692W WO2023037570A1 WO 2023037570 A1 WO2023037570 A1 WO 2023037570A1 JP 2022002692 W JP2022002692 W JP 2022002692W WO 2023037570 A1 WO2023037570 A1 WO 2023037570A1
Authority
WO
WIPO (PCT)
Prior art keywords
self
map
vehicle
processing device
unit
Prior art date
Application number
PCT/JP2022/002692
Other languages
English (en)
French (fr)
Inventor
盛彦 坂野
秀行 粂
茂規 早瀬
竜彦 門司
Original Assignee
日立Astemo株式会社
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 日立Astemo株式会社 filed Critical 日立Astemo株式会社
Priority to DE112022002250.7T priority Critical patent/DE112022002250T5/de
Publication of WO2023037570A1 publication Critical patent/WO2023037570A1/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
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3837—Data obtained from a single source
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3848—Data obtained from both position sensors and additional sensors
    • 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
    • G—PHYSICS
    • G09—EDUCATION; CRYPTOGRAPHY; DISPLAY; ADVERTISING; SEALS
    • G09B—EDUCATIONAL OR DEMONSTRATION APPLIANCES; APPLIANCES FOR TEACHING, OR COMMUNICATING WITH, THE BLIND, DEAF OR MUTE; MODELS; PLANETARIA; GLOBES; MAPS; DIAGRAMS
    • G09B29/00—Maps; Plans; Charts; Diagrams, e.g. route diagram

Definitions

  • the present invention relates to an in-vehicle processing device that generates a map from sensor observation results and estimates its own position.
  • Patent Document 1 Japanese Patent Application Laid-Open No. 2019-144041 describes a first acquisition unit that acquires an external situation signal indicating the external situation of a moving body, and map data that includes information indicating characteristics affected by weather. a second acquisition unit for acquiring; a third acquisition unit for acquiring weather information indicating weather conditions; and a self-position estimation unit for estimating the self-position of the mobile object based on the external situation signal,
  • the self-position estimation device is characterized in that the self-position estimation unit evaluates accuracy of the external situation signal based on the map data and the weather information.
  • Patent Document 1 enables highly accurate self-position estimation by estimating the self-position while considering the reliability of the observation point group based on the weather information.
  • problems such as erroneous self-localization and low self-localization accuracy. If the self-localization accuracy is low, it will be difficult to apply it to automatic driving and driving support.
  • the in-vehicle processing device includes a past map storage unit that stores past generated maps, a self-map generating unit that generates a map during driving based on sensor data acquired during driving, and a self-position estimating unit that compares the generated map with the map while driving to estimate the self-position; and a data selection unit for selecting from a map.
  • the self-position can be estimated with high accuracy. Problems, configurations, and effects other than those described above will be clarified by the following description of the embodiments.
  • FIG. 1 is a block diagram showing the configuration of a map generation/self-position estimation device of Example 1.
  • FIG. It is a functional block diagram showing operation
  • FIG. 3 illustrates global position estimation and local position estimation; It is a figure which shows the attachment state, observation range, and division of a camera, a stereo camera, and a sonar. It is a figure which shows an example of the determination element of an integrated determination part. It is a figure which shows an example of the determination element of an integrated diagnostic part.
  • 4 is a flow chart showing the operation of a self-map generation unit; It is a flowchart which shows operation
  • FIG. 3 is a block diagram showing the configuration of a vehicle according to a second embodiment; FIG. It is a figure which shows map sharing by a map generation and a self-position estimation apparatus.
  • FIG. 1 An embodiment of the map generation/self-position estimation device 100 according to the present invention will be described below with reference to FIGS. 1 to 9.
  • FIG. 1 An embodiment of the map generation/self-position estimation device 100 according to the present invention will be described below with reference to FIGS. 1 to 9.
  • FIG. 1 is a block diagram showing the configuration of the map generation/self-position estimation device 100 according to the first embodiment of the present invention.
  • the map generation/self-position estimation device 100 is mounted on a vehicle 105, and includes cameras 121 to 124, a stereo camera 125, a sonar 126, an external sensor 127, a vehicle speed sensor 131, a steering angle sensor 132, and an interface 180. and an in-vehicle processing device 101 .
  • the cameras 121 to 124, the stereo camera 125, the sonar 126, the other external sensor 127, the vehicle speed sensor 131, the steering angle sensor 132, and the interface 180 are connected to the in-vehicle processing device 101 by signal lines. Exchanges various data with 101.
  • the in-vehicle processing device 101 has a CPU 110 , a ROM 111 , a RAM 112 and a recording section 113 . All or part of the arithmetic processing may be performed by another arithmetic processing device such as an FPGA.
  • the CPU 110 is an arithmetic device that reads and executes various programs and parameters from the ROM 111 and operates as an execution unit of the in-vehicle processing device 101 .
  • the RAM 112 is a readable/writable storage area and operates as a main storage device of the in-vehicle processing device 101 .
  • the RAM 112 stores a self-generated map 161 and a driving situation 162, which will be described later.
  • the ROM 111 is a read-only storage area, and stores programs described later. This program is developed in RAM 112 and executed by CPU 110 . By reading and executing the programs, the CPU 110 operates an odometry estimation unit 141, a landmark detection unit 142, a point group generation unit 143, a position estimation determination unit 144, a driving situation diagnosis unit 145, an integrated determination unit 146, and an integrated diagnosis unit 140. , data selector 147 , self-map generator 148 , and self-position estimator 149 . The ROM 111 also stores sensor parameters 150 .
  • the sensor parameters 150 include the position and orientation relationship with the vehicle 105 and unique information for each of the cameras 121 to 124, the stereo camera 125, the sonar 126, and the other external sensor 127.
  • the sensor parameters 150 of the cameras 121 to 124 and the stereo camera 125 include lens distortion coefficients, the center of the optical axis, the focal length, the number of pixels of the imaging device, and the dimensions, and the sensor parameters 150 of the other external sensor 127 are for each sensor. Contains specific information.
  • the recording unit 113 is a non-volatile storage device and operates as an auxiliary storage device for the in-vehicle processing device 101 .
  • a self-generated map 151 and a driving situation 152 are stored in the recording unit 113 .
  • the cameras 121 to 124 are attached around the vehicle 105, for example, and shoot the surroundings of the vehicle 105.
  • the position/attitude relationship between the cameras 121 to 124 and the vehicle 105 is stored in the ROM 111 as sensor parameters 150 .
  • the mounting method of the cameras 121 to 124 is not limited to the method described above, and other mounting methods may be used. Also, the number of cameras 121 to 124 does not necessarily have to be four.
  • the stereo camera 125 is attached to the vehicle 105, for example, inside the windshield of the vehicle interior, and photographs the front of the vehicle 105.
  • the position and orientation relationship between stereo camera 125 and vehicle 105 is stored in ROM 111 as sensor parameter 150 .
  • the mounting method of the stereo camera 125 is not limited to the method described above, and other mounting methods may be used.
  • the stereo camera 125 may be one unit in which two cameras are arranged side by side with a predetermined base line length apart, or two cameras may be roughly attached and calibrated to form a stereo camera. , two cameras arranged vertically, or three or more cameras arranged side by side.
  • the stereo camera 125 outputs to the in-vehicle processing device 101 the image obtained by photographing and the parallax necessary for distance calculation.
  • the stereo camera 125 may output only the captured image to the in-vehicle processing device 101, and the CPU 110 may calculate the parallax.
  • a plurality of sonars 126 are mounted on the vehicle 105, for example, and observe the surroundings of the vehicle 105.
  • the position and orientation relationship between sonar 126 and vehicle 105 is stored in ROM 111 as sensor parameters 150 .
  • the other external sensor 127 is, for example, a LiDAR mounted on the vehicle 105 and observes the surroundings of the vehicle 105 .
  • the position/orientation relationship between the other external sensor 127 and the vehicle 105 is stored in the ROM 111 as a sensor parameter 150 .
  • the cameras 121-124 and the stereo camera 125 have lenses and imaging elements.
  • the sensor parameters 150 include the characteristics of these cameras 121 to 124 and 125, for example, lens distortion coefficients that indicate lens distortion, optical axis centers, focal lengths, internal parameters such as the number of pixels and dimensions of the image sensor, and sensor parameters. It includes the position and orientation relationship indicating the state of attachment to the vehicle 105 , the relative relationship between the two cameras of the stereo camera 125 , and information specific to each sensor of the sonar 126 and other external sensor 127 , and is stored in the ROM 111 .
  • the position and orientation relationship between each of the cameras 121 to 124, stereo camera 125, sonar 126, and other external sensor 127 and the vehicle 105 is determined by the in-vehicle processing unit 101 using captured images, parallax, vehicle speed sensor 131, and steering angle sensor 132. It may be estimated by the CPU 110 .
  • the cameras 121 to 124, stereo camera 125, sonar 126, and other external sensor 127 may be mounted in various numbers and combinations as long as at least one of the cameras 121 to 124 and stereo camera 125 is included. good.
  • Each of the vehicle speed sensor 131 and the steering angle sensor 132 measures the vehicle speed and steering angle of the vehicle 105 on which the in-vehicle processing device 101 is mounted and outputs them to the in-vehicle processing device 101 .
  • the in-vehicle processing unit 101 uses the outputs of the vehicle speed sensor 131 and the steering angle sensor 132 to calculate the amount and direction of movement of the vehicle 105 on which the in-vehicle processing unit 101 is mounted by a known dead reckoning technique.
  • the interface 180 provides, for example, a GUI that accepts instruction input from the user. Also, other information may be input and output in other forms.
  • FIG. 2 is a functional block diagram representing the operation of the map generation/self-position estimation device 100, and shows the flow of data between the functional blocks executed by the CPU 110, the RAM 112, the ROM 111, and the recording unit 113.
  • functions of an odometry estimation unit 141, a landmark detection unit 142, a point group generation unit 143, an integrated diagnosis unit 140, a data selection unit 147, a self-map generation unit 148, and a self-position estimation unit 149 are shown as functional blocks. show.
  • the self-map generation/self-position estimation unit 250 includes the sensor value acquisition unit 201, the odometry estimation unit 141, the landmark detection unit 142, the point group generation unit 143, the integrated diagnosis unit 140, the data selection unit 147, the self-map generation unit 148, It has a self-position estimation unit 149 and a recording unit 113 .
  • the integrated diagnosis unit 140 has a position estimation determination unit 144 , a driving situation diagnosis unit 145 and an integration determination unit 146 .
  • the combination of the sensor value acquisition unit 201, the odometry estimation unit 141, the landmark detection unit 142, the point group generation unit 143, the self-map generation unit 148, and the recording unit 113 is the map generation unit.
  • sensor value acquisition unit 201 , odometry estimation unit 141 , landmark detection unit 142 , point group generation unit 143 , integrated diagnosis unit 140 , data selection unit 147 , self-map generation unit 148 , self-position estimation unit 149 , and recording unit 113 operates in self-localization mode 203 .
  • a point cloud map is generated from the self observation results.
  • sensor information observed at each time is collected by the sensor value acquisition unit 201, the landmark detection unit 142, the point group generation unit 143, the odometry estimation unit 141, the self-map generation unit 148, and the recording unit 113.
  • the point cloud map is self-generated by synthesizing in chronological order while considering the vehicle motion. Details of each functional block will be described later.
  • the map generation mode 202 after the point cloud map is recorded in the recording unit 113 as the self-generated map 151, subsequent functional blocks related to self-position estimation are not executed.
  • the self-generated map 151 is read from the recording unit 113 and the self-position is estimated on the self-generated map 151 .
  • the self-position estimation unit 149 the map recorded in the recording unit 113 in the past, the sensor value acquisition unit 201, the landmark detection unit 142, the point group generation unit 143, the odometry estimation unit 141, The self-map generating unit 148 and the recording unit 113 synthesize the sensor information observed at each time in a time-series manner while considering the vehicle motion, and collate the self-generated point cloud map to determine the self-position. presume. Details of each functional block will be described later.
  • the sensor value acquisition unit 201 acquires the signal output from the sensor. Images are obtained from the cameras 121 to 124 and images and parallax are obtained from the stereo camera 125 . Cameras 121 to 124 and stereo camera 125 continuously shoot at high frequency, eg, 30 times per second. The sonar 126 and the external sensor 127 output observation results at a frequency determined by each sensor. The sensor value acquisition unit 201 receives these images and signals at a predetermined frequency and outputs them to the landmark detection unit 142 and the point cloud generation unit 143 . Subsequent processing, that is, functional blocks 142 to 149, may operate each time an observation result is received, or may operate at predetermined intervals.
  • the odometry estimation unit 141 estimates the motion of the vehicle 105 using the speed and steering angle of the vehicle 105 transmitted from the vehicle speed sensor 131 and steering angle sensor 132 . For example, it may be estimated using known dead reckoning, may be estimated using known visual odometry technology using a camera, or may be estimated using a known Kalman filter or the like. Vehicle speed, steering angle, vehicle motion information, vehicle motion trajectory (odometry) obtained by integrating the vehicle motion, etc. are stored in the recording unit 113 as the driving situation 162 and output to the self-map generation unit 148 . The odometry estimator 141 may receive position data by GNSS and output odometry based on absolute coordinates estimated from the vehicle speed, steering angle, and position data. In this case, it is possible to select the map closest to the current position from among multiple maps, or refer to information combining the generated map and the actual map.
  • the landmark detection unit 142 first detects landmarks on the road surface using images input from the cameras 121 to 124 and the stereo camera 125 and acquired by the sensor value acquisition unit 201 .
  • Landmarks on the road surface are features on the road surface that can be identified by sensors, such as lane marks, which are a type of road surface paint, stop lines, pedestrian crossings, and other regulatory signs. In this embodiment, moving objects such as vehicles and people, and obstacles such as building walls that hinder vehicle travel are not treated as landmarks on the road surface.
  • the landmark detection unit 142 detects landmarks on the road surface existing around the vehicle 105, that is, features on the road surface that can be identified by the sensor, based on information input from the cameras 121 to 124 and the stereo camera 125. .
  • Landmark information may be obtained on a pixel-by-pixel basis or as an object obtained by grouping pixels. Image recognition may or may not identify the type of landmark (eg, lane mark, pedestrian crossing, etc.).
  • the landmark detection unit 142 generates a point group representing landmarks on the road surface based on the obtained landmark information.
  • this point group may be two-dimensional or three-dimensional, the present embodiment will be described as a three-dimensional point group.
  • the distance to the road surface at each pixel in the images acquired from the cameras 121 to 124 can be calculated with relatively high accuracy. You can calculate the original coordinates. Using the calculated three-dimensional coordinates, point cloud data representing pixels where landmarks are present or three-dimensional coordinates of the grouped objects is output to the self-map generator 148 .
  • the distance can be calculated according to the principle of triangulation. Furthermore, from the calculated distance, the three-dimensional coordinates of the object appearing in the corresponding pixel can be calculated.
  • the Uniqueness Ratio which is a parameter for parallax calculation by stereo matching, is adjusted in advance so that a highly accurate parallax value is output. High-precision 3D coordinates are calculated using the output high-precision parallax values, and the calculated 3D coordinates are used to represent pixels where road landmarks exist or 3D coordinates of grouped objects.
  • the point cloud data is output to the self-map generator 148 .
  • the point cloud data obtained by the landmark detection unit 142 is hereinafter referred to as a road surface point cloud in order to distinguish it from the point cloud to be described later.
  • the three-dimensional coordinates are represented by relative coordinate values from the sensor, like the observed values in each sensor coordinate system.
  • the point cloud generation unit 143 uses the image and sensor information acquired by the sensor value acquisition unit 201 to generate a point cloud representing tall landmarks such as building walls.
  • This point group may be two-dimensional or three-dimensional, but in this embodiment, a three-dimensional point group will be described.
  • three-dimensional coordinates can be calculated from the images acquired by the cameras 121-124 using a known motion stereo method.
  • the motion stereo method changes in the posture of the camera are measured by time-series motion of feature points, such as the corners of the object, which are characteristic points of the image, and three-dimensional coordinates are calculated by the principle of triangulation.
  • Calculation of three-dimensional coordinates by the motion stereo method is difficult for arbitrary pixels with high accuracy, and only points that are easy to track as feature points can be calculated with high accuracy. Points that can be easily tracked as feature points can be selected by a known technique.
  • Three-dimensional coordinates are measured for points that can be easily tracked as feature points, and points higher than the road surface are output to the self-map generation unit 148 as a point group.
  • the distance can be measured by the principle of triangulation from the parallax value and the sensor parameters 150 stored in the ROM 111. Furthermore, from the distance information, the three-dimensional coordinates of the object shown in the corresponding pixel can be measured.
  • the Uniqueness Ratio which is a parameter for calculating parallax by stereo matching, is adjusted in advance so that a highly accurate parallax value is output. Three-dimensional coordinates are calculated using this highly accurate parallax value, and a group of points at positions higher than the road surface is output to the self-map generation unit 148 .
  • the sonar 126 is a sensor that directly observes point clouds, and is assumed to be attached so as not to observe the road surface. do.
  • points arranged linearly are grouped, and the points are interpolated with the linear point group so as to have predetermined intervals, and output.
  • a label indicating that a linear point group is formed is added.
  • the other-world sensor 127 is a sensor that directly observes the point group, and outputs the point group at positions higher than the road surface to the self-map generation unit 148 among the acquired point groups.
  • the point cloud data obtained by the point cloud generation unit 143 is called a high point cloud in order to distinguish it from the road surface point cloud described above.
  • the road surface point group and the high point group based on the observations of the cameras 121 to 124 and the stereo camera 125 are compared, if the observation distance is the same, the road surface point group has a feature of higher accuracy.
  • these three-dimensional coordinates are represented by relative coordinate values from the sensor, like the observed values in each sensor coordinate system.
  • the self-map generation unit 148 uses the point cloud information obtained from both the landmark detection unit 142 and the point cloud generation unit 143 and represented by the relative coordinate values of the sensors as the driving situation 162 obtained from the odometry estimation unit 141.
  • the vehicle motion information and the sensor parameters 150 stored in the ROM 111 are used to convert to world coordinates, and the converted point cloud information is combined in time series to generate a point cloud map.
  • a world coordinate value is a coordinate value based on a certain coordinate and a certain axis.
  • the position at which the in-vehicle processing device 101 is started may be defined as the origin
  • the direction in which the in-vehicle processor 101 first traveled may be defined as the X-axis
  • the directions orthogonal to the X-axis may be defined as the Y-axis and the Z-axis.
  • the map obtained by the self-map generator 148 is stored in the RAM 112 as the self-generated map 161.
  • the self-generated map 161 is read from the RAM 112, the point groups newly obtained from the landmark detection unit 142 and the point group generation unit 143 are coordinate-transformed, and the motion information of the vehicle is used to chronologically Synthesize.
  • the process ends in the self-map generation unit 148 and waits for input of the next time.
  • the self-map generation unit 148 outputs the self-generated map 161 and the driving situation 162 to the position estimation determination unit 144 and the driving situation diagnosis unit 145 .
  • the generated self-generated map and travel situation are recorded in the recording unit 113 as the self-generated map 151 and travel situation 152 .
  • the processing flow of the self-map generator 148 will be described later with reference to FIG.
  • the integrated diagnosis unit 140 has a position estimation determination unit 144, a driving situation diagnosis unit 145, and an integrated determination unit 146.
  • the integrated diagnosis unit 140 integrates the determination result by the position estimation determination unit 144 and the diagnosis result by the driving condition diagnosis unit 145 in the integration determination unit 146 to determine data to be used for position estimation.
  • the position estimation determination unit 144 determines whether or not global position estimation has been successful based on the estimation result of the self-position estimation unit 149, which will be described later, based on the point group. Whether or not global position estimation is successful will be described later in detail with reference to FIG. It refers to the state where there is no Whether or not global position estimation is successful is determined, for example, by whether the sum of the distance errors of the corresponding point group during matching is equal to or less than a predetermined value, or whether the maximum value of the distance of the corresponding point group during matching is a predetermined value. It can be determined by an index such as whether the difference between the vehicle movement amount difference and the self-position estimation difference is equal to or less than a predetermined value, or a combination thereof. The positional deviation may be corrected based on this determination result.
  • the position estimation determination unit 144 does not operate at the time of the first operation because it is in a non-matching state, and determines that global position estimation has failed. Then, it determines whether global position estimation is successful, and transmits the determination result to the integrated determination unit 146 .
  • the driving situation diagnosis unit 145 diagnoses the driving situation using the driving situation 162 when the self-generated map 161 was generated by the self-map generation unit 148 and the past driving situation 152 recorded in the recording unit 113 .
  • the driving condition is determined for each point group obtained from each sensor based on whether or not the driving condition affects the deterioration of accuracy, and the determination result is transmitted to the integrated determination unit 146 .
  • the high point group obtained from the cameras 121 to 124 via the landmark detection unit 142 has the property that the accuracy decreases as the speed of the vehicle 105 increases. Determine whether or not
  • the driving situation 162 is a vehicle behavior that greatly changes the observation direction in a short time, such as a turning operation, when the map is generated Since the observation target is changing, there is a possibility that it will become noise during point cloud matching and reduce accuracy.
  • the turning-back operation if the movement of the vehicle is the same as when the map was generated, a large difference does not occur between the observation point group when the past map was generated and the current observation point group, so the decrease in accuracy is small.
  • the density of the point cloud changes.
  • the integrated determination unit 146 comprehensively determines the state from the determination result of the position estimation determination unit 144 and the diagnosis result of the driving condition diagnosis unit 145, and determines point cloud data to be selected for improving accuracy. A determination method based on the determination result of the position estimation determination unit 144 and the diagnosis result of the driving condition diagnosis unit 145 will be described with reference to FIG. Integrated determination section 146 transmits the determination result to data selection section 147 .
  • the integrated diagnostic unit 140 is a functional block that summarizes diagnostic functions composed of a position estimation determination unit 144, a driving situation diagnosis unit 145, and an integrated determination unit 146.
  • This configuration is an example, and for example, it may be configured with only the position estimation determination unit 144, only with the driving condition diagnosis unit 145, or may be configured with another diagnosis method.
  • An example of determination elements when the integrated diagnosis unit 140 is configured only by the position estimation determination unit 144 will be described with reference to FIG.
  • the data selection unit 147 selects point cloud data for each sensor based on the diagnosis result of the integrated diagnosis unit 140 and transmits it to the self-position estimation unit 149 .
  • the self-position estimation unit 149 collates the self-generated map 161 obtained from the self-map generation unit 148 in the current running situation with the previously generated self-generated map 151 stored in the recording unit 113, The self position on the generated map 151 is estimated.
  • the point cloud used for matching is the point cloud sent from the data selection unit 147 .
  • an ICP (Iterative Closest Point) algorithm which is a known point group matching technology, can be used.
  • the amount of coordinate transformation from the current self-generated map 161 to the previously generated self-generated map 151 can be calculated.
  • Self-location can be estimated. A processing flow of the self-position estimation unit 149 will be described later with reference to FIG.
  • FIG. 3(a) and 3(b) show the state before global position estimation is successful
  • FIG. 3(c) shows the state after global position estimation is successful.
  • the point group 302 of the currently observed self-generated map 161 generated during driving is collated with the point group 301 of the past self-generated map 151 recorded in the recording unit 113 using, for example, the ICP algorithm.
  • the estimated self-position 303 is estimated.
  • FIG. 3B there is a state in which neither local position estimation in a narrow range nor global position estimation in a wide range is successful.
  • local position estimation in a narrow range is successful due to factors such as the initial matching position where the ICP algorithm is started and the similar point cloud arrangement even when sliding. Even if it is, global position estimation in a wide range is not successful, and the position of the point cloud may deviate from the original position, resulting in erroneous self-position estimation.
  • the ICP algorithm operates to minimize the sum of distance errors between corresponding point clouds. The point cloud may be matched to the location with the smallest error, resulting in an erroneous self-localization.
  • the position estimation determination unit 144 determines whether or not global position estimation has been successful based on the state of the point cloud or the like. Outputs the status of whether global position estimation is successful.
  • FIG. 4 is a diagram showing the mounting state of the cameras 121 to 124, the stereo camera 125, and the sonar 126, their observation ranges, and their divisions.
  • the stereo camera 125 has a narrow angle of view and can observe relatively distant observation ranges 401-403. Cameras 121 - 124 and sonar 126 are mounted around vehicle 105 and observe a relatively near field of view 404 . The mounting position and observation range of each sensor are examples. Cameras 121 to 124 and stereo camera 125 observe the road surface point group and the high point group. The point cloud observed by the cameras 121 to 124 and the stereo camera 125 has the property that both the road surface point cloud and the effective point cloud have high accuracy near the vehicle 105 and low accuracy far from the vehicle 105 . Therefore, self-position estimation using a point group observed in the vicinity of the vehicle 105 is desirable for improving self-position estimation accuracy. However, estimating the self-position using only point clouds observed near the vehicle 105 may fail to estimate the global position, as shown in FIGS. 3(a) and 3(b).
  • the stereo camera 125 can observe a relatively long distance, it is preferable to divide the points into a near point group 401, a middle point group 402, and a far point group 403, and use the point groups properly according to the determination result of the integrated determination unit 146. .
  • both the cameras 121 to 124 and the stereo camera 125 have higher accuracy in the road surface point cloud, so they are considered when the integrated determination unit 146 makes a determination.
  • observation point groups may be broadly classified into a near road point group, a middle road point group, a far road point group, a near high point group, a middle high point group, and a far effective point group. Since the observation range of the cameras 121 to 124 is narrow, it is preferable to distinguish between the road surface point group and the high point group instead of dividing by the distance. This is an example, and the cameras 121 to 124 may also be classified according to the distance, and the integrated determination unit 146 may use them properly. Also in this case, both the road surface point cloud and the effective point cloud have the property that the closer the distance from the vehicle 105, the higher the accuracy.
  • the point cloud obtained from the sonar 126 is a high point cloud, the effective point cloud is given line labels and point labels. It is better to use groups separately.
  • FIG. 5 is a diagram showing an example of determination elements of the integrated determination unit 146.
  • FIG. 5 is a diagram showing an example of determination elements of the integrated determination unit 146.
  • FIG. 5(a) shows the determination elements regarding the stereo camera 125
  • FIG. 5(b) the determination elements regarding the cameras 121 to 124
  • FIGS. 5(c) and 5(d) the determination elements regarding the sonar 126.
  • the stereo camera 125 In the determination of the stereo camera 125 shown in FIG. 5(a), a decrease in accuracy is likely to occur due to a movement such as a turning motion. Accuracy loss is less likely to occur. Therefore, a distinction is made between when the travel path difference between the travel situation 152 at the time of map generation and the travel situation 162 during current travel is smaller than or greater than a predetermined value. In general, the trajectory difference is small when traveling in a straight line, and the trajectory difference is large when traveling in a mixture of straight lines and curves.
  • the running locus difference may be determined by dividing the running locus into sections by a predetermined distance or time, and determining the locus difference for each divided section.
  • the accuracy of the high point cloud decreases due to the nature of motion stereo.
  • the road surface point cloud is not affected by the speed and is highly accurate compared to the high point cloud.
  • a wide range of points are used to avoid errors in global localization, and after global self-localization, only high-precision points are used to locally estimate the position. enables highly accurate position estimation.
  • high point clouds and road surface point clouds are selected, and after low speed and global position estimation, only road surface point clouds are selected, and high speed and before global position estimation are selected. uses only the road surface point cloud, and after fast global position estimation, uses only the road surface point cloud.
  • the sonar 126 is a sensor that uses sound waves, its accuracy fluctuates due to temperature changes. Therefore, a point and a line are selected when the temperature difference is small and before global position estimation, a point and a line are selected when the temperature difference is small and after global position estimation, and a point and line are selected when the temperature difference is large and before global position estimation. , select a line, and select a line when the temperature difference is large and before global position estimation.
  • the driving conditions may include the weather, sunshine conditions, and the like.
  • the stages for classifying the determination conditions may be multi-stages of three or more instead of two stages.
  • FIG. 6 is a diagram showing an example of determination elements of the integrated diagnosis unit 140, and shows an example of determination elements when the integrated diagnosis unit 140 is configured only with the position estimation determination unit 144.
  • FIG. 6 is a diagram showing an example of determination elements of the integrated diagnosis unit 140, and shows an example of determination elements when the integrated diagnosis unit 140 is configured only with the position estimation determination unit 144.
  • FIG. 6(a) shows the determination elements regarding the stereo camera 125
  • FIG. 6(b) the determination elements regarding the cameras 121 to 124
  • FIG. 6(c) the determination elements regarding the sonar 126.
  • FIG. 6A, 6B, and 6C a wide range of points are used before global position estimation, and only points with high accuracy are used after global position estimation.
  • FIG. 7 is a flow chart showing the operation of the self-map generator 148.
  • the self-map generation unit 148 Upon receiving an execution command from the landmark detection unit 142 and the point group generation unit 143, the self-map generation unit 148 executes the following operations. The execution subject of each step described below is the CPU 110 .
  • the landmark detection unit 142 acquires from the RAM 112 the road surface point group generated from the outputs of the cameras 121 to 124 and the stereo camera 125 .
  • the road point cloud is represented by each sensor coordinate system. Proceed to next step 702 .
  • the high point cloud of each sensor generated by the point cloud generation unit 143 is acquired from the RAM 112.
  • the high point group is represented by each sensor coordinate system. Go to next step 703 .
  • the coordinate system of the road surface point cloud acquired in the landmark acquisition step 701 and the high point cloud acquired in the point cloud acquisition step 702 are transformed into the coordinate system of the vehicle 105.
  • sensor parameters 150 for each sensor stored in the ROM 111 are used.
  • the coordinate-transformed point group is output to the map generation step 704, and the process proceeds to the next step 704.
  • the vehicle motion information obtained from the odometry estimation unit 141 is used to convert the coordinate point cloud obtained in the coordinate conversion step 703 into world coordinates, and the point cloud map at the previous time stored in the RAM 112 is converted to world coordinates.
  • a point cloud map is self-generated by time-series synthesis of The self-generated map, the vehicle speed, the steering angle, the vehicle motion information obtained from the odometry estimation unit 141, and the vehicle motion trajectory obtained by integrating the vehicle motion are output to the point cloud map output step 705. Go to step 705 .
  • the point cloud map generated in the map generation step 704 is output to the RAM 112 as the self-generated map 161 or to the recording unit 113 as the self-generated map 151.
  • the driving situation obtained from the map generating step 704 is output to the RAM 112 as the driving situation 162 or to the recording unit 113 as the driving situation 152, and the process is terminated.
  • the map generation mode 202 basically, the self-generated map 161 and the driving situation 162 are output to the RAM 112, and then the sensor value acquisition unit 201 acquires sensor values.
  • the point group map generated in the map generation step 704 is output to the recording unit 113 as the self-generated map 151 and the driving situation 152, and the process is terminated.
  • the position estimation determination unit 144 and the driving situation diagnosis unit 145 start processing.
  • FIG. 8 is a flowchart showing the operation of the self-position estimation unit 149.
  • the execution subject of each step described below is the CPU 110 . This process is executed only in the self-position estimation mode 203.
  • the self-generated map 151 stored in the recording unit 113 is acquired.
  • the self-generated map 151 is a map obtained by past travel. Proceed to next step 802 .
  • the selected point group of the self-generated map 161 observed during the current run determined by the integrated determination unit 146 and selected by the data selection unit 147 , is acquired from the RAM 112 . Go to next step 803 .
  • Point cloud matching can use, for example, the ICP (Iterative Closest Point) algorithm, which is a known point cloud matching technique.
  • ICP Intelligent Closest Point
  • point groups are collated by repeating the process of calculating the correspondence between points and minimizing the distance error in the calculated correspondence.
  • the coordinate transformation amount for transforming the self-generated map 161 obtained from the self-generated map generator 148 onto the self-generated map 151 can be calculated. You can get top self position. The self-position on the self-generated map 151 is output and the processing shown in FIG. 8 is terminated. After the end, the sensor value acquisition unit 201 starts processing.
  • Example 2 of the present invention will be described below.
  • differences from the above-described first embodiment will be mainly described, and the same reference numerals will be given to the same configurations and processes as in the first embodiment, and the description thereof will be omitted.
  • FIG. 9 is a block diagram showing the configuration of the vehicle 105 of Example 2 of the present invention.
  • the vehicle 105 has a map generation/self-position estimation device 100 and a group of vehicle control devices 170 to 173 that control automatic driving in the vehicle 105 .
  • the map generation/self-position estimation device 100 includes cameras 121 to 124, a stereo camera 125, a sonar 126, an external sensor 127, a vehicle speed sensor 131, a steering angle sensor 132, an interface 180, and a GNSS receiver 181. , a communication device 182 , and a display device 183 .
  • the vehicle control device groups 170 to 173 are connected to the in-vehicle processing device 101 by signal lines, and exchange various data with the in-vehicle processing device 101 .
  • the GNSS receiver 181 receives signals transmitted from a plurality of satellites constituting a satellite navigation system (eg, GPS), and calculates the position of the GNSS receiver 181, that is, latitude and longitude, by calculation using the received signals. .
  • the accuracy of the latitude and longitude calculated by the GNSS receiver 181 may be low, and may include an error of several meters to 10 meters, for example.
  • the GNSS receiver 181 outputs the calculated latitude and longitude to the in-vehicle processing device 101 .
  • the communication device 182 is a communication device used for exchanging information wirelessly between equipment external to the vehicle 105 and the in-vehicle processing device 101 . For example, when the user is outside the vehicle 105, it communicates with the portable terminal worn by the user to exchange information.
  • the target with which the communication device 182 communicates is not limited to the mobile terminal of the user, and may be a device that provides data related to automatic driving.
  • the display device 183 is, for example, a liquid crystal display, and displays information output from the in-vehicle processing device 101 .
  • the vehicle control device 170 controls the steering device 171, the driving device 172, and At least one of the braking devices 173 is controlled.
  • the steering device 171 operates the steering of the vehicle 105 .
  • Driving device 172 provides driving force to vehicle 105 .
  • the driving device 172 increases or decreases the driving force of the vehicle 105 by, for example, increasing or decreasing the target rotation speed of the engine or motor provided in the vehicle 105 .
  • the braking device 173 applies a braking force to the vehicle 105 to decelerate the vehicle 105 .
  • the in-vehicle processing device 101 may store the self-generated map 151 stored in the recording unit 113 in a server or the like via the communication device 182.
  • the in-vehicle processing device 101 may store the self-generated map 151 mounted on another vehicle 105 and stored in the server in the recording unit 113 via the communication device 182 and utilize it for self-position estimation.
  • the in-vehicle processing unit 101 attaches the reception position of each time of the GNSS receiver 181 to the self-generated map 161, and when estimating the self-position, a value close to the current position information by GNSS is attached. You may search maps and select a map to match. If the GNSS signal cannot be received, the odometry estimator 141 may estimate the absolute position from the difference from the most recent absolute position observed by GNSS.
  • FIG. 10 is a diagram showing map sharing by the map generation/self-position estimation device 100.
  • FIG. 10 is a diagram showing map sharing by the map generation/self-position estimation device 100.
  • the own vehicle 105 and the other vehicle 1103 have the map generation/self-position estimation device 100 having the communication device 182 .
  • Self-vehicle 105 uses communication device 182 to record self-generated map 151 and driving situation 152 in external recording device 1102 connected to server 1101 on the cloud via a communication line.
  • the other vehicle 1103 uses the communication device 182 to acquire the self-generated map 151 generated by the vehicle 105 and the driving situation 152 from the external recording device 1102 connected to the server 1101 on the cloud. use.
  • the own vehicle 105 may utilize the self-generated map 151 and the driving situation 152 generated by the other vehicle 1103 .
  • the in-vehicle processing device 101 of this embodiment includes a past map storage unit (recording unit 113) that stores past generated maps, A self-map generation unit 148 that generates a middle map, a self-position estimation unit 149 that estimates the self-position by collating the previously generated map and the map while driving, and based on the characteristics of the data included in the map while driving, the self-position Since the data selection unit 147 selects the data used by the position estimation unit 149 from the running map, the accuracy of the point group generated from the image taken by the camera and the local adaptation at the time of position estimation using the point group It is possible to estimate the self-position by selecting the optimum point group while considering the occurrence of , and to estimate the self-position with high accuracy.
  • a past map storage unit recording unit 113
  • a self-map generation unit 148 that generates a middle map
  • a self-position estimation unit 149 that estimates the self-position by collating the previously generated map and the map while driving, and based on the characteristics of the data included in
  • the self-position estimation unit 149 can perform first matching (global matching) with a wide matching range between the previously generated map and the running map and second matching (local matching) with a narrow matching range. Since the data selection unit 147 selects different data for the first verification and the second verification, the global verification that reduces the error as a whole including distant points and the points with high sensing accuracy are used. Self-location can be estimated with high accuracy by local matching.
  • the self-location estimating unit 149 since the self-location estimating unit 149 performs the second matching after the first matching, it prevents misestimation of the self-location by performing the local matching before the global matching. It can be estimated with high accuracy.
  • an integrated determination unit 146 is provided for specifying the type of data used for position estimation based on the driving conditions at the time of map generation and the self-position estimation result. Since the driving situation is included, the accuracy of estimating the self-position can be improved.
  • the self-map generation unit 148 generates a running map converted into absolute coordinates estimated by GNSS and odometry, and is connected to a server 1101 that stores the map via a communication line.
  • the map during driving is transmitted to the server 1101 and the map generated by the other vehicle 1103 is used as the map generated in the past, so that the map can be shared by many vehicles and a wide range of maps can be used.
  • self-position estimation unit 149 outputs the estimated self-position to vehicle control device 170 , and vehicle control device 170 controls steering device 171 , drive device 172 based on the self-position output from in-vehicle processing device 101 . and the braking device 173, automatic driving and driving assistance can be realized based on the map generated by itself.
  • the present invention is not limited to the above-described embodiments, and includes various modifications and equivalent configurations within the scope of the attached claims.
  • the above-described embodiments have been described in detail for easy understanding of the present invention, and the present invention is not necessarily limited to those having all the described configurations.
  • Other aspects conceivable within the scope of the technical idea of the present invention are also included in the scope of the present invention.
  • part of the configuration of one embodiment may be replaced with the configuration of another embodiment.
  • the configuration of another embodiment may be added to the configuration of one embodiment.
  • additions, deletions, and replacements of other configurations may be made to a part of the configuration of each embodiment.
  • the map generation/self-position estimation device 100 may have an input/output interface (not shown), and read a program from another device via the input/output interface and a usable medium when necessary.
  • a medium is, for example, a storage medium removable from an input/output interface, or a communication medium, that is, a wired, wireless, or optical network, or a carrier wave or digital signal that propagates through the network.
  • each configuration, function, processing unit, processing means, etc. described above may be realized by hardware, for example, by designing a part or all of them with an integrated circuit, and the processor realizes each function. It may be realized by software by interpreting and executing a program to execute. Also, part or all of the functions realized by the program may be realized by a hardware circuit or FPGA.
  • Information such as programs, tables, and files that implement each function can be stored in storage devices such as memory, hard disks, SSDs (Solid State Drives), or recording media such as IC cards, SD cards, and DVDs.
  • storage devices such as memory, hard disks, SSDs (Solid State Drives), or recording media such as IC cards, SD cards, and DVDs.
  • control lines and information lines indicate those that are considered necessary for explanation, and do not necessarily indicate all the control lines and information lines necessary for implementation. In practice, it can be considered that almost all configurations are interconnected.

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Theoretical Computer Science (AREA)
  • Educational Technology (AREA)
  • Educational Administration (AREA)
  • Business, Economics & Management (AREA)
  • Mathematical Physics (AREA)
  • Navigation (AREA)
  • Instructional Devices (AREA)
  • Traffic Control Systems (AREA)

Abstract

車載処理装置であって、過去に生成された過去生成地図を記憶する過去地図記憶部と、走行中に取得したセンサデータに基づいて走行中地図を生成する自己地図生成部と、前記過去生成地図と前記走行中地図とを照合して自己位置を推定する自己位置推定部と、前記走行中地図に含まれるデータの特性に基づいて、前記自己位置推定部が使用するデータを前記走行中地図から選択するデータ選択部とを備える。

Description

車載処理装置、車両制御装置、及び自己位置推定方法 参照による取り込み
　本出願は、令和３年（２０２１年）９月９日に出願された日本出願である特願２０２１－１４６８４４の優先権を主張し、その内容を参照することにより、本出願に取り込む。
　本発明は、センサの観測結果から地図を生成し、自己位置を推定する車載処理装置に関する。
　自動運転システムや運転支援システムの適用において、自己位置を推定するための詳細な地図情報が重要である。しかしながら、高速道路においては、運転支援システム用の詳細地図が整備されつつあるが、一般道や自宅周辺などの住宅街において詳細地図の整備の目途は立っていない。自動運転システムや運転支援システムの適用範囲を拡大するためには、自ら地図を生成し、自己位置を推定できる技術が求められる。
　本技術分野の背景技術として、以下の先行技術がある。特許文献１（特開２０１９－１４４０４１号公報）には、移動体の外部の状況を示す外部状況信号を取得する第１取得部と、天候の影響を受ける特徴を示す情報を含んだ地図データを取得する第２取得部と、天候の状態を示す天候情報を取得する第３取得部と、前記外部状況信号に基づいて当該移動体の自己位置を推定する自己位置推定部と、を備え、前記自己位置推定部は、前記地図データ及び前記天候情報に基づいて前記外部状況信号の確度を評価する、ことを特徴とする自己位置推定装置が記載されている。
　特許文献１に記載される発明では、天候情報に基づいて観測点群の信頼度を考慮しつつ自己位置を推定することで、高精度な自己位置推定を可能としている。しかしながら、カメラ点群の観測精度に関する性質や、カメラ点群による位置推定時の局所適合の発生が考慮されていないため、自己位置推定を誤ったり、自己位置推定精度が低い問題がある。自己位置推定精度が低ければ、自動運転や運転支援への活用は困難である。
　そこで、本発明は、点群の観測精度に関する性質を考慮しつつ、最適な点群を選定して、高精度に自己位置を推定することを目的とする。
　本願において開示される発明の代表的な一例を示せば以下の通りである。すなわち、車載処理装置であって、過去に生成された過去生成地図を記憶する過去地図記憶部と、走行中に取得したセンサデータに基づいて走行中地図を生成する自己地図生成部と、前記過去生成地図と前記走行中地図とを照合して自己位置を推定する自己位置推定部と、前記走行中地図に含まれるデータの特性に基づいて、前記自己位置推定部が使用するデータを前記走行中地図から選択するデータ選択部とを備えることを特徴とする。
　本発明の一態様によれば、自己位置を高精度に推定できる。前述した以外の課題、構成及び効果は、以下の実施例の説明によって明らかにされる。
実施例１の地図生成・自己位置推定装置の構成を示すブロック図である。 地図生成・自己位置推定装置の動作を表す機能ブロック図である。 大局的な位置推定及び局所的な位置推定を示す図である。 カメラ、ステレオカメラ、ソナーの取り付け状態と観測範囲と区分を示す図である。 統合判定部の判定要素の一例を示す図である。 統合診断部の判定要素の一例を示す図である。 自己地図生成部の動作を示すフローチャートである。 自己位置推定部の動作を示すフローチャートである。 実施例２の車両の構成を示すブロック図である。 地図生成・自己位置推定装置による地図共有を示す図である。
　以下、図１～図９を参照して、本発明にかかる地図生成・自己位置推定装置１００の実施例を説明する。
　図１は、本発明の実施例１の地図生成・自己位置推定装置１００の構成を示すブロック図である。
　地図生成・自己位置推定装置１００は、車両１０５に搭載され、カメラ１２１～１２４と、ステレオカメラ１２５と、ソナー１２６と、他外界センサ１２７と、車速センサ１３１と、舵角センサ１３２と、インターフェース１８０と、車載処理装置１０１を有する。カメラ１２１～１２４と、ステレオカメラ１２５と、ソナー１２６と、他外界センサ１２７と、車速センサ１３１と、舵角センサ１３２と、インターフェース１８０は、車載処理装置１０１と信号線で接続され、車載処理装置１０１と各種データを授受する。
　車載処理装置１０１は、ＣＰＵ１１０と、ＲＯＭ１１１と、ＲＡＭ１１２と、記録部１１３を有する。ＦＰＧＡなど他の演算処理装置で全部、又は一部の演算処理を実行するように構成してもよい。
　ＣＰＵ１１０は、ＲＯＭ１１１から各種プログラム及びパラメータを読み込んで実行する演算装置であり、車載処理装置１０１の実行部として動作する。
　ＲＡＭ１１２は、読み書き可能な記憶領域であり、車載処理装置１０１の主記憶装置として動作する。ＲＡＭ１１２には、後述する自己生成地図１６１、走行状況１６２が格納される。
　ＲＯＭ１１１は、読み取り専用の記憶領域であり、後述するプログラムが格納される。このプログラムはＲＡＭ１１２に展開されて、ＣＰＵ１１０により実行される。ＣＰＵ１１０は、プログラムを読み込んで実行することによって、オドメトリ推定部１４１、ランドマーク検出部１４２、点群生成部１４３、位置推定判定部１４４、走行状況診断部１４５、統合判定部１４６、統合診断部１４０、データ選択部１４７、自己地図生成部１４８、自己位置推定部１４９として動作する。また、ＲＯＭ１１１は、センサパラメータ１５０を格納する。センサパラメータ１５０は、カメラ１２１～１２４、ステレオカメラ１２５、ソナー１２６及び他外界センサ１２７の各々について、車両１０５との位置姿勢関係及び各センサ固有の情報を含む。例えば、カメラ１２１～１２４及びステレオカメラ１２５のセンサパラメータ１５０は、レンズ歪み係数、光軸中心、焦点距離、撮像素子の画素数、及び寸法を含み、他外界センサ１２７のセンサパラメータ１５０は各センサに固有の情報を含む。
　記録部１１３は、不揮発性の記憶装置であり、車載処理装置１０１の補助記憶装置として動作する。記録部１１３には自己生成地図１５１及び走行状況１５２が格納される。
　カメラ１２１～１２４は、例えば、車両１０５の周囲に取り付けられ、車両１０５の周囲を撮影する。カメラ１２１～１２４と車両１０５との位置姿勢関係は、センサパラメータ１５０として、ＲＯＭ１１１に格納される。カメラ１２１～１２４の取り付け方は、前述の方法に限らず、他の取り付け方でもよい。また、カメラ１２１～１２４は、必ず四つでなくともよい。
　ステレオカメラ１２５は、車両１０５の、例えば車室内フロントガラスの内側に取り付けられ、車両１０５の前方を撮影する。ステレオカメラ１２５と車両１０５との位置姿勢関係は、センサパラメータ１５０として、ＲＯＭ１１１に格納される。ステレオカメラ１２５の取り付け方は、前述の方法に限らず、他の取り付け方でもよい。ステレオカメラ１２５は、二つのカメラが所定基線長離れて横に並んで配置されて１ユニット化されているものでもよいし、二つのカメラがラフに取り付けられた状態で、キャリブレーションにより、ステレオカメラとして活用できるようにしたものでもよいし、二つのカメラが縦並びで構成されたものでもよいし、三つ以上のカメラが並んで配置されたものでもよい。本実施例では、二つのカメラが所定基線長離れて横に並んで配置されて１ユニット化されているステレオカメラを想定して説明する。ステレオカメラ１２５は、撮影して得られた画像、及び距離算出のために必要な視差を、車載処理装置１０１に出力する。ステレオカメラ１２５が撮影画像のみを車載処理装置１０１に出力して、ＣＰＵ１１０が視差を算出してもよい。
　ソナー１２６は、例えば車両１０５に複数個搭載され、車両１０５の周囲を観測する。ソナー１２６と車両１０５との位置姿勢関係は、センサパラメータ１５０として、ＲＯＭ１１１に格納される。
　他外界センサ１２７は、例えば、車両１０５に搭載されるＬｉＤＡＲであり、車両１０５の周囲を観測する。他外界センサ１２７と車両１０５の位置姿勢関係は、センサパラメータ１５０としてＲＯＭ１１１に格納される。
　カメラ１２１～１２４及びステレオカメラ１２５は、レンズ及び撮像素子を有する。センサパラメータ１５０は、これらのカメラ１２１～１２４、１２５の特性、例えばレンズの歪みを示すパラメータであるレンズ歪み係数、光軸中心、焦点距離、撮像素子の画素数及び寸法などの内部パラメータ、センサの車両１０５への取り付け状態を示す位置姿勢関係、ステレオカメラ１２５の二つのカメラの相対関係、及び、ソナー１２６、他外界センサ１２７の各センサ固有の情報を含み、ＲＯＭ１１１に格納される。カメラ１２１～１２４、ステレオカメラ１２５、ソナー１２６、他外界センサ１２７の各々と車両１０５の位置姿勢関係は、車載処理装置１０１において、撮影画像、視差、車速センサ１３１、舵角センサ１３２を用いて、ＣＰＵ１１０で推定してもよい。
　カメラ１２１～１２４、ステレオカメラ１２５、ソナー１２６及び他外界センサ１２７は、少なくともカメラ１２１～１２４及びステレオカメラ１２５のうち、どれか一つが含まれていれば、様々な数や組み合わせで搭載されてもよい。
　車速センサ１３１及び舵角センサ１３２の各々は、車載処理装置１０１が搭載された車両１０５の車速及び舵角を測定し車載処理装置１０１に出力する。車載処理装置１０１は、車速センサ１３１及び舵角センサ１３２の出力を用いて、公知のデッドレコニング技術によって、車載処理装置１０１が搭載された車両１０５の移動量及び移動方向を算出する。
　インターフェース１８０は、例えば、ユーザからの指示入力を受け付けるＧＵＩを提供する。また、他の情報を他の形で入出力してもよい。
　図２は、地図生成・自己位置推定装置１００の動作を表す機能ブロック図であり、ＣＰＵ１１０が実行する機能ブロックと、ＲＡＭ１１２と、ＲＯＭ１１１と、記録部１１３の間のデータの流れを示す。図２では、機能ブロックとして、オドメトリ推定部１４１、ランドマーク検出部１４２、点群生成部１４３、統合診断部１４０、データ選択部１４７、自己地図生成部１４８、自己位置推定部１４９が有する機能を示す。
　自己地図生成・自己位置推定部２５０は、センサ値取得部２０１、オドメトリ推定部１４１、ランドマーク検出部１４２、点群生成部１４３、統合診断部１４０、データ選択部１４７、自己地図生成部１４８、自己位置推定部１４９、及び記録部１１３を有する。統合診断部１４０は、位置推定判定部１４４、走行状況診断部１４５、及び統合判定部１４６を有する。
　自己地図生成・自己位置推定部２５０のうち、センサ値取得部２０１、オドメトリ推定部１４１、ランドマーク検出部１４２、点群生成部１４３、自己地図生成部１４８、及び記録部１１３の組み合わせが地図生成モード２０２において動作し、センサ値取得部２０１、オドメトリ推定部１４１、ランドマーク検出部１４２、点群生成部１４３、統合診断部１４０、データ選択部１４７、自己地図生成部１４８、自己位置推定部１４９、及び記録部１１３の組み合わせが自己位置推定モード２０３において動作する。
　地図生成モード２０２では、自己の観測結果から点群地図が生成される。地図生成モード２０２では、センサ値取得部２０１、ランドマーク検出部１４２、点群生成部１４３、オドメトリ推定部１４１、自己地図生成部１４８、及び記録部１１３によって、各時刻に観測されたセンサ情報を、車両運動を考慮しつつ時系列的に合成し、点群地図を自己生成する。各機能ブロックの詳細は後述する。地図生成モード２０２では、点群地図を自己生成地図１５１として記録部１１３に記録した後、後段の自己位置推定に関わる機能ブロックは実行されない。
　自己位置推定モード２０３では、自己生成地図１５１が記録部１１３から読み出され、自己生成地図１５１上で自己位置が推定される。自己位置推定部１４９において、過去に記録部１１３に記録された地図と、地図生成モード２０２と同様に、センサ値取得部２０１、ランドマーク検出部１４２、点群生成部１４３、オドメトリ推定部１４１、自己地図生成部１４８、及び記録部１１３によって、各時刻に観測されたセンサ情報を、車両運動を考慮しつつ時系列的に合成し、自己生成した点群地図を照合することによって、自己位置を推定する。各機能ブロックの詳細は後述する。
　センサ値取得部２０１は、センサから出力される信号を取得する。カメラ１２１～１２４からは画像、ステレオカメラ１２５からは画像と視差を取得する。カメラ１２１～１２４及びステレオカメラ１２５は、連続して高い頻度、例えば毎秒３０回撮影する。ソナー１２６及び他外界センサ１２７は、各センサで定められた頻度で観測結果を出力する。センサ値取得部２０１は、これらの画像や信号を所定の頻度で受信し、ランドマーク検出部１４２及び点群生成部１４３に出力する。以降の処理、すなわち各機能ブロック１４２～１４９は、観測結果を受信するたびに動作するとよいが、所定の周期で動作してもよい。
　オドメトリ推定部１４１は、車速センサ１３１及び舵角センサ１３２から送信される車両１０５の速度及び舵角を用いて、車両１０５の運動を推定する。例えば、公知のデッドレコニングを用いて推定してもよく、カメラを用いた公知のビジュアルオドメトリ技術を用いて推定してもよく、公知のカルマンフィルタ等を併用して推定してもよい。車速、舵角、車両運動情報、及び車両運動を積分した車両運動軌跡（オドメトリ）等は、走行状況１６２として記録部１１３に格納され、自己地図生成部１４８に出力される。なお、オドメトリ推定部１４１には、ＧＮＳＳによる位置データが入力され、車速と舵角と位置データから推定した絶対座標によるオドメトリを出力してもよい。この場合、複数地図の中から現在位置と最も近い地図を選択したり、生成された地図と実際の地図を組み合わせた情報を参照できる。
　ランドマーク検出部１４２は、カメラ１２１～１２４及びステレオカメラ１２５から入力され、センサ値取得部２０１が取得した画像を用いて、まず、路面上のランドマークを検出する。路面上のランドマークとは、センサにより識別可能な特徴を有する路面上の特徴であり、例えば路面ペイントの１種であるレーンマークや、停止線、横断歩道や、その他の規制表示などである。本実施例では、移動体である車両や人間、及び車両の走行の妨げとなる障害物である建物の壁等は路面上のランドマークとして扱わない。
　ランドマーク検出部１４２は、カメラ１２１～１２４及びステレオカメラ１２５から入力される情報に基づいて、車両１０５の周辺に存在する路面上のランドマーク、すなわちセンサが識別可能な路面上の特徴を検出する。ランドマークの情報は、画素単位で得られても、画素がグルーピングされた物体として得られてもよい。画像認識によって、ランドマークの種別（例えばレーンマーク、横断歩道など）が識別されてもよいし、識別されなくてもよい。
　次に、ランドマーク検出部１４２は、得られたランドマークの情報に基づいて、路面上のランドマークを表現する点群を生成する。この点群は二次元でも三次元でもよいが、本実施例では三次元点群として説明する。
　例えば、センサパラメータ１５０に含まれるカメラ１２１～１２４に関する情報を用いて、カメラ１２１～１２４から取得した画像中の各画素における路面までの距離を比較的高精度に計算でき、計算された距離から三次元座標を計算できる。計算された三次元座標を用いて、ランドマークが存在する画素、又はグルーピングされた物体の三次元座標を表す点群データを自己地図生成部１４８に出力する。
　また、例えば、ステレオカメラ１２５が取得した視差値とセンサパラメータ１５０から、三角測量の原理によって距離を計算できる。さらに、計算された距離から、対応する画素に映る対象の三次元座標を計算できる。本実施例では、高精度な三次元点群を出力するために、ステレオマッチングによる視差算出時のパラメータであるUniqueness Ratioを予め調整して、高精度な視差値が出力されるように調整する。出力される高精度な視差値を用いて高精度な三次元座標を計算し、計算された三次元座標を用いて、路面ランドマークが存在する画素、又はグルーピングされた物体の三次元座標を表す点群データを自己地図生成部１４８に出力する。
　ランドマーク検出部１４２で得られる点群データは、後述する点群と区別するために、以後、路面点群と呼称する。また、三次元座標は、各センサ座標系における観測値と同様に、センサからの相対座標値で表される。
　点群生成部１４３は、センサ値取得部２０１が取得した画像及びセンサ情報を用いて、建物の壁などの高さがあるランドマークを表す点群を生成する。この点群は、二次元でも三次元でもよいが、本実施例では三次元点群として説明する。
　例えば、公知のモーションステレオ法を用いて、カメラ１２１～１２４が取得した画像から三次元座標を計算できる。モーションステレオ法では、カメラの姿勢変動を、物体の角などの画像の特徴的な点である特徴点の時系列的な動きを測定し、三角測量の原理によって三次元座標を計算する。モーションステレオ法による三次元座標の計算は、任意の画素について高精度での計算は難しく、特徴点として追跡しやすい点のみ、高精度で計算できる。特徴点として追跡しやすい点については、公知の技術により選別できる。特徴点として追跡しやすい点について三次元座標を測定し、路面より高い点を点群として自己地図生成部１４８に出力する。
　また、例えば、ステレオカメラ１２５の場合、前述の通り、視差値とＲＯＭ１１１に格納されたセンサパラメータ１５０から、三角測量の原理によって距離を測定できる。さらに、その距離情報から、対応する画素に映る対象の三次元座標を測定できる。前述と同様に、高精度な三次元点群を出力するために、ステレオマッチングによる視差算出時のパラメータであるUniqueness Ratioを予め調整して、高精度な視差値が出力されるように調整する。この高精度な視差値を用いて三次元座標を計算し、路面より高い位置の点群を自己地図生成部１４８に出力する。
　また、例えば、ソナー１２６は、点群を直接観測するセンサであり、路面を観測しないように取り付けられることが想定されるため、取得される点群全てを点群として自己地図生成部１４８に出力する。取得される点群のうち、線状に並んでいる点についてはグルーピングし、予め定めた所定間隔となるように線状の点群で点を補間して出力する。また、線状の点群を構成することを示すラベルを付与する。
　また、例えば、他外界センサ１２７は、点群を直接観測するセンサであり、取得される点群のうち、路面より高い位置の点群を自己地図生成部１４８に出力する。
　点群生成部１４３で得られる点群データは、前述の路面点群と区別するために、有高点群と呼称する。カメラ１２１～１２４及びステレオカメラ１２５の観測に基づく路面点群と有高点群を比較した場合、観測距離が同一であれば、路面点群のほうが高精度である特徴がある。また、この三次元座標は、各センサ座標系における観測値と同様に、センサからの相対座標値で表される。
　自己地図生成部１４８は、ランドマーク検出部１４２及び点群生成部１４３の双方から得られ、センサの相対座標値で表される点群情報を、オドメトリ推定部１４１から得られた走行状況１６２の車両運動情報、及びＲＯＭ１１１に格納されたセンサパラメータ１５０を用いて世界座標に変換し、変換された点群情報を時系列的に合成して点群地図を生成する。世界座標値は、ある座標及びある軸を基準とした座標値である。例えば、車載処理装置１０１が起動した位置を原点、最初に進んだ方向をＸ軸、Ｘ軸に直交する方向をＹ軸及びＺ軸の様に定義してもよい。
　自己地図生成部１４８で得られた地図は、自己生成地図１６１としてＲＡＭ１１２に格納される。次時刻以降は、ＲＡＭ１１２から自己生成地図１６１を読み出し、新たにランドマーク検出部１４２及び点群生成部１４３から得られた点群を座標変換し、自車の運動情報を用いて時系列的に合成する。
　地図生成モード２０２では、自己地図生成部１４８で処理終了となり、次時刻の入力を待つ。自己位置推定モード２０３では、自己地図生成部１４８は、自己生成地図１６１及び走行状況１６２を、位置推定判定部１４４及び走行状況診断部１４５に出力する。インターフェース１８０を介して地図記録の指示がある場合、生成された自己生成地図及び走行状況を自己生成地図１５１及び走行状況１５２として記録部１１３に記録する。自己地図生成部１４８の処理フローは、図７を参照して後述する。
　統合診断部１４０は、位置推定判定部１４４、走行状況診断部１４５、及び統合判定部１４６を有する。統合診断部１４０は、位置推定判定部１４４による判定結果、及び走行状況診断部１４５による診断結果を統合判定部１４６で統合して、位置推定に使用するデータを決定する。
　位置推定判定部１４４は、後述する自己位置推定部１４９の推定結果について、大局的な位置推定に成功しているか否かを、点群によって判定する。大局的な位置推定に成功しているか否かについては、図３を参照して後に詳述するが、自己位置推定結果に大きな（例えばメートルレベルの）位置誤差がなく、ランドマーク等の大きなズレがない状態を指す。大局的な位置推定に成功しているか否かは、例えば、照合時の対応点群の距離誤差の総和が所定値以下になっているか、照合時の対応点群の距離の最大値が所定値以下になっているか、自車移動量差分と自己位置推定差分の差が所定値以下かなどの指標や、それらの組み合わせによって判定できる。この判定結果に基づいて、位置ずれを補正してもよい。
　位置推定判定部１４４は、初回動作時は未照合状態のため動作せず、大局的な位置推定に失敗しているものとして判定し、その後は、後段の自己位置推定部１４９の照合結果を参照して、大局的な位置推定に成功しているかを判定し、判定結果を統合判定部１４６に送信する。
　走行状況診断部１４５は、自己地図生成部１４８による自己生成地図１６１の生成時の走行状況１６２及び記録部１１３に記録された過去の走行状況１５２を用いて、走行状況を診断する。走行状況は、各センサから得られる点群ごとに、精度の低下に影響がある走行状況であるか否かで判定され、判定結果を統合判定部１４６に送信する。例えば、カメラ１２１～１２４から、ランドマーク検出部１４２を介して得られる有高点群は、車両１０５の速度が高くなると、精度が低下する性質があり、地図生成時の速度が所定の速度以上か否かを判定する。
　また、例えば、ステレオカメラ１２５の場合、ステレオカメラの画角と観測距離の関係から、走行状況１６２が、切り返し動作のような、観測方向を短時間で大きく変える車両挙動であれば、地図生成時とは観測対象が変化しており、点群照合時にノイズとなって精度を低下する可能性がある。ただし、切り返し動作でも、地図生成時と同等の車両の動きであれば、過去地図生成時と現在の観測点群に大きな差が生じないので、精度の低下は小さい。
　また、例えば、ソナー１２６の場合、速度が変化すると点群の粗密が変化するため、地図生成時と速度が異なる場合は、精度低下の一因となる。この例に限らず、各センサから得られる点群の性質を考慮し、その性質に関する車両挙動について、条件を満たしているか否かを判定する。
　統合判定部１４６は、位置推定判定部１４４の判定結果、及び走行状況診断部１４５の診断結果から統合的に状態を判定し、精度向上のために選択すべき点群データを判定する。位置推定判定部１４４の判定結果、及び走行状況診断部１４５の診断結果に基づく判定方法は、図５を参照して説明する。統合判定部１４６は、判定結果をデータ選択部１４７に送信する。
　統合診断部１４０は、位置推定判定部１４４、走行状況診断部１４５、統合判定部１４６から構成される診断機能を纏めた機能ブロックである。この構成は一例であり、例えば、位置推定判定部１４４だけから構成されても、走行状況診断部１４５だけから構成されても、他の診断方法で構成されてもよい。なお、統合診断部１４０を位置推定判定部１４４のみで構成する場合の判定要素の例は、図６を参照して説明する。
　データ選択部１４７は、統合診断部１４０の診断結果に基づいて、各センサごとの点群データを選定し、自己位置推定部１４９に送信する。
　自己位置推定部１４９は、現在走行中の状況において自己地図生成部１４８から得られた自己生成地図１６１と、記録部１１３に格納された、過去に生成した自己生成地図１５１を照合して、自己生成地図１５１上の自己位置を推定する。照合に使用される点群は、データ選択部１４７から送信された点群である。照合は、例えば、既知の点群マッチング技術であるＩＣＰ（Iterative Closest Point）アルゴリズムを利用できる。これにより、現在走行中の自己生成地図１６１から過去に生成した自己生成地図１５１への座標変換量を計算でき、座標変換された自己生成地図１６１における現在車両の位置から、自己生成地図１５１上の自己位置を推定できる。自己位置推定部１４９の処理フローは、図８を参照して後述する。
　図３を参照して大局的な位置推定の成否について説明する。図３（ａ）及び図３（ｂ）は、大局的な位置推定成功前の状態を示し、図３（ｃ）は大局的な位置推定成功後の状態を示す。記録部１１３に記録された過去の自己生成地図１５１の点群３０１に対して、走行中に生成された現在観測している自己生成地図１６１の点群３０２について、例えばＩＣＰアルゴリズム等を持ちいて照合して推定自己位置３０３を推定する。図３（ｂ）に示すように、狭い範囲における局所的な位置推定も広い範囲における大局的な位置推定のいずれにも成功していない状態がある。また、図３（ａ）に示すように、ＩＣＰアルゴリズムを開始する初期照合位置や、スライドしても類似した点群配置になっている等の要因によって、狭い範囲における局所的な位置推定は成功していても、広い範囲における大局的な位置推定は成功していない状態となり、本来の点群位置からずれて、結果的に自己位置推定を誤る場合がある。また、ＩＣＰアルゴリズムは対応点群間の距離誤差の総和を最小化するように動作するが、ＩＣＰアルゴリズムを開始する初期照合位置によっては、全体的には照合が失敗して、周辺位置の中では誤差が最も小さい位置に点群が照合されて、自己位置推定を誤る場合がある。
　大局的な位置推定に成功している場合は、広範囲の点群、すなわち遠方点群の利用によって、ある程度（例えば、数十ｃｍ程度）の位置推定誤差が残るものの、メートルレベルの誤差はなく、図３（ｃ）に示すように、ある程度正しい位置が推定できる。位置推定判定部１４４は、点群の状態等から大局的な位置推定に成功しているか否かを判定する。大局的な位置推定に成功しているか否かの状態を出力する。
　図４は、カメラ１２１～１２４、ステレオカメラ１２５、ソナー１２６の取り付け状態と観測範囲と区分を示す図である。
　ステレオカメラ１２５は画角が狭く、比較的遠方の観測範囲４０１～４０３を観測できる。カメラ１２１～１２４及びソナー１２６は、車両１０５の周囲に取り付けられており、比較的近傍の観測範囲４０４を観測する。各センサの取り付け位置や観測範囲は一例である。カメラ１２１～１２４及びステレオカメラ１２５は、路面点群と有高点群を観測する。カメラ１２１～１２４及びステレオカメラ１２５によって観測される点群は、路面点群及び有効点群ともに、車両１０５の近傍では高精度で、車両１０５から遠方では低精度となる性質を有する。従って、自己位置推定精度の向上には、車両１０５の近傍で観測された点群を使った自己位置推定が望ましい。しかし、車両１０５の近くで観測された点群だけを使って自己位置を推定すると、図３（ａ）及び図３（ｂ）に示すように、大局的な位置推定に失敗する場合がある。
　一方、遠方の点も使用すると照合が全体的に補正され、図３（ｃ）に示すように、大局的な位置推定に成功する可能性が高まる。従って、大局的な位置推定前は遠方の点も使用し、大局的な位置推定後は、精度が悪い遠方の点を除去して、近傍の点を使用した自己位置推定が望ましい。
　ステレオカメラ１２５は、比較的遠方まで観測できるため、近点群４０１、中点群４０２、遠点群４０３のように区分して、統合判定部１４６の判定結果に応じて点群を使い分けるとよい。これは一例であり、３区分でなくてもよい。
　また、路面点群と有高点群では、カメラ１２１～１２４及びステレオカメラ１２５ともに、路面点群の方が高精度であるため、統合判定部１４６での判定時の使い分けで考慮する。
　ステレオカメラ１２５では、近路面点群、中路面点群、遠路面点群、近有高点群、中有高点群、遠有効点群に観測点群を大別するとよい。カメラ１２１～１２４では、観測範囲が狭いため距離で分けず、路面点群と有高点群で区別するとよい。これは一例であり、カメラ１２１～１２４でも距離によって区分し、統合判定部１４６で使い分けてもよい。この場合も、路面点群及び有効点群ともに、車両１０５からの距離が近いほど高精度であるという性質を有する。ソナー１２６から得られる点群は有高点群であるが、有効点群には線ラベルと点ラベルが付与されているので、統合判定部１４６で線ラベルの有効点群と点ラベルの有効点群とを使い分けるとよい。
　図５は、統合判定部１４６の判定要素の一例を示す図である。
　位置推定判定部１４４で判定される大局的位置推定の成否と、走行状況診断部１４５で判定される走行状態を元に、各センサごとに、選択すべき点群を決定する。図５（ａ）がステレオカメラ１２５に関する判定要素、図５（ｂ）がカメラ１２１～１２４に関する判定要素、図５（ｃ）及び図５（ｄ）がソナー１２６に関する判定要素である。
　図５（ａ）に示すステレオカメラ１２５に関する判定では、切り返し動作のような動きで精度低下が発生しやすいが、地図生成時の走行状況１５２と現在走行中の走行状況１６２が同じであれば、精度低下は発生しにくい。従って、地図生成時の走行状況１５２と現在走行中の走行状況１６２の走行軌跡差が予め定めた所定の値より小さいときと、大きいときで区別する。一般に、直線走行時は軌跡差が小さくなり、直線と曲線が混じる走行では軌跡差が大きくなる。走行軌跡差は、走行軌跡の所定の距離又は時間で区間に分け、分けられた区間毎に軌跡の差を判定するとよい。また、大局的自己位置推定前は、大局的位置推定を誤らないために、広範囲の点を使用し、大局的自己位置推定後は、高精度点のみを使用して局所的に位置を推定した方が高精度な位置推定が可能となる。これらを鑑みて、走行軌跡差小かつ大局的位置推定前は、近・中・遠有高点群及び近・中・遠路面点群を選択し、走行軌跡差小かつ大局的位置推定後は、近有高点群、近路面点群を選択し、走行軌跡差大かつ大局的位置推定前は、近・中有高点群、及び近・中路面点群を選択し、走行軌跡差大かつ大局的位置推定後は、近路面点群を選定する。
　図５（ｂ）に示すカメラ１２１～１２４に関する判定では、速度が高い場合に、モーションステレオの性質から有高点群の精度が低下する。路面点群は速度の影響を受けず、有高点群と比較しても高精度である。また、大局的自己位置推定前は、大局的位置推定を誤らないために広範囲の点を使用し、大局的自己位置推定後は、高精度点のみを使用して局所的に位置を推定した方が高精度な位置推定が可能となる。これらを鑑みて、低速かつ大局的位置推定前は、有高点群、路面点群を選択し、低速かつ大局的位置推定後は、路面点群のみを選択し、高速かつ大局的位置推定前は、路面点群のみを利用し、高速かつ大局的位置推定後は、路面点群のみを利用する。
　図５（ｃ）（ｄ）に示すソナー１２６に関する判定では、地図生成時の走行状況１５２と現在走行中の走行状況１６２の速度差が大きいときに、点群の粗密が変化し、精度が低下する。但し、線については、粗密の変化の影響を受けにくい。また、ソナー１２６の場合、観測距離によって精度が大きく変動することはない。これらを鑑みて、速度差小かつ大局的位置推定前は、点、及び線を選択し、速度差小かつ大局的位置推定後は、点、及び線を選択し、速度差大かつ大局的位置推定前は、線を選択し、速度差大かつ大局的位置推定前は、線を選択する。また、ソナー１２６は、音波を用いるセンサであるため、気温変化により精度が変動する。従って、気温差小かつ大局的位置推定前は、点、及び線を選択し、気温差小かつ大局的位置推定後は、点、及び線を選択し、気温差大かつ大局的位置推定前は、線を選択し、気温差大かつ大局的位置推定前は、線を選択する。
　これらの判定方法は一例であり、これ以外の組み合わせや点群選定も使い方に合わせて設定してよい。また、走行状況は天候や日照条件等も含めてもよい。また、判定条件を区分する段階も２段階でなく、３以上の多段階でもよい。
　図６は、統合診断部１４０の判定要素の一例を示す図であり、統合診断部１４０が位置推定判定部１４４のみで構成される場合の判定要素例を示す。
　統合診断部１４０が位置推定判定部１４４のみで構成される場合、位置推定判定部１４４で判定される大局的位置推定の成否のみに基づいて、各センサ毎に、選択すべき点群を決定する。図６（ａ）がステレオカメラ１２５に関する判定要素、図６（ｂ）がカメラ１２１～１２４に関する判定要素、図６（ｃ）がソナー１２６に関する判定要素である。図６（ａ）（ｂ）（ｃ）ともに大局的位置推定前は広範囲の点を活用し、大局的位置推定後は、精度が高い点のみを活用する。
　図７は、自己地図生成部１４８の動作を示すフローチャートである。自己地図生成部１４８は、ランドマーク検出部１４２、点群生成部１４３から実行指令を受けると、以下の動作を実行する。以下に説明する各ステップの実行主体はＣＰＵ１１０である。
　ランドマーク取得ステップ７０１では、ランドマーク検出部１４２でカメラ１２１～１２４及びステレオカメラ１２５の出力から生成された路面点群をＲＡＭ１１２から取得する。路面点群は各センサ座標系で表現されている。次のステップ７０２に進む。
　点群取得ステップ７０２では、点群生成部１４３で生成された各センサの有高点群をＲＡＭ１１２から取得する。有高点群は、各センサ座標系で表されている。次のステップ７０３に進む。
　座標変換ステップ７０３では、ランドマーク取得ステップ７０１で取得した路面点群と、点群取得ステップ７０２で取得した有高点群の座標系を、車両１０５の座標系に変換する。変換にはＲＯＭ１１１に格納されている各センサ毎のセンサパラメータ１５０を用いる。座標変換した点群を地図生成ステップ７０４に出力し、次のステップ７０４に進む。
　地図生成ステップ７０４では、オドメトリ推定部１４１から得られる車両運動情報を用いて、座標変換ステップ７０３で得られた座標点群を世界座標に変換し、ＲＡＭ１１２に格納された前時刻の点群地図との時系列的な合成によって、点群地図を自己生成する。自己生成した地図、及び、オドメトリ推定部１４１から得られる車速、舵角、車両運動情報、及び車両運動を積分した車両運動軌跡等の走行状況を、点群地図出力ステップ７０５に出力し、次のステップ７０５に進む。
　点群地図出力ステップ７０５では、地図生成ステップ７０４で生成された点群地図を、自己生成地図１６１としてＲＡＭ１１２に、又は自己生成地図１５１として記録部１１３に出力する。同様に、地図生成ステップ７０４から得られた走行状況を　走行状況１６２としてＲＡＭ１１２に、又は走行状況１５２として記録部１１３に出力し、処理を終了する。地図生成モード２０２の場合、基本的には、自己生成地図１６１及び走行状況１６２をＲＡＭ１１２に出力し、その後、センサ値取得部２０１がセンサ値を取得する。インターフェース１８０を介して、ユーザから地図の記録指示を受けた場合、地図生成ステップ７０４で生成された点群地図を自己生成地図１５１及び走行状況１５２として、記録部１１３に出力し、処理を終了する。自己位置推定モード２０３の場合、位置推定判定部１４４及び走行状況診断部１４５が処理を開始する。
　図８は、自己位置推定部１４９の動作を示すフローチャートである。自己位置推定部１４９は、データ選択部１４７から実行指令を受けると、以下の動作を実行する。以下に説明する各ステップの実行主体はＣＰＵ１１０である。本処理は、自己位置推定モード２０３のみで実行される。
　過去地図情報取得ステップ８０１では、記録部１１３に格納された自己生成地図１５１を取得する。自己生成地図１５１は、過去の走行により取得された地図である。次のステップ８０２に進む。
　現在地図情報取得ステップ８０２では、統合判定部１４６によって判定され、データ選択部１４７によって選択された、現在走行中に観測された自己生成地図１６１の選択点群をＲＡＭ１１２から取得する。次のステップ８０３に進む。
　地図照合ステップ８０３では、過去地図情報取得ステップ８０１で取得した過去の自己生成地図１５１と、現在地図情報取得ステップ８０２で取得した現在走行中に観測された自己生成地図１６１から、精度向上の観点で統合判定部１４６及びデータ選択部１４７で選択された点群の照合によって、過去の自己生成地図１５１上の自己位置を計算する。点群の照合は、例えば、既知の点群マッチング技術であるＩＣＰ（Iterative Closest Point）アルゴリズムを利用できる。ＩＣＰアルゴリズムでは、各点同士の対応関係を計算し、計算された対応関係における距離誤差を最小化する処理を繰り返して、点群同士を照合する。この地図同士の照合によって、自己地図生成部１４８から得られた自己生成地図１６１を自己生成地図１５１上に変換する座標変換量を計算でき、座標変換された現在車両の位置から、自己生成地図１５１上の自己位置を得ることができる。自己生成地図１５１上の自己位置を出力して図８に示す処理を終了する。終了後、センサ値取得部２０１が処理を開始する。
　以下、本発明の実施例２を説明する。実施例２では、前述した実施例１との相違点を主に説明し、実施例１と同じ構成及び処理には同じ符号を付し、それらの説明は省略する。
　図９は、本発明の実施例２の車両１０５の構成を示すブロック図である。
　車両１０５は、地図生成・自己位置推定装置１００と、車両１０５における自動運転を制御する車両制御装置群１７０～１７３とを有する。地図生成・自己位置推定装置１００は、カメラ１２１～１２４と、ステレオカメラ１２５と、ソナー１２６と、他外界センサ１２７と、車速センサ１３１と、舵角センサ１３２と、インターフェース１８０と、ＧＮＳＳ受信機１８１と、通信装置１８２と、表示装置１８３を有する。カメラ１２１～１２４と、ステレオカメラ１２５と、ソナー１２６と、他外界センサ１２７と、車速センサ１３１と、舵角センサ１３２と、インターフェース１８０と、ＧＮＳＳ受信機１８１と、通信装置１８２と、表示装置１８３と、車両制御装置群１７０～１７３は、車載処理装置１０１と信号線で接続され、車載処理装置１０１と各種データ授受する。
　ＧＮＳＳ受信機１８１は、衛星航法システム（例えばＧＰＳ）を構成する複数の衛星から送信された信号を受信し、受信した信号を用いた演算によってＧＮＳＳ受信機１８１の位置、すなわち緯度及び経度を計算する。なお、ＧＮＳＳ受信機１８１が計算する緯度及び経度の精度は低くてもよく、例えば数ｍ～１０ｍ程度の誤差が含まれてもよい。ＧＮＳＳ受信機１８１は、計算した緯度及び経度を車載処理装置１０１に出力する。
　通信装置１８２は、車両１０５の外部の機器と車載処理装置１０１とが無線を介して情報を授受するために用いられる通信装置である。例えば、ユーザが車両１０５の外にいるときに、ユーザが身に着けている携帯端末と通信を行い、情報を授受する。通信装置１８２が通信を行う対象はユーザの携帯端末に限定されず、自動運転に関するデータを提供する装置でもよい。
　表示装置１８３は、例えば液晶ディスプレイであり、車載処理装置１０１から出力される情報を表示する。
　車両制御装置１７０は、車載処理装置１０１から出力される情報、例えば、自己位置推定部１４９から出力される、自己生成地図１５１上の現在の自己位置に基づいて、操舵装置１７１、駆動装置１７２及び制動装置１７３の少なくとも一つを制御する。操舵装置１７１は、車両１０５のステアリングを操作する。駆動装置１７２は、車両１０５に駆動力を与える。駆動装置１７２は例えば、車両１０５が備えるエンジンやモータの目標回転数を増減して車両１０５の駆動力を増減する。制動装置１７３は、車両１０５に制動力を与えて車両１０５を減速する。
　車両１０５において、車載処理装置１０１は、記録部１１３に格納された自己生成地図１５１を、通信装置１８２を介してサーバ等に格納してもよい。また、車載処理装置１０１は、他の車両１０５に搭載されてサーバに格納された自己生成地図１５１を、通信装置１８２を介して記録部１１３に格納し、自己位置推定に活用してもよい。さらに、車載処理装置１０１は、ＧＮＳＳ受信機１８１の各時刻の受信位置を自己生成地図１６１に付しておき、自己位置推定する際には、現在のＧＮＳＳによる位置情報と近い値が付された地図を検索して、照合する地図を選択してもよい。ＧＮＳＳ信号が受信できない場合、オドメトリ推定部１４１が、直近でＧＮＳＳによって観測された絶対位置からの差から絶対位置を推定してもよい。
　図１０は、地図生成・自己位置推定装置１００による地図共有を示す図である。
　自車両１０５及び他車両１１０３は、通信装置１８２を有する地図生成・自己位置推定装置１００を有する。自車両１０５は通信装置１８２を用いて、通信回線を介して、クラウド上のサーバ１１０１に接続された外部記録装置１１０２に、自己生成地図１５１及び走行状況１５２を記録する。他車両１１０３は、通信装置１８２を用いて、クラウド上のサーバ１１０１に接続された外部記録装置１１０２から、車両１０５が生成した自己生成地図１５１及び走行状況１５２を取得し、他車両１１０３の地図として活用する。また、他車両１１０３が生成した自己生成地図１５１及び走行状況１５２を自車両１０５が活用してもよい。
　以上に説明したように、本実施例の車載処理装置１０１は、過去に生成された過去生成地図を記憶する過去地図記憶部（記録部１１３）と、走行中に取得したセンサデータに基づいて走行中地図を生成する自己地図生成部１４８と、過去生成地図と走行中地図とを照合して自己位置を推定する自己位置推定部１４９と、走行中地図に含まれるデータの特性に基づいて、自己位置推定部１４９が使用するデータを走行中地図から選択するデータ選択部１４７とを備えるので、カメラが撮影した画像から生成される点群の精度や、点群を用いて位置推定時の局所適合の発生を考慮しつつ、最適な点群を選定して自己位置を推定し、自己位置を高精度に推定できる。
　また、自己位置推定部１４９は、過去生成地図と走行中地図との照合範囲が広い第１の照合（大局的照合）と照合範囲が狭い第２の照合（局所的照合）とを実行可能であり、データ選択部１４７は、第１の照合と第２の照合とで異なるデータを選択するので、遠くの点も含めて全体として誤差を小さくする大局的照合と、センシング精度が高い点を用いた局所的照合によって、自己位置を高精度に推定できる。
　また、自己位置推定部１４９は、第１の照合後に第２の照合を実行するので、大局的照合の前に局所的照合を行うことによる自己位置を間違った推定を防止して、自己位置を高精度に推定できる。
　また、地図生成時の走行状況と自己位置の推定結果に基づいて、位置推定に用いるデータの種別を特定する統合判定部１４６を備え、走行状況は、過去生成地図生成時の走行状況と現在の走行状況を含むので、自己位置の推定精度を向上できる。
　また、自己地図生成部１４８は、ＧＮＳＳ及びオドメトリによって推定した絶対座標に変換された走行中地図を生成し、さらに、地図を格納するサーバ１１０１と通信回線を介して接続されており、他車両１１０３が使用可能とするために、走行中地図をサーバ１１０１に送信し、他車両１１０３が生成した地図を過去生成地図として使用するので、多くの車両で地図を共有でき、広範囲な地図を使用できる。
　また、自己位置推定部１４９は、推定された自己位置を車両制御装置１７０に出力し、車両制御装置１７０は、車載処理装置１０１から出力された自己位置に基づいて、操舵装置１７１、駆動装置１７２及び制動装置１７３の少なくとも一つを制御するので、自ら生成した地図に基づいて自動運転や運転支援を実現できる。
　なお、本発明は前述した実施例に限定されるものではなく、添付した特許請求の範囲の趣旨内における様々な変形例及び同等の構成が含まれる。例えば、前述した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに本発明は限定されない。本発明の技術的思想の範囲内で考えられるその他の態様も本発明の範囲内に含まれる。また、ある実施例の構成の一部を他の実施例の構成に置き換えてもよい。また、ある実施例の構成に他の実施例の構成を加えてもよい。また、各実施例の構成の一部について、他の構成の追加・削除・置換をしてもよい。
　また、地図生成・自己位置推定装置１００が不図示の入出力インターフェースを備え、必要なときに入出力インターフェースと利用可能な媒体を介して、他の装置からプログラムを読み込んでもよい。媒体とは、例えば入出力インターフェースに着脱可能な記憶媒体、又は通信媒体、すなわち有線、無線、光などのネットワーク、又は当該ネットワークを伝搬する搬送波やディジタル信号である。
　また、前述した各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等により、ハードウェアで実現してもよく、プロセッサがそれぞれの機能を実現するプログラムを解釈し実行することにより、ソフトウェアで実現してもよい。また、プログラムにより実現される機能の一部又は全部がハードウェア回路やＦＰＧＡにより実現されてもよい。
　各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリ、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）等の記憶装置、又は、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に格納することができる。
　また、制御線や情報線は説明上必要と考えられるものを示しており、実装上必要な全ての制御線や情報線を示しているとは限らない。実際には、ほとんど全ての構成が相互に接続されていると考えてよい。

Claims (10)

  1. 　車載処理装置であって、
    　過去に生成された過去生成地図を記憶する過去地図記憶部と、
    　走行中に取得したセンサデータに基づいて走行中地図を生成する自己地図生成部と、
    　前記過去生成地図と前記走行中地図とを照合して自己位置を推定する自己位置推定部と、
    　前記走行中地図に含まれるデータの特性に基づいて、前記自己位置推定部が使用するデータを前記走行中地図から選択するデータ選択部とを備えることを特徴とする車載処理装置。
  2. 　請求項１に記載の車載処理装置であって、
    　前記自己位置推定部は、前記過去生成地図と前記走行中地図との照合範囲が広い第１の照合と照合範囲が狭い第２の照合とを実行可能であり、
    　前記データ選択部は、前記第１の照合と前記第２の照合とで異なるデータを選択することを特徴とする車載処理装置。
  3. 　請求項２に記載の車載処理装置であって、
    　前記自己位置推定部は、前記第１の照合後に前記第２の照合を実行することを特徴とする車載処理装置。
  4. 　請求項１に記載の車載処理装置であって、
    　地図生成時の走行状況と前記自己位置の推定結果に基づいて、位置推定に用いるデータの種別を特定する統合判定部を備えることを特徴とする車載処理装置。
  5. 　請求項４に記載の車載処理装置であって、
    　前記走行状況は、前記過去生成地図生成時の走行状況と現在の走行状況を含むことを特徴とする車載処理装置。
  6. 　請求項１に記載の車載処理装置であって、
    　前記自己地図生成部は、ＧＮＳＳ及びオドメトリによって推定した絶対座標に変換された走行中地図を生成することを特徴とする車載処理装置。
  7. 　請求項６に記載の車載処理装置であって、
    　地図を格納するサーバと通信回線を介して接続されており、
    　他車両が使用可能とするために、前記走行中地図を前記サーバに送信し、
    　他車両が生成した地図を前記サーバから受信して、前記過去生成地図として使用することを特徴とする車載処理装置。
  8. 　請求項１から７のいずれか一つに記載の車載処理装置であって、
    　前記自己位置推定部は、前記推定された自己位置を車両制御装置に出力することを特徴とする車載処理装置。
  9. 　車両制御装置であって、
    　請求項８に記載の車載処理装置から出力された自己位置に基づいて、操舵装置、駆動装置及び制動装置の少なくとも一つを制御することを特徴とする車両制御装置。
  10. 　車載処理装置が自己位置を推定する自己位置推定方法であって、
    　前記車載処理装置は、所定の演算処理を実行する演算装置と、前記演算装置がアクセス可能な記憶装置とを有し、
    　前記記憶装置は、過去に生成された過去生成地図と、走行中に取得したセンサデータに基づいて生成された走行中地図とを格納し、
    　前記自己位置推定方法は、
    　走行中に取得したセンサデータに基づいて走行中地図を生成する走行中地図生成手順と、
    　前記過去生成地図と前記走行中地図とを照合して自己位置を推定する自己位置推定手順と、
    　前記自己位置推定手順で使用されるデータの特性に基づいて、前記自己位置推定手順で使用されるデータを前記走行中地図から選択するデータ選択手順とを含むことを特徴とする自己位置推定方法。
PCT/JP2022/002692 2021-09-09 2022-01-25 車載処理装置、車両制御装置、及び自己位置推定方法 WO2023037570A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
DE112022002250.7T DE112022002250T5 (de) 2021-09-09 2022-01-25 Bordeigene verarbeitungsvorrichtung, fahrzeugsteuervorrichtung und verfahren zur schätzung der eigenen position

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2021146844A JP2023039626A (ja) 2021-09-09 2021-09-09 車載処理装置、車両制御装置、及び自己位置推定方法
JP2021-146844 2021-09-09

Publications (1)

Publication Number Publication Date
WO2023037570A1 true WO2023037570A1 (ja) 2023-03-16

Family

ID=85507288

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2022/002692 WO2023037570A1 (ja) 2021-09-09 2022-01-25 車載処理装置、車両制御装置、及び自己位置推定方法

Country Status (3)

Country Link
JP (1) JP2023039626A (ja)
DE (1) DE112022002250T5 (ja)
WO (1) WO2023037570A1 (ja)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018061084A1 (ja) * 2016-09-27 2018-04-05 日産自動車株式会社 自己位置推定方法及び自己位置推定装置
JP2018128314A (ja) * 2017-02-07 2018-08-16 富士通株式会社 移動体位置推定システム、移動体位置推定端末装置、情報格納装置、及び移動体位置推定方法
JP2020060496A (ja) * 2018-10-12 2020-04-16 パイオニア株式会社 情報処理装置
JP2021099280A (ja) * 2019-12-23 2021-07-01 フォルシアクラリオン・エレクトロニクス株式会社 位置推定装置、及び位置推定方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7356208B2 (ja) 2018-02-19 2023-10-04 ジオテクノロジーズ株式会社 自己位置推定装置、自己位置推定方法、および自己位置推定プログラム
JP7384717B2 (ja) 2020-03-18 2023-11-21 本田技研工業株式会社 乗員保護装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018061084A1 (ja) * 2016-09-27 2018-04-05 日産自動車株式会社 自己位置推定方法及び自己位置推定装置
JP2018128314A (ja) * 2017-02-07 2018-08-16 富士通株式会社 移動体位置推定システム、移動体位置推定端末装置、情報格納装置、及び移動体位置推定方法
JP2020060496A (ja) * 2018-10-12 2020-04-16 パイオニア株式会社 情報処理装置
JP2021099280A (ja) * 2019-12-23 2021-07-01 フォルシアクラリオン・エレクトロニクス株式会社 位置推定装置、及び位置推定方法

Also Published As

Publication number Publication date
JP2023039626A (ja) 2023-03-22
DE112022002250T5 (de) 2024-02-22

Similar Documents

Publication Publication Date Title
US10788830B2 (en) Systems and methods for determining a vehicle position
CN106352867B (zh) 用于确定车辆自身位置的方法和设备
CN109313031B (zh) 车载处理装置
EP3137850B1 (en) Method and system for determining a position relative to a digital map
EP2133662B1 (en) Methods and system of navigation using terrain features
US20210248768A1 (en) Generation of Structured Map Data from Vehicle Sensors and Camera Arrays
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法
EP3939863A1 (en) Overhead-view image generation device, overhead-view image generation system, and automatic parking device
WO2020146102A1 (en) Robust lane association by projecting 2-d image into 3-d world using map information
WO2018131258A1 (ja) 車載処理装置
KR102037129B1 (ko) 차량의 위치 보정 장치 및 방법과 이를 이용한 차량 위치 보정 시스템 및 무인 운행이 가능한 차량
CN110945578A (zh) 车载处理装置
US11754415B2 (en) Sensor localization from external source data
EP3994043A1 (en) Sourced lateral offset for adas or ad features
CN115135963A (zh) 用于在场景地图中生成3d参考点的方法
US11131552B2 (en) Map generation system
WO2023037570A1 (ja) 車載処理装置、車両制御装置、及び自己位置推定方法
EP4113063A1 (en) Localization of autonomous vehicles using camera, gps, and imu
Juang Map aided self-positioning based on lidar perception for autonomous vehicles
CN115917255A (zh) 基于视觉的位置和转弯标记预测
JP2020073931A (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2022153586A1 (ja) 自己地図生成装置、および自己位置推定装置
WO2023139935A1 (ja) 演算装置、自己位置推定装置、地図情報生成方法
RU2772620C1 (ru) Создание структурированных картографических данных с помощью датчиков транспортного средства и массивов камер
Janssen et al. Bootstrapping Computer vision and sensor fusion for absolute and relative vehicle positioning

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: 22866906

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 112022002250

Country of ref document: DE