WO2020137314A1 - 位置測定装置 - Google Patents

位置測定装置 Download PDF

Info

Publication number
WO2020137314A1
WO2020137314A1 PCT/JP2019/046204 JP2019046204W WO2020137314A1 WO 2020137314 A1 WO2020137314 A1 WO 2020137314A1 JP 2019046204 W JP2019046204 W JP 2019046204W WO 2020137314 A1 WO2020137314 A1 WO 2020137314A1
Authority
WO
WIPO (PCT)
Prior art keywords
moving body
control unit
map
measuring device
image
Prior art date
Application number
PCT/JP2019/046204
Other languages
English (en)
French (fr)
Inventor
典 岡田
Original Assignee
パナソニックＩｐマネジメント株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by パナソニックＩｐマネジメント株式会社 filed Critical パナソニックＩｐマネジメント株式会社
Publication of WO2020137314A1 publication Critical patent/WO2020137314A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • 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
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles

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 can accurately calculate the self position.
  • 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 first position information indicating the 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 first position information, and determines whether or not the position of the moving body in the environment is specified based on the search result.
  • the control unit calculates the position of the moving body in the environment based on the specified position on the image, and determines that the position of the moving body in the environment is not specified. In that case, the position of the moving body in the environment is calculated based on the motion information.
  • the position measurement device of the present disclosure it is possible to accurately calculate the self position.
  • 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.
  • FIG. 3 is a diagram for explaining steps for determining whether or not the magnitude of angular velocity is equal to or greater than a threshold value.
  • 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 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.
  • 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.
  • control unit 4 may predict the camera pose of the camera 2c at the time t based only on the camera poses of the past cameras 2a and 2b without using the measurement result of the IMU 3. For example, the control unit 4 calculates the camera pose of the camera 2c from the camera pose of the camera 2b, assuming that the difference between the camera poses of the cameras 2a and 2b is equal to the difference between the camera poses of the cameras 2c and 2b. To do.
  • 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.
  • FIG. 9 is a flowchart showing the detailed flow of the self-position calculation step S40.
  • step S36 shown in FIG. 6 when the number of feature points for which feature point matching has succeeded does not exceed the predetermined threshold value, the control unit 4 determines the geometrical shape of the feature points in the captured image at time t- ⁇ t and time t.
  • the self position cannot be calculated based on the physical position relationship.
  • the control unit 4 determines the time t ⁇ t and the geometrical positional relationship of the feature points in the captured image at the time t.
  • the fact that the camera pose cannot be calculated is called “lost”.
  • the control unit 4 turns on the lost flag in step S46 described below.
  • step S41 shown in FIG. 9 the control unit 4 determines whether or not the lost flag is ON.
  • the lost flag has an ON state and an OFF state, and the initial state is OFF.
  • the lost flag being ON at the present time (time t) means that the lost flag is in the lost state one frame before (time t- ⁇ t), and the lost flag is turned ON in step S46 at time t- ⁇ t.
  • the control unit 4 calculates the self-position based on the detection result of the IMU 3 (S42). For example, the control unit 4 calculates the rotation angle by integrating the angular velocity detected by the IMU 3 between time t- ⁇ t and time t. The control unit 4 can calculate the camera pose at time t based on the obtained rotation angle and the camera pose calculated at step S40 at time t- ⁇ t. For example, the control unit 4 multiplies the camera pose calculated at step S40 at time t- ⁇ t by a rotation matrix representing the rotation angle between time t- ⁇ t and time t to determine the camera pose at time t. It can be calculated.
  • step S42 the control unit 4 turns off the lost flag that is in the ON state (S43). Then, it progresses to step S50 shown in FIG.
  • step S41 determines whether the lost flag is OFF in step S41, that is, if the lost has not occurred one frame before (time t ⁇ t).
  • the control unit 4 determines whether the lost has occurred in the current frame (time t). It is determined (S44). For example, the control unit 4 determines that a loss has occurred when the number of feature points for which the feature point matching has succeeded does not exceed the predetermined threshold value.
  • control unit 4 calculates the self-position based on the detection result of the IMU 3 (S45). Next, the control unit 4 turns on the lost flag that is in the OFF state (S46). Then, it progresses to step S50 shown in FIG.
  • step S44 determines 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 based on the geometrical positional relationship between and. Then, it progresses to step S50 shown in FIG.
  • 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 a 3D map 51 that indicates the spatial position of the feature points in the environment.
  • the control unit 4 searches the captured image for a position on the image corresponding to the spatial position indicated by the 3D map 51, and determines whether or not the position of the mobile body 1 in the environment is specified based on the search result. ..
  • the control unit 4 calculates the position of the mobile body 1 in the environment based on the specified position on the image, and the position of the mobile body 1 in the environment is calculated.
  • the position of the mobile body 1 in the environment is calculated based on the motion information.
  • the control unit 4 can calculate the self-position based on the motion information detected by the IMU 3. You can As described above, in the present embodiment, it is possible to obtain the position measuring device 100 having high robustness that does not interrupt the self-position calculation operation even if the lost state is once generated.
  • FIG. 10 is a block diagram showing the configuration of the position measuring device 200 according to this embodiment.
  • a 3D map obtained by registering the feature points extracted in the past as map points in the storage unit 5 (hereinafter, In addition to the "old map") 51, a new map 52 newly created when a loss occurs is stored.
  • the storage unit 5 may store an integrated map 53 in which information on the old map 51 and information on the new map 52 are integrated.
  • the old map 51, the new map 52, and the integrated map 53 are created by the control unit 4 that functions as the map management unit 45b.
  • FIG. 11 is a flowchart showing the operation flow of the position measuring device 200. Unlike the first embodiment, the control unit 4 determines whether the lost flag is ON after step S20 of extracting the feature points (S21).
  • step S22 Map points corresponding to the feature points extracted in step S20 at time t are registered in the new map 52. Map points corresponding to the feature points extracted after time t+ ⁇ t are also registered in the new map 52. After step S22, the process proceeds to step S40 for calculating the self position.
  • the control unit 4 may create an integrated map 53 that integrates the information of the old map 51 and the information of the new map 52.
  • FIG. 12 is a flowchart showing the flow of the operation of creating the integrated map 53.
  • the operation of creating the integrated map 53 may be performed in the new map creating step S22 shown in FIG.
  • the operation of creating the integrated map 53 may be performed at a predetermined time interval different from ⁇ t, separately from the self-position calculation operation performed at each time interval ⁇ t shown in FIG. 11.
  • the control unit 4 creates a pre-integrated map 54 in which map points in the old map 51 and map points in the new map 52 are arranged (S201).
  • FIG. 13 is a schematic diagram illustrating the pre-integrated map 54.
  • the map points in the old map 51 are indicated by crosses, and the map points in the new map 52 are indicated by circles.
  • control unit 4 corresponds to the camera pose corresponding to the camera frame registered in the new map 52 and the camera frame registered in the old map 51.
  • the difference between the camera pose and the pose is calculated (S202).
  • control unit 4 selects a set of a camera frame registered in the new map 52 and a camera frame registered in the old map 51, in which the calculated camera pose difference is less than or equal to a threshold value (S203). ).
  • control unit 4 executes feature point matching between the selected camera frames (S204).
  • Steps S203 and S204 of the present embodiment will be described using the example shown in FIG.
  • the feature points corresponding to the map points indicated by the crosses in the old map 51 are within the camera frame imaged by the camera 2d.
  • the feature points corresponding to the map points indicated by the circles in the new map 52 are within the camera frame imaged by the camera 2e.
  • the control unit 4 executes feature point matching between the camera frame of the camera 2d and the camera frame of the camera 2e.
  • the map points in the old map 51 are projected onto the camera frame of the camera 2e recorded in the new map 52, and the feature amount of the projected point and the feature points in the camera frame of the camera 2e are projected. Is performed by comparing with the feature amount of. This comparison is performed, for example, by calculating the similarity as in step S35 of the first embodiment shown in FIG. 6 and identifying the feature points corresponding to the projection points as in step S36.
  • the map points in the new map 52 are projected onto the camera frame of the camera 2d recorded in the old map 51, and the feature amount of the projected point and the feature points in the camera frame of the camera 2d are projected. It may be performed by comparing with the feature amount of.
  • control unit 4 puts together the map points in the pre-integrated map 54 that are determined to correspond as a result of the feature point matching.
  • control unit 4 combines the map points into one by using the average of the position information of the old and new map points associated by the feature point matching as the map point position information of the integrated map 53.
  • the control unit 4 when the control unit 4 determines that the position of the moving body in the environment is not specified based on the search result, the control unit 4 indicates the spatial position in the environment of the feature points extracted thereafter.
  • the map 52 is generated.
  • the position of the moving body 1 is complemented by using the motion information detected by the IMU 3 to generate the new map 52, thereby continuing the generation of the map information. be able to.
  • the control unit 4 may generate an integrated map 53 that integrates the old map 51 and the new map 52. As a result, the information included in the old map 51 obtained before the time when the lost state occurs can be efficiently used.
  • FIG. 15 is a block diagram showing the configuration of the position measuring device 300 according to this embodiment.
  • the position measuring device 300 detects the angular velocity, acceleration, etc. measured by the IMU 3 when the control unit 4 realizes the function of the map management unit 45c. The difference is that motion information is used.
  • the control unit 4 acquires and uses the angular velocity from the IMU 3 will be described.
  • FIG. 16 is a flowchart showing the operation flow of the position measuring device 300 in this embodiment. Unlike the second embodiment, in the operation of the present embodiment shown in FIG. 16, when the lost flag is ON (Yes in S21), the control unit 4 determines that the magnitude of the angular velocity measured by the IMU 3 is equal to or larger than the threshold value ⁇ th . It is determined whether there is any (S23).
  • step S23 for determining whether or not the magnitude of the angular velocity is equal to or larger than the threshold value ⁇ th will be described with reference to FIG.
  • the control unit 4 acquires the angular velocities ⁇ 1 to ⁇ 7 measured by the IMU 3 from the time t ⁇ t to the time t.
  • the number of angular velocity measurement values measured by the IMU 3 from time t- ⁇ t to time t is not limited to seven. This number may change depending on the frame rate (1/ ⁇ t) of the camera 2 and the measurement interval of the IMU 3.
  • control unit 4 may determine that the magnitude of the angular velocity is equal to or greater than the threshold ⁇ th when the magnitude of at least one of the measured angular velocity values ⁇ 1 to ⁇ 7 is equal to or greater than the threshold ⁇ th (S23). And yes).
  • the control unit 4 determines that the magnitude of the angular velocity is equal to or greater than the threshold ⁇ th (Yes in S23). To do.
  • control unit 4 may determine that the magnitude of the angular velocity is equal to or greater than the threshold ⁇ th (Yes in S23).
  • step S40 when it is determined that the magnitude of the angular velocity is equal to or larger than the threshold value ⁇ th (Yes in S23), the control unit 4 proceeds to step S40 without creating a new map. On the other hand, when determining that the magnitude of the angular velocity is less than the threshold value ⁇ th (No in S23), the control unit 4 proceeds to step S22 for creating a new map. After step S22, the process proceeds to step S40 for calculating the self position.
  • the control unit 4 determines whether or not the moving amount of the mobile body 1 is equal to or larger than a predetermined threshold value based on the motion information of the mobile body 1 acquired from the IMU 3. The control unit 4 does not generate the new map 52 when it determines that the movement amount is equal to or greater than the predetermined threshold value. When determining that the movement amount is less than the predetermined threshold value, the control unit 4 generates the new map 52.
  • the camera 2 acquires captured images at a predetermined time interval ⁇ t
  • the IMU 3 detects the motion information once at the predetermined time interval ⁇ t
  • the movement amount is defined by the detection result of the motion information. May be.
  • the camera 2 acquires the captured image at a predetermined time interval ⁇ t
  • the IMU 3 detects the motion information a plurality of times at the predetermined time interval ⁇ t, and the movement amount depends on the detection result of the motion information at each time. May be specified.
  • the camera 2 acquires the captured image at a predetermined time interval ⁇ t
  • the IMU 3 detects the motion information a plurality of times at the predetermined time interval ⁇ t
  • the movement amount corresponds to the detected plurality of motion information. It may be defined by an average.
  • the moving amount of the moving body 1 is at least one of the angular velocity and the acceleration of the moving body 1.
  • the timing of creating the new map 52 can be delayed according to the motion information such as the angular velocity of the moving body 1 when a loss occurs.
  • the motion information such as the angular velocity of the moving body 1 when a loss occurs.
  • 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.
  • control unit 4 may delete the old map 51 and execute the map initialization process of creating the new map 52.
  • the present disclosure can be applied to a position measuring device that measures the position of a moving body.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (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 (10)

  1. 　移動体の位置を測定する位置測定装置であって、
    　前記移動体に搭載され、前記移動体の周囲の環境を撮像して撮像画像を取得する撮像部と、
    　前記移動体の動きを示す動き情報を検出する検出部と、
    　前記撮像画像から特徴点を抽出する制御部と、
    　前記特徴点の前記環境における空間位置を示す第１位置情報を記憶する記憶部とを備え、
    　前記制御部は、
    　　前記撮像画像上で、前記第１位置情報が示す空間位置に対応する画像上の位置を探索し、探索結果に基づいて前記環境における前記移動体の位置が特定されるか否かを判断し、
    　　前記環境における前記移動体の位置が特定されると判断した場合、特定された前記画像上の位置に基づいて前記環境における前記移動体の位置を算出し、
    　　前記環境における前記移動体の位置が特定されないと判断した場合、前記動き情報に基づいて前記環境における前記移動体の位置を算出する、
    位置測定装置。
  2. 　前記制御部は、前記探索結果に基づいて前記環境における前記移動体の位置が特定されないと判断した場合、それ以降に抽出された前記特徴点の前記環境における空間位置を示す第２位置情報を生成する、請求項１に記載の位置測定装置。
  3. 　前記制御部は、前記第１位置情報と前記第２位置情報とを統合した第３位置情報を生成する、請求項２に記載の位置測定装置。
  4. 　前記制御部は、
    　　前記動き情報に基づいて、前記移動体の移動量が所定の閾値以上であるか否かを判定し、
    　　前記移動量が所定の閾値以上であると判定したとき、前記第２位置情報を生成せず、
    　　前記移動量が所定の閾値以上でないと判定したとき、前記第２位置情報を生成する、
    請求項２又は３に記載の位置測定装置。
  5. 　前記撮像部は、所定の時間間隔で前記撮像画像を取得し、
    　前記検出部は、前記所定の時間間隔に前記動き情報を１回検出し、
    　前記移動量は、前記動き情報の検出結果によって規定される、
    請求項４に記載の位置測定装置。
  6. 　前記撮像部は、所定の時間間隔で前記撮像画像を取得し、
    　前記検出部は、前記所定の時間間隔に前記動き情報を複数回検出し、
    　前記移動量は、各回の動き情報の検出結果によって規定される、
    請求項４に記載の位置測定装置。
  7. 　前記撮像部は、所定の時間間隔で前記撮像画像を取得し、
    　前記検出部は、前記所定の時間間隔に前記動き情報を複数回検出し、
    　前記移動量は、検出された複数の動き情報の平均によって規定される、
    請求項４に記載の位置測定装置。
  8. 　前記移動量は、前記移動体の角速度及び加速度の少なくとも一方である、
    請求項４～７のいずれかに記載の位置測定装置。
  9. 　前記検出部は、慣性計測装置、加速度センサ及びジャイロセンサのうちの少なくとも１つを含む、請求項１～８のいずれかに記載の位置測定装置。
  10. 　前記撮像画像は、距離画像及び色画像を含む、請求項１～９のいずれかに記載の位置測定装置。
PCT/JP2019/046204 2018-12-28 2019-11-26 位置測定装置 WO2020137314A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018247818A JP2022037260A (ja) 2018-12-28 2018-12-28 位置測定装置
JP2018-247818 2018-12-28

Publications (1)

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

Family

ID=71129717

Family Applications (1)

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

Country Status (2)

Country Link
JP (1) JP2022037260A (ja)
WO (1) WO2020137314A1 (ja)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018031616A (ja) * 2016-08-23 2018-03-01 住友電気工業株式会社 位置検出システム、車載装置、中央装置および位置検出プログラム
WO2018047392A1 (ja) * 2016-09-06 2018-03-15 Cyberdyne株式会社 モビリティ及びモビリティシステム
JP2018165889A (ja) * 2017-03-28 2018-10-25 カシオ計算機株式会社 自律移動装置、方法およびプログラム

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018031616A (ja) * 2016-08-23 2018-03-01 住友電気工業株式会社 位置検出システム、車載装置、中央装置および位置検出プログラム
WO2018047392A1 (ja) * 2016-09-06 2018-03-15 Cyberdyne株式会社 モビリティ及びモビリティシステム
JP2018165889A (ja) * 2017-03-28 2018-10-25 カシオ計算機株式会社 自律移動装置、方法およびプログラム

Also Published As

Publication number Publication date
JP2022037260A (ja) 2022-03-09

Similar Documents

Publication Publication Date Title
JP6658001B2 (ja) 位置推定装置、プログラム、位置推定方法
JP6435750B2 (ja) ３次元座標算出装置、３次元座標算出方法および３次元座標算出プログラム
US10529076B2 (en) Image processing apparatus and image processing method
JP6237326B2 (ja) 姿勢推定装置、姿勢推定方法及び姿勢推定用コンピュータプログラム
EP2917754B1 (en) Image processing method, particularly used in a vision-based localization of a device
US9679384B2 (en) Method of detecting and describing features from an intensity image
WO2016035324A1 (en) Method for estimating motion, mobile agent and non-transitory computer-readable medium encoded with a computer program code for causing a processor to execute a method for estimating motion
CN104981680A (zh) 相机辅助的运动方向和速度估计
US20220156973A1 (en) Information processing apparatus, information processing method, and program
JP2019132664A (ja) 自車位置推定装置、自車位置推定方法、及び自車位置推定プログラム
US10607350B2 (en) Method of detecting and describing features from an intensity image
TW201926141A (zh) 影像目標追蹤方法及裝置
US11257177B2 (en) Moving object action registration apparatus, moving object action registration system, and moving object action determination apparatus
CN111382637A (zh) 行人检测跟踪方法、装置、终端设备及介质
El Bouazzaoui et al. Enhancing RGB-D SLAM performances considering sensor specifications for indoor localization
JPWO2017094140A1 (ja) 物体検出装置及び物体検出方法
JP2018205950A (ja) 自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、及び自車位置推定プログラム
JP7056840B2 (ja) 自車位置推定装置、自車位置推定方法、および自車位置推定プログラム
WO2020137314A1 (ja) 位置測定装置
US20220291009A1 (en) Information processing apparatus, information processing method, and storage medium
JP6886136B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
WO2020137313A1 (ja) 位置測定装置
KR20190116039A (ko) 이동형 단말기에서의 증강 현실을 제공하기 위한 위치 인식 방법 및 시스템
CN117649619B (zh) 无人机视觉导航定位回收方法、系统、装置及可读存储介质
JP7019113B2 (ja) 相対位置検出装置、相対位置検出方法、及び相対位置検出プログラム

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

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19903868

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: JP