JP2018156538A - Autonomous mobile device, image processing method and program - Google Patents

Abstract

[Problem] To improve the technology of using created maps. An autonomous mobile device 100 includes an imaging section 41, a map creation section 11, a ceiling estimation section 13, a determination section 14, and a determination result processing section. The map creation unit 11 creates map information using information on a plurality of images taken by the imaging unit 41. The ceiling estimating unit 13 estimates a ceiling plane using information on a plurality of images taken by the imaging unit 41. The determining unit 14 uses the ceiling plane estimated by the ceiling estimating unit 13 to determine whether the map information includes more errors than a reference value. The determination result processing unit performs predetermined processing according to the result determined by the determination unit 14. [Selection diagram] Figure 3

Description

  The present invention relates to an autonomous mobile device, an image processing method, and a program.

  Autonomous mobile devices that move autonomously according to their use have become widespread. For example, an autonomous mobile device that moves autonomously for indoor cleaning is known. In general, such an autonomous mobile device needs to both create a map in real space and estimate its own position in the real space.

  A technique called SLAM (Simultaneous Localization And Mapping) is known as a technique for simultaneously creating a map in real space and estimating its own position in the real space. In the SLAM method, a feature point is extracted from each frame of a moving image captured by a camera, and the same feature point is tracked in different frames, so that the three-dimensional position (camera position) of the own device and the three-dimensional position of the feature point are detected. (This collects and constitutes the information of a map) and the process which estimates alternately is performed. For example, Patent Document 1 discloses a mobile robot that can move while creating a surrounding map using the SLAM method.

JP, 2006-52515, A

  The mobile robot disclosed in Patent Document 1 can move while creating a map by using the SLAM method. However, there is room for improvement in the technique using the created map in the mobile robot.

  An object of this invention is to improve the technique using the created map.

The autonomous mobile device of the present invention is
An imaging unit;
A map creation unit that creates map information using information of a plurality of images captured by the imaging unit;
A ceiling estimation unit that estimates a ceiling plane using information of a plurality of images captured by the imaging unit;
Using the ceiling plane estimated by the ceiling estimation unit, a determination unit that determines whether the map information includes more errors than the reference,
A determination result processing unit that performs a predetermined process according to a result determined by the determination unit;
Is provided.

  ADVANTAGE OF THE INVENTION According to this invention, the autonomous mobile apparatus, the image processing method, and program which improved the technique using the created map can be provided.

It is a figure which shows the external appearance of the autonomous mobile apparatus which concerns on Embodiment 1 of this invention. It is a figure which shows an example of the image which the imaging part with which the autonomous mobile device which concerns on Embodiment 1 is provided. It is a figure which shows the structure of the autonomous mobile apparatus which concerns on Embodiment 1. FIG. It is a figure which shows the flowchart of the whole autonomous movement control process which concerns on Embodiment 1. FIG. It is a figure which shows the flowchart of the initialization process of the own position estimation thread | sled in the autonomous movement control process which concerns on Embodiment 1. FIG. It is a figure which shows the flowchart of the own position estimation process after the initialization process of the own position estimation thread | sled in the autonomous movement control process which concerns on Embodiment 1. FIG. It is a figure which shows the flowchart of the process of the map creation thread | sled in the autonomous movement control process which concerns on Embodiment 1. FIG. It is a figure which shows the flowchart of the process of the loop closing thread | sled in the autonomous movement control process which concerns on Embodiment 1. FIG. It is a figure which shows the flowchart of the process of the error determination thread | sled in the autonomous movement control process which concerns on Embodiment 1. FIG. It is a figure explaining real space in three-dimensional Hough transform. It is a figure explaining the Hough space in three-dimensional Hough transform. It is a figure explaining that the image of the ceiling which can be acquired becomes small when the distance to a front wall becomes small. It is a figure which shows the flowchart of a process of the error determination thread | sled concerning Embodiment 2 of this invention. It is a figure which shows the flowchart of the process of the error determination thread | sled which concerns on Embodiment 3 of this invention. It is a figure which shows an example of a structure which provides one part, such as a control part, in an external server.

  Hereinafter, an autonomous mobile device, an autonomous mobile method, and a program according to embodiments of the present invention will be described with reference to the drawings. In the drawings, the same or corresponding parts are denoted by the same reference numerals.

(Embodiment 1)
An autonomous mobile device according to an embodiment of the present invention is a device that autonomously moves indoors depending on the application while creating a map of the surroundings. Examples of this use include security monitoring, indoor cleaning, pets, and toys.

  As shown in FIG. 1, the autonomous mobile device 100 includes an imaging unit 41 and a drive unit 42 in appearance.

  The imaging unit 41 includes a monocular imaging device (camera). The imaging unit 41 acquires an image (frame) at 30 fps, for example. The autonomous mobile device 100 performs autonomous movement while recognizing its own position and surrounding environment in real time based on images sequentially acquired by the imaging unit 41. The imaging unit 41 uses a wide-angle lens so that an image in the ceiling direction can be acquired, and acquires an image including the ceiling as shown in FIG.

