WO2019208101A1 - 位置推定装置 - Google Patents

位置推定装置 Download PDF

Info

Publication number
WO2019208101A1
WO2019208101A1 PCT/JP2019/013964 JP2019013964W WO2019208101A1 WO 2019208101 A1 WO2019208101 A1 WO 2019208101A1 JP 2019013964 W JP2019013964 W JP 2019013964W WO 2019208101 A1 WO2019208101 A1 WO 2019208101A1
Authority
WO
WIPO (PCT)
Prior art keywords
image
imaging device
current position
error
virtual
Prior art date
Application number
PCT/JP2019/013964
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 US17/049,656 priority Critical patent/US11538241B2/en
Priority to DE112019001542.7T priority patent/DE112019001542T5/de
Publication of WO2019208101A1 publication Critical patent/WO2019208101A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/20—Scenes; Scene-specific elements in augmented reality scenes
    • 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/0967—Systems involving transmission of highway information, e.g. weather, speed limits
    • G08G1/096766—Systems involving transmission of highway information, e.g. weather, speed limits where the system is characterised by the origin of the information transmission
    • G08G1/096791—Systems involving transmission of highway information, e.g. weather, speed limits where the system is characterised by the origin of the information transmission where the origin of the information is another vehicle
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/161—Decentralised systems, e.g. inter-vehicle communication
    • G08G1/163—Decentralised systems, e.g. inter-vehicle communication involving continuous checking
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/166—Anti-collision systems for active traffic, e.g. moving vehicles, pedestrians, bikes

