WO2020137313A1 - 位置測定装置 - Google Patents

位置測定装置 Download PDF

Info

Publication number
WO2020137313A1
WO2020137313A1 PCT/JP2019/046198 JP2019046198W WO2020137313A1 WO 2020137313 A1 WO2020137313 A1 WO 2020137313A1 JP 2019046198 W JP2019046198 W JP 2019046198W WO 2020137313 A1 WO2020137313 A1 WO 2020137313A1
Authority
WO
WIPO (PCT)
Prior art keywords
control unit
image
moving body
captured image
unit
Prior art date
Application number
PCT/JP2019/046198
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 JP2020562956A priority Critical patent/JPWO2020137313A1/ja
Priority to EP19906078.1A priority patent/EP3904995A4/en
Publication of WO2020137313A1 publication Critical patent/WO2020137313A1/ja
Priority to US17/357,173 priority patent/US20210318690A1/en

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G06T2207/10021—Stereoscopic video; Stereoscopic image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Definitions

  • the present disclosure relates to a position measuring device that measures the position of a moving body.
  • Patent Document 1 acquires an image captured by an image capturing unit provided on a moving body, and associates feature points included in an image captured before the movement and an image captured after the movement with each other.
  • a tracking unit that performs tracking
  • an area estimation unit that acquires information related to movement, and based on this information, estimates a region in which the two-dimensional position change of the feature point seen from the moving body is small before and after the movement, and tracking.
  • Disclosed is an information processing device including an estimation processing unit that estimates a self-position of a moving body by using feature points that are associated with each other and that are within an area.
  • the present disclosure provides a position measurement device that efficiently measures the position of a moving body based on motion information of the moving body.
  • the position measuring device measures the position of a moving body.
  • the position measuring device is mounted on a moving body, and an image capturing unit that captures a captured image by capturing an environment around the moving body, a detecting unit that detects motion information indicating the motion of the moving body, and a feature point from the captured image.
  • a storage unit that stores position information indicating a spatial position of the feature point in the environment.
  • the control unit searches the captured image for a position on the image corresponding to the spatial position indicated by the positional information, calculates the positional relationship between the spatial position indicated by the positional information and the imaging unit, and detects the position of the moving body in the environment. Calculate the position.
  • the control unit sets a reference point for searching the spatial position on the captured image, according to the motion information detected by the detection unit.
  • the position measuring device measures the position of a moving body.
  • the position measuring device is mounted on a moving body, and an image capturing unit that captures a captured image by capturing an environment around the moving body, a detecting unit that detects motion information indicating the motion of the moving body, and a feature point from the captured image.
  • a storage unit that stores position information indicating a spatial position of the feature point in the environment.
  • the control unit searches the captured image for a position on the image corresponding to the spatial position indicated by the positional information, calculates the positional relationship between the spatial position indicated by the positional information and the imaging unit, and detects the position of the moving body in the environment. Calculate the position.
  • the control unit changes the search range for searching the spatial position on the captured image, according to the motion information detected by the detection unit.
  • the position measurement device of the present disclosure it is possible to efficiently measure the position of the moving body based on the movement information of the moving body.
  • FIG. 3 is a diagram for explaining the feature point matching process in the first embodiment.
  • FIG. 3 is a diagram for explaining a process of calculating the similarity of the feature amount according to the first embodiment.
  • the flowchart which shows the detailed flow of the step which calculates the estimated camera pose in Embodiment 1.
  • FIG. 6 is a diagram for explaining a step of specifying a search range according to the second embodiment.
  • FIG. 3 is a diagram for explaining a vibration detection process performed by the control unit.
  • the flowchart which shows the detailed flow of the feature point matching step in Embodiment 3.
  • the position measurement device is mounted on a moving body such as a manned cargo-handling vehicle, an automated guided vehicle (AGV), and an autonomous mobile baggage carrying robot. Measure the position.
  • a moving body such as a manned cargo-handling vehicle, an automated guided vehicle (AGV), and an autonomous mobile baggage carrying robot. Measure the position.
  • AGV automated guided vehicle
  • FIG. 1 is a diagram illustrating a configuration of the mobile unit 1.
  • the moving body 1 includes, for example, a luggage carrier 1a for carrying luggage.
  • the position measuring device 100 according to the present embodiment is mounted on the moving body 1.
  • the position measuring device 100 includes a camera 2 that captures an image of the surroundings of the moving body 1, and an inertial measurement device (Inertial measurement unit, hereinafter referred to as “IMU”) 3.
  • IMU Inertial measurement unit, hereinafter referred to as “IMU”) 3.
  • the IMU 3 is a device that detects the acceleration and the angular velocity of the moving body 1.
  • the position measuring device 100 can be applied with, for example, a Visual-SLAM (Simultaneous Localization and Mapping) technology that measures the self-position and generates 3D map information based on sequentially captured images.
  • a Visual-SLAM Simultaneous Localization and Mapping
  • the position measuring device 100 extracts feature points in the image captured by the camera 2.
  • the feature points include, for example, edges, corners, etc. of objects, roads, buildings, and the like.
  • the position measuring apparatus 100 creates the 3D map by converting the coordinates of the extracted feature points on the image into world coordinates and registering the map points corresponding to the feature points on the image in the world coordinate space.
  • the position measuring device 100 captures the surroundings of the moving body 1 with the camera 2 at a constant frame rate while moving the moving body 1, and makes the feature points on the image at the time of capturing correspond to the map points on the 3D map. Perform point matching processing.
  • the position measuring apparatus 100 determines the position and orientation of the camera 2 (hereinafter, referred to as “camera pose”) based on the geometrical positional relationship between the feature point of the current frame and the feature point of the immediately preceding frame. calculate.
  • the position measuring device 100 can determine the position of the position measuring device 100 and thus the position of the moving body 1 based on the calculated position of the camera 2.
  • the position information measured by the position measuring device 100 is stored in, for example, an external server and can be used for various data management in the environment through which the mobile body 1 has passed.
  • the position measuring device 100 may be used to move the mobile unit 1 based on the calculated position information of the mobile unit 1 and the created 3D map information.
  • FIG. 2 is a block diagram showing the configuration of the position measuring device 100.
  • the position measuring device 100 includes a camera 2, an IMU 3, a control unit 4, a storage unit 5, a communication interface (I/F) 7, and a drive unit 8.
  • I/F communication interface
  • the camera 2 is an example of the imaging unit of the present disclosure.
  • the camera 2 is installed in the mobile body 1, captures an image around the mobile body 1, and generates color image data and range image data.
  • the camera 2 may include a depth sensor such as an RGB-D camera or a stereo camera.
  • the camera 2 may be configured by an RGB camera that captures a color image and a ToF (Time of Flight) sensor that captures a distance image.
  • ToF Time of Flight
  • the IMU 3 is an example of the detection unit according to the present disclosure.
  • the IMU 3 includes an acceleration sensor that detects the acceleration of the moving body 1 and a gyro sensor that detects the angular velocity of the moving body 1.
  • the control unit 4 includes a general-purpose processor such as a CPU or MPU that realizes a predetermined function in cooperation with software.
  • the control unit 4 realizes various functions such as the feature point extraction unit 41, the feature point matching unit 42, the position calculation unit 44, and the map management unit 45 by reading and executing the program stored in the storage unit 5, The overall operation of the position measuring device 100 is controlled.
  • the control unit 4 executes a program that realizes the position measuring method of this embodiment or a program that realizes the SLAM algorithm.
  • the control unit 4 is not limited to one that realizes a predetermined function by cooperation of hardware and software, and is a hardware circuit such as FPGA, ASIC, or DSP designed as a dedicated circuit for realizing the predetermined function. It may be configured.
  • the storage unit 5 is a recording medium that records various information including programs and data necessary for realizing the functions of the position measuring device 100.
  • the storage unit 5 stores, for example, a 3D map 51 and image data.
  • the storage unit 5 is realized by, for example, a flash memory, a semiconductor memory device such as an SSD, a magnetic storage device such as a hard disk, or another storage device alone or in combination thereof.
  • the storage unit 5 may include a volatile memory such as SRAM or DRAM capable of operating at high speed for temporarily storing various information.
  • the volatile memory functions as, for example, a work area of the control unit 4 or a frame memory that temporarily stores image data for each frame.
  • the communication I/F 7 is an interface circuit for enabling communication connection between the position measuring device 100 and an external device such as the server 150 via the network 50.
  • the communication I/F 7 performs communication according to a standard such as IEEE802.3, IEEE802.11 or Wi-Fi.
  • the drive unit 8 is a mechanism that moves the moving body 1 according to an instruction from the control unit 4.
  • the drive unit 8 includes an engine drive circuit, a steering circuit, and a brake circuit that are connected to the tires of the moving body 1.
  • FIG. 3 is a flowchart showing the flow of the operation of the position measuring device 100. Each process of the flowchart of FIG. 3 is executed by the control unit 4 of the position measuring device 100.
  • the control unit 4 acquires a captured image at time t (S10).
  • the captured image is image data of the environment around the moving body 1 captured by the camera 2.
  • FIG. 4 is a diagram illustrating the captured image 10 and the characteristic points therein.
  • the circled portions indicate the feature points extracted from the captured image 10.
  • the control unit 4 extracts pixels or pixel groups whose luminance values or colors can be distinguished from surrounding pixels or pixel groups as feature points.
  • the feature points specify, for example, edges, corners and patterns of objects, roads and buildings.
  • a known FAST (Features from Accelerated Segment Test) technique may be used to detect the feature points from the captured image 10.
  • control unit 4 also performs a process of calculating the self-position of the moving body 1 and a process of creating the 3D map 51.
  • the control unit 4 functioning as the map management unit 45 converts the coordinates of the feature points on the captured image 10 into world coordinates, and registers the map points corresponding to the feature points on the captured image 10 in the world coordinate space.
  • map points corresponding to the characteristic points on the captured image 10, the camera frame showing the captured image 10, and the camera pose of the camera 2 when capturing the captured image are recorded.
  • Information on the created 3D map 51 is stored in the storage unit 5.
  • the control unit 4 can generate the 3D map 51 as illustrated in FIG. 5 by acquiring a captured image and registering feature points at each time interval ⁇ t while the moving body 1 is moving, for example.
  • FIG. 5 is a schematic diagram illustrating the 3D map 51. Map points corresponding to the feature points on the captured image 10 are registered in the 3D map 51. Each map point has world coordinates. In FIG. 5, map points are indicated by circles. The structural information indicated by the broken line in FIG. 5 is not recorded in the 3D map 51, but is shown for convenience of description.
  • the control unit 4 can reproduce the structural information as shown by the broken line by acquiring the captured image at each time interval ⁇ t and registering the feature points.
  • the control unit 4 functioning as the feature point matching unit 42 associates the feature points in the captured image 10 at the time t extracted in step S20 with the map points of the 3D map 51.
  • a feature point matching process is performed (S30).
  • the control unit 4 uses, for example, a known KLT (Kanade-Lucas-Tomasi) tracker technology as the feature point matching process, and the feature points in the captured image 10 at time t- ⁇ t and the captured image at time t.
  • the feature points in 10 may be associated with each other.
  • the control unit 4 functioning as the position calculation unit 44 calculates the camera pose at time t.
  • the control unit 4 can obtain the position (self-position) of the position measuring device 100 and eventually the moving body 1 based on the calculated camera pose (S40).
  • the camera pose at time t is calculated, for example, based on the geometrical positional relationship between the feature points in the image captured at time t and the feature points in the image captured at time t- ⁇ t.
  • the camera pose at time t is calculated, for example, based on the camera pose at time t- ⁇ t and the detection result of the IMU 3.
  • the control unit 4 repeats the above steps S10 to S40 at every predetermined time interval ⁇ t until it determines the end of processing (S50) (S60).
  • the determination of the process end is made, for example, when the user inputs a process end instruction.
  • the control unit 4 transmits information such as the created 3D map 51 to the server 150.
  • FIG. 6 is a flowchart showing a detailed flow of the feature point matching step S30.
  • the control unit 4 calculates a predicted camera pose at time t (S31).
  • FIG. 7 is a diagram for explaining the feature point matching process.
  • FIG. 7 shows a camera 2a having the camera pose calculated in step S40 at time t ⁇ 2 ⁇ t and a camera 2b having the camera pose calculated in step S40 at time t ⁇ t.
  • the camera pose of the camera 2c at time t is predicted based on the past camera poses of the cameras 2a and 2b and the measurement results of the IMU 3 up to the present time.
  • the cameras 2a, 2b, and 2c are the same as the camera 2, but are on different time axes, and are therefore assigned different reference numerals to be distinguished.
  • the cameras 2a, 2b, and 2c image a cube-shaped object 55.
  • the object 55 is an example of a component of the surrounding environment of the moving body 1 (see FIG. 5).
  • the captured image 10a is an image captured by the camera 2a at time t-2 ⁇ t.
  • the captured image 10a includes the feature points Fa1 and Fa2 (see step S20 in FIG. 3).
  • the captured image 10b is an image captured by the camera 2b at time t- ⁇ t.
  • the captured image 10b includes feature points Fb1 and Fb2. In order not to complicate the drawing, in FIG. 7, the images that should appear in the captured images 10a and 10b are omitted.
  • map points M1 and M2 are registered in the 3D map 51.
  • the map point M1 is registered on the 3D map 51 before time t- ⁇ t based on the feature point Fa1 or Fb1.
  • the map point M2 is registered on the 3D map 51 before time t- ⁇ t based on the feature point Fa2 or Fb2.
  • step S31 the control unit 4 selects one map point on the 3D map 51 (S32).
  • the control unit 4 projects the selected map point on the captured image, assuming that the camera 2 has the predicted camera pose calculated in step S31 (S33).
  • the image coordinates of the projection point are calculated by projectively converting the world coordinates of the map point on the 3D map 51. That is, the control unit 4 projects the selected map point on the image coordinate plane to obtain the image coordinates of the projected point.
  • Step S32 and step S33 will be described with reference to the example of FIG.
  • the control unit 4 selects the map point M1 on the 3D map 51.
  • step S33 the control unit 4 projects the map point M1 on the captured image 10c.
  • the projection point is P1.
  • the control unit 4 repeats steps S31 to S36 until all map points are projected (S37). For example, when the map point M2 is selected in step S32 in the next loop that is determined to be No in step S37, the control unit 4 projects the map point M2 to the projection point P2 in step S33.
  • the control unit 4 identifies the search range D around the projection point P1 (S34).
  • the search range D may be a rectangle centered on the projection point P1 and having a predetermined size, but is not limited thereto.
  • the search range D may be a circle centered on the projection point and having a predetermined radius.
  • Step S35 the control unit 4 calculates the degree of similarity between the feature amount of the projected point and the feature amount of the feature point within the search range D (S35).
  • Step S35 will be described with reference to FIG.
  • the captured image 10c in FIG. 8 is a captured image captured by the camera 2c at time t and acquired by the control unit 4 in step S10 shown in FIG.
  • the feature points extracted in step S20 shown in FIG. 3 are indicated by circles.
  • the projected point P1 projected in step 33 and corresponding to the map point M1 is shown by an X mark.
  • step S35 the control unit 4 calculates the degree of similarity between the feature amount of the projection point P1 and the feature amounts of the feature points Fc1, Fc3, Fc4, and Fc5 within the predetermined search range D around the projection point P1. To do.
  • the feature amount of the feature point is, for example, a SURF feature amount obtained by the SURF (Speeded-Up Robust Features) technique, a SIFT feature amount obtained by the SIFT (Scale-Invariant Feature Transform) technique, or an ORB (OrientedFAST and RotatedBRIEF). ) An ORB feature amount obtained by the technique.
  • the feature amount of a feature point is represented by, for example, a vector having one or more dimensions.
  • the SURF feature amount is represented by a 64-dimensional vector
  • the SIFT feature amount is represented by a 128-dimensional vector.
  • the feature amount of the projection point is acquired when the feature point is extracted from the captured image captured before the time t- ⁇ t, and is stored in the storage unit 5 together with the feature point.
  • the similarity calculated in step S35 is calculated as a distance such as a Euclidean distance between feature amounts.
  • the control unit 4 identifies the feature point corresponding to the projection point based on the similarity calculated in step S35 (S36).
  • the control unit 4 identifies the feature point Fc1 as a feature point having a feature amount similar to the feature amount of the projection point P1.
  • the feature point Fc1 at time t is matched as the one corresponding to the feature point Fb1 at time t- ⁇ t via the projection point P1 and the map point M1 (see FIG. 7).
  • step S36 if the similarity between the projection point and the feature point is less than a predetermined threshold value, the control unit 4 does not specify that the feature point is a feature point corresponding to the projection point.
  • the control unit 4 does not specify that the feature point is a feature point corresponding to the projection point.
  • step S36 when there are a plurality of feature points having a similarity between the projection point and the threshold value or more in the search range D, the control unit 4 sets the feature point having the highest similarity as the feature point corresponding to the projection point. Identify.
  • step S36 the control unit 4 determines whether or not all the map points in the 3D map 51 are projected on the captured image 10c (S37). If all map points have not been projected (No in S37), the process returns to step S32, and the control unit 4 selects one map point that has not been projected and executes the processes of steps S33 to S37. If all are projected (Yes in S37), the feature point matching S30 ends.
  • step S31 for calculating the predicted camera pose at time t will be described.
  • step t40 When the change of the camera pose is constant over time, in other words, when the mobile body 1 moves so that the change of the camera pose is constant over time, at step t40 (FIG.
  • the camera pose of the camera 2c at time t is calculated using the difference between the camera poses of the cameras 2a and 2b (see FIG. 7) calculated in step 3), the projection point P1 and the feature point Fc1 corresponding to the projection point P1 ( (See FIG. 8) with high accuracy.
  • the change in camera pose may not be constant over time because the camera pose shakes or the moving body 1 accelerates or rotates.
  • the positions of the projection point P1 and the feature point Fc1 that should correspond thereto are dissociated on the captured image 10c. Even if such dissociation occurs, if the feature point Fc1 exists in the search range D, feature point matching can be performed.
  • the projection point P1 is projected to a place far from the feature point Fc1, and the feature point Fc1 is located in the search range D.
  • the feature point Fc1 that should originally correspond to the projection point P1 cannot be made to correspond to the projection point P1, and the feature point matching fails.
  • the acceleration and/or the angular velocity measured by the IMU 3 shown in FIG. 2 are used to predict the camera pose.
  • the control unit 4 can efficiently perform feature point matching even when the moving body 1 accelerates or rotates.
  • FIG. 9 is a flowchart showing the detailed flow of step S31 for calculating the predicted camera pose at time t. Each process of the flowchart of FIG. 9 is executed by the control unit 4 functioning as the camera pose prediction unit 43 shown in FIG.
  • control unit 4 acquires from the IMU 3 the acceleration and the angular velocity of the mobile unit 1 between time t- ⁇ t and time t (S311). Next, the control unit 4 integrates the acceleration and the angular velocity with respect to time to calculate the amount of change in the camera pose from time t- ⁇ t to time t (S312).
  • step S313 acquires the camera pose calculated at time t- ⁇ t (S313).
  • the camera pose acquired in step S313 is calculated by the control unit 4 in a step corresponding to step S4 (see FIG. 3) at time t- ⁇ t. Note that step S313 may be performed before step S312 or before step S311.
  • control unit 4 determines the time based on the camera pose at time t ⁇ t acquired in step S313 and the amount of change in the camera pose from time t ⁇ t to time t calculated in step S312.
  • a predicted camera pose at t is calculated (S314).
  • the acceleration and/or the angular velocity measured by the IMU 3 are reflected in the prediction of the camera pose, and the feature point matching can be efficiently performed even when the moving body 1 accelerates or rotates. Becomes
  • the position measuring device 100 measures the position of the moving body 1.
  • the position measuring device 100 is mounted on the moving body 1 and detects a camera 2 for picking up an environment around the moving body 1 to obtain a picked-up image and motion information such as acceleration and angular velocity indicating the movement of the moving body 1.
  • the IMU 3 includes a control unit 4 that extracts feature points from the captured images 10a and 10b, and a storage unit 5 that stores map points M1 and M2 indicating spatial positions of the feature points in the environment.
  • the control unit 4 searches for a position on the image corresponding to the spatial position indicated by the map points M1 and M2 on the captured image 10c (S30), and detects a position between the spatial position indicated by the map points M1 and M2 and the camera 2. The positional relationship is calculated to calculate the position of the moving body 1 in the environment (S40). The control unit 4 sets the reference point P1 for searching the spatial position on the captured image 10c according to the motion information detected by the IMU 3.
  • the position measuring device 100 even when the moving body 1 accelerates or rotates, the spatial position is searched on the captured image 10c according to the motion information detected by the IMU 3, so that the position is efficiently searched. Feature point matching can be performed.
  • the IMU 3 may detect motion information between the first time t ⁇ t and the second time t when the camera 2 temporally moves forward and backward.
  • the control unit 4 can predict the camera pose at the second time t from the camera pose at the first time t- ⁇ t according to the motion information (S31).
  • the IMU 3 includes at least one of an inertial measurement device, an acceleration sensor and a gyro sensor.
  • the captured image includes a range image and a color image.
  • FIG. 10 is a block diagram showing a configuration of the position measuring device 200 according to the second embodiment of the present disclosure.
  • the position measuring device 200 has the same configuration as the position measuring device 100 of the first embodiment except that the processing performed by the control unit 4 is different from that of the first embodiment.
  • the control unit 4 of the position measuring device 200 calculates the position of the moving body 1 by executing steps S10 to S60 as shown in FIG. However, in the second embodiment, the content of the feature point matching step S30 is different from the first embodiment.
  • FIG. 11 is a flowchart showing a detailed flow of the feature point matching step in the second embodiment. Compared to FIG. 6 of the first embodiment, in the second embodiment, step S34b is different from the search range identification step S34 of the first embodiment.
  • step S34b the control unit 4 functioning as the feature point matching unit 242 specifies the search range D based on the acquired measurement result of the IMU 3, for example, the angular velocity.
  • the control unit 4 changes the size of the search range D according to the measurement result of the IMU 3, for example, the value of the angular velocity.
  • FIG. 12 is a diagram for explaining step S34b for specifying the search range D.
  • FIG. 12 shows the camera 2 and the captured image 10 captured by the camera 2.
  • the x-axis, the y-axis, and the z-axis which are orthogonal to each other are coordinate axes of a camera coordinate system whose origin is the optical center of the camera 2.
  • the optical center of the camera 2 is, for example, the center of the lens of the camera 2.
  • the z-axis is the optical axis of the camera 2.
  • the captured image 10 captured by the camera 2 is in the image plane.
  • the points in the captured image 10 are represented by u-coordinates and v-coordinates in mutually orthogonal image coordinate systems.
  • the position of the map point M in the 3D map 51 can be represented by a camera coordinate system or world coordinates X, Y, and Z.
  • the map point M is projected on the captured image 10 in step S33 of FIG. In FIG. 12, the projection point is represented by P.
  • the control unit 4 sets the search range D to the length u0 in the u direction with the projection point P as the center and the length in the v direction in the acquisition step S34b. It is a rectangle of size v0.
  • u0 and v0 are initial values of the lengths of the predetermined search range D in the u and v directions.
  • the lengths in the u direction and the v direction are represented by the number of pixels, for example.
  • the control unit 4 sets the length of the search range D in the u direction to u1 which is larger than u0. To do. The difference between u1 and u0 becomes larger as the angular velocity around the y-axis increases, for example.
  • the control unit 4 sets the length of the search range D in the v direction to v1 larger than v0. And The difference between v1 and v0 becomes larger as the angular velocity around the x-axis increases, for example.
  • the control unit 4 rotates the search range D in the rolling direction.
  • the rotation angle is set to be larger as the angular velocity around the z axis is larger, for example.
  • the control unit 4 may make the size of the search range D larger than the initial value (u0 ⁇ v0) when the vibration is detected based on the acceleration and/or the angular velocity detected by the IMU 3. For example, as shown in FIG. 13, when the positive/negative of the acceleration ay in the y-axis direction is changed over a predetermined threshold number of times from time t ⁇ t to t, the control unit 4 causes the IMU 3 and thus the moving body 1 to vibrate in the y-axis direction. I judge that I did. When it is determined that the moving body 1 vibrates in the y-axis direction, the control unit 4 sets the length of the search range D in the v direction to v1 which is larger than the initial value v0 (see FIG. 12).
  • the control unit 4 determines the enlargement ratio v1/v0 of the search range D in the v direction on the basis of the maximum value ay1 of the absolute value of the acceleration between time t ⁇ t and time t. To do. For example, the control unit 4 increases v1/v0 as ay1 increases.
  • the control unit 4 determines that the IMU 3 and thus the moving body 1 vibrates in the x-axis direction when the positive/negative of the acceleration ax in the x-axis direction is exchanged a predetermined threshold number of times or more from time t- ⁇ t to t. To do.
  • the control unit 4 sets the length of the search range D in the u direction to u1, which is larger than the initial value u0.
  • the control unit 4 changes the search range D in which the spatial position is searched on the captured image 10c according to the motion information detected by the IMU 3 (S34b).
  • the current frame (imaged image at time t) in the current frame (imaged image at time t) to be associated with the feature point in the previous frame (time t- ⁇ t) It is possible to prevent a situation in which the feature points do not fall within the search range D. Therefore, the efficiency of the feature point matching and the accuracy of the calculation of the position of the moving body 1 are improved.
  • control unit 4 calculates the position of the moving body 1 by executing steps S10 to S60 as shown in FIG.
  • the content of the feature point matching step S30 is different from the first embodiment.
  • FIG. 14 is a flowchart showing the detailed flow of the feature point matching step in the third embodiment. Compared to FIG. 6 of the first embodiment, in the third embodiment, step S34c is different from the search range identifying step S34 of the first embodiment.
  • step S34c the control unit 4 specifies the search range D based on the acquired measurement result of the IMU 3, for example, the angular velocity.
  • 15a and 15b are diagrams for explaining step S34c for specifying the search range D. As shown in steps S10 and S60 of FIG. 3, the control unit 4 acquires captured images at regular time intervals ⁇ t.
  • FIG. 15a is a diagram illustrating a captured image 310a captured at time t- ⁇ t.
  • FIG. 15b is a diagram illustrating a captured image 310b captured at time t.
  • 15a and 15b show the camera coordinates x, y and z and the image coordinates u and v.
  • the circled portions indicate the feature points extracted from the captured images 310a and 310b.
  • the area S in the captured image 310b at time t in FIG. 15b is a new area that is not shown in the captured image 310a at time t ⁇ t in FIG. 15a. Therefore, the feature points in the new area S in FIG. 15b are not associated with the feature points in the captured image 310a in FIG. 15a. Therefore, in the third embodiment, the control unit 4 limits the search range D based on the acquired measurement result of the IMU 3. Specifically, the control unit 4 excludes the new region S from the search range D and excludes the feature points in the new region S from the feature point matching targets.
  • the control unit 4 determines the position and size of the new area S in the captured image based on the acquired measurement result of the IMU 3. For example, the control unit 4 acquires the angular velocity detected by the IMU 3 between the time t- ⁇ t and the time t, and integrates the obtained angular velocity to integrate the acquired angular velocity so that the rotation angle of the camera 2 from the time t- ⁇ t to the time t. Calculate ⁇ . The control unit 4 calculates the position and size of the new area S from the calculated rotation angle ⁇ , the rotation direction, and the internal parameters of the camera 2.
  • the length u S [pixel] of the new region S in the u direction in FIG. It can be calculated by the formula (1).
  • u S U ⁇ u / ⁇ u (1)
  • U[pixel] is the total length of the captured image 310b in the u direction
  • ⁇ u is the angle of view of the camera 2 in the u direction.
  • v S V ⁇ v / ⁇ v (2)
  • V[pixel] is the total length of the captured image 310b in the v direction
  • ⁇ v is the angle of view of the camera 2 in the v direction.
  • control unit 4 limits the search range D according to the angle of view of the captured image 10 captured by the camera 2.
  • the control unit 4 calculates the position and size in the captured image of the new area S based on the acquired measurement result of the IMU 3, and excludes the feature points in the new area S from the target of feature point matching.
  • the feature point in the new area S of the current frame (captured image at time t) 310b and the feature point in the previous frame (captured image at time t- ⁇ t) 310a are not associated with each other.
  • the efficiency of matching and thus the accuracy of calculation of the position of the moving body 1 are improved.
  • the number of feature points in the current frame (captured image at time t) 310b that is the target of feature point matching is reduced, the amount of calculation processing of the control unit 4 can be reduced.
  • the first to third embodiments have been described as examples of the technique disclosed in the present application.
  • the technique in the present disclosure is not limited to this, and can be applied to the embodiment in which changes, replacements, additions, omissions, etc. are appropriately made. It is also possible to combine the constituent elements described in the first to third embodiments to form a new embodiment.
  • the present disclosure can be applied to a position measuring device that measures the position of a moving body.

Landscapes

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

Abstract

位置測定装置（１００）は、移動体に搭載され、移動体の周囲の環境を撮像して撮像画像を取得する撮像部（２）と、移動体の動きを示す動き情報を検出する検出部（３）と、撮像画像から特徴点を抽出する制御部（４）と、特徴点の環境における空間位置を示す位置情報を記憶する記憶部（５）とを備える。制御部（４）は、撮像画像上で、位置情報が示す空間位置に対応する画像上の位置を探索し、位置情報が示す空間位置と撮像部との間の位置関係を算出して環境における移動体の位置を算出する。制御部（４）は、検出部によって検出された動き情報に応じて、撮像画像上で空間位置の探索を行う基準点を設定する。

Description

位置測定装置
　本開示は、移動体の位置を測定する位置測定装置に関する。
　特許文献１は、移動体に設けられた撮像部によって撮像された画像を取得し、移動の前に撮像された画像と、移動の後に撮像された画像と、に含まれる特徴点の対応付けを行うトラッキング部と、移動に関する情報を取得し、この情報に基づいて、移動の前後で、移動体から見た特徴点の二次元的な位置の変化が小さい領域を推定する領域推定部と、トラッキング部によって対応付けられ、かつ、領域内にある特徴点を用いて、移動体の自己位置推定を行う推定処理部とを具備する情報処理装置を開示している。これにより、カメラの向きが急激に変化する場合にも、特徴点のトラッキングを良好に行うことができる、ロバスト性の高い情報処理装置が得られる。
国際公開第２０１６／０３１１０５号
　本開示は、移動体の動き情報に基づいて効率良く移動体の位置を測定する位置測定装置を提供する。
　本開示の一態様は、移動体の位置を測定する位置測定装置を提供する。位置測定装置は、移動体に搭載され、移動体の周囲の環境を撮像して撮像画像を取得する撮像部と、移動体の動きを示す動き情報を検出する検出部と、撮像画像から特徴点を抽出する制御部と、特徴点の環境における空間位置を示す位置情報を記憶する記憶部とを備える。制御部は、撮像画像上で、位置情報が示す空間位置に対応する画像上の位置を探索し、位置情報が示す空間位置と撮像部との間の位置関係を算出して環境における移動体の位置を算出する。制御部は、検出部によって検出された動き情報に応じて、撮像画像上で空間位置の探索を行う基準点を設定する。
　本開示の他の態様は、移動体の位置を測定する位置測定装置を提供する。位置測定装置は、移動体に搭載され、移動体の周囲の環境を撮像して撮像画像を取得する撮像部と、移動体の動きを示す動き情報を検出する検出部と、撮像画像から特徴点を抽出する制御部と、特徴点の環境における空間位置を示す位置情報を記憶する記憶部とを備える。制御部は、撮像画像上で、位置情報が示す空間位置に対応する画像上の位置を探索し、位置情報が示す空間位置と撮像部との間の位置関係を算出して環境における移動体の位置を算出する。制御部は、検出部によって検出された動き情報に応じて、撮像画像上で空間位置の探索を行う探索範囲を変化させる。
　本開示の位置測定装置によると、移動体の動き情報に基づいて効率良く移動体の位置を測定することができる。
本開示の実施形態１に係る位置測定装置を搭載した移動体の構成を例示する図 実施形態１に係る位置測定装置の構成を示すブロック図 実施形態１～３に係る位置測定装置の動作の流れを示すフローチャート 撮像画像とその中の特徴点とを例示する図 ３Ｄ地図を例示する模式図 実施形態１における特徴点マッチングステップの詳細な流れを示すフローチャート 実施形態１における特徴点マッチング処理を説明するための図 実施形態１における特徴量の類似度を算出する処理を説明するための図 実施形態１における予測カメラポーズを算出するステップの詳細な流れを示すフローチャート 本開示の実施形態２に係る位置測定装置の構成を示すブロック図 実施形態２における特徴点マッチングの詳細な流れを示すフローチャート 実施形態２における探索範囲を特定するステップを説明するための図 制御部による振動の検出処理を説明するための図 実施形態３における特徴点マッチングステップの詳細な流れを示すフローチャート 時刻ｔ－Δｔに撮像された撮像画像を例示する図 時刻ｔに撮像された撮像画像を例示する図
　以下、本開示に係る実施形態について図面を参照して説明する。なお、以下の各実施形態において、同様の構成要素については同一の符号を付している。
（実施形態１）
１．概要
　本開示の実施形態１に係る位置測定装置は、例えば、有人の荷役自動車、無人搬送車（Automated guided vehicle、ＡＧＶ）及び自律移動型の荷物運搬ロボット等の移動体に搭載され、移動体の位置を測定する。
　図１は、移動体１の構成を例示する図である。移動体１は、例えば、荷物を搭載する荷台１ａを備える。移動体１には、本実施形態に係る位置測定装置１００が搭載されている。位置測定装置１００は、移動体１の周囲を撮像するカメラ２と、慣性計測装置（Inertial measurement unit、以下、「ＩＭＵ」という。）３とを備える。ＩＭＵ３は、移動体１の加速度及び角速度を検出する装置である。
　位置測定装置１００には、例えば、逐次撮像される画像に基づいて、自己位置の測定と３Ｄ地図情報を生成するＶｉｓｕａｌ－ＳＬＡＭ（Simultaneous Localization and Mapping）技術を適用することができる。
　位置測定装置１００は、カメラ２によって撮像された画像内の特徴点を抽出する。特徴点は、例えば、物、道路及び建造物等のエッジ、角等を含む。位置測定装置１００は、抽出された特徴点の画像上の座標を世界座標に変換し、画像上の特徴点に対応するマップポイントを世界座標空間に登録することによって、３Ｄ地図を作成する。位置測定装置１００は、移動体１を移動させながら一定のフレームレートでカメラ２により移動体１の周囲を撮像し、撮像時の画像上の特徴点と３Ｄ地図上のマップポイントとを対応させる特徴点マッチング処理を行う。位置測定装置１００は、現フレームの特徴点と、１つ前のフレームの特徴点と、の幾何学的位置関係に基づいて、カメラ２の位置及び向き（以下、「カメラポーズ」という。）を算出する。位置測定装置１００は、算出されたカメラ２の位置に基づいて、位置測定装置１００ひいては移動体１の位置を求めることができる。
　位置測定装置１００によって測定された位置情報は、例えば外部サーバに蓄積され、移動体１の通った環境内の各種データ管理に採用可能である。
　位置測定装置１００は、算出した移動体１の位置情報と作成した３Ｄ地図の情報とに基づいて、移動体１を移動させるために用いられてもよい。
２．構成
　図２は、位置測定装置１００の構成を示すブロック図である。位置測定装置１００は、カメラ２、ＩＭＵ３、制御部４、記憶部５、通信インタフェース（Ｉ／Ｆ）７及び駆動部８を備える。
　カメラ２は、本開示の撮像部の一例である。カメラ２は、移動体１に設置され、移動体１の周囲を撮像し、色画像データ及び距離画像データを生成する。カメラ２は、ＲＧＢ－Ｄカメラ、ステレオカメラ等のデプスセンサを含んでもよい。また、カメラ２は、色画像を撮像するＲＧＢカメラと、距離画像を撮像するＴｏＦ（Time of Flight）センサとによって構成されてもよい。
　ＩＭＵ３は、本開示の検出部の一例である。ＩＭＵ３は、移動体１の加速度を検出する加速度センサと、移動体１の角速度を検出するジャイロセンサとを備える。
　制御部４は、ソフトウェアと協働して所定の機能を実現するＣＰＵ又はＭＰＵのような汎用プロセッサを含む。制御部４は、記憶部５に格納されたプログラムを読み込んで実行することによって特徴点抽出部４１、特徴点マッチング部４２、位置算出部４４、地図管理部４５等の各種の機能を実現し、位置測定装置１００の全体動作を制御する。例えば、制御部４は、本実施形態の位置測定方法を実現するプログラム又はＳＬＡＭアルゴリズムを実現するプログラムを実行する。制御部４は、ハードウェアとソフトウェアの協働により所定の機能を実現するものに限定されず、所定の機能を実現するための専用回路として設計されたＦＰＧＡ、ＡＳＩＣ、ＤＳＰ等のハードウェア回路で構成されてもよい。
　記憶部５は、位置測定装置１００の機能を実現するために必要なプログラム及びデータを含む種々の情報を記録する記録媒体である。記憶部５には、例えば３Ｄ地図５１や画像データが格納される。記憶部５は、例えば、フラッシュメモリ、ＳＳＤなどの半導体メモリ装置、ハードディスク等の磁気記憶装置、その他の記憶デバイス単独で又はそれらを適宜組み合わせて実現される。記憶部５は、種々の情報を一時的に記憶する高速動作可能なＳＲＡＭ、ＤＲＡＭなどの揮発性メモリを含んでもよい。揮発性メモリは、例えば制御部４の作業領域や画像データをフレーム毎に一時的に記憶するフレームメモリとして機能する。
　通信Ｉ／Ｆ７は、ネットワーク５０を介して、位置測定装置１００とサーバ１５０等の外部機器との通信接続を可能とするためのインタフェース回路である。通信Ｉ／Ｆ７は、ＩＥＥＥ８０２．３、ＩＥＥＥ８０２．１１又はＷｉ－Ｆｉ等の規格に従って通信を行う。
　駆動部８は、制御部４からの指示に従って移動体１を移動させる機構である。例えば、駆動部８は、移動体１のタイヤに接続されたエンジンの駆動回路、ステアリング回路及びブレーキ回路を含む。
３．動作
３－１．動作の概要
　図３は、位置測定装置１００の動作の流れを示すフローチャートである。図３のフローチャートの各処理は、位置測定装置１００の制御部４によって実行される。
　まず、制御部４は、時刻ｔにおける撮像画像を取得する（Ｓ１０）。ここで、撮像画像は、カメラ２によって撮像された移動体１の周囲の環境の画像データである。
　次に、特徴点抽出部４１として機能する制御部４は、撮像画像を解析して特徴点を抽出する（Ｓ２０）。図４は、撮像画像１０とその中の特徴点とを例示する図である。図４中、丸で示した部分は、撮像画像１０から抽出された特徴点を示している。制御部４は、輝度値又は色が周囲の画素又は画素群と区別できる画素又は画素群を特徴点として抽出する。特徴点は、例えば、物、道路及び建造物等のエッジ、角及び模様等を特定する。撮像画像１０から特徴点を検出するために、例えば公知のＦＡＳＴ（Features from Accelerated Segment Test）技術が使用されてもよい。
　なお、制御部４は、移動体１の自己位置の算出処理の他、３Ｄ地図５１の作成処理も行う。地図管理部４５として機能する制御部４は、特徴点の撮像画像１０上の座標を世界座標に変換し、撮像画像１０上の特徴点に対応するマップポイントを世界座標空間に登録することによって、３Ｄ地図５１を作成する。３Ｄ地図５１には、撮像画像１０上の特徴点に対応するマップポイントとともに、撮像画像１０を示すカメラフレームと、当該撮像画像を撮像した際のカメラ２のカメラポーズと、が記録される。作成された３Ｄ地図５１の情報は、記憶部５に格納される。制御部４は、例えば移動体１の移動中に、時間間隔Δｔ毎に撮像画像を取得して特徴点を登録することにより、図５に例示したような３Ｄ地図５１を生成することができる。
　図５は、３Ｄ地図５１を例示する模式図である。３Ｄ地図５１には、撮像画像１０上の特徴点に対応するマップポイントが登録されている。マップポイントは、それぞれ世界座標を有する。図５には、マップポイントが丸印で示されている。図５の破線で示した構造上の情報は、３Ｄ地図５１には記録されないが、説明の便宜のために示している。制御部４は、時間間隔Δｔ毎に撮像画像を取得して特徴点を登録することにより、破線で示したような構造上の情報を再現することができる。
　図３に戻り、ステップＳ２０の後、特徴点マッチング部４２として機能する制御部４は、ステップＳ２０で抽出された時刻ｔにおける撮像画像１０中の特徴点と、３Ｄ地図５１のマップポイントとを対応させる特徴点マッチング処理を行う（Ｓ３０）。あるいは、制御部４は、特徴点マッチング処理として、例えば、公知のＫＬＴ（Kanade-Lucas-Tomasi）トラッカ技術を用いて、時刻ｔ－Δｔにおける撮像画像１０中の特徴点と、時刻ｔにおける撮像画像１０中の特徴点と、を対応させてもよい。
　次に、位置算出部４４として機能する制御部４は、時刻ｔにおけるカメラポーズを算出する。制御部４は、算出されたカメラポーズに基づいて、位置測定装置１００ひいては移動体１の位置（自己位置）を求めることができる（Ｓ４０）。時刻ｔにおけるカメラポーズは、例えば、時刻ｔに撮像された画像中の特徴点と、時刻ｔ－Δｔに撮像された画像中の特徴点と、の幾何学的位置関係に基づいて算出される。あるいは、時刻ｔにおけるカメラポーズは、例えば、時刻ｔ－Δｔにおけるカメラポーズと、ＩＭＵ３の検出結果とに基づいて算出される。 
　制御部４は、以上のステップＳ１０～Ｓ４０を、処理終了の判断を行うまで（Ｓ５０）、所定の時間間隔Δｔ毎に繰り返す（Ｓ６０）。処理終了の判断は、例えば、ユーザによる処理終了指示の入力があった場合に行われる。処理終了時（Ｓ５０でＹｅｓ）には、制御部４は、例えば作成した３Ｄ地図５１等の情報をサーバ１５０に送信する。
３－２．特徴点マッチング
　以下、図３に示した特徴点マッチングステップＳ３０の詳細を説明する。図６は、特徴点マッチングステップＳ３０の詳細な流れを示すフローチャートである。まず、制御部４は、時刻ｔにおける予測カメラポーズを算出する（Ｓ３１）。
　図７は、特徴点マッチング処理を説明するための図である。図７には、時刻ｔ－２ΔｔにおけるステップＳ４０で算出されたカメラポーズを有するカメラ２ａと、時刻ｔ－ΔｔにおけるステップＳ４０で算出されたカメラポーズを有するカメラ２ｂとが示されている。本実施形態のステップＳ３１では、過去のカメラ２ａ、２ｂのカメラポーズと、現在までのＩＭＵ３の測定結果とに基づいて、時刻ｔにおけるカメラ２ｃのカメラポーズを予測する。カメラ２ａ、２ｂ及び２ｃは、カメラ２と同一のものであるが、異なる時間軸上にあるものであるため異なる符号を付して区別する。
　図７では、カメラ２ａ、２ｂ及び２ｃは、立方体形状のオブジェクト５５を撮像している。オブジェクト５５は、移動体１の周囲環境の構成物の一例である（図５参照）。
　撮像画像１０ａは、時刻ｔ－２Δｔのカメラ２ａによって撮像された画像である。撮像画像１０ａには、特徴点Ｆａ１及びＦａ２が含まれている（図３のステップＳ２０参照）。撮像画像１０ｂは、時刻ｔ－Δｔのカメラ２ｂによって撮像された画像である。撮像画像１０ｂには、特徴点Ｆｂ１及びＦｂ２が含まれている。図面を複雑にしないため、図７では、撮像画像１０ａ及び１０ｂに写っているはずの画像を省略している。
　図７では、３Ｄ地図５１には、マップポイントＭ１及びＭ２が登録されている。マップポイントＭ１は、特徴点Ｆａ１又はＦｂ１に基づいて、時刻ｔ－Δｔ以前に３Ｄ地図５１上に登録されたものである。マップポイントＭ２は、特徴点Ｆａ２又はＦｂ２に基づいて、時刻ｔ－Δｔ以前に３Ｄ地図５１上に登録されたものである。
　図６に戻り、ステップＳ３１の後、制御部４は、３Ｄ地図５１上のマップポイントを１つ選択する（Ｓ３２）。次に、制御部４は、選択されたマップポイントを、カメラ２がステップＳ３１で算出された予測カメラポーズを有するものとして、撮像画像に射影する（Ｓ３３）。射影点の画像座標は、３Ｄ地図５１上のマップポイントの世界座標を射影変換することによって算出される。すなわち、制御部４は、選択されたマップポイントを画像座標平面に射影し、射影点の画像座標を求める。
　図７の例を参照して、ステップＳ３２及びステップＳ３３について説明する。ステップ３２では、例えば、制御部４は、３Ｄ地図５１上のマップポイントＭ１を選択する。次に、ステップＳ３３において、制御部４は、マップポイントＭ１を撮像画像１０ｃに射影する。図７の例では、射影点はＰ１である。また、後述のように、制御部４は、全てのマップポイントを射影するまでステップＳ３１～Ｓ３６を繰り返す（Ｓ３７）。例えば、ステップＳ３７でＮｏと判断されて移行した次のループにおいて、ステップＳ３２でマップポイントＭ２が選択された場合、制御部４は、ステップＳ３３においてマップポイントＭ２を射影点Ｐ２に射影する。
　図６に戻り、ステップＳ３３の後、制御部４は、射影点Ｐ１の周囲の探索範囲Ｄを特定する（Ｓ３４）。探索範囲Ｄは、図８に例示するように、射影点Ｐ１を中心とする所定の大きさの矩形であってもよいが、これに限定されない。例えば、探索範囲Ｄは、射影点を中心とする所定の半径を有する円であってもよい。
　次に、制御部４は、射影点の特徴量と、探索範囲Ｄ内にある特徴点の特徴量と、の類似度を算出する（Ｓ３５）。ステップＳ３５を、図８を用いて説明する。図８の撮像画像１０ｃは、時刻ｔにおいてカメラ２ｃによって撮像され、図３に示したステップＳ１０において制御部４によって取得された撮像画像である。図８には、図３に示したステップＳ２０において抽出された特徴点が丸印で示されている。また、図８には、ステップ３３において射影された、マップポイントＭ１に対応する射影点Ｐ１が×印で示されている。
　ステップＳ３５では、制御部４は、射影点Ｐ１の特徴量と、射影点Ｐ１の周囲の所定の探索範囲Ｄ内にある特徴点Ｆｃ１、Ｆｃ３、Ｆｃ４及びＦｃ５の特徴量と、の類似度を算出する。
　特徴点の特徴量は、例えば、ＳＵＲＦ（Speeded-Up Robust Features）技術によって得られるＳＵＲＦ特徴量、ＳＩＦＴ（Scale-Invariant Feature Transform）技術によって得られるＳＩＦＴ特徴量、又は、ＯＲＢ（Oriented FAST and Rotated BRIEF）技術によって得られるＯＲＢ特徴量である。
　特徴点の特徴量は、例えば、１以上の次元を有するベクトルで表される。例えば、ＳＵＲＦ特徴量は６４次元のベクトルで表され、ＳＩＦＴ特徴量は１２８次元のベクトルで表される。
　射影点の特徴量は、時刻ｔ－Δｔ以前に撮像された撮像画像から特徴点を抽出する際に取得され、特徴点とともに記憶部５に格納されている。
　ステップＳ３５において算出される類似度は、例えば、特徴量間のユークリッド距離等の距離として算出される。
　ステップＳ３５の後、制御部４は、ステップＳ３５で算出された類似度に基づいて、射影点に対応する特徴点を特定する（Ｓ３６）。図８に示した例では、制御部４は、特徴点Ｆｃ１を、射影点Ｐ１の特徴量に類似する特徴量を有する特徴点として特定する。これにより、時刻ｔにおける特徴点Ｆｃ１が、射影点Ｐ１とマップポイントＭ１（図７参照）とを介して、時刻ｔ－Δｔにおける特徴点Ｆｂ１に対応するものとしてマッチングされる。
　ステップＳ３６では、射影点と特徴点との類似度が所定の閾値未満である場合、制御部４は、当該特徴点が射影点に対応する特徴点であると特定しない。探索範囲Ｄ内に射影点と閾値以上の類似度を有する特徴点が存在しない場合は、射影点に対応する特徴点であると特定される特徴点は存在しない。言い換えれば、特徴点マッチングは失敗する。
　ステップＳ３６では、探索範囲Ｄ内に射影点と閾値以上の類似度を有する特徴点が複数存在する場合、制御部４は、最も高い類似度を有する特徴点を、射影点に対応する特徴点と特定する。
　制御部４は、ステップＳ３６を終えると、３Ｄ地図５１内の全てのマップポイントを撮像画像１０ｃに射影したか否かを判断する（Ｓ３７）。マップポイントを全て射影していない場合（Ｓ３７でＮｏ）、ステップＳ３２に戻り、制御部４は、射影していないマップポイントを１つ選択し、ステップＳ３３～Ｓ３７の処理を実行する。全て射影している場合（Ｓ３７でＹｅｓ）、特徴点マッチングＳ３０を終了する。
３－３．予測カメラポーズの算出
　以下、時刻ｔにおける予測カメラポーズを算出するステップＳ３１の詳細について説明する。
　カメラポーズの変化が時間的に一定である場合、言い換えればカメラポーズの変化が時間的に一定となるように移動体１が移動する場合、時刻ｔ－２Δｔと時刻ｔ－ΔｔにおいてステップＳ４０（図３参照）で算出されたカメラ２ａ、２ｂ（図７参照）のカメラポーズの差分を用いて時刻ｔにおけるカメラ２ｃのカメラポーズを算出すると、投影点Ｐ１と投影点Ｐ１に対応する特徴点Ｆｃ１（図８参照）とは精度良く一致する。
　実際には、カメラポーズがぶれたり、移動体１が加速や回転をすることによって、カメラポーズの変化は時間的に一定とならないことがある。このような場合、図８に示したように、射影点Ｐ１と、これに対応すべき特徴点Ｆｃ１との撮像画像１０ｃ上の位置が解離する。このような解離があったとしても、特徴点Ｆｃ１が探索範囲Ｄの中に存在すれば、特徴点マッチングを行うことができる。
　しかしながら、時刻ｔ－Δｔと時刻ｔとの間に移動体１が大きく加速したり回転したりすると、射影点Ｐ１が特徴点Ｆｃ１から遠く離れた場所に射影され、特徴点Ｆｃ１が探索範囲Ｄの外に存在する事態が発生する。このような場合、本来射影点Ｐ１に対応すべき特徴点Ｆｃ１を射影点Ｐ１に対応させることができず、特徴点マッチングは失敗する。
　そこで、本実施の形態では、図２に示したＩＭＵ３によって計測された加速度及び／又は角速度をカメラポーズの予測に利用する。これにより、制御部４は、移動体１が加速や回転をした場合でも効率的に特徴点マッチングを行うことができる。
　図９は、時刻ｔにおける予測カメラポーズを算出するステップＳ３１の詳細な流れを示すフローチャートである。図９のフローチャートの各処理は、図２に示したカメラポーズ予測部４３として機能する制御部４によって実行される。
　まず、制御部４は、時刻ｔ－Δｔと時刻ｔとの間における移動体１の加速度及び角速度をＩＭＵ３から取得する（Ｓ３１１）。次に、制御部４は、加速度及び角速度をそれぞれ時間で積分し、時刻ｔ－Δｔから時刻ｔまでのカメラポーズの変化量を算出する（Ｓ３１２）。
　次に、制御部４は、時刻ｔ－Δｔにおいて算出されたカメラポーズを取得する（Ｓ３１３）。ステップＳ３１３で取得されるカメラポーズは、時刻ｔ－ΔｔにおけるステップＳ４（図３参照）に相当するステップにおいて制御部４によって算出されたものである。なお、ステップＳ３１３は、ステップＳ３１２の前又はステップＳ３１１の前に行われてもよい。
　次に、制御部４は、ステップＳ３１３で取得された時刻ｔ－Δｔのカメラポーズと、ステップＳ３１２において算出された時刻ｔ－Δｔから時刻ｔまでのカメラポーズの変化量と、に基づいて、時刻ｔにおける予測カメラポーズを算出する（Ｓ３１４）。
　これにより、本実施の形態では、ＩＭＵ３によって計測された加速度及び／又は角速度をカメラポーズの予測に反映させ、移動体１が加速や回転をした場合でも効率的に特徴点マッチングを行うことが可能となる。
４．まとめ
　以上のように、本実施形態に係る位置測定装置１００は、移動体１の位置を測定する。位置測定装置１００は、移動体１に搭載され、移動体１の周囲の環境を撮像して撮像画像を取得するカメラ２と、移動体１の動きを示す加速度及び角速度等の動き情報を検出するＩＭＵ３と、撮像画像１０ａ、１０ｂから特徴点を抽出する制御部４と、特徴点の環境における空間位置を示すマップポイントＭ１、Ｍ２を記憶する記憶部５とを備える。制御部４は、撮像画像１０ｃ上で、マップポイントＭ１、Ｍ２が示す空間位置に対応する画像上の位置を探索し（Ｓ３０）、マップポイントＭ１、Ｍ２が示す空間位置とカメラ２との間の位置関係を算出して環境における移動体１の位置を算出する（Ｓ４０）。制御部４は、ＩＭＵ３によって検出された動き情報に応じて、撮像画像１０ｃ上で空間位置の探索を行う基準点Ｐ１を設定する。
　本実施形態に係る位置測定装置１００によると、移動体１が加速や回転をした場合でも、ＩＭＵ３によって検出された動き情報に応じて、撮像画像１０ｃ上で空間位置の探索を行うにより、効率的に特徴点マッチングを行うことができる。
　本実施形態において、ＩＭＵ３は、カメラ２が時間的に前後する第１の時刻ｔ－Δｔと第２の時刻ｔとの間における動き情報を検出してもよい。制御部４は、動き情報に応じて、第１の時刻ｔ－Δｔにおけるカメラポーズから第２の時刻ｔにおけるカメラポーズを予測することができる（Ｓ３１）。
　本実施形態において、ＩＭＵ３は、慣性計測装置、加速度センサ及びジャイロセンサのうちの少なくとも１つを含む。
　本実施形態において、撮像画像は、距離画像及び色画像を含む。
（実施形態２）
　実施形態２では、ＩＭＵ３の測定結果に応じて探索範囲Ｄの大きさを変化させる例を説明する。
　図１０は、本開示の実施形態２に係る位置測定装置２００の構成を示すブロック図である。位置測定装置２００は、制御部４によって行われる処理が実施形態１と異なる以外は、実施形態１の位置測定装置１００と同様の構成を有する。
　位置測定装置２００の制御部４は、図３に示したようなステップＳ１０～Ｓ６０を実行することによって、移動体１の位置を算出する。もっとも、実施形態２では、特徴点マッチングステップＳ３０の内容が実施形態１と異なる。
　図１１は、実施形態２における特徴点マッチングステップの詳細な流れを示すフローチャートである。実施形態１の図６と比較すると、実施形態２では、ステップＳ３４ｂが、実施形態１の探索範囲特定ステップＳ３４と異なる。
　ステップＳ３４ｂでは、特徴点マッチング部２４２として機能する制御部４は、取得したＩＭＵ３の測定結果、例えば角速度に基づいて、探索範囲Ｄを特定する。言い換えれば、制御部４は、ＩＭＵ３の測定結果に応じて、例えば角速度の値に応じて、探索範囲Ｄの大きさを変更する。
　図１２は、探索範囲Ｄを特定するステップＳ３４ｂを説明するための図である。図１２には、カメラ２と、カメラ２によって撮像された撮像画像１０が示されている。互いに直交するｘ軸、ｙ軸、ｚ軸は、カメラ２の光学中心を原点とするカメラ座標系の座標軸である。カメラ２の光学中心は、例えばカメラ２のレンズの中心である。ｚ軸は、カメラ２の光軸である。
　カメラ２によって撮像される撮像画像１０は、画像平面内にある。撮像画像１０内の点は、互いに直交する画像座標系のｕ座標とｖ座標で表される。
　３Ｄ地図５１内のマップポイントＭの位置は、カメラ座標系で表すこともできるし、世界座標Ｘ、Ｙ及びＺで表すこともできる。マップポイントＭは、図１１のステップＳ３３において撮像画像１０に射影される。図１２では、射影点をＰで表している。
　例えば、ＩＭＵ３によって検出された加速度及び角速度がゼロであった場合、制御部４は、取得ステップＳ３４ｂにおいて、探索範囲Ｄを、射影点Ｐを中心とするｕ方向の長さｕ０、ｖ方向の長さｖ０の矩形とする。ｕ０及びｖ０は、予め定められた探索範囲Ｄのｕ方向及びｖ方向の長さの初期値である。ｕ方向及びｖ方向の長さは、例えば画素数で表される。
　また、例えば、ＩＭＵ３によって検出された角速度により、カメラ２がｙ軸を中心に回転（ヨーイング）したと判断した場合、制御部４は、探索範囲Ｄのｕ方向の長さをｕ０より大きいｕ１とする。ｕ１とｕ０との差は、例えばｙ軸周りの角速度が大きいほど大きいものとされる。
　同様に、例えば、ＩＭＵ３によって検出された角速度により、カメラ２がｘ軸を中心に回転（ピッチング）したと判断した場合、制御部４は、探索範囲Ｄのｖ方向の長さをｖ０より大きいｖ１とする。ｖ１とｖ０との差は、例えばｘ軸周りの角速度が大きいほど大きいものとされる。
　同様に、例えば、ＩＭＵ３によって検出された角速度により、カメラ２がｚ軸を中心に回転（ローリング）したと判断した場合、制御部４は、探索範囲Ｄをローリング方向に回転させる。回転角度は、例えばｚ軸周りの角速度が大きいほど大きいものとされる。
　制御部４は、ＩＭＵ３によって検出された加速度及び／又は角速度に基づいて振動を検出した場合に、探索範囲Ｄの大きさを初期値（ｕ０×ｖ０）より大きくしてもよい。例えば、制御部４は、図１３に示すようにｙ軸方向の加速度ａｙの正負が時刻ｔ－Δｔからｔまでに所定の閾値回数以上入れ替わった場合、ＩＭＵ３ひいては移動体１がｙ軸方向に振動したと判断する。移動体１がｙ軸方向に振動したと判断した場合、制御部４は、例えば探索範囲Ｄのｖ方向の長さを初期値ｖ０より大きいｖ１とする（図１２参照）。
　探索範囲Ｄをどれだけ大きくするかは、例えば、時刻ｔ－Δｔからｔまでの加速度ａｙの絶対値の大きさに基づいて決定される。図１３に示した例では、制御部４は、時刻ｔ－Δｔと時刻ｔとの間における加速度の絶対値の最大値ａｙ１に基づいて、探索範囲Ｄのｖ方向の拡大率ｖ１／ｖ０を決定する。例えば、制御部４は、ａｙ１が大きいほどｖ１／ｖ０を大きくする。
　同様に、例えば、制御部４は、ｘ軸方向の加速度ａｘの正負が時刻ｔ－Δｔからｔまでに所定の閾値回数以上入れ替わった場合、ＩＭＵ３ひいては移動体１がｘ軸方向に振動したと判断する。移動体１がｘ軸方向に振動したと判断した場合、制御部４は、例えば探索範囲Ｄのｕ方向の長さを初期値ｕ０より大きいｕ１とする。
　以上のように、本実施形態では、制御部４は、ＩＭＵ３によって検出された動き情報に応じて、撮像画像１０ｃ上で前記空間位置の探索を行う探索範囲Ｄを変化させる（Ｓ３４ｂ）。これにより、移動体１の回転又は加速に起因するカメラポーズの変化によって、前フレーム（時刻ｔ－Δｔにおける撮像画像）中の特徴点に対応付けられるべき現フレーム（時刻ｔにおける撮像画像）中の特徴点が探索範囲Ｄの中に入らない事態を防止することができる。したがって、特徴点マッチングの効率ひいては移動体１の位置の算出の精度が向上する。
（実施形態３）
　本開示の実施形態３では、ＩＭＵ３の測定結果に応じて、特徴点の探索を行わない領域を判断する例を説明する。
　実施形態３では、制御部４は、図３に示したようなステップＳ１０～Ｓ６０を実行することによって、移動体１の位置を算出する。もっとも、実施形態３では、特徴点マッチングステップＳ３０の内容が実施形態１と異なる。
　図１４は、実施形態３における特徴点マッチングステップの詳細な流れを示すフローチャートである。実施形態１の図６と比較すると、実施形態３では、ステップＳ３４ｃが、実施形態１の探索範囲特定ステップＳ３４と異なる。
　ステップＳ３４ｃでは、制御部４は、取得したＩＭＵ３の測定結果、例えば角速度に基づいて、探索範囲Ｄを特定する。図１５ａ及び図１５ｂは、探索範囲Ｄを特定するステップＳ３４ｃを説明するための図である。制御部４は、図３のステップＳ１０及びＳ６０に示すように、一定の時間間隔Δｔ毎に撮像画像を取得する。
　図１５ａは、時刻ｔ－Δｔに撮像された撮像画像３１０ａを例示する図である。図１５ｂは、時刻ｔに撮像された撮像画像３１０ｂを例示する図である。図１５ａ及び図１５ｂには、カメラ座標ｘ、ｙ及びｚと、画像座標ｕ及びｖとを示している。図１５ａ及び図１４中、丸で示した部分は、撮像画像３１０ａ、３１０ｂから抽出された特徴点を示している。
　図１５ｂの時刻ｔの撮像画像３１０ｂ中の領域Ｓは、図１５ａの時刻ｔ－Δｔの撮像画像３１０ａには写っていない新規領域である。したがって、図１５ｂの新規領域Ｓ中の特徴点は、図１５ａの撮像画像３１０ａ中の特徴点に対応付けられることはない。そこで、実施形態３では、制御部４は、取得したＩＭＵ３の測定結果に基づいて、探索範囲Ｄを制限する。具体的には、制御部４は、新規領域Ｓを探索範囲Ｄから除外し、新規領域Ｓ中の特徴点を特徴点マッチングの対象から除外する。
　制御部４は、取得したＩＭＵ３の測定結果に基づいて新規領域Ｓの撮像画像における位置及び寸法を決定する。例えば、制御部４は、時刻ｔ－Δｔと時刻ｔとの間にＩＭＵ３によって検出された角速度を取得し、取得した角速度を積分することによって時刻ｔ－Δｔから時刻ｔまでのカメラ２の回転角度φを算出する。制御部４は、算出された回転角度φと回転方向とカメラ２の内部パラメータとから、新規領域Ｓの位置及び寸法を算出する。
　例えば、カメラ２が時刻ｔ－Δｔと時刻ｔとの間にｙ軸周りに回転角度φｕだけ回転したとすると、図１５ｂの新規領域Ｓのｕ方向の長さｕＳ［ｐｉｘｅｌ］は、次式（１）により算出できる。
　　　ｕＳ＝Ｕ×φｕ／θｕ　　　　・・・（１）
　ここで、Ｕ［ｐｉｘｅｌ］は撮像画像３１０ｂのｕ方向の全長であり、θｕは、カメラ２のｕ方向の画角である。
　同様に、カメラ２が時刻ｔ－Δｔと時刻ｔとの間にｘ軸周りに回転角度φｖだけ回転したとすると、新規領域Ｓのｖ方向の長さｖＳ［ｐｉｘｅｌ］は、次式（２）により算出できる。
　　　ｖＳ＝Ｖ×φｖ／θｖ　　　　・・・（２）
　ここで、Ｖ［ｐｉｘｅｌ］は撮像画像３１０ｂのｖ方向の全長であり、θｖは、カメラ２のｖ方向の画角である。
　以上のように、本実施形態では、制御部４は、カメラ２における撮像画像１０の画角に応じて、探索範囲Ｄを制限する。
　すなわち、制御部４は、取得したＩＭＵ３の測定結果に基づいて新規領域Ｓの撮像画像における位置及び寸法を算出し、新規領域Ｓ中の特徴点を特徴点マッチングの対象から除外する。これにより、現フレーム（時刻ｔの撮像画像）３１０ｂの新規領域Ｓ中の特徴点と、前フレーム（時刻ｔ－Δｔの撮像画像）３１０ａ中の特徴点と、を対応付けることがなくなるため、特徴点マッチングの効率ひいては移動体１の位置の算出の精度が向上する。また、特徴点マッチングの対象である現フレーム（時刻ｔの撮像画像）３１０ｂ中の特徴点の数が減少するため、制御部４の演算処理量を低減できる。
　以上のように、本出願において開示する技術の例示として、実施の形態１～３を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、上記実施の形態１～３で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。
　本開示は、移動体の位置を測定する位置測定装置に適用可能である。
　１　移動体
　２　カメラ
　３　ＩＭＵ
　４　制御部
　５　記憶部
　７　通信Ｉ／Ｆ
　８　駆動部
　１０　撮像画像
　５１　３Ｄ地図
　１００　位置測定装置

Claims (9)

  1. 　移動体の位置を測定する位置測定装置であって、
    　前記移動体に搭載され、前記移動体の周囲の環境を撮像して撮像画像を取得する撮像部と、
    　前記移動体の動きを示す動き情報を検出する検出部と、
    　前記撮像画像から特徴点を抽出する制御部と、
    　前記特徴点の前記環境における空間位置を示す位置情報を記憶する記憶部とを備え、
    　前記制御部は、前記撮像画像上で、前記位置情報が示す空間位置に対応する画像上の位置を探索し、前記位置情報が示す空間位置と前記撮像部との間の位置関係を算出して前記環境における前記移動体の位置を算出し、
    　前記制御部は、前記検出部によって検出された動き情報に応じて、前記撮像画像上で前記空間位置の探索を行う基準点を設定する、
    位置測定装置。
  2. 　前記検出部は、前記撮像部が時間的に前後する第１の時刻と第２の時刻との間における前記動き情報を検出し、
    　前記制御部は、前記動き情報に応じて、第１の時刻における前記撮像部の位置及び向きから第２の時刻における前記撮像部の位置及び向きを予測する、
    請求項１に記載の位置測定装置。
  3. 　前記制御部は、前記検出部によって検出された動き情報に応じて、前記撮像画像上で前記空間位置の探索を行う探索範囲を変化させる、請求項１又は２に記載の位置測定装置。
  4. 　前記検出部は、前記動き情報として前記移動体の角速度を検出し、
    　前記制御部は、検出された角速度が大きいほど、前記探索範囲を増大させる、
    請求項３に記載の位置測定装置。
  5. 　前記制御部は、前記撮像部における前記撮像画像の画角に応じて、前記探索範囲を制限する、請求項３又は４に記載の位置測定装置。
  6. 　前記検出部は、前記動き情報として前記移動体の振動を検出し、
    　前記制御部は、検出された振動が大きいほど、前記探索範囲を増大させる、
    請求項３～５のいずれかに記載の位置測定装置。
  7. 　前記検出部は、慣性計測装置、加速度センサ及びジャイロセンサのうちの少なくとも１つを含む、請求項１～６のいずれかに記載の位置測定装置。
  8. 　前記撮像画像は、距離画像及び色画像を含む、請求項１～７のいずれかに記載の位置測定装置。
  9. 　移動体の位置を測定する位置測定装置であって、
    　前記移動体に搭載され、前記移動体の周囲の環境を撮像して撮像画像を取得する撮像部と、
    　前記移動体の動きを示す動き情報を検出する検出部と、
    　前記撮像画像から特徴点を抽出する制御部と、
    　前記特徴点の前記環境における空間位置を示す位置情報を記憶する記憶部とを備え、
    　前記制御部は、前記撮像画像上で、前記位置情報が示す空間位置に対応する画像上の位置を探索し、前記位置情報が示す空間位置と前記撮像部との間の位置関係を算出して前記環境における前記移動体の位置を算出し、
    　前記制御部は、前記検出部によって検出された動き情報に応じて、前記撮像画像上で前記空間位置の探索を行う探索範囲を変化させる、
    位置測定装置。
PCT/JP2019/046198 2018-12-28 2019-11-26 位置測定装置 WO2020137313A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
JP2020562956A JPWO2020137313A1 (ja) 2018-12-28 2019-11-26
EP19906078.1A EP3904995A4 (en) 2018-12-28 2019-11-26 LOCATION DEVICE
US17/357,173 US20210318690A1 (en) 2018-12-28 2021-06-24 Positioning device

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-247816 2018-12-28
JP2018247816 2018-12-28

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US17/357,173 Continuation US20210318690A1 (en) 2018-12-28 2021-06-24 Positioning device

Publications (1)

Publication Number Publication Date
WO2020137313A1 true WO2020137313A1 (ja) 2020-07-02

Family

ID=71127158

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/046198 WO2020137313A1 (ja) 2018-12-28 2019-11-26 位置測定装置

Country Status (4)

Country Link
US (1) US20210318690A1 (ja)
EP (1) EP3904995A4 (ja)
JP (1) JPWO2020137313A1 (ja)
WO (1) WO2020137313A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7553090B2 (ja) 2020-10-09 2024-09-18 Ｎｅｃソリューションイノベータ株式会社 位置推定装置、位置推定方法、及びプログラム

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021157136A1 (ja) * 2020-02-07 2021-08-12 パナソニックＩｐマネジメント株式会社 測位システム

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007303886A (ja) * 2006-05-09 2007-11-22 Sony Corp 位置推定装置、位置推定方法及びプログラム記録媒体
JP2009156611A (ja) * 2007-12-25 2009-07-16 Toyota Motor Corp 移動状態推定装置
WO2009150793A1 (ja) * 2008-06-09 2009-12-17 パナソニック株式会社 撮像装置、撮像方法
WO2016031105A1 (ja) 2014-08-26 2016-03-03 ソニー株式会社 情報処理装置、情報処理方法、及びプログラム
JP2016148956A (ja) * 2015-02-10 2016-08-18 株式会社デンソーアイティーラボラトリ 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
JP2018009833A (ja) * 2016-07-12 2018-01-18 株式会社Ｓｏｋｅｎ 自車位置特定装置、及び自車位置特定方法
WO2018212292A1 (ja) * 2017-05-19 2018-11-22 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110149126A1 (en) * 2009-12-22 2011-06-23 Olympus Corporation Multiband image pickup method and device
GB2506338A (en) * 2012-07-30 2014-04-02 Sony Comp Entertainment Europe A method of localisation and mapping
US9767372B2 (en) * 2014-05-20 2017-09-19 Nissan Motor Co., Ltd. Target detection apparatus and target detection method
CN110264509B (zh) * 2018-04-27 2022-10-14 腾讯科技（深圳）有限公司 确定图像捕捉设备的位姿的方法、装置及其存储介质
KR102645368B1 (ko) * 2018-08-16 2024-03-11 소니그룹주식회사 정보 처리 장치와 정보 처리 방법과 프로그램
WO2021157116A1 (ja) * 2020-02-07 2021-08-12 パナソニックＩｐマネジメント株式会社 位置測定装置
WO2021157136A1 (ja) * 2020-02-07 2021-08-12 パナソニックＩｐマネジメント株式会社 測位システム

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007303886A (ja) * 2006-05-09 2007-11-22 Sony Corp 位置推定装置、位置推定方法及びプログラム記録媒体
JP2009156611A (ja) * 2007-12-25 2009-07-16 Toyota Motor Corp 移動状態推定装置
WO2009150793A1 (ja) * 2008-06-09 2009-12-17 パナソニック株式会社 撮像装置、撮像方法
WO2016031105A1 (ja) 2014-08-26 2016-03-03 ソニー株式会社 情報処理装置、情報処理方法、及びプログラム
JP2016148956A (ja) * 2015-02-10 2016-08-18 株式会社デンソーアイティーラボラトリ 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
JP2018009833A (ja) * 2016-07-12 2018-01-18 株式会社Ｓｏｋｅｎ 自車位置特定装置、及び自車位置特定方法
WO2018212292A1 (ja) * 2017-05-19 2018-11-22 パイオニア株式会社 情報処理装置、制御方法、プログラム及び記憶媒体

Non-Patent Citations (1)

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

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7553090B2 (ja) 2020-10-09 2024-09-18 Ｎｅｃソリューションイノベータ株式会社 位置推定装置、位置推定方法、及びプログラム

Also Published As

Publication number Publication date
EP3904995A4 (en) 2022-02-23
JPWO2020137313A1 (ja) 2020-07-02
US20210318690A1 (en) 2021-10-14
EP3904995A1 (en) 2021-11-03

Similar Documents

Publication Publication Date Title
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
JP6658001B2 (ja) 位置推定装置、プログラム、位置推定方法
US10529076B2 (en) Image processing apparatus and image processing method
KR101775591B1 (ko) 데이터베이스 생성의 목적을 위한 대화식 및 자동 3-d 오브젝트 스캐닝 방법
KR101072876B1 (ko) 이동 로봇에서 자신의 위치를 추정하기 위한 방법 및 장치
US20190355173A1 (en) Leveraging crowdsourced data for localization and mapping within an environment
CN108955718A (zh) 一种视觉里程计及其定位方法、机器人以及存储介质
KR101880185B1 (ko) 이동체 포즈 추정을 위한 전자 장치 및 그의 이동체 포즈 추정 방법
US20140254874A1 (en) Method of detecting and describing features from an intensity image
TWI640931B (zh) 影像目標追蹤方法及裝置
JP6782903B2 (ja) 自己運動推定システム、自己運動推定システムの制御方法及びプログラム
US11263777B2 (en) Information processing apparatus and information processing method
US10607350B2 (en) Method of detecting and describing features from an intensity image
KR102303779B1 (ko) 복수 영역 검출을 이용한 객체 탐지 방법 및 그 장치
WO2018142533A1 (ja) 位置姿勢推定装置および位置姿勢推定方法
WO2020137313A1 (ja) 位置測定装置
El Bouazzaoui et al. Enhancing RGB-D SLAM performances considering sensor specifications for indoor localization
CN111382637A (zh) 行人检测跟踪方法、装置、终端设备及介质
Zhu et al. Robust plane-based calibration of multiple non-overlapping cameras
CN115112123A (zh) 基于视觉-imu融合的多移动机器人协同定位方法及系统
JP6886136B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
CN117067261A (zh) 机器人监控方法、装置、设备及存储介质
Strasdat et al. Multi-cue localization for soccer playing humanoid robots
WO2020137314A1 (ja) 位置測定装置
CN114973075A (zh) 运动状态确定方法、装置、设备及存储介质

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

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2020562956

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2019906078

Country of ref document: EP

Effective date: 20210728