The drive unit 42 is an independent two-wheel drive type, and is a moving means including wheels and a motor. The autonomous mobile device 100 translates back and forth (translation) by driving the two wheels in the same direction, and rotates (changes direction) on the spot by driving the two wheels in the reverse direction. A swivel movement (translation + rotation (direction change) movement) can be performed by the changed drive. Each wheel is also equipped with a rotary encoder, which measures the rotational speed of the wheel with the rotary encoder and uses the geometric relationship such as the wheel diameter and the distance between the wheels to translate and move. The amount can be calculated. For example, if the wheel diameter is D and the rotation speed is R (measured by a rotary encoder), the translational movement amount at the ground contact portion of the wheel is π · D · R. Further, if the wheel diameter is D, the distance between the wheels is I, the rotation speed of the right wheel is R _R , and the rotation speed of the left wheel is _RL , the rotation amount of the direction change is 360 (when the right rotation is positive). ° × D × (R _L −R _R ) / (2 × I). By sequentially adding the translational movement amount and the rotation amount, the drive unit 42 functions as a so-called odometry, and measures its own position (position and orientation with reference to the position and orientation at the start of movement). Can do.

  The accuracy of the position and orientation of the own aircraft obtained from odometry often becomes low due to wheel wear or slip. In particular, due to the accumulation of errors, the accuracy deteriorates with time. However, regarding the rotational component (direction information) of the odometry, the accuracy can be improved by using an angular velocity sensor that measures the angular velocity (angular movement amount per unit time). In addition, by using an azimuth sensor (not shown) that detects geomagnetism and identifies the azimuth, information on the absolute direction using geomagnetism can be obtained regardless of the obtained value from the odometry.

  In addition, you may make it provide a crawler instead of a wheel, and you may make it move by providing a plurality (for example, two) leg | foot and walking with a leg | foot. In these cases as well, the position and orientation of the subject aircraft can be measured based on the movements of the two crawlers and the movements of the feet as in the case of the wheels.

  As illustrated in FIG. 3, the autonomous mobile device 100 includes a control unit 10, a storage unit 20, a sensor unit 30, an input unit 43, a communication unit 44, and a power supply 45 in addition to the imaging unit 41 and the drive unit 42.

  The control unit 10 is configured by a CPU (Central Processing Unit) or the like, and by executing a program stored in the storage unit 20, each unit described later (a map creation unit 11, a position estimation unit 12, a ceiling estimation unit 13, a determination) The functions of the unit 14 and the map editing unit 15) are realized.

  The storage unit 20 includes a ROM (Read Only Memory), a RAM (Random Access Memory), and the like, and includes an image storage unit 21, a map storage unit 22, and a ceiling estimated value storage unit 23. The ROM stores a program executed by the CPU of the control unit 10 (for example, a program related to an SLAM method calculation and an autonomous movement control process described later) and data necessary for executing the program in advance. The RAM stores data that is created or changed during program execution.

  The image storage unit 21 stores an image captured by the imaging unit 41. However, in order to save the storage capacity, the image storage unit 21 may not store all the images captured by the imaging unit 41, and may store the feature amount of the image instead of the image itself. . For important images (key frames, which will be described later), information on the position of the own device (the position and orientation of the own device) estimated by the position estimation unit 12 using the SLAM method described later is also used. It is stored in the image storage unit 21 together with information (in association with the image).

  The map storage unit 22 stores map information (feature points and information on three-dimensional positions of obstacles) created by the map creation unit 11 based on information from the SLAM method and the obstacle sensor 31 described later.

  The ceiling estimated value storage unit 23 stores the coefficient of the equation representing the plane of the ceiling estimated by the ceiling estimation unit 13. The equation representing the ceiling plane will be described later.

  As the sensor unit 30, an obstacle sensor 31 and a floor sensor 32 are provided. The obstacle sensor 31 is a distance sensor that can detect an object (obstacle) present in the surroundings and measure the distance to the object (obstacle), and is, for example, an infrared sensor or an ultrasonic sensor. . In addition, you may make it detect an obstruction using the imaging part 41, without mounting the independent obstruction sensor 31. FIG. Moreover, you may provide the bumper sensor (not shown) which detects having collided with the other object. The floor sensor 32 is a sensor that detects whether or not the autonomous mobile device 100 is in contact with the floor surface. While the autonomous mobile device 100 is moving on the floor surface, the floor sensor 32 detects that the autonomous mobile device 100 is in contact with the floor surface. For example, when a person lifts the autonomous mobile device 100, the floor sensor 32 detects that the autonomous mobile device 100 has left the floor.

  As the input unit 43, an operation button for operating the autonomous mobile device 100 is provided. The operation buttons include, for example, a power button, a mode switching button (switching between the cleaning mode, the pet mode, and the like), an initialization button (re-creating the map), and the like. As the input unit 43, a microphone (not shown) that inputs sound and a voice recognition unit that recognizes a voice of an operation instruction to the autonomous mobile device 100 may be provided.

  The communication unit 44 is a module for communicating with an external device, and is a wireless module including an antenna when performing wireless communication with the external device. For example, the communication unit 44 is a wireless module for performing short-range wireless communication based on Bluetooth (registered trademark). The communication unit 44 exchanges data between the autonomous mobile device 100 and the outside.

  The power source 45 is a power source for operating the autonomous mobile device 100, and is generally a built-in rechargeable battery, but may be a solar cell or a system that is wirelessly powered from the floor. Also good. When the power supply 45 is a rechargeable battery, it is charged by connecting the autonomous mobile device 100 to a charger (charging station).

  Next, a functional configuration of the control unit 10 of the autonomous mobile device 100 will be described. The control unit 10 realizes the functions of the map creation unit 11, the position estimation unit 12, the ceiling estimation unit 13, the determination unit 14, and the map editing unit 15, the SLAM method calculation described later, movement control of the autonomous mobile device 100, and the like. I do. The control unit 10 supports a multi-thread function, and can proceed with a plurality of threads (different processing flows) in parallel.

  The map creation unit 11 estimates a three-dimensional position of a feature point, which will be described later, using the SLAM method based on information on the image stored in the image storage unit 21 and information on the position and orientation of the own device at the time of image capture. Map information is created by acquiring the three-dimensional position of the obstacle based on the position and orientation information of the own device when the obstacle sensor 31 detects the obstacle. Then, the estimated three-dimensional position of the feature point, the acquired three-dimensional position of the obstacle, and the like are stored in the map storage unit 22 as map information. A feature point corresponding to a feature point existing in a two-dimensional space on an image in a three-dimensional space of SLAM coordinates described later is referred to as a Map point. One Map point may be observed from a plurality of images. That is, one Map point may be associated with a plurality of feature points (on the image).

  The position estimation unit 12 estimates the position and orientation of its own device as visual odometry based on the SLAM method described later.

  The ceiling estimation unit 13 extracts a feature point (ceiling feature point) existing on the ceiling from the feature points stored in the map storage unit 22 by a three-dimensional Hough transformation described later, and the ceiling plane ( Estimate the ceiling plane. Then, the coefficient of the equation representing the estimated ceiling plane (ceiling coefficient) is stored in the ceiling estimated value storage unit 23.

  The determination unit 14 determines that a distance from a ceiling plane represented by a ceiling coefficient stored in the ceiling estimated value storage unit 23 among the Map points corresponding to feature points included in a key frame described later is a reference distance (for example, If the number of Map points equal to or less than 10 cm) is obtained and the number is less than the reference number (for example, 2), the map information stored in the map storage unit 22 includes more errors than the reference. judge.

  The determination of the determination unit 14 will be supplementarily described. As shown in FIG. 2, the image acquired by the imaging unit 41 includes a large portion of the ceiling. Therefore, usually, a map point corresponding to a feature point in the key frame includes a large number of images existing on the ceiling. . The distance between the Map point existing on the ceiling and the ceiling plane is zero. Therefore, if the ceiling plane represented by the ceiling coefficient stored in the ceiling estimated value storage unit 23 is the correct ceiling plane, the ceiling estimated value storage unit among the Map points corresponding to the feature points included in the key frame. Many Map points whose distance from the ceiling plane represented by the ceiling coefficient stored in 23 is almost 0 (that is, not more than the reference distance) are included (more than the reference number).

  That is, among the Map points corresponding to the feature points included in the key frame, the number of Map points whose distance from the ceiling plane represented by the ceiling coefficient stored in the ceiling estimated value storage unit 23 is equal to or less than the reference distance. Is less than the reference number (assuming that the ceiling coefficient is almost correct) means that the three-dimensional position of the Map point is incorrect. Since the information on the 3D position of the Map point is a part of the map information stored in the map storage unit 22, the fact that the 3D position of the Map point is incorrect is stored in the map storage unit 22. It can be inferred that the map information contains many errors. Therefore, by the above determination, it can be determined whether or not the map information stored in the map storage unit 22 includes more errors than the reference.

  If the determination unit 14 determines that there are many errors included in the map information stored in the map storage unit 22, the map editing unit 15 edits the map. The simplest process for editing a map is to delete the map.

  The functional configuration of the autonomous mobile device 100 has been described above. Next, the autonomous movement control process of the autonomous mobile device 100 will be described with reference to FIG. FIG. 4 is a flowchart of the entire autonomous movement control process of the autonomous mobile device 100. The autonomous mobile device 100 starts this autonomous movement control process when the power is turned on. First, the control unit 10 of the autonomous mobile device 100 starts each of the own machine position estimation thread (step S101), the map creation thread (step S102), the loop closing thread (step S103), and the error determination thread (step S104). To do. Details of these threads will be described later.

  Thereafter, the control unit 10 determines whether or not to end the operation by the autonomous movement control process (step S105). If the operation ends (step S105; Yes), the operation ends. If not, the operation ends (step S105; No), the map creation unit 11 creates and updates map information (step S106). Next, in order to move autonomously, the control unit 10 instructs the drive unit 42 to perform a desired operation (step S107), and returns to step S105. By this autonomous movement control process, the map information can be updated as appropriate while autonomously moving based on the map information.

  A typical example of autonomous movement control processing by the autonomous mobile device 100 will be introduced. When the autonomous mobile device 100 first turns on the power supply 45 in a state where it is placed on the charger, it relies on the obstacle sensor 31 to move through each room of the house, and the obstacle sensor 31 positions obstacles such as walls. And map information including obstacle positions can be created. When a map is created to some extent, it is possible to know an area that has no map information but is considered to be movable, and it is possible to encourage the creation of a wider map by moving autonomously to that area. It becomes like this. And if the information of the map of the almost movable whole area is created, the efficient movement operation | movement using the information of a map will be attained. For example, it is possible to return to the charger through the shortest route from any position in the room or to efficiently clean the room.

  Next, the own position estimation thread activated in step S101 of the overall flowchart of the autonomous movement control process (FIG. 4) will be described with reference to FIG. This thread is a process in which the position estimation unit 12 first performs an initialization process, and then continues its own position estimation (estimating its own position by visual odometry using an image acquired by the imaging unit 41).

  The position estimation unit 12 first clears the initialized flag (step S201). The initialized flag is a flag variable indicating whether or not the initializing process that the position estimating unit 12 first performs in order to estimate the position of the aircraft has been completed. When the initialization process is completed, 1 is set to the initialized flag. Next, it is determined whether or not to end the operation by the autonomous movement control process (step S202). If the operation ends (step S202; Yes), the operation ends. If the operation does not end (step S202; No), it is determined whether or not the operation has been completed (step S203). This determination is made based on whether the initialized flag is 1 (initialized) or 0 (not initialized). If it has been initialized (step S203; Yes), the own position estimation process from step S231 shown in FIG. 6 is performed. If it has not been initialized (step S203; No), the process proceeds to step S204 to perform the initialization process. First, the initialization process will be described.

  In the initialization process, the position estimation unit 12 first sets −1 to the frame counter N (step S204). Then, if it is detected by the floor sensor 32 that the autonomous mobile device 100 is in contact with the floor surface, the position estimation unit 12 acquires an image with the imaging unit 41 (step S205). Step S205 is also called an imaging step. While the floor sensor 32 detects that the floor sensor 32 is not in contact with the floor surface (lifted), the position estimation unit 12 does not perform the processes in and after step S205 until it is detected that the floor sensor 32 has contacted the floor surface. Keep on waiting. The image can be acquired at 30 fps, for example. The image acquired by the imaging unit 41 is also called a frame. Next, feature points are acquired from the acquired image (step S206). A feature point refers to a characteristic part in an image such as a corner part in the image. The feature points can be acquired by using an algorithm such as SIFT (Scale-Invariant Future Transform) or SURF (Speed-Up Robust Features). Note that algorithms other than these may be used to obtain feature points. In step S206, the position estimation unit 12 functions as a feature point acquisition unit.

  If the number of acquired feature points is small, the posture cannot be calculated by the two-view structure from motion method described later. Therefore, the position estimation unit 12 determines whether or not the number of feature points acquired is greater than or equal to a reference value (for example, 10) (step S207). If the number of feature points acquired is less than the reference value (step S207; No), the process returns to step S205, and image acquisition and feature point acquisition are repeated until the number of feature points equal to or greater than the reference value is obtained. At this point, the map information has not yet been created. However, in the above-described typical example, for example, the initialization process is started because the obstacle sensor 31 is being relied on to start moving all rooms. If the image acquisition and the feature point acquisition are repeated, the image acquisition is repeated while moving, so that various images can be acquired, and any of them can be expected to acquire an image with a large number of feature points.

  If the number of feature points acquired is equal to or greater than the reference value (step S207; Yes), the position estimation unit 12 increments the frame counter N (step S208). Then, it is determined whether or not the frame counter N is 0 (step S209). If the frame counter N is 0 (step S209; Yes), it means that only one image has been acquired, so the process returns to step S205 to acquire the second image. Although not shown in the flowchart shown in FIG. 5, the position of the own machine when the first image is acquired is somewhat distant from the position of the own machine when the second image is acquired. However, the accuracy of the posture estimated in the subsequent processing is improved. Therefore, when returning from step S209 to step S205, a process of waiting until the translation distance by odometry becomes a predetermined distance (for example, 1 m) or more according to an operation instruction to the drive unit 42 in step S107 of the overall flowchart (FIG. 4). May be added.

  If the frame counter N is not 0 (step S209; No), it means that two images have been acquired, so the position estimation unit 12 acquires the correspondence of the feature points between these two images (step S210). Here, a supplementary explanation will be given regarding the correspondence between the feature points. When the same point in the real environment exists as a feature point in each of these two images, the set of these two feature points is called a feature point correspondence. Acquiring such a set of feature points is referred to as acquisition of feature point correspondences, and the number of feature point sets that can be acquired is referred to as feature point correspondence numbers.

  If the number of corresponding feature points is less than 5, it is impossible to estimate the relative posture between two images, which will be described later. Therefore, the position estimation unit 12 determines whether the number of corresponding feature points is less than 5 (step) S211). If the number of corresponding feature points is less than 5 (step S211; Yes), the process returns to step S204 to reacquire the initial image. If the number of corresponding feature points is 5 or more (step S211; No), the position estimation unit 12 uses the Two-view Structure from Motion method to acquire a relative posture between the two images (respective images are acquired). The difference between the positions (translation vector t) and the difference between the directions (rotation matrix R)) are calculated (step S212).

  Specifically, the calculation of the relative posture between the two images is performed by obtaining a basic matrix E from corresponding feature points and decomposing the basic matrix E into a translation vector t and a rotation matrix R. In addition, the value of each element of the translation vector t obtained by this calculation (having three elements of X, Y, and Z with the position where the first image is acquired as the origin, assuming movement in a three-dimensional space) Is different from the value in the real environment (the Two-view Structure from Motion method cannot obtain the value in the real environment itself, but obtains the value in a space similar to the real environment). These are regarded as values on the SLAM space, and will be described below using coordinates on the SLAM space (SLAM coordinates).

  When the relative orientation (translation vector t and rotation matrix R) between the two images is obtained, the values are based on the first image (the position where the first image was acquired is the origin of the SLAM coordinates, the translation vector is the 0 vector, the rotation When the matrix is the unit matrix I), the orientation of the second image (position (translation vector t) and orientation (rotation matrix R) of the own image when the second image is acquired)) and Become. Here, when the postures of the two images (the position (translation vector t) and direction (rotation matrix R) of the own device at the time of photographing the image (frame) are also referred to as frame postures) are obtained. Based on the following concept, the map creation unit 11 obtains the three-dimensional position in the SLAM coordinates of the feature points (corresponding feature points) that can be matched between the two images (step S213). In this process, the map creation unit 11 functions as a three-dimensional position acquisition unit.