Definitions

  • the present invention relates to a technique for estimating the position of a moving body such as a robot or an automobile.
  • a moving body such as a robot and a car collects information around the mobile body, estimates the current position and traveling state of the moving body, and controls the traveling of the moving body.
  • Various sensors are used to detect information around the moving body.
  • sensors for measuring surrounding information include an imaging device such as a camera, a laser sensor, and a millimeter wave radar.
  • a GPS (Global Positioning System) or an IMU (Internal Measurement Unit) is used as a sensor for measuring the position of the moving body.
  • the position (self-position) of the moving body is estimated by integrating the speed or angular velocity of the moving body calculated by the IMU or using GPS positioning in a control device mounted on the moving body. To do.
  • a SLAM Simultaneous Localization and Mapping
  • position correction is essential.
  • the control device collects surrounding information with a laser sensor or a camera, and detects the position and shape of a three-dimensional object that serves as a reference for position estimation.
  • the current position of the moving object is corrected by comparing the position of the detected three-dimensional object with map information (map matching). Therefore, when the position error of the detected three-dimensional object is large, the position of the moving body cannot be corrected. Before correction, if the position error is already large, the current position cannot be corrected even if map matching is performed.
  • Non-Patent Document 1 creates a plurality of virtual current positions in order to estimate an accurate current position, and uses each virtual position, map information, internal and external parameters of the sensor, Create an intensity image and a parallax image.
  • Each created virtual intensity image and parallax image are matched with the actually acquired intensity image and parallax image, and a matching error is calculated by assigning a fixed weight to the matching result between each intensity image and parallax image.
  • the virtual position with the smallest matching error is set as the current position.
  • Non-Patent Document 1 when calculating a matching error from each virtual position, a fixed weight is applied to the matching result of the intensity image and the matching result of the parallax image. The reliability of the information acquired by the imaging device due to the error in the position is lowered, and the current position accuracy estimated by the matching is lowered.
  • the present invention has been made in view of the above points, and an object of the present invention is to provide a position estimation device capable of performing position estimation with high accuracy.
  • a position estimation device of the present invention is an imaging device mounted on a moving body, A position estimation device for estimating a current position of a moving object equipped with an imaging device, Current position estimating means for estimating a current position of the moving body; Virtual position creating means for creating a plurality of virtual positions based on the current position estimated by the current position estimating means; Virtual image creation means for creating each of a plurality of virtual images when it is assumed that images are taken at the plurality of virtual positions by the imaging device; Image matching means for comparing each of the plurality of virtual images with an image captured at the current position by the imaging device and calculating a respective comparison error; A weight is calculated based on at least one of the information acquired by the imaging device and the current position error information of the moving object obtained by the current position estimation means, and the respective comparisons are performed using the weight.
  • a weighting means for weighting the error; Position correcting means for correcting the current position estimated by the current position estimating means based on the weighted comparison error; It is characterized by
  • the block diagram of the position estimation apparatus of the moving body which concerns on one Embodiment of this invention.
  • the flowchart which shows the image processing procedure which an image process part performs.
  • the figure explaining an example of the virtual position creation method The figure explaining an example of the virtual image creation method.
  • the figure explaining an example of the method of calculating the weight by a calibration error The figure explaining an example of the method of calculating the weight by a parallax image error.
  • the figure explaining the method of calculating the weight by image intensity The figure explaining the method of calculating the weight by image resolution.
  • the figure explaining the method of calculating the weight by a feature point The figure explaining an example of the image matching method.
  • the figure explaining the Example of this invention The figure explaining the Example of this invention.
  • the figure explaining the Example of this invention Explanation of abnormal cases. Explanation of obstacle effects and countermeasures.
  • FIG. 1 is a configuration diagram of a position estimation apparatus 1 according to an embodiment of the present invention.
  • the position estimation device 1 estimates a current position of a moving body on which an imaging device is mounted, and is mounted on a moving body 100 such as an automobile or a robot.
  • the position estimation device 1 includes one or more imaging devices 12 (12a, 12b,... 12n) and an information processing device 13.
  • the imaging device 12 is, for example, a still camera or a video camera.
  • the imaging device 12 may be a monocular camera or a compound eye camera.
  • the information processing device 13 processes the image picked up by the image pickup device 12 and calculates the position or amount of movement of the moving body 100.
  • the information processing device 13 may perform display according to the calculated position or movement amount, or may output a signal related to the control of the moving body 100.
  • the information processing device 13 is, for example, a general computer, and includes an image processing unit 14 that processes an image captured by the imaging device 12, a control unit (CPU) 15 that performs processing based on the image processing result, and a memory 16. And a display unit 17 such as a display, and a bus 18 for connecting these components to each other.
  • the information processing device 13 performs the following processing when the image processing unit 14 and the control unit 15 execute predetermined computer programs.
  • the imaging device 12a is installed in front of the moving body 100, for example.
  • the lens of the imaging device 12 a is directed to the front of the moving body 100.
  • the imaging device 12a images, for example, a distant view in front of the moving body 100.
  • the other image pickup devices 12b,..., The image pickup device 12n are installed at different positions from the image pickup device 12a and pick up an image pickup direction or region different from that of the image pickup device 12a.
  • the imaging device 12b may be installed, for example, downward behind the moving body 100.
  • the imaging device 12b may capture a close view behind the moving body 100.
  • the imaging device 12 is a monocular camera
  • the pixel position on the image and the actual ground positional relationship (x, y) are constant, so the distance from the imaging device 12 to the feature point is geometrically determined. Can be calculated automatically.
  • the imaging device 12 is a stereo camera, the distance to the feature point on the image can be measured more accurately.
  • a camera having a monocular standard lens is used will be described. However, other cameras (such as a camera having a wide-angle lens or a stereo camera) may be used.
  • the objects that the imaging device 12a, the imaging device 12b,..., And the imaging device 12n image at a certain time may be different from each other.
  • the imaging device 12a may capture a distant view in front of the moving body 100.
  • a feature point such as a three-dimensional object or a landmark for position estimation may be extracted from an image obtained by capturing a distant view.
  • the imaging device 12b may image a close view such as a road surface around the moving body 100.
  • a white line around the moving body 100, road surface paint, or the like may be detected from an image obtained by capturing a near view.
  • the imaging device 12a, the imaging device 12b,..., The imaging device 12n may be installed on the moving body 100 under conditions that are not affected by environmental disturbances such as rain and sunlight.
  • the imaging device 12a may be installed forward in front of the moving body 100, whereas the imaging device 12b may be installed rearward or downward in the rear of the moving body 100.
  • the imaging device 12a may be installed forward in front of the moving body 100, whereas the imaging device 12b may be installed rearward or downward in the rear of the moving body 100.
  • the imaging device 12b may be installed rearward or downward in the rear of the moving body 100.
  • the imaging device 12b may be clear.
  • the imaging device 12a, the imaging device 12b,..., The imaging device 12n may shoot under different imaging conditions (aperture value, white balance, etc.). For example, by mounting an imaging device in which parameters are adjusted for a bright place and an imaging device in which parameters are adjusted for a dark place, it may be possible to take an image regardless of the brightness of the environment.
  • Imaging device 12a, imaging device 12b,... Imaging device 12n captures an image when receiving a command to start shooting from control unit 15, or at regular time intervals.
  • the data of the captured image and the imaging time are stored in the memory 16.
  • the memory 16 includes a main storage device (main memory) of the information processing device 13 and an auxiliary storage device such as a storage.
  • the image processing unit 14 performs various image processing based on the image data stored in the memory 16 and the imaging time.
  • image processing for example, an intermediate image is created and stored in the memory 16.
  • the intermediate image may be used for determination and processing by the control unit 15 in addition to processing by the image processing unit 14.
  • the bus 18 can be composed of IEBUS (Inter Equipment Bus), LIN (Local Interconnect Network), CAN (Controller Area Network), and the like.
  • IEBUS Inter Equipment Bus
  • LIN Local Interconnect Network
  • CAN Controller Area Network
  • the map 19 has information on the environment in which the moving body 100 travels (environment information storage means).
  • the map 19 stores, for example, information on the shape and position of a stationary object (a tree, a building, a road, a lane, a signal, a sign, a road surface paint, a road edge, etc.) in the traveling environment.
  • Each information of the map 19 may be expressed by a mathematical expression.
  • the line information is not composed of a plurality of points, and only the slope and intercept of the line may be used.
  • you may represent with the point cloud, without distinguishing the information of the map 19.
  • the point group may be represented by 3D (x, y, z), 4D (x, y, z, color), or the like.
  • the information on the map 19 may be in any form.
  • the image processing unit 14 projects map information on an image captured by the imaging device 12 based on the current position of the moving body 100 and the internal parameters of the imaging device 12 stored in the map 19 and the memory 16.
  • the image processing unit 14 identifies a plurality of position candidates of the moving body 100 based on the image captured by the imaging device 12, and the position of the moving body 100 based on the plurality of position candidates and the moving speed of the moving body 100. Is estimated.
  • the image processing unit 14 processes, for example, an image captured by the imaging device 12 while the moving body 100 is traveling, and estimates the position of the moving body 100. For example, the image processing unit 14 calculates the movement amount of the moving body 100 from the video image captured by the imaging device 12, and adds the movement amount to the past position to estimate the current position.
  • the image processing unit 14 may extract feature points from each frame image of the video image. The image processing unit 14 further extracts the same feature point in the subsequent and subsequent frame images. Then, the image processing unit 14 calculates the movement amount of the moving body 100 by tracking the feature points.
  • the control unit 15 may output a command regarding the moving speed to the moving body 100 based on the result of the image processing of the image processing unit 14. For example, the control unit 15 instructs or decreases the moving speed of the moving body 100 according to the number of pixels of the three-dimensional object in the image, the number of outliers among the feature points in the image, the type of image processing, or the like. Or a command to maintain may be output.
  • FIG. 2 is a flowchart showing an image processing procedure performed by the image processing unit 14.
  • the current position estimation unit 201 is a unit that estimates the current position of the moving body 100.
  • the current position estimating means 201 is constituted by, for example, a GPS that estimates an absolute position. Further, the current position estimating means 201 may be configured by odometry that estimates a relative position from a certain reference point. In the position estimation method based on the GPS or odometry described above, a position error occurs, but is corrected by a position correction unit 211 described later.
  • the environment information extraction unit 202 is a unit that extracts environment information around the moving body 100 acquired by the imaging device 12a, the imaging device 12b,.
  • the environment information extraction unit 202 acquires a grayscale image and a color image around the moving body 100 captured by the imaging device 12a, the imaging device 12b,. Moreover, you may acquire the parallax image produced from the image imaged with the imaging device 12a, the imaging device 12b, ... imaging device 12n. And the information of the feature point on the image imaged with the imaging device 12a, the imaging device 12b, ... imaging device 12n may be acquired. Moreover, you may acquire the line information on the image imaged with the imaging device 12a, the imaging device 12b, ... imaging device 12n.
  • the virtual position creation unit 203 is a unit that creates a virtual position based on the current position estimated by the current position estimation unit 201.
  • the number of virtual positions created by the virtual position creating means 203 is one or more and is not a fixed value but is changed according to the driving environment. Details of the virtual position creation means 203 will be described later.
  • the map reference means 204 is means for acquiring information from the map 19. Information is acquired from the map 19 based on a command from the control unit 15. All the information on the map 19 may be acquired by a command from the control unit 15. Further, only information around the mobile object 100 may be acquired.
  • the virtual image creation means 205 creates each virtual image using the map information acquired by the map reference means 204 and each virtual position created by the virtual position creation means 203.
  • the virtual image is an image when it is assumed that the image is captured from the virtual position by the imaging device. For example, when the map 19 ⁇ ⁇ ⁇ ⁇ is composed of a 3D (x, y, z) point group, each point is converted into a pixel (u, v). Further, the virtual image creating means 205 creates an image in accordance with the information on the map 19.
  • each point is converted into a pixel (u, v), and distance information is added to the pixel (u, v). Therefore, a virtual image composed of (u, v, distance) is used.
  • each point is composed of (u, v, color) by adding color information to the pixel (u, v). It may be a virtual image.
  • map 19 is composed of 3D (x, y, z) point groups, depth information is added to each pixel (u, v), and the map is composed of (u, v, depth).
  • a virtual parallax image may be used.
  • the color information is the intensity of the pixel.
  • the image is displayed in color or gray scale, and in the case of gray scale, the intensity of each pixel of the image is 0 to 255 (black to white), and in the case of color, R (0 to 255) + G (0 to 255) + B There are three channels (0 to 255). And depth information is the length of the Z-axis direction which is a detection direction of sensors, such as an imaging device.
  • the image matching unit 206 matches (compares) the image actually captured by the imaging device 12a, the imaging device 12b,... With the environment information extracting unit 202 with the virtual image created by the virtual image creating unit 205. ) Means.
  • the image matching unit 206 will be described later.
  • the conditional branch 209 checks whether the last virtual position has been created by the virtual position creation means 203. If the last virtual position has not been created, the process proceeds to virtual position creating means 203. If the last virtual position has been created, the process proceeds to weighting means 210.
  • the weighting means 210 is a means for weighting each matching result (comparison error) obtained by the image matching means 206.
  • the weighting unit 210 calculates a weight according to the traveling environment and the state of the sensor.
  • the weighting unit 210 calculates a weight based on at least one of the information acquired by the imaging device and the information on the current position error of the moving body 100 obtained by the current position estimating unit.
  • the information acquired by the imaging device includes the calibration error information of the imaging device, the feature point information on the image extracted from the image captured by the imaging device, and the image captured by the imaging device. At least one of the information on the parallax image error, the information on the image intensity acquired from the image captured by the imaging device, and the information on the image resolution acquired from the image captured by the imaging device.
  • the weighting unit 210 includes an imaging device calibration error estimation unit 210a, a feature point extraction unit 210b, a parallax image error estimation unit 210c, a current position error estimation unit 210d, an image intensity acquisition unit 210e, an image resolution acquisition unit 210f, and the like. .
  • the imaging device calibration error estimation unit 210a of the weighting unit 210 estimates the calibration errors of the imaging device 12a, the imaging device 12b,.
  • the above-described calibration is a step for correcting each distortion of the image pickup device 12a, the image pickup device 12b,..., And the image pickup device 12a, the image pickup device 12b,. Imaging is performed by the imaging device 12n.
  • the imaging device calibration error estimation unit 210 a obtains the aforementioned error and outputs the result to the weighting unit 210.
  • the feature point extraction means 210b of the weighting means 210 extracts feature points on the image captured by the imaging device 12a, the imaging device 12b,.
  • the above-described feature points are corners, edges, maximum values, minimum values, etc. on the image captured by the image capturing device 12a, the image capturing device 12b,.
  • the feature points extracted by the feature point extraction unit 210 b are output to the weighting unit 210.
  • the parallax image error estimation unit 210c of the weighting unit 210 estimates an error in creating a parallax image using images captured by the imaging device 12a, the imaging device 12b,.
  • the aforementioned parallax image error is, for example, an error caused by calibration for creating a parallax image.
  • When creating a parallax image it is necessary to accurately estimate the relative positions of the imaging device 12a, the imaging device 12b,..., The imaging device 12n, and if an error occurs, the error of the output parallax image increases. . Further, when creating a parallax image, it is necessary to search and search information captured in the imaging device 12a, imaging device 12b,... The error of the output parallax image becomes large.
  • the current position error estimating means 210d of the weighting means 210 estimates the position error estimated by the current position estimating means 201. For example, when the current position estimation unit 201 estimates the current position of the moving body 100 by GPS, an error output by the GPS is set as the current position error. Further, in the method of estimating the relative position from a certain reference, when the current position of the moving body 100 is estimated by the current position estimating unit 201, an error proportional to the travel distance may be used as the current position error.
  • the image intensity acquisition unit 210e of the weighting unit 210 is a unit that acquires the pixel intensity (image intensity) of each of the images acquired by the imaging device 12a, the imaging device 12b,. After acquiring the pixel intensity of the image acquired by the imaging device 12a, the imaging device 12b,..., The imaging device 12n, a search is made for pixels that are too dark below the lower limit and pixels that are too bright above the upper limit.
  • the image resolution acquisition means 210f of the weighting means 210 is means for acquiring the respective resolutions of the images acquired by the imaging device 12a, the imaging device 12b,. Details of the weighting means 210 will be described later.
  • the position correcting unit 211 is a unit that uses the weighting unit 210 to correct the current position estimated by the current position estimating unit 201. Among the matching errors (comparison errors) of the virtual images weighted by the weighting means 210, the minimum error is searched. Then, the virtual position having the minimum error is set as the current position.
  • FIG. 3 is a diagram for explaining an example of a virtual position creation method.
  • the details of the virtual position creation means 203 will be described with reference to FIG.
  • the coordinate system 301 uses the current position estimated by the current position estimating means 201 of the moving body 100 as the origin.
  • the virtual positions (302A, 302B,... 302N) are positions on the coordinate system 301, and the coordinates of the respective virtual positions are 303A, 303B,.
  • the estimated position error 304 is the position error of the moving body 100 estimated by the current position error estimating means 210d.
  • the range of the position error 304 is a range centered on the current position of the moving body 100 estimated by the current position estimation unit 201, and the size thereof changes according to the position error estimated by the current position error estimation unit 210d. To do.
  • the range R305 is a range where a virtual position is created.
  • the virtual position creating unit 203 creates a plurality of virtual positions based on the current position of the moving body 100 estimated by the current position estimating unit 201. Since the number N of virtual positions created by the virtual position creating means 203 depends on the traveling environment and situation of the moving body 100, the range R305 is set based on the position error 304 estimated by the current position error estimating means 210d.
  • the virtual position is a position and orientation set within a predetermined range centered on the estimated current position of the mobile object.
  • the mutual interval k306 between the virtual positions is an interval obtained by dividing the current position 301 to the virtual position range R305 estimated by the current position estimation unit 201 of the moving body 100 by a predetermined number.
  • the interval k306 is adjusted with the obtained positional accuracy and the accuracy of the imaging device 12a, imaging device 12b,.
  • the position accuracy estimated by the position estimation unit 211 is R
  • a virtual position is created using a preset interval k, but k may be adjusted based on the position estimation accuracy required for the traveling environment. For example, when the position accuracy specified by the position estimation unit 211 is R, the position accuracy corrected by the position correction unit 211 using the interval k is k.
  • FIG. 3 shows XY coordinates, but a virtual position may be created using Z, roll, pitch, and yaw (6 DOF).
  • a virtual position is created according to the error of each dimension.
  • the moving body 100 is a one-lane road and the white line detection function is mounted on the moving body 100, the error in the Y direction of the moving body 100 is reduced. It is not necessary to create a virtual position. Further, different intervals k306 in the X direction and Y direction may be set, and the number of virtual positions in each direction may be adjusted. Further, when the environment in which the moving body 100 travels is less uneven, the virtual positions of the Z direction, pitch, and roll are not created, and only X, Y, and yaw are accurately estimated in a short processing time.
  • the direction of the vehicle at the virtual position may be set using a certain range ⁇ and an interval K obtained by dividing the range ⁇ .
  • FIG. 4 is a diagram illustrating an example of a virtual image creation method.
  • the virtual image creation means 205 will be described with reference to FIG.
  • the map information 401, 402, 403 is information described on the map 19.
  • the virtual image 404A is an image when it is assumed that the imaging device 12a, the imaging device 12b,... 12n are imaged from the virtual position 302A, and the virtual image 404N is the imaging device 12a, the imaging device 12b, ... An image when it is assumed that the image is taken by the imaging device 12n.
  • the map information 401, 402, 403 is displayed on the virtual image 404A.
  • Information 401A, 402A, and 403A are displayed, and information 401N, 402N, and 403N are displayed on the virtual image 404N.
  • the information of the map 19 is three-dimensional (x, y, z), so the virtual image 404A and the virtual image 404N are displayed. (U, v, distance) or (u, v, depth). Also, if the position (x, y, z) and the color information of the driving environment are described in the map information 19, the information on the map 19 is four-dimensional (x, y, z, color), so the virtual image 404A. And the virtual image 404N can be represented by (u, v, color).
  • the processing time becomes long when a virtual image is created using all information. Therefore, only information within a predetermined threshold (height, distance, etc.) from the current position of the moving body 100 may be converted into an image. Further, since the imaging range of the imaging device 12a, imaging device 12b,..., The imaging device 12n is determined, a virtual image may be created using information within the imaging range.
  • FIG. 5A is a diagram for explaining an example of a method for calculating a weight due to a calibration error, and represents weighting using calibration parameters of the imaging device 12a, the imaging device 12b,.
  • the imaging device calibration error estimation means 210a estimates the calibration errors of the imaging device 12a, imaging device 12b,.
  • the pattern 500 is a pattern captured by the imaging device 12a, the imaging device 12b,.
  • An image 501 is an image acquired before the imaging device 12a, the imaging device 12b,. Since the acquired image is distorted before calibration and the pattern 500 is imaged by the imaging device 12a, the imaging device 12b,.
  • an image 502 represents an image corrected by a conventional calibration technique.
  • the image 502 captured by the imaging device 12a, the imaging device 12b,..., The imaging device 12n is the same as the actual pattern 500.
  • the corner positions of the pattern 500 are arranged at equal intervals. Therefore, the corrected corner position can be estimated, and the estimated corner position is defined as a pixel 503.
  • the actual position on the corner image 502 is the pixel 504
  • an error E505 from the estimated pixel 503 occurs. Therefore, an error map including errors E1, E2,... ET is calculated by calculating an error between the estimated pixel position and the actual position for each pixel of the imaging device 12a, imaging device 12b,. 506 (information on calibration error) is obtained.
  • a weight ⁇ c is added from the equation (1) and the error map 506 described above.
  • the parameter I increases.
  • a ratio of the parameter I to all pixels (U ⁇ V) is set as a weight ⁇ c.
  • the calibration may be performed periodically while the mobile object 100 is traveling, for example, and the weight may be calculated every time calibration is performed.
  • FIG. 5B is a diagram illustrating an example of a method for calculating a weight due to a parallax image error. 5B will be used to explain weighting using parameters when a parallax image is created by the imaging device 12a, the imaging device 12b,...
  • the image 507L is a left image for creating a parallax image
  • the image 507R is a right image for creating a parallax image.
  • a search is performed using pixels of the image 507R having the same information as the pixel information of the image 507L.
  • Conventional parallax image creation technology searches by pixel intensity and matches the pixel with the closest intensity.
  • a graph 509 is obtained.
  • the vertical axis C of the graph 509 is an error amount obtained by comparing the pixels of the left and right images.
  • Let 508R be the pixel with the smallest error (Cmin510).
  • Cmin510 the minimum error amount obtained for each pixel of the image 507L
  • Cthreshold the threshold value
  • the ratio of the parameter J to all pixels (U ⁇ V) is defined as a weight ⁇ s. As the parallax image error decreases, the weight ⁇ s increases.
  • FIG. 5C is a diagram illustrating a method for calculating a weight based on image intensity. The weighting based on the pixel intensity will be described with reference to FIG. 5C.
  • An image 511 is an image acquired by the imaging device 12a, the imaging device 12b,.
  • the road surface 512 is the road surface of the road on which the moving body 100 is traveling.
  • the weight ⁇ I based on the pixel intensity is calculated using Equation (3).
  • the description has been made with dark pixels.
  • the intensity of the pixel is too high (too bright), the reliability is low, so the weight ⁇ I is low in Equation (3).
  • the weight ⁇ I is low when the pixel intensity such as rainy weather is too weak and when the pixel intensity such as backlight is too strong.
  • FIG. 5D is a diagram illustrating a method for calculating weights based on image resolution. The weighting by the resolution of the image imaged with the imaging device 12a, the imaging device 12b, ... imaging device 12n is demonstrated using FIG. 5d.
  • the image 515 is the maximum resolution that can be captured by the imaging device 12a, the imaging device 12b,.
  • the image 516 has a lower resolution than the image 515 (V ′ ⁇ V, U ′ ⁇ U).
  • the image 517 is an image having a lower resolution than the image 516 (V ′′ ⁇ V ′, U ′′ ⁇ U ′). Therefore, when the position is estimated using the image 516, the reliability is lower than the position estimation of the image 515, and the reliability when the position is estimated using the image 517 is further reduced.
  • weighting is performed according to the image resolution of the image captured by the imaging device 12a, the imaging device 12b,. The weight increases as the image resolution increases.
  • FIG. 5E is a diagram for explaining a method of calculating weights based on feature points. The weighting by the number of feature points extracted on the imaging device 12n will be described with reference to FIG. 5E.
  • the image 520 is an image acquired by the imaging device 12a, the imaging device 12b,.
  • the stationary object 521 is a stationary object shown in the image 520.
  • the feature point 522 is a feature point extracted from the stationary object 521 on the image 520.
  • the image 523 is an image in an environment different from that when the image 520 is acquired by the imaging device 12a, the imaging device 12b,.
  • a stationary object 524 is a stationary object shown in the image 523.
  • the feature point 525 is a feature point extracted from the stationary object 524 on the image 523.
  • the weight is calculated using the number of feature points as shown in Equation (5). The weight increases as the number of feature points increases.
  • FIG. 6 is a diagram illustrating an example of an image matching method.
  • the image matching means 206 will be described with reference to FIG.
  • the virtual image and the actually captured image are matched, and the virtual distance image and the distance image created from the actually captured image are matched.
  • a comparison error obtained by combining the comparison error of the virtual image and the comparison error of the virtual distance image and calculating an average.
  • the virtual images 601A, 601B,..., 601N are created virtual images (u, v, color) from the virtual positions 302A, 302B,.
  • the virtual distance images 602A, 602B,..., 602N are virtual distance images (u, v, distance) created from the virtual positions 302A, 302B,.
  • An image 603 is an image (u, v, color) actually captured by the imaging device 12a, the imaging device 12b,..., The imaging device 12n from the current position of the moving body 100.
  • An image 604 is a distance image (u, v, distance) actually captured from the current position of the moving body 100 by the imaging device 12a, the imaging device 12b,.
  • the virtual images 601A, 601B,..., 601N created from the respective virtual positions 302A, 302B,..., 302N are compared with the actually captured image 603, and the respective comparison errors are EiA, EiB,. , EiN.
  • the color of the pixel (u, v) of the actually captured image 603 is defined as a color r. Therefore, the above-described comparison is, for example, a difference for each pixel (u, v) as shown in Expression (6).
  • the average of EiA, EiB,..., EiN obtained at each pixel is EiA ′, EiB ′,.
  • EiA (u, v)
  • EiB (u, v)
  • ⁇ ⁇ ⁇ EiN (u, v)
  • EiA ′ [EiA (1, 0) +... + EiA (U, V)] / (U ⁇ V) (6 ′)
  • EiB ′ [EiB (1, 0) +... + EiB (U, V)] / (U ⁇ V)
  • ⁇ ⁇ ⁇ EiN ′ [EiN (1, 0) +... + EiN (U, V)] / (U ⁇ V)
  • the virtual distance images 602A, 602B,..., 602N created from the respective virtual positions 302A, 302B,..., 302N are compared with the distance images 604 created from the images actually captured by the imaging device.
  • the comparison errors are EsA, EsB,..., EsN.
  • the distances of the pixels (u, v) of the virtual images 601A, 601B, ..., 601N created from the respective virtual positions 302A, 302B, ..., 302N are distance A, distance B, ..., distance N.
  • the distance of the pixel (u, v) of the actually captured distance image 604 is a distance r.
  • the above-described comparison is, for example, a difference for each pixel (u, v) as shown in Expression (7).
  • the average of EsA, EsB,..., EsN obtained at each pixel is set as EsA ′, EsB ′,.
  • EsA (u, v)
  • EsB (u, v)
  • ⁇ ⁇ ⁇ EsN (u, v)
  • EsA ′ [EsA (1,0) +... + EsA (U, V)] / (U ⁇ V) (7 ′)
  • EsB ′ [EsB (1, 0) +... + EsB (U, V)] / (U ⁇ V)
  • ⁇ ⁇ ⁇ EsN ′ [EsN (1,0) +... + EsN (U, V)] / (U ⁇ V)
  • the weight Wi of the image 603 actually captured from the current position of the moving body 100 is calculated as shown in Expression (8) using the weight described in FIG.
  • Wi ⁇ c ⁇ ⁇ I ⁇ ⁇ r ⁇ ⁇ T (8)
  • the weight Ws of the distance image 604 actually captured from the current position of the moving body 100 is calculated as shown in the equation (9) using the weight described in FIG.
  • the average error EiA ′ obtained from the virtual images 601A, 601B,..., 601N and the virtual distance images 602A, 602B,. , EiB ′,..., EiN ′ and EsA ′, EsB ′,..., EsN ′ are set as comparison errors EfA, EfB,.
  • Expression (10) represents details of the comparison errors EfA, EfB,..., EfN.
  • EfA (Wi ⁇ EiA ′ + Ws ⁇ EsA ′) / (Wi + Ws) (10)
  • the position correction means 211 obtains the minimum error among the comparison errors EfA, EfB,..., EfN obtained by the equation (10), and sets the virtual position of the minimum error as the current position. For example, when the minimum error calculated by the position correction unit 211 is EfA, the current position 301 (X, Y, Z) estimated by the current position estimation unit 201 is calculated as shown in the equation (11), as shown in Equation (11). , ZA).
  • the position correction means 211 may correct the position based on the errors EfA, EfB,..., EfN obtained by Expression (10). For example, as shown in Expression (12), the current position 301 is calculated.
  • X (XA / EfA +... + XN / EfN) / (1 / EfA +... + 1 / EfN)
  • Y (YA / EfA +... + YN / EfN) / (1 / EfA +... + 1 / EfN)
  • Z (ZA / EfA +... + ZN / EfN) / (1 / EfA +... + 1 / EfN) (12)
  • a road 700a in FIG. 7A is a road on which the moving body 100 is traveling.
  • the error 701a is a current position error obtained by the current position error estimating means 210d.
  • the shadow 702 is a shadow on the road 700 on which the moving body 100 is traveling.
  • the following conditions (a1) to (f1) are assumed.
  • (A1) The error obtained by the imaging device calibration error estimating means 210a is large.
  • (B1) The number of feature points extracted by the feature point extracting means 210b is large.
  • the parallax image error obtained by the parallax image error estimating means 210c is Small (d1) Current position error 701a estimated by current position error estimation means 210d is small (e1) Since shadow 702, the number of low intensity pixels obtained by image intensity acquisition means 210e is large (f1) Image resolution acquisition The image resolution acquired by means 210f is high.
  • the virtual position generating unit 203 does not generate a virtual position, but the current position estimated by the current position estimating unit 201 Environment information extraction means 202, map reference 204, virtual image creation 205, and image matching 206 are executed from only the position.
  • the average error is calculated by the equations (6) and (7), and the weights of the virtual image and the virtual distance image are calculated by the equations (8) and (9).
  • the calibration error of the imaging device 12a, imaging device 12b,..., Imaging device 12n obtained by the imaging device calibration error estimation means 210a is large, the weight of ⁇ c is low.
  • the weight ⁇ I becomes low.
  • the resolution of the imaging device 12a, imaging device 12b,..., Imaging device 12n obtained by the image resolution acquisition unit 210f is high, the weight ⁇ r is high, and the error obtained by the parallax image error estimation unit 210c is small. Therefore, the weight ⁇ s is increased. Therefore, when calculating the weight Wi of the virtual image and the weight Ws of the virtual distance image using the equations (8) and (9), Wi ⁇ Ws.
  • FIG. 8 is a diagram for explaining an abnormal case.
  • FIG. 8 shows a situation where the road 800 can be traveled without restriction as usual in the map information, but the lane is temporarily restricted due to an accident or the like.
  • a road 800 is a road on which the moving body 100 is traveling.
  • a travelable area 801 a is a travelable area of the road 800.
  • the travel prohibition area 801b is an area in which the road 800 cannot travel temporarily.
  • the obstacle 802 is an obstacle for distinguishing the travel prohibition area 801.
  • a route 803 is a route created to avoid the obstacle 802.
  • the moving body 804 is a preceding vehicle in the travelable area 801a.
  • the estimated error 805 is an error estimated by the current position error estimating means 210d of the moving body 100.
  • the obstacle 802 is detected using the imaging device 12a, the imaging device 12b,.
  • the error 805 estimated by the current position error estimating means 210d of the moving body 100 is not large, but the area 801a that can be traveled is narrower than the road 800 because of the obstacle 802. Therefore, it is necessary to travel while estimating the position more accurately than when traveling on the road 800. For this reason, the number of virtual positions created by the virtual position creating means 203 and the number of virtual images created by the virtual image creating means 205 are increased.
  • the width of the road 800 returns to the original and becomes wider than the travelable area 801a. Therefore, a virtual position is created based on the error 805 estimated by the current position error estimating means 210d as usual.
  • the obstacle 802 is recognized by the imaging device 12a, the imaging device 12b,..., The imaging device 12n, but information on the obstacle 802 may be described on the map 19.
  • the mobile unit 804 may recognize the obstacle 802 and immediately write it on the map 19 and share it with other vehicles.
  • the obstacle 802 information may be directly transmitted from the mobile body 804 that has traveled the area 801a to the mobile body 100 that will travel in the future using the V2V (Vehicle to Vehicle) technology without being described in the map 19.
  • the information for increasing the virtual position may be transmitted from the moving body 804 to the moving body 100 without transmitting the information of the obstacle 802 by the V2V technology.
  • FIG. 9 illustrates the effects of obstacles and countermeasures.
  • a road 900 is a road on which the moving body 100 is traveling.
  • the position error 901 is the current position error of the moving body 100 estimated by the current position error estimating means 210d of the weighting means 210.
  • An image 902 is an image acquired by the imaging device 12a, the imaging device 12b,.
  • the stationary object 903 is a stationary object on the road 900, and is reflected on the image 902 as a stationary object 903a.
  • the obstacle 904 is an obstacle on the road 900 and is shown as an obstacle 904a on the image 902.
  • the stationary object 905 is a stationary object on the road 900, and is reflected as a stationary object 905a on the image 902.
  • Point 906 is map information on the road 900 projected on the image 902.
  • a point 907 is map information on the stationary object 903 projected on the image 902.
  • a point 908 is map information on the stationary object 905 projected on the image 902.
  • the obstacle 904a is reflected in front of the stationary object 905a.
  • the information of the map 19 is in the form of (x, y, z)
  • the pixel information of the point 908 is (u, v, distance A), but the imaging device 12a, imaging device 12b,.
  • the actual distance to the stationary object 905 detected in step 1 is not the distance A, but the distance B to the obstacle 904 in front of the stationary object 905. Therefore, the position error due to the position correction means 211 becomes large.
  • Matching is performed by distinguishing map information of the environment in which the mobile unit 100 is traveling. For example, since there is a high probability that other vehicles and pedestrians exist on the road 900, the map information 906 on the road 900 is not used by the position correction unit 211. Also, since the map information on the stationary object 905 is highly likely to be affected by surrounding obstacles, the map information 908 on the stationary object 905 is not used. Accordingly, the position correction unit 211 performs position estimation using only map information of a certain height or higher based on the position estimated by the current position estimation unit 201 of the moving body 100. Then, since the position is estimated by the position correction unit 211 only at the point 907 having a low probability of being affected by the obstacle, the influence of the obstacle in the traveling environment of the moving body 100 can be reduced.
  • the position estimation of the moving body 100 can be performed by the above-mentioned measures.
  • the position correction unit 211 performs high-precision position estimation using only the point 907
  • the distances between the points 907 and 908 that are not used by the position correction unit 211 are determined as the imaging device 12a, the imaging device 12b,. It measures with the apparatus 12n.
  • the distance to the stationary object 905 based on the information of the map 19 is the distance B, but since the actually measured distance is the distance A, the point 908 on the image 902 is an outlier or an obstacle. It becomes clear that it is a thing. Therefore, the accuracy of the position estimated by the position correcting unit 211 using only the point 907 and the position of the obstacle 904 detected using the point 908 are increased.
  • the point 908 is described as one point, but a plurality of points may be used.
  • the obstacle detection may be performed by comparing the distance measured from the position estimated by the position correction unit 211 using only the point 907 and the point 907.
  • the image 902 may be divided into various regions, and a point 908 in a certain region may be used as one obstacle.
  • the map information 19 and the imaging device 12a, the imaging device 12b,... Information of the imaging device 12n in FIG. Detection may be performed. Also, obstacle detection is performed based on the current position error 901 of the moving body 100.
  • the position correction unit 211 estimates the position and determines whether it is an obstacle.
  • the position correction unit 211 performs position estimation several times, and detects the obstacle after the reliability is increased.
  • information on the current position error of the moving body 100 obtained by the current position estimation unit information on the calibration error of the imaging apparatus, which is information acquired by the imaging apparatus, and an image captured by the imaging apparatus.
  • Information on feature points on the image extracted from the image information on the parallax image error acquired from the image captured by the imaging device, information on the image intensity acquired from the image captured by the imaging device, and imaging
  • the weight is calculated using image resolution information acquired from the image captured by the apparatus, the weight may be calculated based on at least one of these information.
  • the weight is calculated based on the information acquired by the imaging device 12 and the information on the current position error of the moving body 100 obtained by the current position estimation unit, and the plurality of virtual images are actually obtained.
  • a comparison error calculated by comparing each of the captured images is weighted, and a process of setting a virtual position with the smallest weighted comparison error as the current position of the moving body 100 from a plurality of virtual positions is performed.
  • the weight is not fixed as in the prior art, but a value corresponding to the situation is calculated, the reliability of the information is improved, and highly accurate position estimation can be performed. Therefore, even in situations where it is difficult to match by images, such as when rain or sunlight is strong (backlight, reflection, reflection) or when the calibration error of the imaging device is large, matching can be performed with high accuracy according to the driving situation. .
  • the present invention is not limited to the above-described embodiments, and various designs can be made without departing from the spirit of the present invention described in the claims. It can be changed.
  • the above-described embodiment has been described in detail for easy understanding of the present invention, and is not necessarily limited to one having all the configurations described.
  • a part of the configuration of an embodiment can be replaced with the configuration of another embodiment, and the configuration of another embodiment can be added to the configuration of an embodiment.