If the coordinates (frame coordinates: known) of the feature point in the image are (u, v) and the three-dimensional position (unknown) of the feature point in the SLAM coordinates is (X, Y, Z), these are homogeneous. These relationships when expressed in coordinates are expressed by the following formula (1) using the perspective projection matrix P. Here, the symbol “to” represents “equal except for a non-zero constant multiple” (that is, equal or is a constant (non-zero) multiple), and the symbol “′” represents “transpose”. Represents.
(U v 1) ′ to P (XY Y 1) ′ (1)

In the above equation (1), P is a 3 × 4 matrix, and from the 3 × 3 matrix A indicating the internal parameters of the camera and the external parameters R and t indicating the posture (frame posture) of the image, the following equation It is represented by (2). Here, (R | t) represents a matrix in which the translation column vector t is arranged to the right of the rotation matrix R.
P = A (R | t) (2)

  In the above equation (2), R and t are obtained as the frame postures as described above. Further, since the internal parameter A of the camera is determined by the focal length and the image sensor size, it is a constant if the image capturing unit 41 is determined.

One of the feature points corresponding to the two images is the frame coordinates (u ₁ , v ₁ ) of the first image and the frame coordinates (u ₂ , v ₂ ) of the _second image. The following equations (3) and (4) can be obtained. Here, I is a unit matrix, 0 is a zero vector, and (L | r) is a matrix in which a column vector r is arranged to the right of the matrix L.
(U ₁ v ₁ 1) ′ to A (I | 0) (XY Z 1) ′ (3)
(U ₂ v ₂ 1) ′ to A (R | t) (XY Z 1) ′ (4)

In the above formulas (3) and (4), because there are formulas for u ₁ , v ₁ , u ₂ , and v _2, there are four formulas, but there are three unknowns, X, Y, and Z. , X, Y, Z can be obtained, and this is the three-dimensional position of the feature point in the SLAM coordinates. Since the number of equations is larger than the number of unknowns, for example, X, Y, Z obtained from u ₁ , v ₁ , u ₂ and X, Y, Z obtained from u ₁ , v ₁ , v ₂ are used. May be different. In such a case, it becomes a simultaneous linear equation with an excess condition, and there is generally no solution, but the map creation unit 11 finds the most probable X, Y, and Z using the least square method.

  When the three-dimensional position (X, Y, Z) of the feature point in the SLAM coordinate is obtained, it is used as a Map point, and the map creation unit 11 is also referred to as a Map point database (Map point DB (Database), which is stored in the map storage unit 22. Registered) (step S214). As elements to be registered in the Map point database, at least “X, Y, Z which is a three-dimensional position of a feature point in SLAM coordinates” and “a feature amount of the feature point” (for example, a feature amount obtained by SIFT or the like) is necessary. Further, if a “time stamp” (a value at the time of registration in the Map point database of a key frame counter NKF (a variable representing the current key frame number) described later) is added to the registered elements in the Map point database. This is convenient when editing the Map point database (such as returning to the past state).

  Then, the map creating unit 11 determines whether or not all the feature points (corresponding feature points) corresponding to each other between the two images are registered in the Map point database (step S215). If not all have been registered yet (step S215; No), the process returns to step S213, and if all have been registered (step S215; Yes), the process proceeds to step S216.

  Next, the position estimation unit 12 initializes NKF (a variable representing a counter of a key frame (pointing to an image to be processed in a subsequent thread)) to 0 (step S216), and the second image is a key frame. Is registered in a frame database (also referred to as a frame DB (Database), which is stored in the image storage unit 21) (step S217).

  Elements to be registered in the frame database are “key frame number” (value of the key frame counter NKF at the time of registration), “posture” (position (translation vector t) in the SLAM coordinates of the own device at the time of image capture) Direction (rotation matrix R)), “all extracted feature points”, “a point where a three-dimensional position is known as a Map point among all feature points”, and “a feature of the key frame itself”.

  Among the above, “feature of the key frame itself” is data for improving the processing for obtaining the image similarity between the key frames, and it is usually preferable to use a histogram of feature points in the image, The image itself may be a “characteristic of the key frame itself”.

  Next, in order to notify the map creation thread that the key frame has been generated, the position estimation unit 12 stores the key frame queue of the map creation thread (the queue has a first-in first-out data structure). The key frame counter NKF is set (step S218).

  Since the initialization process of the own position estimation thread is completed as described above, the position estimation unit 12 sets 1 to the initialized flag (step S219).

  Then, in order to obtain the scale correspondence between the SLAM coordinates and the real environment coordinates, the position estimation unit 12 translates the translation distance (obtained by the coordinates in the real environment) by odometry in the SLAM coordinates estimated by the above processing. The scale Sc is obtained by dividing by the distance sd (step S220). Actually, the scale Sc is a value determined by the imaging range, the number of imaging pixels, and the like, and therefore becomes a constant if the imaging unit 41 is determined. Therefore, a ratio between the translation distance in the actual environment and the translation distance in the SLAM coordinates may be experimentally obtained in advance and set as the scale Sc in advance. By setting the scale Sc in advance, the visual odometry can be used instead of the odometry as will be described later, even if the drive unit 42 does not have the odometry function.

  Then, the control unit 10 clears variables pa, pb, pc, and pd (these are stored in the estimated ceiling value storage unit 23) used in an error determination thread described later (step S221). And it progresses to step S231 shown in FIG. 6 which is a process in the case of having completed initialization via step S202 and step S203.

  Now, with reference to FIG. 6, the process in the case of initialization is described. This process is a normal process of the own machine position estimation thread, and the position estimation unit 12 sequentially estimates the current position and orientation of the own machine (translation vector t and rotation matrix R in SLAM coordinates). It is processing.

  If it is detected by the floor sensor 32 that the autonomous mobile device 100 is in contact with the floor surface, the position estimation unit 12 acquires an image with the imaging unit 41 (step S231). Step S231 is also called a photographing step. While it is detected by the floor sensor 32 that it is not in contact with the floor surface (lifted), the position estimation unit 12 does not perform the processing after step S231 until it is detected that the floor sensor 32 is in contact with the floor surface. Keep on waiting. If it is in contact with the floor, then the position estimation unit 12 increments the frame counter N (step S232), and acquires the feature points included in the captured image (step S233). In step S233, the position estimation unit 12 functions as a feature point acquisition unit. Next, from the information of the previous key frame registered in the frame database (for example, the image whose key frame number is NKF), the three-dimensional position is known among the feature points included in the information of the image. A feature point (which is a Map point registered in the Map point database) is acquired, and a feature point (corresponding feature point) that can be correlated with the image that has just been captured is extracted (step S234).

  Then, the position estimation unit 12 determines whether or not the number of corresponding feature points is less than a predetermined number (for example, 10; hereinafter referred to as “reference corresponding feature number”) (step S235). In step S235; Yes), since the accuracy of the posture estimated by the SLAM method is deteriorated, the process returns to the image acquisition in step S231 without estimating the position. Here, instead of immediately returning to step S231, the process returns to step S234 to search the key frame registered in the frame database for the number of corresponding feature points equal to or greater than the reference corresponding feature number. Also good. In this case, if no key feature registered in the frame database has a number of corresponding feature points equal to or greater than the reference corresponding feature number, the process returns to step S231.

When the position estimation unit 12 can extract the corresponding feature points that are equal to or more than the reference corresponding feature points (step S235; No), the position estimation unit 12 acquires the three-dimensional positions (X _i , Y _i , Z _i ) of the corresponding feature points from the Map point database. (Step S236). The frame coordinates of the corresponding feature point included in the image that has just been taken are (u _i , v _i ), and the three-dimensional position of the corresponding feature point is (X _i , Y _i , Z _i ) (i is 1 To the number of corresponding feature points) and the values (ux _i , vx) obtained by projecting the three-dimensional positions (X _i , Y _i , Z _i ) of the corresponding feature points onto the frame coordinate system by the following equation (5) _i ) and the frame coordinates (u _i , v _i ) should ideally match.
(Ux _i vx _i 1) ′ to A (R | t) (X _i Y _i Z _i 1) ′ (5)

Actually, since (X _i , Y _i , Z _i ) and (u _i , v _i ) also contain errors, (ux _i , vx _i ) and (u _i , v _i ) match. I rarely do it. And the unknowns are only R and t (three-dimensional in the three-dimensional space, and 3 + 3 = 6 is the number of unknowns), but there are twice the number of corresponding feature points (for one corresponding feature point) Since there are equations for u and v of the frame coordinates), it becomes a simultaneous linear equation with an excess condition, and is obtained by the least square method as described above. Specifically, the position estimation unit 12 obtains an attitude (translation vector t and rotation matrix R) that minimizes the cost function E1 of the following equation (6). This is the attitude of the own machine in the SLAM coordinates obtained by the SLAM method (the position and orientation of the own machine represented by the translation vector t and the rotation matrix R). In this way, the position estimation unit 12 estimates the attitude of the own device (step S237).

  Since the current attitude (translation vector t and rotation matrix R) in the SLAM coordinates has been obtained, the position estimation unit 12 obtains VO (visual odometry) by multiplying this by the scale Sc (step S238). The VO can be used as the position and orientation of the aircraft in the real environment.

  Next, the position estimating unit 12 captures a predetermined distance (for example, 1 m, for example, “reference” below) from the position of the own device when the immediately preceding key frame registered in the frame DB (an image having a key frame number of NKF) is captured. It is referred to as “translation distance”.) It is determined whether or not it is moving (step S239). The travel distance of the aircraft compared to the reference translation distance is the odometry of the translation distance from the previous key frame to the current frame (the absolute value of the difference vector between the translation vectors of both frames (the square root of the sum of the squares of the elements)). Or may be obtained from the above-described VO (visual odometry). As described above, the contents to be registered in the frame DB are “key frame number”, “posture”, “all extracted feature points”, and “a point whose 3D position is known as a Map point among all feature points”. , “Characteristics of the key frame itself”.

  If it has moved less than the reference translation distance (step S239; No), the process returns to step S202 of FIG. If it has moved beyond the reference translational distance (step S239; Yes), after incrementing the key frame counter NKF (step S240), the current frame is registered as a key frame in the frame DB (step S241).

  Then, the position estimation unit 12 sets the key frame counter NKF in the key frame queue of the map creation thread in order to notify the map creation thread that a new key frame has occurred (step S242). Then, the process returns to step S202 in FIG. The key frame counter NKF, the scale Sc, the map point DB, and the frame DB are stored in the storage unit 20 so that values can be referred to across threads.

  Next, the map creation thread activated in step S102 of the overall flowchart of the autonomous movement control process (FIG. 4) will be described with reference to FIG. In this thread, the map creation unit 11 creates the map information (Map point DB) by calculating the three-dimensional position of the corresponding feature point in the key frame.

  First, the map creation unit 11 determines whether or not to end the operation by the autonomous movement control process (step S301). If the operation ends (step S301; Yes), the operation ends. If the operation does not end (step S301; No), it is determined whether the key frame queue is empty (step S302). If the key frame queue is empty (step S302; Yes), the process returns to step S301. If the key frame queue is not empty (step S302; No), data is extracted from the key frame queue and is processed by the MKF (key frame processed by the map creation thread). (A variable representing a number) (step S303). The map creation unit 11 determines whether MKF is greater than 0 (step S304). If MKF is 0 (step S304; No), the map creation unit 11 returns to step S301 and waits for data to enter the key frame queue. When MKF is 1 or more (step S304; Yes), the process proceeds to the following process.

  The map creation unit 11 refers to the frame DB and corresponds to the feature point of the previous key frame (key frame number MKF-1) and the feature point of the current key frame (key frame number MKF key frame). Feature points (corresponding feature points) that can be taken are extracted (step S305). Since the posture (translation vector t and rotation matrix R) of each key frame is also registered in the frame DB, the corresponding feature points are three-dimensionally processed in the same manner as in the process of initializing the own position estimation thread. The position can be calculated. The map creation unit 11 registers the three-dimensional position of the corresponding feature point for which the three-dimensional position has been calculated as a Map point in the Map point DB (step S306). Step S306 is also called a map creation step. The map creation unit 11 also registers the three-dimensional position for the feature points for which the current three-dimensional position has been calculated in the frame DB (step S307).

  If the corresponding feature point extracted by the map creation unit 11 has already been registered in the Map point DB, the processing for the next corresponding feature point (one that has not been registered in the Map point DB) is skipped. Alternatively, the three-dimensional position calculation may be performed again to update the three-dimensional position registered in the Map point DB and the corresponding three-dimensional position in the frame DB.

  Next, the map creating unit 11 determines whether or not the key frame queue is empty (step S308). If it is empty (step S308; Yes), bundle adjustment processing is performed on the orientations of all key frames and the three-dimensional positions of all map points to improve accuracy (step S309), and the process proceeds to step S310. . If the key frame queue is not empty (step S308; No), the process proceeds to step S310. Next, the map creating unit 11 sets MKF in the key frame queue of the loop closing thread (step S310), and returns to step S301.

  The bundle adjustment process is a non-linear optimization method that simultaneously estimates the camera posture (key frame posture) and the three-dimensional position of the Map point, and an error that occurs when the Map point is projected onto the key frame. The optimization is performed so that is minimized.

  By performing the bundle adjustment process, it is possible to improve the accuracy of the key frame posture and the three-dimensional position of the Map point. However, even if this process is not performed, the accuracy cannot be improved and a special problem does not occur. Therefore, even when there is no other process (for example, the key frame queue is empty), this process does not need to be performed every time.

  Further, when bundle adjustment processing is performed, a Map point may be found in which an error when projected onto a key frame is larger than a predetermined value. Such a Map point with a large error has an adverse effect on SLAM estimation. Therefore, a flag for identifying the Map point that requires a large error and is deleted from the Map point DB and the frame DB is set. You can put it. Note that the bundle adjustment process is treated as an option in the present embodiment, and the details of the process are omitted here.

  Next, the loop closing thread activated in step S103 in the overall flowchart of the autonomous movement control process (FIG. 4) will be described with reference to FIG. In this thread, the control unit 10 continues to check whether or not the loop closing process can be performed, and if possible, performs the loop closing process. Note that the loop closing process uses the difference between the posture value and the current posture value when you were in the same location before when you recognized that you have returned to the same location that you have been before. This refers to correcting the three-dimensional position of the key frame and the related Map point in the trajectory from the previous time to the present.

  First, the control unit 10 determines whether or not to end the operation by the autonomous movement control process (step S401). If the operation ends (step S401; Yes), the process ends. If the operation is not finished (step S401; No), it is determined whether or not the key frame queue is empty (step S402). If the key frame queue is empty (step S402; Yes), the process returns to step S401. If the key frame queue is not empty (step S402; No), data is extracted from the key frame queue and processed by the LKF (loop closing thread). It is set to a variable representing a key frame number (step S403). Next, the control unit 10 determines whether LKF is greater than 1 (step S404). If LKF is 0 or 1 (step S404; No), the process returns to step S401 and waits for data to enter the key frame queue. When LKF is 2 or more (step S404; Yes), the following processing is performed.

  The control unit 10 refers to the frame DB, and the similarity between the current key frame (key frame whose key frame number is LKF) and “characteristic of the key frame itself” is a predetermined similarity (for example, 0.9, hereinafter “reference image”). (Referred to as “similarity”) Key frames having the above values are searched from the frame DB (step S405). Here, when the feature of the image (key frame) is represented by a feature vector, the similarity is obtained by normalizing the absolute value of the inner product of the feature vectors of the two images to 1, or the features of the two images. An inner product of those obtained by normalizing the absolute value of the vector (the square root of the square sum of the elements) to 1 can be used as the similarity between the two images. Also, the reciprocal of the distance (square root of the sum of squares of the differences between the elements) of the feature vectors of the two images (the absolute value normalized to 1) may be used as the similarity.

  The control unit 10 determines whether or not a key frame with a similarity of “characteristic of the key frame itself” equal to or higher than the reference image similarity is found (step S406), and if not found (step S406; No), step S408. If it is found (step S406; Yes), the posture of the key frame in the locus from the found key frame to the current key frame and the three-dimensional position of the Map point included in the key frame in the locus are corrected. (Step S407). For example, the control unit 10 corrects the posture of the current key frame as the same posture as the found key frame. Then, using the difference between the attitude of the found key frame and the attitude of the current key frame, linear correction is applied to the attitude of each key frame in the trajectory from the found key frame to the current key frame. Further, the three-dimensional position of the Map point included in each key frame is also corrected according to the correction amount of the posture of each key frame.

  Next, the control unit 10 sets LKF in the key frame queue of the error determination thread and returns to step S401 (step S408).

  Next, the error determination thread activated in step S104 of the overall flowchart of the autonomous movement control process (FIG. 4) will be described. This error determination thread assumes that the image acquired by the imaging unit 41 often includes a part of the ceiling, and the feature point existing in the vicinity of the ceiling in the image acquired by the imaging unit 41. If the state that is not included continues, the determination unit 14 determines that the error included in the map information is large (the map information includes more errors than the reference). When the determination unit 14 determines that the error included in the map information is large (the map information includes more errors than the reference), the map editing unit 15 performs a process of deleting the map information. . Specifically, the ceiling estimation unit 13 estimates a ceiling plane by three-dimensional Hough transform, and the determination unit 14 is the number K of map points whose distance from the ceiling plane is a reference ceiling distance ncd (for example, 10 cm) or less. If it is less than (for example, two), it is determined that the error included in the map information is large (the map information includes more errors than the reference).

  Usually, various things such as tables and chairs are often placed on the floor in the room, but nothing in particular exists on the ceiling except lighting. Therefore, it can be assumed that among the feature points included in the image acquired by the imaging unit 41, the feature points present on the plane of the ceiling can be acquired stably. Based on this assumption, the error determination thread determines whether or not the error included in the map information is large (whether or not the map information includes more errors than the reference).

  Now, with reference to FIG. 9, the processing of the error determination thread will be described. First, the control unit 10 determines whether or not to end the operation by the autonomous movement control process (step S501). If the operation ends (step S501; Yes), the operation ends. If not, the operation ends (step S501; No). Then, it is determined whether or not the key frame queue of the error determination thread is empty (step S502). If the key frame queue is empty (step S502; Yes), the process returns to step S501 to wait for data to enter the key frame queue.

  If the key frame queue is not empty (step S502; No), the control unit 10 extracts the data from the key frame queue and sets it to IKF (a variable representing the key frame number of the key frame to be processed by the error determination thread) (step S502). S503). Next, the control unit 10 determines whether or not the variables pa, pb, pc, and pd stored in the ceiling estimated value storage unit 23 are all 0 (step S504). The variables pa, pb, pc, and pd are set with the values of the coefficients of the equations representing the ceiling plane estimated by the ceiling estimation unit 13 as described later. As described above, the variables pa, pb, pc, and pd are initialized in step S221 of FIG. All cleared to zero. Therefore, it is possible to determine whether the ceiling plane estimation by the ceiling estimation unit 13 has not yet been performed based on whether the variables pa, pb, pc, pd are all 0 or not.

  If the variables pa, pb, pc, and pd are all 0 (step S504; Yes), the ceiling estimation unit 13 uses the Map point that exists in the ceiling direction of the current key frame (key frame number is IKF). Then, the ceiling plane is estimated by the three-dimensional Hough transform (step S505). Since the autonomous mobile device 100 initializes the SLAM coordinates by horizontally moving the floor surface in the initialization process of the own position estimation thread shown in FIG. 5, the Map point existing in the ceiling direction based on the SLAM coordinates. Can be extracted from the Map point DB. For simplicity, the Map point included in the upper half area in the image may be “Map point existing in the ceiling direction”.