Abstract

高精度の位置推定を行うことができる位置推定装置を提供する。本発明の位置推定装置１は、撮像装置１２を搭載した移動体１００の現在位置を推定する位置推定装置１であって、移動体１００の現在位置を推定し、その現在位置に基づいて複数の仮想位置を作成し、複数の仮想位置での仮想画像をそれぞれ作成し、複数の仮想画像を実際画像と比較して比較誤差を算出し、撮像装置１２により取得された情報と、移動体の現在位置誤差の情報の少なくとも一つに基づいて重みを算出し、重みを用いて比較誤差に重み付けを行い、重み付けされた比較誤差に基づいて現在位置を補正する。

Description

位置推定装置
　本発明は，ロボットや自動車などの移動体の位置を推定する技術に関する。
　ロボット及び自動車などの移動体がその周囲の情報を収集し，移動体の現在位置及び走行状態を推定し，移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。移動体の周囲の情報検出のためには様々なセンサが用いられる。一般に，周囲の情報を計測するためのセンサとしては，カメラなどの撮像装置，レーザセンサ，ミリ波レーダなどがある。移動体の位置を測定するためのセンサとしては，ＧＰＳ（Ｇｌｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）またはＩＭＵ（Ｉｎｅｒｔｉａｌ　Ｍｅａｓｕｒｅｍｅｎｔ　Ｕｎｉｔ）が用いられる。
　自律走行制御では，移動体に搭載された制御装置において，例えばＩＭＵで算出した移動体の速度または角速度を積算したり，ＧＰＳ測位を用いたりして，移動体自身の位置（自己位置）を推定する。また，地図情報とランドマークがなく，ＧＰＳも使えない場合は，移動体の周囲に存在する物体との相対位置を推定しながら走行中環境の地図を作成するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）法が用いられる。ＳＬＡＭで推定した相対位置の誤差は，蓄積するため，位置修正が必須となる。位置修正は，例えば，制御装置は，レーザセンサあるいはカメラなどで周囲の情報を収集し，位置推定の際の基準となる立体物の位置や形状を検出する。そして，検出された立体物の位置を地図情報と比較（マップマッチング）することにより，移動体の現在位置を修正する。従って，検出された立体物の位置誤差が大きい場合，移動体の位置を修正できない。また，修正前は，すでに位置誤差が大きい場合は，マップマッチングを行っても，現在位置を修正できない。
　ここで，例えば，非特許文献１は，正確な現在位置を推定するために，複数の仮想の現在位置を作成し，それぞれの仮想位置，地図情報，センサの内部と外部パラメータを用い，仮想の強度画像と視差画像を作成する。それぞれの作成した仮想の強度画像と視差画像を実際に取得した強度画像と視差画像とマッチングし，それぞれの強度画像と視差画像のマッチング結果に固定の重みを付けてマッチング誤差を算出する。最もマッチング誤差の小さい仮想位置を現在位置とする。
Yuquan Xu, et al. 3D Point Cloud Map Based Vehicle Localization Using Stereo Camera. 2017 IEEE Intelligent Vehicles Symposium. USA.
　しかし，非特許文献１のように，それぞれの仮想位置からマッチング誤差を算出する際，強度画像のマッチング結果と視差画像のマッチング結果に固定の重みを付けるため，走行環境の明るさや撮像装置のキャリブレーション誤差による撮像装置が取得した情報の信頼度が低くなり，マッチングで推定した現在位置精度が低くなる。
　本発明は、上記の点に鑑みてなされたものであり、その目的とするところは、高精度の位置推定を行うことができる位置推定装置を提供することである。
　上記課題を解決するために，本発明の位置推定装置は，移動体に搭載した撮像装置であって，
　撮像装置を搭載した移動体の現在位置を推定する位置推定装置であって、
　前記移動体の現在位置を推定する現在位置推定手段と、
　該現在位置推定手段により推定された現在位置に基づいて複数の仮想位置を作成する仮想位置作成手段と、
　前記撮像装置により前記複数の仮想位置で撮像したと仮定した場合の複数の仮想画像をそれぞれ作成する仮想画像作成手段と、
　前記複数の仮想画像を前記撮像装置により前記現在位置で撮像された画像と比較してそれぞれの比較誤差を算出する画像マッチング手段と、
　前記撮像装置により取得された情報と、前記現在位置推定手段で得られた前記移動体の現在位置誤差の情報との少なくとも一つに基づいて重みを算出し、該重みを用いて前記それぞれの比較誤差に重み付けを行う重み付け手段と、
　前記現在位置推定手段で推定されている現在位置を前記重み付けされた比較誤差に基づいて補正する位置補正手段と、
　を有することを特徴とする。
　本発明によれば、雨天や日差しが強い条件（逆光、照り返し、反射）や撮像装置のキャリブレーション誤差が大きい場合などのように画像によるマッチングが難しい状況でも，走行状況に応じて高精度にマッチングができる。
　本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。また、上記した以外の、課題、構成及び効果は、以下の実施形態の説明により明らかにされる。
本発明の一実施形態に係る移動体の位置推定装置の構成図。 画像処理部が行う画像処理手順を示すフローチャート。 仮想位置作成方法の一例を説明する図。 仮想画像作成方法の一例を説明する図。 キャリブレーション誤差による重みを算出する方法の一例を説明する図。 視差画像誤差による重みを算出する方法の一例を説明する図。 画像強度による重みを算出する方法を説明する図。 画像解像度による重みを算出する方法を説明する図。 特徴点による重みを算出する方法を説明する図。 画像マッチング方法の一例を説明する図。 本発明の実施例を説明する図。 本発明の実施例を説明する図。 異常な場合の説明。 障害物の影響と対策の説明。
　以下，本発明の実施形態に係る移動体の位置推定装置について，図面を用いて説明する。
　図１は，本発明の一実施形態に係る位置推定装置１の構成図である。
　位置推定装置１は，撮像装置を搭載した移動体の現在位置を推定するものであり、自動車またはロボットなどの移動体１００に搭載されている。位置推定装置１は，一台以上の撮像装置１２（１２ａ，１２ｂ，・・・１２ｎ）と，情報処理装置１３を有する。撮像装置１２は，例えばスチルカメラまたはビデオカメラである。また，撮像装置１２は単眼カメラまたは複眼カメラでもよい。
　情報処理装置１３は，撮像装置１２で撮像された画像を処理して移動体１００の位置または移動量を算出する。情報処理装置１３は，算出された位置または移動量に応じた表示を行ってもよく，または移動体１００の制御に関する信号を出力してもよい。
　情報処理装置１３は，例えば一般的なコンピュータであって，撮像装置１２によって撮像された画像を処理する画像処理部１４と，画像処理結果に基づく処理を行う制御部（ＣＰＵ）１５と，メモリ１６と，ディスプレイなどの表示部１７と，これら構成要素を相互に接続するバス１８とを有する。情報処理装置１３は，画像処理部１４及び制御部１５が所定のコンピュータプログラムを実行することにより，以下の処理を行う。
　撮像装置１２ａは，例えば，移動体１００の前方に設置されている。撮像装置１２ａのレンズは移動体１００の前方に向けられている。撮像装置１２ａは，例えば，移動体１００の前方の遠景を撮像する。他の撮像装置１２ｂ，・・・撮像装置１２ｎは，撮像装置１２ａと異なる位置に設置され，撮像装置１２ａと異なる撮像方向または領域を撮像する。撮像装置１２ｂは，例えば，移動体１００の後方で下方に向けて設置されていてもよい。撮像装置１２ｂは，移動体１００後方の近景を撮像するものでよい。
　撮像装置１２が単眼カメラの場合，路面が平らであれば，画像上のピクセル位置と実際の地上位置関係（ｘ，ｙ）が一定になるため，撮像装置１２から特徴点までの距離を幾何学的に計算できる。撮像装置１２がステレオカメラの場合，画像上の特徴点までの距離をより正確に計測できる。以下の説明では，単眼の標準レンズを有するカメラを採用した事例について説明するが，これ以外のカメラ（広角レンズを有するカメラまたはステレオカメラなど）でもよい。
　また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎが，ある時刻で撮像する対象物は，それぞれ互いに異なるものでよい。例えば，撮像装置１２ａは，移動体１００の前方の遠景を撮像するものでよい。この場合，遠景を撮像した画像からは，立体物，または位置推定のためのランドマークなどの特徴点が抽出されるようにしてもよい。撮像装置１２ｂは，移動体１００周辺の路面などの近景を撮像するようにしてもよい。この場合，近景を撮像した画像からは，移動体１００の周囲の白線，または路面ペイントなどが検出されるようにしてもよい。
　また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎは，同時に雨や日差しなどの環境外乱の影響を受けないような条件で移動体１００に設置されてもよい。例えば，撮像装置１２ａは移動体１００の前方で前向きに設置されるのに対して，撮像装置１２ｂは移動体１００の後方で後ろ向きまたは下向きに設置されてもよい。これにより，例えば，降雨時に撮像装置１２ａのレンズに雨滴が付着した場合でも，進行方向の逆向きまたは下向きの撮像装置１２ｂのレンズには雨滴が付着しにくい。このため，撮像装置１２ａが撮像した画像が雨滴の影響で不鮮明であっても，撮像装置１２ｂが撮像した画像は雨滴の影響を受けにくい。あるいは，日差しの影響で撮像装置１２ａの画像が不鮮明であっても，撮像装置１２ｂが撮像した画像は鮮明である可能性がある。
　また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎは，互いに異なる撮像条件（絞り値，ホワイトバランス，等）で撮影してもよい。例えば，明るい場所用にパラメータを調整した撮像装置と，暗い場所用にパラメータを調整した撮像装置とを搭載することで，環境の明暗によらず撮像可能としてもよい。
　撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎは，制御部１５から撮影開始の指令を受けたとき，または一定の時間間隔で画像を撮像する。撮像された画像のデータ及び撮像時刻は，メモリ１６に格納される。メモリ１６は，情報処理装置１３の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置を含む。
　画像処理部１４がメモリ１６に格納された画像データ及び撮像時刻に基づいて，様々な画像処理を行う。この画像処理では，例えば，中間画像が作成されてメモリ１６に保存される。中間画像は，画像処理部１４による処理の他，制御部１５などの判断や処理に利用されてもよい。
　バス１８は，ＩＥＢＵＳ（Ｉｎｔｅｒ　Ｅｑｕｉｐｍｅｎｔ　Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ　Ｉｎｔｅｒｃｏｎｎｅｃｔ　Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ　Ａｒｅａ　Ｎｅｔｗｏｒｋ）などで構成できる。
　地図１９は，移動体１００が走行する環境の情報を有する（環境情報記憶手段）。地図１９は，例えば，走行環境にある静止物（木，建物，道路，車線，信号，標識，路面ペイント，路端など）の形状や位置の情報を記憶している。地図１９のそれぞれの情報を数式で表してもよい。例えば，線情報を複数点で構成せず，線の傾きと切片のみでよい。また，地図１９の情報を区別せずに，点群で表してもよい。点群は３Ｄ（ｘ，ｙ，ｚ），４Ｄ（ｘ，ｙ，ｚ，色）などで表してもよい。最終的に，移動体１００の現在位置から走行環境を検出し，マップマッチングができれば，地図１９の情報をどんな形にしてもよい。
　画像処理部１４は，地図１９とメモリ１６に格納された移動体１００の現在位置と撮像装置１２の内部パラメータに基づいて，撮像装置１２で撮像された画像に地図情報を投影する。画像処理部１４は，撮像装置１２で撮像された画像に基づいて移動体１００の複数の位置候補を特定し，その複数の位置候補と移動体１００の移動速度とに基づいて移動体１００の位置を推定する。
　画像処理部１４は，例えば，移動体１００の走行中に撮像装置１２が撮像した画像を処理して，移動体１００の位置を推定する。例えば，画像処理部１４は，撮像装置１２が撮像したビデオ画像で移動体１００の移動量を算出し，過去の位置に移動量を加算して現在位置を推定する。画像処理部１４は，ビデオ画像の各フレーム画像で特徴点を抽出してもよい。画像処理部１４は，さらに，次以降のフレーム画像で同じ特徴点を抽出する。そして，画像処理部１４は，特徴点のトラッキングにより移動体１００の移動量を算出する。
　制御部１５は，画像処理部１４の画像処理の結果に基づいて，移動体１００に対して移動速度に関する指令を出力してもよい。例えば，制御部１５は，画像内の立体物の画素数，画像内の特徴点のうちの外れ値の数または画像処理の種類等に応じて，移動体１００の移動速度を増加させる指令，減少させる指令または維持させる指令を出力してもよい。
　図２は，画像処理部１４が行う画像処理手順を示すフローチャートである。
　現在位置推定手段２０１は，移動体１００の現在位置を推定する手段である。現在位置推定手段２０１は，例えば，絶対位置を推定するＧＰＳで構成する。また，現在位置推定手段２０１は，ある基準点から相対位置を推定するオドメトリで構成してもよい。前述のＧＰＳやオドメトリに基づく位置推定手法には，位置誤差が発生するが，後述する位置補正手段２１１で補正する。
　環境情報抽出手段２０２は，移動体１００に搭載されている撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した移動体１００の周囲の環境情報を抽出する手段である。環境情報抽出手段２０２では，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像された移動体１００の周囲のグレースケール画像やカラー画像を取得する。また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像から作成された視差画像を取得してもよい。そして，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像された画像上の特徴点の情報を取得してもよい。また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像上の線情報を取得してもよい。
　仮想位置作成手段２０３は，現在位置推定手段２０１で推定した現在位置に基づいて仮想位置を作成する手段である。仮想位置作成手段２０３で作成する仮想位置の個数は１個以上であり、固定値にせず，走行環境に応じて変更する。仮想位置作成手段２０３の詳細は後述する。
　地図参照手段２０４は，地図１９から情報を取得する手段である。制御部１５の指令に基づいて，地図１９から情報を取得する。制御部１５の指令で，地図１９にある情報を全て取得してもよい。また，移動体１００の周囲の情報のみを取得してもよい。
　仮想画像作成手段２０５は，地図参照手段２０４で取得した地図情報と仮想位置作成手段２０３で作成したそれぞれの仮想位置を用い，それぞれの仮想画像を作成する。仮想画像は、撮像装置で仮想位置から撮像したと仮定した場合の画像である。例えば，地図１９ が３Ｄ（ｘ，ｙ，ｚ）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に変換する。また，仮想画像作成手段２０５は，地図１９の情報に合わせて画像を作成する。例えば，地図１９ が３Ｄ（ｘ，ｙ，ｚ）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に変換し，ピクセル（ｕ，ｖ）に距離情報を追加する。従って，（ｕ，ｖ，距離）で構成した仮想画像とする。
　また，地図１９ が４Ｄ（ｘ，ｙ，ｚ，色）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に色情報を追加し，（ｕ，ｖ，色）で構成した仮想画像にしてもよい。また，地図１９ が３Ｄ（ｘ，ｙ，ｚ）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に深さ情報を追加し，（ｕ，ｖ，深さ）で構成した仮想視差画像にしてもよい。色情報は、ピクセルの強度である。画像はカラーかグレースケールで表示され、グレースケールの場合、画像のそれぞれのピクセルの強度は０から２５５（黒から白）となり、カラーの場合、Ｒ（０～２５５）＋Ｇ（０～２５５）＋Ｂ（０～２５５）の３チャネルとなる。そして、深さ情報は、撮像装置などのセンサの検出方向であるＺ軸方向の長さである。
　画像マッチング手段２０６は，環境情報抽出手段２０２で撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎにより実際に撮像した画像を、仮想画像作成手段２０５で作成した仮想画像とマッチングする（比較する）手段である。画像マッチング手段２０６については後述する。
　条件分岐２０９は，仮想位置作成手段２０３で最後の仮想位置が作成されたかをチェックする。最後の仮想位置が作成されなかった場合，仮想位置作成手段２０３に進み，最後の仮想位置が作成された場合，重み付け手段２１０に進む。
　重み付け手段２１０は，画像マッチング手段２０６で得られたそれぞれのマッチング結果（比較誤差）に重みを付ける手段である。重み付け手段２１０は，走行環境やセンサの状態に応じた重みを算出する。重み付け手段２１０は，撮像装置により取得された情報と、現在位置推定手段で得られた移動体１００の現在位置誤差の情報の少なくとも一つに基づいて重みを算出する。撮像装置により取得された情報には、撮像装置のキャリブレーション誤差の情報と、撮像装置により撮像された画像から抽出された画像上の特徴点の情報と、撮像装置により撮像された画像から取得された視差画像誤差の情報と、撮像装置により撮像された画像から取得された画像強度の情報と、撮像装置により撮像された画像から取得された画像解像度の情報の少なくとも一つが含まれる。重み付け手段２１０は，撮像装置キャリブレーション誤差推定手段２１０ａ，特徴点抽出手段２１０ｂ，視差画像誤差推定手段２１０ｃ，現在位置誤差推定手段２１０ｄ，画像強度取得手段２１０ｅ，画像解像度取得手段２１０ｆなどで構成される。
　重み付け手段２１０の撮像装置キャリブレーション誤差推定手段２１０ａは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのそれぞれのキャリブレーション誤差を推定する。前述のキャリブレーションは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのそれぞれの歪みを補正するためのステップであり，既知のパターンを用いて撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像する。一方，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像の枚数，又は，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した時の距離，位置，角度などによって，キャリブレーション結果が異なるため，誤差が発生する。撮像装置キャリブレーション誤差推定手段２１０ａが前述の誤差を求めて，重み付け手段２１０に結果を出力する。
　重み付け手段２１０の特徴点抽出手段２１０ｂは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像上で特徴点を抽出する。前述の特徴点は，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像上でのコーナー，エッジ，最大値，最小値などである。特徴点抽出手段２１０ｂで抽出した特徴点を重み付け手段２１０に出力する。
　重み付け手段２１０の視差画像誤差推定手段２１０ｃは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像された画像を用いて視差画像を作成する際の誤差を推定する。前述の視差画像誤差は，例えば，視差画像を作成するためのキャリブレーションによる誤差である。視差画像を作成する場合，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのお互いの相対位置を正確に推定する必要があり，誤差が発生する場合，出力の視差画像の誤差が大きくなる。また，視差画像を作成する場合，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎに写る情報をそれぞれの撮像装置の画像で探索し，検索する必要があるため，探索誤差が発生するときは，出力の視差画像の誤差が大きくなる。
　重み付け手段２１０の現在位置誤差推定手段２１０ｄは，現在位置推定手段２０１で推定した位置の誤差を推定する。例えば，現在位置推定手段２０１でＧＰＳにより移動体１００の現在位置を推定する場合，ＧＰＳが出力する誤差を現在位置誤差とする。また，ある基準から相対位置を推定する手法において現在位置推定手段２０１で移動体１００の現在位置を推定する場合，走行距離に比例した誤差を現在位置誤差としてもよい。
　重み付け手段２１０の画像強度取得手段２１０ｅは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像のそれぞれのピクセル強度（画像強度）を取得する手段である。撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像のそれぞれのピクセル強度を取得した後，下限値以下の暗すぎるピクセルと上限値以上の明るすぎるピクセルを探索する。
　重み付け手段２１０の画像解像度取得手段２１０ｆは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像のそれぞれの解像度を取得する手段である。重み付け手段２１０の詳細は後述する。
　位置補正手段２１１は，重み付け手段２１０を用い，現在位置推定手段２０１で推定した現在位置を補正する手段である。重み付け手段２１０で重み付けたそれぞれの仮想画像のマッチング誤差（比較誤差）の中で，最小誤差を探索する。そして、最小誤差を有した仮想位置を現在位置にする。
　図３は、仮想位置作成方法の一例を説明する図である。この図３を用いて，仮想位置作成手段２０３の詳細を説明する。座標系３０１は移動体１００の現在位置推定手段２０１で推定した現在位置を原点とする。仮想位置（３０２Ａ，３０２Ｂ，・・・３０２Ｎ）は座標系３０１上の位置であり，それぞれの仮想位置の座標を３０３Ａ，３０３Ｂ，・・・３０３Ｎとする。
　推定した位置誤差３０４は，現在位置誤差推定手段２１０ｄで推定した移動体１００の位置誤差である。位置誤差３０４の範囲は、現在位置推定手段２０１で推定された移動体１００の現在位置を中心とした範囲であり、その大きさは、現在位置誤差推定手段２１０ｄで推定した位置誤差に応じて変化する。
　範囲Ｒ３０５は，仮想位置が作成される範囲である。仮想位置作成手段２０３は、現在位置推定手段２０１で推定された移動体１００の現在位置に基づいて複数の仮想位置を作成する。仮想位置作成手段２０３が作成する仮想位置の個数Ｎは，移動体１００の走行環境や状況に依存するため，現在位置誤差推定手段２１０ｄで推定した位置誤差３０４に基づいて，範囲Ｒ３０５を設定する。仮想位置は、移動体の推定された現在位置を中心とした所定範囲内に設定される位置と向きである。
　仮想位置の互いの間隔ｋ３０６は、移動体１００の現在位置推定手段２０１で推定した現在位置３０１から仮想位置範囲Ｒ３０５までの間を所定数で分割した間隔である。間隔ｋ３０６は求めた位置精度や撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの精度で調整する。位置推定手段２１１で推定した位置精度がＲの場合，予め設定した間隔ｋを用いて仮想位置を作成するが、走行中環境に必要な位置推定精度に基づいてｋを調整してもよい。例えば、位置推定手段２１１で指定した位置精度がＲの場合、間隔ｋを用いて位置補正手段２１１で補正する位置精度がｋとなる。一方、走行中環境に必要な精度が０．５Ｒの場合，間隔ｋから間隔０．５Ｒとし、位置補正する。簡単のため，図３をＸ－Ｙ座標で示しているが，Ｚ，ロール，ピッチ，ヨー（６ＤＯＦ）で仮想位置を作成してもよい。
　一方，次元が多い場合，処理時間が増えるため，それぞれの次元の誤差に応じて仮想位置を作成する。例えば，移動体１００が走行する一車線の道路であり，白線検出機能が移動体１００に搭載された場合，移動体１００のＹ方向の誤差は低くなるため，仮想位置作成手段２０３でＹ方向の仮想位置を作成しなくてもよい。また，Ｘ方向とＹ方向の異なった間隔ｋ３０６を設定し，それぞれの方向の仮想位置の個数を調整してもよい。また，移動体１００が走行する環境に凹凸が少ない場合，Ｚ方向，ピッチ，ロールの仮想位置を作成せず，短い処理時間でＸ，Ｙ，ヨーのみ精度よく推定する。なお、仮想位置における車両の向きとして、ある範囲αとそれを分割した間隔Ｋを用いて設定してもよい。間隔Ｋは、仮想位置を作成したときと同じｋ（ｋ＝Ｋ）を設定するが、現在位置誤差推定手段２１０ｄで推定した現在位置誤差に基づいて、Ｋを調整（Ｋ＞ｋ　or　Ｋ＜ｋ）してもよい。
　図４は、仮想画像作成方法の一例を説明する図である。この図４を用いて，仮想画像作成手段２０５の説明をする。
　地図情報４０１，４０２，４０３は地図１９に記載された情報である。仮想画像４０４Ａは仮想位置３０２Ａから撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎにより撮像したと仮定した場合の画像であり，仮想画像４０４Ｎは仮想位置３０２Ｎから撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像したと仮定した場合の画像である。この時，地図１９に対する座標３０３Ａと座標３０３Ｎの位置と撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの内部パラメータは既知であるため，地図情報４０１，４０２，４０３は仮想画像４０４Ａ上で情報４０１Ａ，４０２Ａ，４０３Ａとして映し出され，仮想画像４０４Ｎ上で情報４０１Ｎ，４０２Ｎ，４０３Ｎとして映し出される。
　地図情報１９に走行環境の位置（ｘ，ｙ，ｚ）情報のみが記載されていれば，地図１９の情報は３次元（ｘ，ｙ，ｚ）となるため，仮想画像４０４Ａと仮想画像４０４Ｎを（ｕ，ｖ，距離）又は（ｕ，ｖ，深さ）で表せる。また，地図情報１９に走行環境の位置（ｘ，ｙ，ｚ）と色情報が記載されていれば，地図１９の情報は４次元（ｘ，ｙ，ｚ，色）となるため，仮想画像４０４Ａと仮想画像４０４Ｎを（ｕ，ｖ，色）で表せる。
　地図１９に移動体１００の現在位置の近傍情報と遠方情報が含まれているため，全ての情報を用いて仮想画像を作成する場合，処理時間が長くなる。従って，移動体１００の現在位置から予め定められた閾値内（高さ，距離など）の情報のみを画像に変換してもよい。また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの撮像範囲が決まっているため，その撮像範囲内の情報を用いて，仮想画像を作成してもよい。
　図５Ａから図５Ｅを用いて重み付け手段２１０の詳細を説明する。
　図５Ａは、キャリブレーション誤差による重みを算出する方法の一例を説明する図であり、撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーションパラメータを用いた重み付けを表す。撮像装置キャリブレーション誤差推定手段２１０ａで、撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのそれぞれのキャリブレーション誤差を推定する。
　パターン５００は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎが撮像するパターンである。画像５０１は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎがキャリブレーションされる前に取得した画像である。キャリブレーション前のため，取得した画像が歪んでいて，パターン５００を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像する場合，誤って撮像する。
　一方，画像５０２は従来のキャリブレーション技術で補正した画像を表す。この場合，歪みが補正されたため，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像５０２は実際のパターン５００と同じとなる。
　キャリブレーションを行っても，完全に補正できない場合がある。キャリブレーションを行う際，パターン５００のコーナー位置が等間隔に配置されているため，補正後のコーナーの位置推定が可能であり，推定したコーナーの位置をピクセル５０３とする。一方，コーナーの画像５０２上での実際の位置がピクセル５０４の場合，推定したピクセル５０３からの誤差Ｅ５０５が発生する。従って，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのピクセル毎に推定したピクセル位置と実際の位置の誤差を算出することで，誤差Ｅ１，Ｅ２，・・・ＥＴを含めた誤差マップ５０６（キャリブレーション誤差の情報）が得られる。ピクセル数がＵ×Ｖの場合，Ｔ＝Ｕ×Ｖの誤差マップとなる。
　式（１）と前述の誤差マップ５０６から重みλｃを付ける。誤差マップ５０６のそれぞれの算出した誤差Ｅ１，Ｅ２，・・・ＥＴがある閾値Ｅｔｈｒｅｓｈｏｌｄより低い場合，パラメータＩが増える。パラメータＩの全ピクセル（Ｕ×Ｖ）に対する割合を重みλｃとする。
ｉｆ　Ｅ　＞　Ｅｔｈｒｅｓｈｏｌｄ，　Ｉ＋＋
λｃ　＝　Ｉ／（Ｕ×Ｖ）・・・（１）
　キャリブレーションは、例えば移動体１００が走行しながら定期的に実施してもよく、キャリブレーションするごとに重みを算出してもよい。
　図５Ｂは、視差画像誤差による重みを算出する方法の一例を説明する図である。この図５Ｂを用いて、撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで視差画像を作成した時のパラメータを用いた重み付けの説明をする。画像５０７Ｌは視差画像を作成するための左画像であり，画像５０７Ｒは視差画像を作成するための右画像である。視差画像を作成する時，画像５０７Ｌのそれぞれのピクセル情報と同じ情報を持つ画像５０７Ｒのピクセルで探索する。従来の視差画像作成技術はピクセル強度で探索を行い，強度の最も近いピクセルとマッチングする。
　例えば，ピクセル５０８Ｌを画像５０７Ｌのピクセルとし，画像５０７Ｒでそれぞれのピクセル強度と比較（探索）すると，グラフ５０９が得られる。グラフ５０９の縦軸Ｃは、左右の画像のピクセル同士を比較した誤差量である。最も誤差の小さい（Ｃｍｉｎ５１０）ピクセルを５０８Ｒとする。式（２）に示すとおり，画像５０７Ｌのそれぞれのピクセルで得られた最小誤差量Ｃｍｉｎが予め定められた閾値Ｃｔｈｒｅｓｈｏｌｄより小さければ，パラメータＪが増える。パラメータＪの全ピクセル（Ｕ×Ｖ）に対する割合を重みλｓとする。視差画像誤差が小さくなるほど重みλｓが高くなる。
ｉｆ　Ｃｍｉｎ　＜　Ｃｔｈｒｅｓｈｏｌｄ，　Ｊ＋＋
λｓ　＝　Ｊ／（Ｕ×Ｖ）・・・（２）
　図５Ｃは、画像強度による重みを算出する方法を説明する図である。この図５Ｃを用いてピクセル強度による重み付けの説明をする。
　画像５１１は現在位置から撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎが取得した画像である。
　路面５１２は、移動体１００が走行している道路の路面である。
この時，静止物５１３の影５１４の影響で，画像５１１の一部が暗くなった（強度が０になった）。この時の画像５１１は、暗いピクセルが多いため，画像５１１で位置推定を行う場合，誤差が大きくなる。従って，明るいピクセル（強度＝１００％）と暗いピクセル（強度＝０％）の個数をパラメータＫで表す。明るいピクセル（強度＝１００％），又は暗いピクセル（強度＝０％）が増えると，信頼度が低くなるため，ピクセル強度による重みλＩは、式（３）を用いて算出される。
ｉｆ　強度＝０％　ＯＲ　強度＝１００％，　Ｋ＋＋
λＩ　＝　１－Ｋ／（Ｕ×Ｖ）・・・（３）
　簡単のため，暗いピクセルで説明したが，ピクセルの強度が高すぎる（明るすぎる）場合でも，信頼度が低くなるため，式（３）で重みλＩが低くなる。上記した式(３)により、例えば雨天等のピクセルの強度が弱すぎる場合及び逆光などのピクセル強度が強すぎる場合には重みλＩが低くなる。
　図５Ｄは、画像解像度による重みを算出する方法を説明する図である。この図５ｄを用いて，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像の解像度による重み付けの説明をする。
　画像５１５は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像できる最大の解像度であり，合計Ｕ×Ｖピクセルで構成される。解像度の高い画像で位置推定する場合，信頼度が高くなる。一方，画像５１６は画像５１５より解像度の低い画像である（Ｖ’＜Ｖ，Ｕ’＜Ｕ）。また，画像５１７は画像５１６より解像度の低い画像である（Ｖ’’＜Ｖ’，Ｕ’’＜Ｕ’）。従って，画像５１６で位置推定する場合，画像５１５の位置推定より信頼度が低くなり，画像５１７で位置推定する場合の信頼度がさらに低くなる。ここで，式（４）のとおり撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像の画像解像度によって重み付ける。画像解像度が大きくなるに応じて重みも高くなる。
λｒ　＝　（画像解像度）／（Ｕ×Ｖ）・・・（４）
　図５Ｅは、特徴点による重みを算出する方法を説明する図である。この図５Ｅを用いて，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎ上での抽出した特徴点数による重み付けの説明をする。
　画像５２０は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像である。静止物５２１は，画像５２０に写っている静止物である。特徴点５２２は，画像５２０上で静止物５２１から抽出した特徴点である。一方，画像５２３は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで画像５２０を取得した時とは違う環境の画像である。静止物５２４は画像５２３に写っている静止物である。特徴点５２５は，画像５２３上で静止物５２４から抽出した特徴点である。
　画像５２０の静止物５２１から抽出した特徴点数は画像５２３の静止物５２４から抽出した特徴点数より多いため，画像５２０のマッチング結果の信頼度が高い。従って，式（５）のとおり，特徴点数を用いて重みを算出する。特徴点数が多くなるに応じて重みも高くなる。
λＴ　＝　（特徴点数）／（Ｕ×Ｖ）・・・（５）
　図６は、画像マッチング方法の一例を説明する図である。この図６を用いて，画像マッチング手段２０６の説明をする。本実施例では、仮想画像と実際に撮像した画像とのマッチングと、仮想距離画像と実際に撮像した画像から作成した距離画像とのマッチングを行い、仮想画像の比較誤差と仮想距離画像の比較誤差とを算出し、仮想画像の比較誤差と仮想距離画像の比較誤差とを組み合わせて平均した比較誤差を算出する。
　仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎは，仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎからのそれぞれの作成した仮想画像（ｕ，ｖ，色）である。仮想距離画像６０２Ａ，６０２Ｂ，・・・，６０２Ｎは，仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎからのそれぞれの作成した仮想距離画像（ｕ，ｖ，距離）である。画像６０３は，移動体１００の現在位置から撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで実際に撮像した画像（ｕ，ｖ，色）である。画像６０４は，移動体１００の現在位置から撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで実際に撮像した距離画像（ｕ，ｖ，距離）である。
　それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎを実際に撮像した画像６０３と比較し，それぞれの比較誤差をＥｉＡ，ＥｉＢ，・・・，ＥｉＮとする。それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎのピクセル（ｕ，ｖ）の色を色Ａ，色Ｂ，・・・，色Ｎとし，実際に撮像した画像６０３のピクセル（ｕ，ｖ）の色を色ｒとする。従って，前述の比較は，例えば，式（６）に示す通り，ピクセル（ｕ，ｖ）毎の差である。それぞれのピクセルで得られたＥｉＡ，ＥｉＢ，・・・，ＥｉＮの平均を式（６’）の通り，ＥｉＡ’，ＥｉＢ’，・・・，ＥｉＮ’とする。
ＥｉＡ（ｕ，ｖ）　＝　｜色ｒ－色Ａ｜・・・（６）
ＥｉＢ（ｕ，ｖ）　＝　｜色ｒ－色Ｂ｜
・
・
・
ＥｉＮ（ｕ，ｖ）　＝　｜色ｒ－色Ｎ｜
ＥｉＡ’＝［ＥｉＡ（１，０）＋・・・＋ＥｉＡ（Ｕ，Ｖ）］／（Ｕ×Ｖ）・・・（６’）
ＥｉＢ’＝［ＥｉＢ（１，０）＋・・・＋ＥｉＢ（Ｕ，Ｖ）］／（Ｕ×Ｖ）
・
・
・
ＥｉＮ’＝［ＥｉＮ（１，０）＋・・・＋ＥｉＮ（Ｕ，Ｖ）］／（Ｕ×Ｖ）
　それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想距離画像６０２Ａ，６０２Ｂ，・・・，６０２Ｎを、撮像装置で実際に撮像した画像から作成した距離画像６０４と比較し，それぞれの比較誤差をＥｓＡ，ＥｓＢ，・・・，ＥｓＮとする。それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎのピクセル（ｕ，ｖ）の距離を距離Ａ，距離Ｂ，・・・，距離Ｎとし，実際に撮像した距離画像６０４のピクセル（ｕ，ｖ）の距離を距離ｒとする。従って，前述の比較は，例えば，式（７）に示す通り，ピクセル（ｕ，ｖ）毎の差である。それぞれのピクセルで得られたＥｓＡ，ＥｓＢ，・・・，ＥｓＮの平均を式（７’）のとおり，ＥｓＡ’，ＥｓＢ’，・・・，ＥｓＮ’とする。
ＥｓＡ（ｕ，ｖ）　＝　｜距離ｒ－距離Ａ｜・・・（７）
ＥｓＢ（ｕ，ｖ）　＝　｜距離ｒ－距離Ｂ｜
・
・
・
ＥｓＮ（ｕ，ｖ）　＝　｜距離ｒ－距離Ｎ｜
ＥｓＡ’＝［ＥｓＡ（１，０）＋・・・＋ＥｓＡ（Ｕ，Ｖ）］／（Ｕ×Ｖ）・・・（７’）
ＥｓＢ’＝［ＥｓＢ（１，０）＋・・・＋ＥｓＢ（Ｕ，Ｖ）］／（Ｕ×Ｖ）
・
・
・
ＥｓＮ’＝［ＥｓＮ（１，０）＋・・・＋ＥｓＮ（Ｕ，Ｖ）］／（Ｕ×Ｖ）
　この時，移動体１００の現在位置から実際に撮像した画像６０３の重みＷｉは，図５で説明した重みを用いて式（８）に示す通り算出する。
Ｗｉ　＝　λｃ・λＩ・λｒ・λＴ・・・（８）
　また，移動体１００の現在位置から実際に撮像した距離画像６０４の重みＷｓは，前述の図５で説明した重みを用いて式（９）に示す通り算出する。
Ｗｓ　＝　λｃ・λｓ・λｒ・λＴ・・・（９）
　従って，それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎと仮想距離画像６０２Ａ，６０２Ｂ，・・・，６０２Ｎで得られた平均誤差ＥｉＡ’，ＥｉＢ’，・・・，ＥｉＮ’とＥｓＡ’，ＥｓＢ’，・・・，ＥｓＮ’の組み合わせを比較誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮとする。式（１０）で比較誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮの詳細を表す。
ＥｆＡ　＝　（Ｗｉ・ＥｉＡ’＋Ｗｓ・ＥｓＡ’）／（Ｗｉ＋Ｗｓ）・・・（１０）
ＥｆＢ　＝　（Ｗｉ・ＥｉＢ’＋Ｗｓ・ＥｓＢ’）／（Ｗｉ＋Ｗｓ）
・
・
・
ＥｆＮ　＝　（Ｗｉ・ＥｉＮ’＋Ｗｓ・ＥｓＮ’）／（Ｗｉ＋Ｗｓ）
　位置補正手段２１１では、式（１０）で得られた比較誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮの中で最小誤差を求め，最小誤差の仮想位置を現在位置とする。例えば，位置補正手段２１１で求めた最小誤差がＥｆＡの場合，現在位置推定手段２０１で推定した現在位置３０１（Ｘ，Ｙ，Ｚ）を式（１１）に示す通り，仮想位置３０２Ａ（ＸＡ，ＹＡ，ＺＡ）とする。
　　　　　Ｘ＝ＸＡ・・・（１１）
　　　　　Ｙ＝ＹＡ
　　　　　Ｚ＝ＺＡ
　また，式（１０）で得られた誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮに基づいて，位置補正手段２１１で位置を補正してもよい。例えば，式（１２）の通り，現在位置３０１を算出する。
Ｘ＝（ＸＡ／ＥｆＡ＋・・・＋ＸＮ／ＥｆＮ）／（１／ＥｆＡ＋・・・＋１／ＥｆＮ）
Ｙ＝（ＹＡ／ＥｆＡ＋・・・＋ＹＮ／ＥｆＮ）／（１／ＥｆＡ＋・・・＋１／ＥｆＮ）
Ｚ＝（ＺＡ／ＥｆＡ＋・・・＋ＺＮ／ＥｆＮ）／（１／ＥｆＡ＋・・・＋１／ＥｆＮ）
・・・（１２）
　図７Ａと図７Ｂを用いて本発明の実施例について説明する。
　図７Ａの道路７００ａは移動体１００が走行中の道路である。誤差７０１ａは，現在位置誤差推定手段２１０ｄで得られた現在位置誤差である。影７０２は，移動体１００が走行している道路７００の上にある影である。実施例の説明のため，以下の条件（ａ１）～（ｆ１）とする。
（ａ１）撮像装置キャリブレーション誤差推定手段２１０ａで得られた誤差が大きい
（ｂ１）特徴点抽出手段２１０ｂで抽出した特徴点数が多い
（ｃ１）視差画像誤差推定手段２１０ｃで得られた視差画像誤差が小さい
（ｄ１）現在位置誤差推定手段２１０ｄで推定した現在位置誤差７０１ａが小さい
（ｅ１）影７０２のため，画像強度取得手段２１０ｅで得られた強度の低いピクセルの数が多い
（ｆ１）画像解像度取得手段２１０ｆで取得した画像解像度が高い
　上記条件（ａ１）～（ｆ１）によれば，現在位置誤差推定手段２１０ｄで推定した誤差７０１ａが小さいため，仮想位置作成手段２０３で仮想位置を作成せず，現在位置推定手段２０１で推定した現在位置のみから環境情報抽出手段２０２，地図参照２０４，仮想画像作成２０５，画像マッチング２０６を実施する。式（６）と式（７）で平均誤差を算出し，式（８）と式（９）で仮想画像と仮想距離画像のそれぞれの重みを算出する。ここで，撮像装置キャリブレーション誤差推定手段２１０ａで得られた撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーション誤差が大きいため，λｃの重みが低くなる。影７０２の影響で画像強度取得手段２１０ｅで取得した撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎでの暗いピクセルが多いため，重みλＩが低くなる。しかし，画像解像度取得手段２１０ｆで得られた撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの解像度が高いため，重みλｒが高くなり，視差画像誤差推定手段２１０ｃで得られた誤差が小さかったため，重みλｓが高くなった。従って，式（８）と式（９）で仮想画像の重みＷｉと仮想距離画像の重みＷｓを算出する場合，Ｗｉ＜Ｗｓとなる。
　一方，図７Ｂの場合，移動体が走行中の道路７００ｂに横断歩道７０３があり，現在位置推定手段２０１で推定した現在位置誤差７０１ｂが大きい。そして、横断歩道７０３を用いて撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーション補正ができたため，
（ａ２）撮像装置キャリブレーション誤差推定手段２１０ａで得られたキャリブレーション誤差が小さい
（ｂ２）特徴点抽出手段２１０ｂで抽出した特徴点数が多い
（ｃ２）視差画像誤差推定手段２１０ｃで得られた視差画像誤差が大きい
（ｄ２）現在位置誤差推定手段２１０ｄで推定した現在位置誤差７０１ｂが大きい
（ｅ２）明るすぎるピクセルと暗すぎるピクセルはないため，画像強度取得手段２１０ｅで得られた強度の低いピクセルか強度の高いピクセルが少ない
（ｆ２）画像解像度取得手段２１０ｆで取得した画像解像度が高い
　上記条件（ａ２）～（ｆ２）によれば，図７Ｂの場合，現在位置誤差推定手段２１０ｄで推定した誤差７０１ｂが大きいため，仮想位置作成手段２０３で仮想位置を複数作成し，それぞれの仮想位置から地図参照手段２０４で地図情報を得て，それぞれの仮想位置から仮想画像作成手段２０５で仮想画像を作成する。そして、それぞれの仮想位置から作成した仮想画像を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで実際に取得した画像とマッチングし，誤差を算出する。ここで，式（６）と式（７）で平均誤差を算出し，式（８）と式（９）で仮想画像と仮想距離画像のそれぞれの重みを算出する。
　図７Ｂの場合，横断歩道７０３を用いて撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーション補正ができるため，撮像装置キャリブレーション誤差推定手段２１０ａで得られたキャリブレーション誤差が小さくなり，λｃの重みが高くなる。そして、画像強度取得手段２１０ｅで得られた強度の低いピクセルか強度の高いピクセルが少ないため，重みλＩが高くなる。また、画像解像度取得手段２１０ｆで取得した画像解像度が高いため，重みλｒが高くなる。一方，距離画像を作成した時の視差画像誤差推定手段２１０ｃで得られた誤差が大きかったため，重みλｓが低くなった。従って，式（８）と式（９）で算出する仮想画像の重みＷｉが仮想距離画像の重みＷｓより高い（Ｗｉ＞Ｗｓ）。
　図８は異常な場合を説明する図である。図８は、普段であれば地図情報のとおりに制限なく道路８００を走行できるが、事故などの影響で一時的に車線が制限された状況を示している。
　道路８００は移動体１００が走行している道路である。走行可能な領域８０１ａは道路８００の走行可能な領域である。走行禁止領域８０１ｂは，道路８００の一時的に走行できない領域である。障害物８０２は，走行禁止領域８０１を区別するための障害物である。経路８０３は，障害物８０２を避けるために作成された経路である。移動体８０４は走行可能な領域８０１ａの先行車である。推定した誤差８０５は移動体１００の現在位置誤差推定手段２１０ｄで推定した誤差である。
　移動体１００が道路８００を走行している時に，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎを用いて障害物８０２を検出する。この時，移動体１００の現在位置誤差推定手段２１０ｄで推定した誤差８０５は大きくないが，障害物８０２のため，走行可能な領域８０１ａは道路８００より狭くなっている。従って，普段道路８００を走行する時より，正確に位置推定を行いながら走行することが必要となる。このため，仮想位置作成手段２０３で作成する仮想位置の個数と仮想画像作成手段２０５で作成する仮想画像の個数を増やす。そして、移動体１００が経路８０３の通り、走行可能な領域８０１ａを走行した後は，道路８００の幅が元に戻り、走行可能な領域８０１ａよりも幅広くなる。したがって，通常どおり現在位置誤差推定手段２１０ｄで推定した誤差８０５に基づいて仮想位置を作成する。
　簡単のため，障害物８０２を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで認識したとしたが，障害物８０２の情報を地図１９に記載してもよい。例えば，移動体８０４が障害物８０２を認識してすぐに地図１９に記載し，他車と共有してもよい。また，地図１９に記載せず，障害物８０２情報をＶ２Ｖ（Ｖｅｈｉｃｌｅ　ｔｏ　Ｖｅｈｉｃｌｅ）技術を用いて領域８０１ａを走行済みの移動体８０４から、これから走行する移動体１００へ直接送信してもよい。また，Ｖ２Ｖ技術で障害物８０２の情報を送信せず，仮想位置を増やすための指令を移動体８０４から移動体１００に送信してもよい。
　図９は障害物の影響と対策を説明する。
　道路９００は，移動体１００が走行中の道路である。位置誤差９０１は，重み付け手段２１０の現在位置誤差推定手段２１０ｄで推定した移動体１００の現在位置誤差である。画像９０２は，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像である。
　静止物９０３は道路９００にある静止物であり，画像９０２上で静止物９０３ａとして写っている。障害物９０４は道路９００にある障害物であり，画像９０２上で障害物９０４ａとして写っている。静止物９０５は道路９００にある静止物であり，画像９０２上で静止物９０５ａとして写っている。
　点９０６は画像９０２に投影した道路９００上での地図情報である。点９０７は画像９０２に投影した静止物９０３上での地図情報である。点９０８は画像９０２に投影した静止物９０５上での地図情報である。この時，障害物９０４ａは静止物９０５ａの前に写っている。地図１９の情報を（ｘ，ｙ，ｚ）の形とした時，点９０８のピクセル情報は（ｕ，ｖ，距離Ａ）となるが，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで検出した静止物９０５への実際の距離は距離Ａではなく，静止物９０５の前にある障害物９０４への距離Ｂとなってしまう。従って，位置補正手段２１１による位置誤差が大きくなる。
　前述の障害物による位置誤差を避けるための対策を説明する。移動体１００が走行中環境の地図情報を区別して，マッチングを行う。例えば，道路９００上には他車や歩行者が存在する確率が高いため，道路９００上での地図情報９０６を位置補正手段２１１で使わない。また，静止物９０５上での地図情報が周囲の障害物に影響される可能性が高いため，静止物９０５上での地図情報９０８も使わない。従って，移動体１００の現在位置推定手段２０１で推定した位置により，ある高さ以上の地図情報のみを用いて、位置補正手段２１１で位置推定を行う。すると，障害物の影響を受ける確率の低い点９０７のみで位置補正手段２１１で位置推定を行うため，移動体１００が走行中環境の障害物の影響を削減できる。
　また，移動体１００の位置推定だけでなく，前述の対策で障害物の位置推定や検出もできる。例えば，点９０７のみで位置補正手段２１１で高精度に位置推定を行った後，位置補正手段２１１で使わなかった点９０７と点９０８への距離を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで計測する。ここで，例えば，地図１９の情報による静止物９０５へ距離は距離Ｂであるが，実際に計測した距離は距離Ａとなってしまうため，画像９０２上での点９０８は外れ値，又は，障害物であることが明確になる。従って，点９０７のみで位置補正手段２１１で推定した位置と，点９０８を用いて検出した障害物９０４の位置の正確さが高くなる。
　簡単のため，点９０８を一点にして説明したが，複数点であってもよい。複数点の場合，それぞれの点の距離と点９０７のみで位置補正手段２１１で推定した位置から計測した距離を比較し，障害物検出を行ってもよい。また，画像９０２を様々な領域に分けて，ある領域内の点９０８をひとつの障害物としてもよい。また，信頼度を向上させるため，図９の時の地図情報１９と撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの情報とは限らず，時系列に観測し，統計的に障害物検出を行ってもよい。また，移動体１００の現在位置誤差９０１に基づいて障害物検出を行う。例えば，移動体１００の現在位置誤差９０１が低い場合，位置補正手段２１１で位置推定し，障害物かどうか判断する。一方，移動体１００の現在位置誤差９０１が高い場合，位置補正手段２１１で位置推定を何回か行って，信頼度が高くなった後に障害物検出を行う。
　上述の実施形態では、現在位置推定手段で得られた移動体１００の現在位置誤差の情報と、撮像装置により取得された情報である撮像装置のキャリブレーション誤差の情報と、撮像装置により撮像された画像から抽出された画像上の特徴点の情報と、撮像装置により撮像された画像から取得された視差画像誤差の情報と、撮像装置により撮像された画像から取得された画像強度の情報と、撮像装置により撮像された画像から取得された画像解像度の情報を用いて重みを算出しているが、これらの情報の少なくとも一つに基づいて算出してもよい。
　上述の実施形態によれば、撮像装置１２により取得された情報と、現在位置推定手段で得られた移動体１００の現在位置誤差の情報に基づいて重みを算出し、複数の仮想画像と実際に撮像した画像とをそれぞれ比較して算出した比較誤差に重み付けを行い、複数の仮想位置の中から、重み付けされた比較誤差が最も小さい仮想位置を移動体１００の現在位置とする処理を行っている。重みは、従来のように固定ではなく、状況に応じた値が算出されるので、情報の信頼性が高くなり、高精度の位置推定を行うことができる。したがって、例えば雨天や日差しが強い条件（逆光、照り返し、反射）や撮像装置のキャリブレーション誤差が大きい場合などのように、画像によるマッチングが難しい状況でも，走行状況に応じて高精度にマッチングができる。
　以上、本発明の実施形態について詳述したが、本発明は、前記の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の精神を逸脱しない範囲で、種々の設計変更を行うことができるものである。例えば、前記した実施の形態は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施形態の構成の一部を他の実施形態の構成に置き換えることが可能であり、また、ある実施形態の構成に他の実施形態の構成を加えることも可能である。さらに、各実施形態の構成の一部について、他の構成の追加・削除・置換をすることが可能である。
１　位置推定装置
１２　撮像装置
１３　情報処理装置
１４　画像処理部
１５　制御部
１６　メモリ
１７　表示部
１００　移動体