The estimation of the ceiling plane in step S505 is performed by acquiring a Map point group existing in the ceiling direction from the Map point DB and using a three-dimensional Hough transform that is an extension of a normal (two-dimensional) Hough transform. Therefore, the three-dimensional Hough transform will be described. A plane S including a point (X _p , Y _p , Z _p ) in the real space is represented by a distance ρ _s from the origin and two angles (azimuth angle θ _s and elevation angle φ _s ) as shown in FIG. 10A. be able to. At this time, these relationships can be expressed by the following equations.
X _p = ρ _s · cos φ _s · cos θ _s (7)
Y _p = ρ _s · cos φ _s · sin θ _s (8)
Z _p = ρ _s · sinφ _s (9)

Then, an arbitrary point p (X _p , Y _p , Z _p ) on the plane S in the real space of the XYZ coordinate system becomes one point s (ρ _s ,) in the Hough space of the ρφθ coordinate system as shown in FIG. 10B. φ _s , θ _s ). That is, an arbitrary point p (X _p , Y _p , Z _p ) on the plane S in the real space is converted into one point s (ρ _s , φ _s , θ _s ).

  Therefore, when the (X, Y, Z) coordinates of each Map point are converted into (ρ, φ, θ) coordinates by three-dimensional Hough transform, more points are added to the points on the Hough space corresponding to the plane in the real space. Map points will be converted. Therefore, when the ρφθ space is divided into grid-like regions (grids), the (X, Y, Z) coordinates of each Map point are converted to (ρ, φ, θ) coordinates by three-dimensional Hough transform, and the most Focusing on the grid in which the Map points are converted, this grid is called the most frequent conversion grid. Then, the plane in the real space corresponding to the coordinates on the Hough space corresponding to the most frequent transformation grid (for example, the ρφθ coordinate of the center point of the most frequent transformation grid) can be estimated as the ceiling plane. In step S505, the ceiling estimation unit 13 determines how many Map points have been converted to the most frequent conversion grid. The number of Map points is the number of points that support the plane converted into the most frequent conversion grid.

  Then, the ceiling estimation unit 13 determines whether or not the number of Map points converted to the most frequent conversion grid (the number of points that support a plane) is equal to or less than a reference conversion number (for example, 10) (Step S506). If the Map point converted into the most frequent conversion grid is equal to or less than the reference conversion number (step S506; No), there is little information for estimating the ceiling plane, and the process returns to step S501 as an estimation failure.