Claims (5)

  1. 　撮像装置を搭載した移動体の現在位置を推定する位置推定装置であって、
    　前記移動体の現在位置を推定する現在位置推定手段と、
    　該現在位置推定手段により推定された現在位置に基づいて複数の仮想位置を作成する仮想位置作成手段と、
    　前記撮像装置により前記複数の仮想位置で撮像したと仮定した場合の複数の仮想画像をそれぞれ作成する仮想画像作成手段と、
    　前記複数の仮想画像を前記撮像装置により前記現在位置で撮像された画像と比較してそれぞれの比較誤差を算出する画像マッチング手段と、
    　前記撮像装置により取得された情報と、前記現在位置推定手段で得られた前記移動体の現在位置誤差の情報との少なくとも一つに基づいて重みを算出し、該重みを用いて前記それぞれの比較誤差に重み付けを行う重み付け手段と、
    　前記現在位置推定手段で推定されている現在位置を前記重み付けされた比較誤差に基づいて補正する位置補正手段と、
    　を有することを特徴とする位置推定装置。
  2. 　前記位置補正手段は、前記複数の仮想位置の中から前記重み付けされた比較誤差が最も小さい仮想位置を前記移動体の現在位置とすることを特徴とする請求項１に記載の位置推定装置。
  3. 　前記移動体の現在位置誤差の情報に基づいて、前記仮想位置の個数と現在位置からの間隔を調整することを特徴とする請求項１または２に記載の位置推定装置。
  4. 　前記移動体の現在位置誤差の情報に基づいて、前記撮像装置の画像解像度を変更することを特徴とする請求項１から請求項３のいずれか一項に記載の位置推定装置。
  5. 　前記撮像装置により取得された情報には、
    　前記撮像装置のキャリブレーション誤差の情報と、
    　前記撮像装置により撮像された画像上の特徴点の情報と、
    　前記撮像装置により撮像された画像から視差画像を作成する際の視差画像誤差の情報と、
    　前記撮像装置により撮像された画像から取得された画像強度の情報と、
    　前記撮像装置により撮像された画像から取得された画像解像度の情報と、
    　の少なくとも一つが含まれることを特徴とする請求項１から請求項4のいずれか一項に記載の位置推定装置。