If the number of Map points converted to the most frequent conversion grid is larger than the reference conversion number (for example, 10) (step S506; Yes), the ceiling estimation unit 13 estimates the coefficient of the plane equation, and uses the estimated coefficient as the variable ca, Set to cb, cc, cd (step S507). Step S507 is also called a ceiling estimation step. The estimation of the coefficient of this plane equation will be described. First, a plane in real space can be expressed by the following plane equation.
A × x + B × y + C × z + D = 0 (13)
In Expression (13), A, B, C, and D are coefficients of the plane equation. Then, if the XYZ coordinates of each Map point converted to the most frequent conversion grid are substituted into Expression (13), an equation having A, B, C, and D as unknowns is obtained. There are four unknowns, A, B, C, and D, but since the number of Map points to be substituted into Equation (13) is more than 10, the coefficients A, B, C, and D of the plane equation are over-conditions. Can be solved by the least squares method.

  Then, the obtained A, B, C, and D are set in variables ca, cb, cc, and cd, respectively. These variables are variables that temporarily store the coefficients of the plane equation. At this time, the estimated normal vector of the ceiling plane is represented by (ca, cb, cc), and the inclination of the normal vector should ideally be perpendicular to the floor surface (XY plane) ( When the floor surface and the XY plane are different, the coordinates are converted in advance so that the floor surface becomes the XY plane.) That is, the value of cc should be sufficiently larger than ca and sufficiently larger than cb (for example, cc> 10 × ca and cc> 10 × cb). Therefore, the ceiling estimation unit 13 determines whether or not the inclination of the normal vector (ca, cb, cc) is an abnormal value (not a value close to perpendicular to the floor surface) (step S508). If the slope of the normal vector is abnormal (step S508; Yes), the process returns to step S501.

  If the inclination of the normal vector is not abnormal (step S508; No), the variables ca, cb, cc, cd are set to the variables pa, pb, pc, pd, respectively (step S509). Then, the counter variable ECT for error determination is cleared (step S510), and the process returns to step S501.

  On the other hand, if at least one of the variables pa, pb, pc, pd is not 0 in step S504 (step S504; No), the ceiling plane is already estimated. Among the Map points existing in the ceiling direction of the current key frame (key frame number is IKF), the distance from the plane equation represented by the coefficients pa, pb, pc, pd is the reference ceiling distance ncd (for example, 10 cm) The number of the following Map points is set in the variable K (step S511).

  Then, the determination unit 14 determines whether or not the value of the variable K is less than a reference number (for example, 2) (step S512). Step S512 is also called a determination step. If the value of the variable K is equal to or greater than the reference number (step S512; No), the process proceeds to step S510. If the value of the variable K is less than the reference number (step S512; Yes), the counter variable ECT for error determination is incremented (step S513).

  Then, the determination unit 14 determines whether or not the counter variable ECT is equal to or less than a reference counter value (for example, 5) (step S514). If the counter variable ECT is less than or equal to the reference counter value (step S514; No), the process returns to step S501.

  If the counter variable ECT is larger than the reference counter value (step S514; Yes), the generated map information (Map point DB information and frame DB information) includes more errors than the reference. The map editing unit 15 clears all of the initialized flag, the Map point DB, the frame DB, and the key frame queue of each thread (step S515). In step S515, the map editing unit 15 functions as a determination result processing unit that performs a predetermined process according to a result determined by the determination unit 14. Step S515 is also called a determination result processing step. Then, the control unit 10 returns to step S501 and starts again.

  As a result of the error determination thread processing described above, if the map information includes more errors than the standard, the initialized flag, the Map point DB, the frame DB, and the key frame queue of each thread are all cleared. All threads will be redone from the beginning. For this reason, it is possible to prevent the autonomous mobile device 100 from continuing to move in a state where the generated map includes more errors than the reference.

  Although not shown in FIG. 9 for the sake of complexity, when it is determined in step S508 that the inclination of the normal vector of the ceiling plane estimated by the ceiling estimation unit 13 is abnormal, the process does not immediately return to step S501, The inclination abnormality counter variable LAC is incremented, and when the value of the counter variable exceeds a specified number of times (for example, 10 times), it is assumed that the map information includes more errors than the reference, and the process of step S515 is performed. You may make it go.

  If the value of the variable K is greater than or equal to the reference number in step S512 (step S512; No), there is a possibility that the map information at that time does not yet contain much error. Therefore, if the determination in step S512 is No, the map editing unit 15 stores the map information (Map point DB) at that time as a “probable map” in the map storage unit (not shown), and then the step. In S510, the map editing unit 15 returns to the previous Map point DB state in which the Map point DB is stored without clearing the initialized flag, the frame DB, and the key frame queue of each thread in Step S515. Anyway. By doing in this way, the information of the map created in the past can be used effectively.

  As a method of returning the Map point DB to the previous state, in addition to the method of saving the information of the entire Map point DB as a “probable map”, time stamps (even if time information is used) are used for each element of the Map point DB. There is also a method of storing the information of the key frame counter NKF (which is reasonable). In this method, when the determination in step S512 is No (when the map information does not include more errors than the reference), the map editing unit 15 determines the time stamp ( (Time or NKF) is set in a variable TS that is a time storage unit (this TS itself may be stacked so that it can be returned to the past one after another). In step S515, the map editing unit 15 Of the information in the Map point DB, the information whose time stamp is after the TS is deleted, and the information before the TS is left as it is. By performing such processing, it is possible to delete only the latest Map point information (considered that the error is large (contains many errors)) while preserving the past Map point information.

  This concept can also be applied to the frame DB. In step S515, the map editing unit 15 sets the key frame number (representing the time at which the frame was captured) among the information in the frame DB to the variable TS (map information). It is also possible to delete information that is larger than (represents the time at which the error is not greater than the reference) and leave the information before the TS as it is. By doing this, it is possible to delete only the latest key frame information (considered to have a large error (including many errors)) while preserving the information of the past probable key frames.

(Embodiment 2)
In the first embodiment, the error determination thread process is performed on the assumption that there are a plurality of feature points on the ceiling plane of the current key frame (key frame whose key frame number is IKF). When the autonomous mobile device 100 moves in the room, as shown in FIG. 11, the area of the ceiling part included in the image acquired by the imaging unit 41 becomes smaller as the distance to the front wall becomes shorter. Therefore, in a state where the distance to the front wall is less than the front distance reference value (for example, 1 m), the image acquired by the imaging unit 41 does not include a ceiling portion having a sufficient area, and the determination unit 14 is sufficient. It is considered that error determination cannot be performed with high accuracy. Therefore, Embodiment 2 in which error determination is not performed when the distance to the front wall is less than the front distance reference value will be described. The second embodiment is the same as the first embodiment except that only a part of the error determination thread is different from the first embodiment. Therefore, the changed part of the error determination thread will be described with reference to FIG.

  The change part in the error determination thread according to the second embodiment is the same as that of the first embodiment except that the process of step S521 is added between step S503 and step S504 in FIG. Therefore, the added process will be described.

  In step S521, the determination unit 14 determines whether the distance between the own device position and the front wall when acquiring the current key frame (key frame with the key frame number IKF) is less than the front distance reference value (for example, 1 m). Determine whether or not. Since the determination unit 14 can acquire the position of the own device from the frame DB, the distance can be calculated if the position of the front wall can be acquired by the obstacle sensor 31. When the position of the front wall has not been acquired yet, the determination unit 14 regards this distance as being equal to or greater than the front distance reference value. If the distance is less than the forward distance reference value (step S521; Yes), the process returns to step S501. If the distance is greater than the forward distance reference value (step S521; No), the process proceeds to step S504.

  By adding the above step S521, the autonomous mobile device 100 according to the second embodiment estimates the ceiling plane and determines the error only when it is assumed that the key frame includes a ceiling part with a sufficient area. To do. Therefore, it is possible to reduce the possibility of failure in error determination due to the fact that the key frame does not include a ceiling portion with a sufficient area.

(Embodiment 3)
In the first and second embodiments described above, in step S511, the number of Map points whose distance from the ceiling plane is equal to or less than the reference ceiling distance ncd is set as the variable K. In step S512, whether the variable K is less than the reference number. By determining whether or not, the determination unit 14 determines whether or not the map information has a large error. However, the ceiling estimation unit 13 estimates the ceiling plane (first ceiling plane) from the Map point in the key frame with the key frame number IKF, and this ceiling plane and the ceiling plane estimated in step S505 (second ceiling plane). The determination unit 14 may determine whether or not the map information has a large error by determining whether or not the distance to the reference ceiling distance is less than or equal to the reference ceiling distance. A third embodiment for making such a determination will be described. The third embodiment is the same as the first embodiment except that a part of the error determination thread is different from the first embodiment. Therefore, the changed part of the error determination thread will be described with reference to FIG.

  The change part in the error determination thread according to the third embodiment is merely replacing the processing from step S511 to step S512 in FIG. 9 with the processing from step S531 to step S534 in FIG. Is the same. Therefore, the replaced part will be described.

  In step S531, as in step S505, the ceiling estimation unit 13 uses a Map point existing in the ceiling direction of the current key frame (key frame number is IKF) to perform a ceiling plane by three-dimensional Hough transform. presume. More specifically, the number of Map points converted to the above-described maximum conversion grid is obtained. In step S532, as in step S506, the ceiling estimation unit 13 determines whether the number of Map points (the number of points that support a plane) converted to the most frequent conversion grid is equal to or less than the reference conversion number (for example, 10). Determine whether. If the number of Map points converted to the most frequent conversion grid is equal to or less than the reference conversion number (step S532; No), there is little information for estimating the ceiling plane, so that error determination is impossible and the process returns to step S501.

  If the number of Map points converted to the most conversion grid is larger than the reference conversion number (for example, 10) (step S532; Yes), the ceiling estimation unit 13 estimates the coefficient of the plane equation as in step S507 (step S507). S533). Step S533 is also called a ceiling estimation step. Then, the determination unit 14 determines whether or not the distance between the plane represented by the coefficient estimated in step S533 and the plane represented by the variables pa, pb, pc, pd is equal to or less than the reference ceiling distance ncd (step). S534). Step S534 is also called a determination step.

  In determining the distance between the planes, the determination unit 14 first compares the normal vectors to determine whether or not the difference between the angles of the two normal vectors is less than a reference angle (for example, 10 degrees). . If there is a difference greater than the reference angle, the determination unit 14 determines that the map information includes more errors than the reference, and considers that the distance between the ceiling planes is greater than the reference ceiling distance ncd (Step S534; No). ), The process proceeds to step S513.

  If the difference between the angles of the two normal vectors is less than the reference angle, the difference in distance from the origin of each plane is compared with the reference ceiling distance ncd as the distance between the two planes. If the difference in distance from the origin of each plane is equal to or less than the reference ceiling distance ncd (step S534; Yes), the process proceeds to step S510, and if greater than the reference ceiling distance ncd (step S534; No), the process proceeds to step S513.

  The error determination thread according to the third embodiment can perform error determination with higher accuracy than the error determination thread according to the first embodiment if the number of Map points included in the key frame is large.

  The above embodiments can be arbitrarily combined. For example, it is possible to perform both the error determination thread processing (step S521) according to the second embodiment and the error determination thread processing (step S531 to step S534) according to the third embodiment. Further, the error determination thread processing according to the first embodiment may be combined with the error determination thread processing according to the third embodiment. For example, when the number of Map points included in the key frame is less than a predetermined number, the error determination thread according to the first embodiment is processed, and when the number is more than the predetermined number, the error determination thread according to the third embodiment is performed. You may make it perform the process of. Further, for example, when both the determination by the processing from step S511 to step S512 and the determination by the processing from step S531 to step S534 are performed, and both determinations are “there is a lot of error in the map information”, The counter variable ECT may be incremented.