PCT/JP2019/013964 2018-04-27 2019-03-29 位置推定装置 WO2019208101A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
US17/049,656 US11538241B2 (en) 2018-04-27 2019-03-29 Position estimating device
DE112019001542.7T DE112019001542T5 (de) 2018-04-27 2019-03-29 Positionsschätzvorrichtung

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018087826A JP7190261B2 (ja) 2018-04-27 2018-04-27 位置推定装置
JP2018-087826 2018-04-27

Publications (1)

Publication Number Publication Date
WO2019208101A1 true WO2019208101A1 (ja) 2019-10-31

Family

ID=68295223

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/013964 WO2019208101A1 (ja) 2018-04-27 2019-03-29 位置推定装置

Country Status (4)

Country Link
US (1) US11538241B2 (ja)
JP (1) JP7190261B2 (ja)
DE (1) DE112019001542T5 (ja)
WO (1) WO2019208101A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7274707B1 (ja) 2021-12-13 2023-05-17 アイサンテクノロジー株式会社 評価システム、コンピュータプログラム、及び評価方法

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102019002662A1 (de) * 2019-04-10 2020-10-15 Friedrich-Alexander-Universität Erlangen-Nürnberg Verfahren zur Auswertung von Radarsystemen
US11372091B2 (en) * 2019-06-28 2022-06-28 Toyota Research Institute, Inc. Systems and methods for correcting parallax
JP7409330B2 (ja) * 2021-01-28 2024-01-09 トヨタ自動車株式会社 自己位置推定精度検証方法、自己位置推定システム
JP7468392B2 (ja) 2021-02-12 2024-04-16 株式会社豊田自動織機 自己位置推定装置
JP2022132882A (ja) * 2021-03-01 2022-09-13 キヤノン株式会社 ナビゲーションシステム、ナビゲーション方法およびプログラム

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000090393A (ja) * 1998-09-16 2000-03-31 Sumitomo Electric Ind Ltd 車載型走行路環境認識装置
JP2005326168A (ja) * 2004-05-12 2005-11-24 Fuji Photo Film Co Ltd 運転支援システム、車両、および運転支援方法
JP2007255979A (ja) * 2006-03-22 2007-10-04 Nissan Motor Co Ltd 物体検出方法および物体検出装置
JP2008175717A (ja) * 2007-01-19 2008-07-31 Xanavi Informatics Corp 現在位置算出装置、現在位置算出方法
JP2010286298A (ja) * 2009-06-10 2010-12-24 Sanyo Electric Co Ltd 位置表示装置
WO2013133129A1 (ja) * 2012-03-06 2013-09-12 日産自動車株式会社 移動物体位置姿勢推定装置及び移動物体位置姿勢推定方法
JP2016188806A (ja) * 2015-03-30 2016-11-04 シャープ株式会社 移動体及びシステム

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8126642B2 (en) * 2008-10-24 2012-02-28 Gray & Company, Inc. Control and systems for autonomously driven vehicles
US10533850B2 (en) * 2013-07-12 2020-01-14 Magic Leap, Inc. Method and system for inserting recognized object data into a virtual world
US9802317B1 (en) * 2015-04-24 2017-10-31 X Development Llc Methods and systems for remote perception assistance to facilitate robotic object manipulation
US9652896B1 (en) * 2015-10-30 2017-05-16 Snap Inc. Image based tracking in augmented reality systems
JP6232649B2 (ja) * 2016-02-18 2017-11-22 国立大学法人名古屋大学 仮想空間表示システム
DE112016006521T5 (de) * 2016-03-28 2018-11-22 Hitachi High-Technologies Corporation Ladungsträgerstrahlvorrichtung und Verfahren zum Einstellen der Ladungsträgerstrahlvorrichtung
KR20180051836A (ko) * 2016-11-09 2018-05-17 삼성전자주식회사 주행 차량을 위한 가상의 주행 차선을 생성하는 방법 및 장치
DE102017222534B3 (de) * 2017-12-12 2019-06-13 Volkswagen Aktiengesellschaft Verfahren, computerlesbares Speichermedium mit Instruktionen, Vorrichtung und System zum Einmessen einer Augmented-Reality-Brille in einem Fahrzeug, für das Verfahren geeignetes Fahrzeug und für das Verfahren geeignete Augmented-Reality-Brille
US10657721B2 (en) * 2018-02-09 2020-05-19 Paccar Inc Systems and methods for providing augmented reality support for vehicle service operations
JP6687306B2 (ja) * 2018-03-13 2020-04-22 三菱電機株式会社 表示制御装置、表示装置、及び表示制御方法
US10767997B1 (en) * 2019-02-25 2020-09-08 Qualcomm Incorporated Systems and methods for providing immersive extended reality experiences on moving platforms
US10719966B1 (en) * 2019-06-11 2020-07-21 Allstate Insurance Company Accident re-creation using augmented reality

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000090393A (ja) * 1998-09-16 2000-03-31 Sumitomo Electric Ind Ltd 車載型走行路環境認識装置
JP2005326168A (ja) * 2004-05-12 2005-11-24 Fuji Photo Film Co Ltd 運転支援システム、車両、および運転支援方法
JP2007255979A (ja) * 2006-03-22 2007-10-04 Nissan Motor Co Ltd 物体検出方法および物体検出装置
JP2008175717A (ja) * 2007-01-19 2008-07-31 Xanavi Informatics Corp 現在位置算出装置、現在位置算出方法
JP2010286298A (ja) * 2009-06-10 2010-12-24 Sanyo Electric Co Ltd 位置表示装置
WO2013133129A1 (ja) * 2012-03-06 2013-09-12 日産自動車株式会社 移動物体位置姿勢推定装置及び移動物体位置姿勢推定方法
JP2016188806A (ja) * 2015-03-30 2016-11-04 シャープ株式会社 移動体及びシステム

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7274707B1 (ja) 2021-12-13 2023-05-17 アイサンテクノロジー株式会社 評価システム、コンピュータプログラム、及び評価方法
JP2023087496A (ja) * 2021-12-13 2023-06-23 アイサンテクノロジー株式会社 評価システム、コンピュータプログラム、及び評価方法