In the above-described embodiment, an example in which the ceiling plane is estimated by three-dimensional Hough transform has been described. However, the ceiling plane may be estimated by using another method (for example, a RANSAC (RANdom Sample Consensus) algorithm). .
In the above embodiment, the determination unit 14 determines whether or not the map information stored in the map storage unit 22 includes more errors than the reference using the ceiling plane estimated by the ceiling estimation unit 13. Using the result, the map editing unit 15 edits the map information stored in the map storage unit 22. However, without being limited to this, if the determination unit 14 determines that the map information stored in the map storage unit 22 includes more errors than the reference, the control unit 10 (determination result processing unit) Some notification may be made to inform the user that there are many errors in the map, or the movement operation may be temporarily stopped.

  In the above-described embodiment, an example of a monocular SLAM in which the imaging unit 41 is a single eye has been described. However, a similar configuration can be adopted even with a compound eye SLAM using a plurality of imaging units. For example, when the autonomous mobile device 100 includes two image capturing units 41, two images can be acquired from the same position without moving. Two images can be acquired by one operation. If each of the two images includes a feature point equal to or greater than the reference value (determined in step S207), “N = N + 2” is performed in step S208, and step S209 always proceeds to “No”. When processing is performed using one of the two images as the previous frame and the other as the current frame, the “odometry translation distance” when calculating the scale Sc in step S220 is the distance between the two imaging units. good. Thereby, even when the accuracy of the position acquired from odometry is low, initialization can be performed stably.

  However, sufficient accuracy cannot be obtained when the distance between the two imaging units is smaller than the distance to the feature point. (For example, when the distance between two imaging units is 30 mm and the distance to a feature point 5 m ahead is measured, etc.) That is, when the accuracy of the translation distance obtained from odometry is high, it is rather similar to monocular SLAM. Higher accuracy may be obtained by performing initialization processing (for example, initialization processing using only the first imaging unit among a plurality of imaging units). Therefore, both the initialization process similar to the monocular SLAM and the initialization process by the two imaging units described in the previous paragraph are performed, and map information (Map point DB and frame DB) is started to be created respectively. A process similar to the error determination thread may be performed in the middle to determine each error, and thereafter, the map information with the smaller error may be adopted.

  In each of the above-described embodiments, the value of the reference translation distance is set to “for example, 1 m”. However, the optimal reference translation distance is determined according to the size and speed of the autonomous mobile device itself, the environment in which the autonomous movement is performed, the movement range, and the like. Since it changes, it supplements about the setting method of reference | standard translation distance.

  As a method for setting the reference translation distance, a value obtained by averaging the distances from the three-dimensional position of all Map points (feature points whose three-dimensional position is known) on the key frame to the own position (= all observed in the key frame) (The average distance of the depth to the Map point) and the translation distance may be set (for example, a distance of 5% of the average distance of the depth to the Map point is set as the reference translation distance). The “all Map points” used here can be extracted by the control unit 10 referring to the Map point DB. Further, according to the size of the translation distance in the actual environment, for example, 10 cm may be set as the reference translation distance if moving on the table, 1 m if moving around the room, 10 m if moving around the open space, etc. Then, a value corresponding to the wheel diameter of the drive unit 42 (for example, 10 times the wheel diameter) may be set as the reference translational distance.

  When setting the reference translation distance as a distance in the actual environment, the translation distance in the SLAM coordinates is multiplied by the scale Sc and converted into the distance in the actual environment (the reverse is also true). When setting the distance in the SLAM coordinates as the reference translation distance, the distance in the real environment is divided by the scale Sc and converted into the distance in the SLAM coordinates for comparison).

  Each function of the autonomous mobile device 100 can also be implemented by a computer such as a normal PC (Personal Computer). Specifically, in the above embodiment, the autonomous movement control processing program performed by the autonomous mobile device 100 has been described as being stored in advance in the ROM of the storage unit 20. However, the program is stored and distributed on a computer-readable recording medium such as a flexible disk, CD-ROM (Compact Disc Read Only Memory), DVD (Digital Versatile Disc), and MO (Magneto-Optical Disc). A computer capable of realizing each of the functions described above may be configured by reading and installing the program on a computer.

The computer may be built in the autonomous mobile device 100 or may exist separately from the autonomous mobile device 100. That is, as shown in FIG. 14, the control unit 50 and the storage unit 60 may be provided in the external server 200. With the cloud computing technology, the control unit 10 of the autonomous mobile device 100 transmits data acquired by the imaging unit 41, the sensor unit 30, and the like to the external server 200 via the communication unit 44, and performs arithmetic processing on the server 200. Then, the calculation result performed by the server 200 may be received via the communication unit 44 to control the drive unit 42 and the like. In FIG. 14, the control unit 50 of the server 200 includes the map creation unit 11, the position estimation unit 12, the ceiling estimation unit 13, the determination unit 14, and the map editing unit 15. Each part should just be provided in either of the control parts 10 of the autonomous mobile device 100. Whether each unit is provided in the control unit 50 of the server 200 or the control unit 10 of the autonomous mobile device 100 is arbitrary.
Further, erasing the information may be that the information is physically deleted from the storage medium, or may be logically erased by setting an erasure flag.

  As mentioned above, although preferable embodiment of this invention was described, this invention is not limited to the specific embodiment which concerns, This invention includes the invention described in the claim, and its equivalent range It is. Hereinafter, the invention described in the scope of claims of the present application will be appended.

(Appendix 1)
An imaging unit;
A map creation unit that creates map information using information of a plurality of images captured by the imaging unit;
A ceiling estimation unit that estimates a ceiling plane using information of a plurality of images captured by the imaging unit;
Using the ceiling plane estimated by the ceiling estimation unit, a determination unit that determines whether the map information includes more errors than the reference,
A determination result processing unit that performs a predetermined process according to a result determined by the determination unit;
An autonomous mobile device comprising:

(Appendix 2)
A map storage unit for storing information of the map created by the map creation unit;
When the determination unit determines that the map information stored in the map storage unit includes more errors than the reference, the determination result processing unit uses the map information stored in the map storage unit. To edit,
The autonomous mobile device according to attachment 1.

(Appendix 3)
When the determination unit determines that the map information stored in the map storage unit includes more errors than a reference, the determination result processing unit stores the map information stored in the map storage unit. Erase
The autonomous mobile device according to attachment 2.

(Appendix 4)
A map storage unit for storing the map information stored in the map storage unit when the determination unit determines that the map information stored in the map storage unit does not include more errors than the reference. Prepared,
When the determination unit determines that the map information stored in the map storage unit includes more errors than the reference, the determination result processing unit stores the map information stored in the map storage unit. Is stored in the map storage unit,
The autonomous mobile device according to appendix 2 or 3.

(Appendix 5)
A time storage unit that stores information on the time when the determination unit determines that the map information stored in the map storage unit does not include more errors than the reference;
The map storage unit stores information on the map together with information on the time when the map was created,
When the determination unit determines that the map information stored in the map storage unit includes more errors than a reference, the determination result processing unit stores the map information stored in the map storage unit. Among them, erase the information of the map created after the time stored in the time storage unit,
The autonomous mobile device according to any one of appendices 2 to 4.

(Appendix 6)
An image storage unit that stores information on the captured image together with information on the time of shooting,
When the determination unit determines that the map information stored in the map storage unit includes more errors than the reference, the determination result processing unit stores information on the image stored in the image storage unit. Among them, the information of images taken after the time stored in the time storage unit is deleted,
The autonomous mobile device according to attachment 5.

(Appendix 7)
A three-dimensional position acquisition unit that acquires a three-dimensional position of a feature point that is a characteristic part included in the image captured by the imaging unit;
When the number of three-dimensional positions whose distance from the ceiling plane estimated by the ceiling estimation unit is less than or equal to a reference ceiling distance among the three-dimensional positions acquired by the three-dimensional position acquisition unit is less than the reference number , It is determined that the map information stored in the map storage unit includes more errors than the standard,
The autonomous mobile device according to any one of appendices 2 to 6.

(Appendix 8)
If the difference between the inclination of the ceiling plane estimated by the ceiling estimation unit and the inclination of the floor surface on which the autonomous mobile device is moving is equal to or greater than a reference angle, the determination unit is stored in the map storage unit. It is determined that the map information contains more errors than the standard.
The autonomous mobile device according to any one of appendices 2 to 7.

(Appendix 9)
The determination unit has an angle between the inclination of the first ceiling plane estimated by the ceiling estimation unit and the inclination of the second ceiling plane estimated by the ceiling estimation unit before the first ceiling plane. If the difference is greater than or equal to a reference angle, it is determined that the map information stored in the map storage unit includes more errors than the reference.
The autonomous mobile device according to any one of appendices 2 to 8.

(Appendix 10)
The determination unit obtains the distance between the aircraft and the front wall, and when the distance is less than a forward distance reference value, the map information stored in the map storage unit includes more errors than the reference. Do not judge
The autonomous mobile device according to any one of appendices 2 to 9.

(Appendix 11)
The ceiling estimation unit estimates a ceiling plane by a three-dimensional Hough transform,
The autonomous mobile device according to any one of supplementary notes 1 to 10.

(Appendix 12)
The imaging unit is a monocular imaging unit,
The map creation unit creates map information based on information of a plurality of images taken by the monocular imaging unit;
The autonomous mobile device according to any one of appendices 1 to 11.

(Appendix 13)
It has a floor sensor that detects whether it is in contact with the floor,
Only when the floor sensor detects that it is in contact with the floor, the map creation unit creates map information,
The autonomous mobile device according to any one of appendices 1 to 12.

(Appendix 14)
Create map information using information from multiple images taken and estimate the ceiling plane.
Determine whether the map information created using the estimated ceiling plane contains more errors than the standard,
A predetermined process is performed according to the determined result.
Image processing method.

(Appendix 15)
Shooting steps,
A map creating step for creating map information using information of a plurality of images photographed in the photographing step;
A ceiling estimation step of estimating a ceiling plane using information of a plurality of images captured in the imaging step;
A determination step of determining whether or not an error includes more than a reference in the map information created in the map creation step using the ceiling plane estimated in the ceiling estimation step;
A determination result processing step for performing predetermined processing according to the result determined in the determination step;
An image processing method comprising:

(Appendix 16)
On the computer,
Shooting steps,
A map creating step for creating map information using information of a plurality of images photographed in the photographing step;
A ceiling estimation step for estimating a ceiling plane using information of a plurality of images photographed in the photographing step;
A determination step of determining whether or not an error includes more than a reference in the map information created in the map creation step using the ceiling plane estimated in the ceiling estimation step; and
A determination result processing step for performing a predetermined process according to the result determined in the determination step;
A program for running

DESCRIPTION OF SYMBOLS 10,50 ... Control part, 11 ... Map preparation part, 12 ... Position estimation part, 13 ... Ceiling estimation part, 14 ... Determination part, 15 ... Map editing part, 20, 60 ... Storage part, 21 ... Image storage part, 22 DESCRIPTION OF SYMBOLS ... Map storage part, 23 ... Ceiling estimated value storage part, 30 ... Sensor part, 31 ... Obstacle sensor, 32 ... Floor sensor, 41 ... Imaging part, 42 ... Drive part, 43 ... Input part, 44, 71 ... Communication Part 45 ... power source 100 ... autonomous mobile device 200 ... server

Claims (16)

An imaging unit;
A map creation unit that creates map information using information of a plurality of images captured by the imaging unit;
A ceiling estimation unit that estimates a ceiling plane using information of a plurality of images captured by the imaging unit;
Using the ceiling plane estimated by the ceiling estimation unit, a determination unit that determines whether the map information includes more errors than the reference,
A determination result processing unit that performs a predetermined process according to a result determined by the determination unit;
An autonomous mobile device comprising: A map storage unit for storing information of the map created by the map creation unit;
When the determination unit determines that the map information stored in the map storage unit includes more errors than the reference, the determination result processing unit uses the map information stored in the map storage unit. To edit,
The autonomous mobile device according to claim 1. When the determination unit determines that the map information stored in the map storage unit includes more errors than a reference, the determination result processing unit stores the map information stored in the map storage unit. Erase
The autonomous mobile device according to claim 2. A map storage unit for storing the map information stored in the map storage unit when the determination unit determines that the map information stored in the map storage unit does not include more errors than the reference. Prepared,
When the determination unit determines that the map information stored in the map storage unit includes more errors than the reference, the determination result processing unit stores the map information stored in the map storage unit. Is stored in the map storage unit,
The autonomous mobile device according to claim 2 or 3. A time storage unit that stores information on the time when the determination unit determines that the map information stored in the map storage unit does not include more errors than the reference;
The map storage unit stores information on the map together with information on the time when the map was created,
When the determination unit determines that the map information stored in the map storage unit includes more errors than a reference, the determination result processing unit stores the map information stored in the map storage unit. Among them, erase the information of the map created after the time stored in the time storage unit,
The autonomous mobile device according to any one of claims 2 to 4. An image storage unit that stores information on the captured image together with information on the time of shooting,
When the determination unit determines that the map information stored in the map storage unit includes more errors than the reference, the determination result processing unit stores information on the image stored in the image storage unit. Among them, the information of images taken after the time stored in the time storage unit is deleted,
The autonomous mobile device according to claim 5. A three-dimensional position acquisition unit that acquires a three-dimensional position of a feature point that is a characteristic part included in the image captured by the imaging unit;
When the number of three-dimensional positions whose distance from the ceiling plane estimated by the ceiling estimation unit is less than or equal to a reference ceiling distance among the three-dimensional positions acquired by the three-dimensional position acquisition unit is less than the reference number , It is determined that the map information stored in the map storage unit includes more errors than the standard,
The autonomous mobile device according to any one of claims 2 to 6. If the difference between the inclination of the ceiling plane estimated by the ceiling estimation unit and the inclination of the floor surface on which the autonomous mobile device is moving is equal to or greater than a reference angle, the determination unit is stored in the map storage unit. It is determined that the map information contains more errors than the standard.
The autonomous mobile device according to any one of claims 2 to 7. The determination unit has an angle between the inclination of the first ceiling plane estimated by the ceiling estimation unit and the inclination of the second ceiling plane estimated by the ceiling estimation unit before the first ceiling plane. If the difference is greater than or equal to a reference angle, it is determined that the map information stored in the map storage unit includes more errors than the reference.
The autonomous mobile device according to any one of claims 2 to 8. The determination unit obtains the distance between the aircraft and the front wall, and when the distance is less than a forward distance reference value, the map information stored in the map storage unit includes more errors than the reference. Do not judge
The autonomous mobile device according to any one of claims 2 to 9. The ceiling estimation unit estimates a ceiling plane by a three-dimensional Hough transform,
The autonomous mobile device according to any one of claims 1 to 10. The imaging unit is a monocular imaging unit,
The map creation unit creates map information based on information of a plurality of images taken by the monocular imaging unit;
The autonomous mobile device according to any one of claims 1 to 11. It has a floor sensor that detects whether it is in contact with the floor,
Only when the floor sensor detects that it is in contact with the floor, the map creation unit creates map information,
The autonomous mobile device according to any one of claims 1 to 12. Create map information using information from multiple images taken and estimate the ceiling plane.
Determine whether the map information created using the estimated ceiling plane contains more errors than the standard,
A predetermined process is performed according to the determined result.
Image processing method. Shooting steps,
A map creating step for creating map information using information of a plurality of images photographed in the photographing step;
A ceiling estimation step of estimating a ceiling plane using information of a plurality of images captured in the imaging step;
A determination step of determining whether or not an error includes more than a reference in the map information created in the map creation step using the ceiling plane estimated in the ceiling estimation step;
A determination result processing step for performing predetermined processing according to the result determined in the determination step;
An image processing method comprising: On the computer,
Shooting steps,
A map creating step for creating map information using information of a plurality of images photographed in the photographing step;
A ceiling estimation step for estimating a ceiling plane using information of a plurality of images photographed in the photographing step;
A determination step of determining whether or not an error includes more than a reference in the map information created in the map creation step using the ceiling plane estimated in the ceiling estimation step; and
A determination result processing step for performing a predetermined process according to the result determined in the determination step;
A program for running

Images