Also Published As

Publication number Publication date
JP7190261B2 (ja) 2022-12-15
US20210256260A1 (en) 2021-08-19
JP2019191133A (ja) 2019-10-31
US11538241B2 (en) 2022-12-27
DE112019001542T5 (de) 2020-12-10

Similar Documents

Publication Publication Date Title
CA3028653C (en) Methods and systems for color point cloud generation
WO2019208101A1 (ja) 位置推定装置
US10860871B2 (en) Integrated sensor calibration in natural scenes
US11061122B2 (en) High-definition map acquisition system
CN111448478A (zh) 用于基于障碍物检测校正高清地图的系统和方法
US11151729B2 (en) Mobile entity position estimation device and position estimation method
JP6552448B2 (ja) 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム
US20200341150A1 (en) Systems and methods for constructing a high-definition map based on landmarks
US11132813B2 (en) Distance estimation apparatus and method
WO2018225480A1 (ja) 位置推定装置
WO2020230410A1 (ja) 移動体
WO2020113425A1 (en) Systems and methods for constructing high-definition map
WO2022133986A1 (en) Accuracy estimation method and system
AU2018102199A4 (en) Methods and systems for color point cloud generation
JP7302966B2 (ja) 移動体
US20230421739A1 (en) Robust Stereo Camera Image Processing Method and System

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

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 19794030

Country of ref document: EP

Kind code of ref document: A1