JP2017134617A - Position estimation device, program and position estimation method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a position estimation device capable of improving accuracy of a position to be estimated.SOLUTION: A position estimation device comprises: an image feature point extraction part 11 for extracting a feature point from an image which is imaged by an imaging device; an image feature point tracking part 12 for tracking the feature point of the image which was imaged earlier in time; an environment map generation part 15 for using a three-dimensional coordinate of a movable body, distance information and a camera parameter of the imaging device for generating a map including the three-dimensional coordinate of the feature point; a position detection part 16 for detecting a position and an attitude of the movable body in which a tolerance of a projection coordinate when the three-dimensional coordinate is projected in the image and the coordinate of the feature point, or a tolerance of a conversion coordinate when the coordinate of the feature point is converted into the three-dimensional coordinate and the three-dimensional coordinate becomes smaller; and a permitted motion defining part 17 for based on movement speed of the movable body which is detected by the position detection part based on a position, rotation speed of the movable body which is detected based on the attitude, and an imaging condition of the image, defining a motion in which an image in which the feature point can be tracked, can be imaged.SELECTED DRAWING: Figure 6

Description

  The present invention relates to a position estimation device, a program, and a position estimation method.

  Development of a technique in which a moving body moves along a route determined autonomously without a driver maneuvering the moving body or maneuvering from the outside is underway. In order for the moving body to move along the route, it is necessary for the moving body to accurately estimate the position even during movement. Then, Visual-SLAM (Simultaneous Localization and Mapping; simultaneous position estimation map construction) that estimates the position and orientation of a moving body using a camera provided in the moving body is known. In Visual-SLAM, it is possible to simultaneously generate an environment map including three-dimensional position information of surrounding stationary objects from a time-series image and estimate the position and orientation of a moving object.

  In the detection of position and orientation using a camera, a plurality of feature points and their three-dimensional coordinates are obtained from images taken in time series, and are associated with the feature points of the image taken at the next timing. . Since the three-dimensional coordinates of the tracked feature points in the next timing image are known, the self-position and orientation are estimated using the correspondence between the three-dimensional coordinates and the feature points in the image.

  When the position and orientation are detected using the camera in this way, it is known that the accuracy of the position and orientation greatly depends on the quality of the captured image. In view of this, a position estimation technique that is robust with respect to the quality of images taken by a camera has been conventionally devised (see, for example, Patent Document 1). In Patent Document 1, corresponding points corresponding to each of a plurality of images captured at different timings are searched, and a plurality of sets of corresponding points are repeatedly selected from all the sets of corresponding points searched, and repeatedly selected. A motion estimation device that calculates motion estimation candidates for the host vehicle based on a plurality of sets of corresponding points is disclosed.

  However, in the conventional technique, there is a problem that the accuracy of the estimated position and orientation may be reduced when the image changes so much that the feature points are difficult to be associated. For example, it is considered that it is desirable for the moving body to arrive at the destination early, but if the change in the position or posture becomes large because the moving body moves quickly, the feature points in the image captured at different timing may be associated with each other. It can be difficult. In this case, since the position and orientation are estimated using images that can be associated in the past, the accuracy of the estimated position and orientation may be reduced.

  In view of the above-described problems, an object of the present invention is to provide a position estimation device that can improve the accuracy of an estimated position.

  The present invention is a position estimation device that estimates a position of a moving body from a surrounding image captured by an imaging device capable of detecting distance information to a subject and creates a surrounding map, and the imaging device captures the image An image feature point extraction unit that extracts a feature point from an image, an image feature point tracking unit that tracks the feature point of the image in the image captured by the imaging device, three-dimensional coordinates of the moving object, and the distance information And an environment map generation unit that generates the map including the three-dimensional coordinates of the feature points using the camera parameters of the imaging device, and the projection coordinates and the feature points when the three-dimensional coordinates are projected onto the image. A position detection unit that detects a position error and a position of the moving body in which the error of the coordinates or the converted coordinates when the coordinates of the feature points are converted into the three-dimensional coordinates and the error of the three-dimensional coordinates becomes smaller; The feature point can be tracked based on at least one of the moving speed of the moving body detected based on the position or the rotational speed of the moving body detected based on the posture and the imaging condition of the image. And an allowable motion definition unit that defines a motion capable of capturing the image.

  A position estimation device capable of improving the accuracy of the estimated position can be provided.

It is an example of the schematic perspective view of a moving body. It is an example of the hardware block diagram of a position measurement part and a mobile body control part. It is an example of the functional block diagram of a position measurement part. It is an example of the figure explaining the correspondence of the coordinate of the feature point in the image in a pinhole camera model, and the three-dimensional coordinate of a world coordinate system. It is an example of the figure which shows the information memorize | stored in a memory | storage part typically. It is an example of the flowchart figure which shows the whole process sequence of the position measurement part in this embodiment. It is an example of the figure for demonstrating a position and orientation measurement process. It is a flowchart which shows an example of an environmental map production | generation process. It is an example of the flowchart figure which shows the procedure in which an allowable motion definition part defines an allowable motion. It is an example of the flowchart figure explaining the process of allowable exercise | movement determination of FIG. It is an example of the flowchart figure which shows the operation | movement procedure of a mobile body control part. It is an example of the figure explaining the calculation method of route cost.

Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings.
FIG. 1 is an example of a schematic perspective view of a moving body. The moving body in FIG. 1 is a flying object 100 called a multicopter or a drone. The flying object 100 moves up and down by rotating a plurality of rotors (rotary blades) 9 arranged radially, and moves back and forth, right and left. The rotational speed of the four rotors 9 is increased at the time of ascent, and the rotational speed of the four rotors 9 is decreased at the time of the ascent. Further, when moving forward or backward, the rotational speed of the symmetrical rotor 9 is changed and the machine body is tilted to move forward or backward.

  The flying object 100 of this embodiment uses Visual-SLAM (Simultaneous Localization and Mapping; simultaneous position estimation map construction) and includes an environment including three-dimensional coordinates of surrounding stationary objects (three-dimensional points) from a time-series image. Map generation and self-position and posture estimation are performed simultaneously.

  The flying object 100 in FIG. 1 includes a position measurement unit 10 that measures a position and a moving body control unit 30 that performs control related to movement. The position measurement unit 10 estimates, measures, or calculates the current position and orientation of the flying object 100, and the moving body control unit 30 moves the flying object 100 to the destination while referring to the current position. The flying object 100 has a stereo camera 50. The stereo camera 50 has a feature that the distance to the subject can be detected. If a means capable of detecting the distance is provided separately, a monocular camera may be used, or a monocular camera capable of acquiring distance information may be used. An example of a means capable of detecting the distance is a laser radar.

  FIG. 2 is an example of a hardware configuration diagram of the position measurement unit 10 and the moving body control unit 30. The position measuring unit 10 can have, for example, a smartphone, a tablet terminal, a notebook PC, a digital camera, or a control device provided in various moving bodies such as the flying object 100, a vehicle, and a mobile robot. Therefore, the position measuring unit 10 has a function as an information processing apparatus.

  The position measurement unit 10 includes an input device 101, an output device 102, a drive device 103, a camera I / F 104, a position measurement device 105, a moving body control unit I / F 106, an auxiliary storage device 107, and a main storage device. 108, a CPU (Central Processing Unit) 109 that performs various controls, a network connection device 110, an attitude sensor 111, and a logic circuit 112, which are connected to each other via a system bus B.

  The input device 101 includes a keyboard operated by a user, a pointing device such as a mouse, a voice input device such as a microphone, and the like, and receives input of various operation signals from the user. Further, a touch panel integrated with the output device 102 may be provided as the input device 101.

  The output device 102 is a display that displays various types of information using an LED, a CUI (Character User Interface), a GUI (Graphical User Interface), or the like that indicates the state of the position measurement unit 10. Is output to the outside. Furthermore, a speaker function for outputting sound may be provided.

  For example, a recording medium 103 a and the like are detachably attached to the drive device 103, and various information recorded on the recording medium 103 a can be read and various information can be written on the recording medium 103 a under the control of the CPU 109. The program executed by the position measuring unit 10 is distributed, for example, in a state stored in the recording medium 103a, or distributed from a server that distributes the program.

  The recording medium 103a is a computer-readable medium that stores various programs and data. The recording medium 103a is a portable type such as a USB (Universal Serial Bus) memory, an SD memory, or a CD-ROM.

  The camera I / F 104 is an interface with the stereo camera 50. The camera I / F 104 is a camera parameter for the stereo camera 50 (mainly internal parameters such as focal length, exposure time, image center, aperture value, ISO sensitivity, aspect ratio, skew correction amount, distortion correction amount, frame interval, etc.). And image data and camera parameters are received from the stereo camera 50.

  The position measuring device 105 acquires position information (coordinates: latitude, longitude, altitude) of the flying object 100 using a GPS (Global Positioning System) function or the like. Location information may be acquired from IMES (Indoor Messaging System) or a beacon indoors or underground.

The mobile body control unit I / F 106 is an interface for communicating with the mobile body control unit 30. Specifically, a USB cable, I ² C, SPI, MicroWire, or the like is connected.

  The auxiliary storage device 107 is, for example, a storage device such as an SSD (Solid State Drive) or an HDD (Hard Disk Drive). The auxiliary storage device 107 stores a program for generating a control signal that the CPU 109 outputs to each block in FIG. 2, an operating system, and the like.

  A program read from the auxiliary storage device 107 is expanded in the main storage device 108 and executed by the CPU 109. The main storage device 108 is, for example, a RAM (Random Access Memory) or the like, but is not limited to this.

  The CPU 109 controls the overall processing of the position measurement unit 10 by outputting various calculations and control signals to each unit in FIG. 2 based on the operating system and program. The execution result of the program can be stored in the auxiliary storage device 107.

  The network connection device 110 is a communication device that connects to various networks based on a control signal from the CPU 109. For example, it connects to a wireless local area network (LAN) or 3G or 4G public wireless communication network. Alternatively, it may be connected to a wired LAN. Thereby, the network connection apparatus 110 can acquire a program, setting information, etc. from the external apparatus etc. which are connected to the network. Further, the network connection device 110 can provide an execution result obtained by executing the program to an external device or the like.

  The attitude sensor 111 is various sensors for detecting the attitude of the flying object 100. For example, a three-axis gyro sensor, a three-axis acceleration sensor, and a magnetic direction sensor are included.

  The logic circuit 112 is hardware that performs specific information processing. Specifically, an adder, a multiplier, a memory, a latch, and the like, and an IC, LSI, ASIC, FPGA, and the like, which are combined using an AND, OR, NAND, NOR circuit, and the like as elements. In this embodiment, the logic circuit 112 performs feature point extraction, tracking, and parallax calculation, which will be described later.

  In the present embodiment, by installing an execution program (for example, a position / orientation measurement program) in the hardware configuration of the computer main body described above, the hardware / resource and the software cooperate to perform the position / orientation measurement process in the present embodiment. Etc. can be realized. Further, the program corresponding to the above-described position and orientation measurement processing may be in a resident state on the apparatus, for example, or may be activated by an activation instruction.

  Next, the moving body control unit 30 will be described. The moving body control unit 30 controls the movement of the flying object 100. Movement means at least one of position, moving speed, and posture. The moving body control unit 30 includes a position measurement unit I / F 201, an arithmetic control device 202, a motor controller 203, and four motors 204.

  The position measurement unit I / F 201 is an interface for communicating with the position measurement unit 10. Specifically, a USB cable, I2C, SPI, MicroWire, or the like is connected. For example, a position estimated by the position measurement unit 10, a moving speed and posture, and an allowable motion map described later are transmitted via the position measurement unit I / F 201.

  The arithmetic and control unit 202 controls the rotation speed of the rotor 9 by controlling the rotation speeds of the four motors 204, and controls the position (position in the three-dimensional space), the moving speed, the posture, and the like. One or more of the four rotors 9 are controlled to operate as a lift generating rotor 9 or a posture controlling rotor 9. Further, when moving, the rotational speed of the left and right or front and rear rotors 9 is unbalanced (with a difference in rotational speed) and is controlled to be tilted so as to be moved. In order to suppress the rotational torque to the flying object 100 generated by the rotation of the rotor 9, two of the four rotors 9 are rotated in the clockwise direction and the remaining two are rotated in the counterclockwise direction. .

  The coordinates of the destination including the waypoint are registered in advance in the arithmetic and control unit 202, and the flying object 100 moves so as to approach the waypoint or the destination based on the position and orientation detected by the position measuring unit 10. Determine the direction. It is assumed that an appropriate moving speed is determined in advance. The arithmetic and control unit 202 outputs to the motor controller 203 a rotation speed instruction signal for instructing the rotation speeds of the four rotors 9 determined by the movement direction and the movement speed.

  The motor controller 203 controls the rotation speed of the motor 204 according to the rotation speed instruction signal. For example, a PWM signal is generated and output to the motor 204 so that a current corresponding to the rotation speed instruction signal flows to the motor 204. The motor controller 203 detects or acquires the rotation speed of the motor 204 and performs feedback control. Also, if there is a minute difference in the number of rotations of the motor 204 that should rotate at the same number of rotations, the current is controlled so that the number of rotations is the same. Further, when a rotational speed difference corresponding to the rotational speed instruction signal cannot be obtained between the two rotors 9, the current flowing in at least one motor 204 is controlled so that the designated rotational speed difference is obtained.

<Functional Configuration of Position Measuring Unit 10>
Next, the function of the position measurement unit 10 will be described with reference to FIG. FIG. 3 is an example of a functional block diagram of the position measurement unit 10. The position measurement unit 10 includes an image feature point extraction unit 11, an image feature point tracking unit 12, a depth measurement unit 13, an imaging unit control unit 14, an environment map generation unit 15, a moving body position measurement unit 16, and an allowable motion definition unit. 17.

  The image feature point extraction unit 11, the image feature point tracking unit 12, and the depth measurement unit 13 are realized by the above-described logic circuit 112. The imaging unit control unit 14, the environment map generation unit 15, the moving body position measurement unit 16, and the allowable motion definition unit 17 are realized by the CPU 109 executing a program developed from the auxiliary storage device 107 to the main storage device 108. Function or means to be performed. However, the image feature point extraction unit 11, the image feature point tracking unit 12, and the depth measurement unit 13 may be realized by the CPU 109 executing a program.

  Further, the position measuring unit 10 has a storage unit 1000. The storage unit 1000 is information storage means constructed by at least one of the auxiliary storage device 107 and the main storage device 108.

  The imaging unit control unit 14 controls the stereo camera 50 by outputting camera parameters to the stereo camera 50. In this embodiment, at least the exposure time and the frame interval are output among the camera parameters. The frame interval (imaging interval) may be fixed, or may be controlled by the imaging unit control unit 14 according to the environment in which the image is captured. In the fixed case, the frame interval is recorded in the auxiliary storage device 107 of the flying object 100 or the like. Further, the imaging unit control unit 14 uses the histogram of the image captured by the stereo camera 50 to determine the exposure time so that, for example, there is no saturated pixel and the pixel is used from low luminance to high luminance.

  The depth measurement unit 13 measures the distance from the stereo camera 50 to the subject for each pixel or pixel block by the stereo measurement method. The image is divided into blocks of a certain size by a so-called block matching method, and the horizontal shift of each block is calculated as parallax. Furthermore, after calculating the block cost, it is preferable to apply a Semi-Global-Mating method that optimizes the parallax in consideration of the smoothness with the surroundings. Details are disclosed in Non-Patent Document 1, for example.

  The image feature point extraction unit 11 extracts feature points (image feature points) from the image captured by the stereo camera 50. The image feature point extraction unit 11 extracts a feature point for each captured image. For example, the image feature point extraction unit 11 extracts a feature point using the luminance value of the image, or extracts a feature point from the shape of an object in the image, but is not limited thereto. Absent.

  As a feature point extraction method, for example, a Harris operator or a FAST (Features from Accelerated Segment Test) operator can be used. The Harris operator is image processing that detects the intersection of two edges as a corner. The FAST operator is image processing that detects a corner when the pixel values of 16 pixels on the circumference around the pixel of interest are n or more consecutively and are brighter or darker than the pixel of interest. In addition, as feature point extraction methods, for example, a SIFT (Scale Invariant Feature Transform) feature descriptor and a SURF (Speeded Up Robust Features) feature descriptor can be used. The image feature point extraction unit 11 records the coordinates of the feature points on the image and their feature amounts (pixel value, density, luminance, etc.) in the storage unit 1000 as image feature point information.

  The image feature point tracking unit 12 tracks feature points of time-series images (captured at different timings). The image feature point tracking unit 12 tracks each feature point by searching, for example, the feature points extracted by the image feature point extracting unit 11 from images captured by the stereo camera 50 after the image from which the feature points are extracted. I do. Further, the image feature point tracking unit 12 determines success or failure (success or failure) of the tracking from the search result, and registers the image feature point information in the storage unit 1000 based on the determination result. That is, success or failure of tracking is registered for each feature point of the image.

  The image feature point tracking unit 12 can track feature points using, for example, a search method based on block matching or feature amounts. When tracking feature points using block matching, the image feature point tracking unit 12 extracts a block including the feature points of the image immediately before the time series, and performs block matching on the image after the time series. I do.

  When performing the search using the above-described feature amount, the image feature point tracking unit 12 performs the same feature point description processing as that of the image feature point extracting unit 11, and is recorded as image feature point information. Search for points and feature points with high correlation. The image feature point tracking unit 12 determines that tracking is successful when the correlation exceeds a predetermined threshold as a result of the search described above, and determines tracking failure when the correlation is less than or equal to the threshold.

  The environment map generation unit 15 generates a three-dimensional map of the surrounding environment from the image feature point information, camera parameters, and the measurement result of the depth measurement unit 13. For example, the initial position of the flying object 100 (three-dimensional coordinates in the world coordinate system described later) is known by the position measurement device 105. At this initial position, since the distance to the feature point is known from the focal length of the camera parameter and the result of the depth measurement unit 13, the three-dimensional coordinates of the feature point can be obtained as shown in FIG.

  FIG. 4A is an example for explaining the correspondence between the feature point coordinates in the image and the three-dimensional coordinates of the world coordinate system in the pinhole camera model. (X, y) are the coordinates of the feature point on the image, and (X, Y, Z) are the three-dimensional coordinates of the feature point in the world coordinate system. f is a focal length. Since the depth (distance) measured by the depth measurement unit 13 is Z, X = Z · (x−cx) / f is obtained from the proportional relationship between x and X and f and Z shown in FIG. . Similarly, Y = Z · (y−cy) / f is obtained. cx and cy are in the image (because the coordinates of feature points in the image have the upper left corner as the origin).

  Therefore, the initial position of the flying object 100 and the three-dimensional coordinates of the three-dimensional points (feature points in the world coordinate system) included in the image captured there are known. The environment map generation unit 15 thereby generates a simple environment map and performs optimization later.

  Returning to FIG. 3, the moving body position measurement unit 16 performs processing for measuring its own position and orientation from the environment map and the image feature point information. When the flying object 100 moves from the initial position and images the surroundings, the same feature points are imaged from different locations. The moving body position measurement unit 16 measures the position and orientation after movement by obtaining a rotation matrix R and a translation vector t for converting the three-dimensional coordinates of the feature points in the world coordinate system to the camera coordinate system as described later. To do. Then, the three-dimensional coordinates of the feature points can be known as shown in FIG. Accordingly, the position and orientation of the flying object 100 at the time of imaging in the world coordinate system can be constantly updated and moved while generating an environment map.

  The allowable motion definition unit 17 defines a motion permitted or restricted by the flying object 100 based on the exposure time and the frame interval, and outputs the motion to the moving body control unit 30. The moving body control unit 30 controls the movement of the flying object 100 within the limits. Details will be described later.

<< Information stored in memory >>
FIG. 5 is an example of a diagram schematically showing information stored in the storage unit 1000. The storage unit 1000 stores an environment map 1001, image feature point information 1002, camera parameters 1003, allowable motion information 1004, moving body position / posture information 1005, and motion parameter using motion 1006.

  The environment map 1001 is a map in which three-dimensional coordinates of feature points are registered. Three-dimensional coordinates are registered for each feature point, and a map of the space in which the flying object 100 has moved is reproduced. Thereby, information on the surrounding environment such as where an object such as a building or a wall, a road, or the like is located can be obtained. The environment map may be two-dimensional information such as surrounding stationary objects.

The image feature point information 1002 includes the following information for each image.
Feature quantity (pixel value, density, brightness, etc.) of the coordinate feature point of the feature point on the image
Tracking success / failure from previous image in time series The camera parameter 1003 is mainly an internal parameter as described above, and includes at least an exposure time and a frame interval. The exposure time is a time during which a CCD or CMOS image sensor is exposed to subject light. The frame interval is a time interval from when the stereo camera 50 captures one image until the next image is captured.

  The allowable motion information 1004 is information for specifying a motion parameter or a motion parameter permitted by the allowable motion definition unit 17 among the plurality of motion parameters. The motion parameter is a parameter that defines the motion content of the flying object 100. For example, the degree of freedom of the rigid body is six (pitch angle θ, roll angle φ, rotation of yaw angle ψ, movement in X, Y, and Z directions), but even when flying object 100 moves in a certain direction, for example. The body control unit 30 controls the plurality of motors 204. For this reason, several motion parameters are set so that the motion of the flying object can be easily controlled. Examples of the motion parameters include, but are not limited to, ascending, descending, turning right, turning left, moving forward, moving backward, moving right, moving left, and the like.

  The moving body position / orientation information 1005 is information related to the position and orientation of the stereo camera 50 that captures the image from which the feature point is detected. The moving body position / orientation information 1005 registers the position and orientation for each captured image.

  The exercise parameter use exercise 1006 indicates an exercise used for each exercise parameter. For example, if it is ascending, no rotation is involved, and therefore, movements that change the pitch angle θ, the roll angle φ, and the yaw angle ψ are not performed. In addition, the position does not change in a posture change that does not involve movement (a parallel movement vector described later is zero). If the movement is in the X direction, the change in posture and the X coordinate change based on the principle of movement. Whether or not the pitch angle θ, the roll angle φ, the yaw angle ψ, the X coordinate, the Y coordinate, and the Z coordinate change is registered in the motion parameter using motion 1006 in association with each motion parameter.

<Processing of the remains measurement unit>
Next, an example of the position / orientation measurement process in the present embodiment will be described with reference to FIGS. FIG. 6 is an example of a flowchart illustrating an overall processing procedure of the position measurement unit 10 in the present embodiment.

  The stereo camera 50 acquires an image based on camera parameters and the like notified from the imaging unit control unit 14 (S10). The stereo camera 50 captures images at a constant cycle and generates images in time series. For example, when the flying object 100 is moving, the stereo camera 50 generates a time-series image during movement. The stereo camera 50 outputs the captured image to the image feature point extraction unit 11, the image feature point tracking unit 12, and the depth measurement unit 13.

  Next, the image feature point extraction unit 11 extracts feature points from the image obtained in step S10 (S20). The feature points are stored in the image feature point information 1002 of the storage unit 1000.

  Next, the image feature point tracking unit 12 tracks the feature points obtained by the process of S20 between successive images (S30). The success or failure of tracking is stored in the image feature point information 1002 of the storage unit 1000.

  The depth measurement unit 13 measures the depth (distance information) for each pixel or pixel block of the image (S40). Depth is referenced with the image because it is embedded in a pixel or pixel block of the image. Alternatively, the image feature point information 1002 in the storage unit 1000 may be stored in association with the feature points.

  As shown in FIG. 4, the environment map generation unit 15 creates an environment map as needed using the feature points (coordinates) of the image stored in the storage unit 1000 and the position, orientation, and depth of the flying object. Keep it.

  Next, the moving body position measurement unit 16 measures the position and orientation of the flying object 100 (or the stereo camera 50) (S50). The position and posture are stored in the moving body position / posture information 1005 of the storage unit 1000.

  Next, the environment map generation unit 15 performs an environment map generation process using the image feature point information 1002 and the moving body position and orientation information 1005 obtained by the process of S50 (S60). In the process of S60, not only the generation of the environment map but also the update (for example, correction, deletion, expansion) of the environment map can be performed.

  Next, the allowable motion definition unit 17 calculates the movement amount of the feature point in the image based on the camera parameter 1003 and the moving speed and the rotational speed calculated by the moving body position measuring unit 16, and determines an allowable motion parameter ( S70).

<Details of processing>
Hereinafter, steps S50, S60, and S70 of FIG. 6 will be described in detail.

<< S50 Position and orientation measurement process >>
The position / orientation measurement process (S50) in the position measurement unit 10 will be specifically described. FIG. 7 is an example of a diagram for explaining the position and orientation measurement processing. The moving body position measurement unit 16 measures the position and orientation using the correspondence relationship between the three-dimensional coordinates of the feature points obtained by generating the environment map and the coordinates of the feature points in the image.

  A coordinate system that moves together with the flying object 100 (stereo camera 50) is referred to as a camera coordinate system, and an overall coordinate system of the three-dimensional space to be handled is referred to as a world coordinate system. The camera coordinate system is a coordinate system having the origin at the center of the image sensor of the camera, for example. The world coordinate system is a WGS84 coordinate system used in GPS, a world geodetic system, a Japanese geodetic system, or the like. Assume that the Z direction of the camera coordinate system is the front surface of the flying object 100, the X direction is the side surface, and the Y direction is the lower surface. The posture generated by rotation around the Y axis is specified by the yaw angle (ψ), the posture generated by rotation around the Z axis is specified by the roll angle (ρ), and the posture generated by rotation around the X axis is the pitch angle (θ). Specified by.

  The position measurement unit 10 obtains a rotation matrix R and a translation vector t that are converted from the world coordinate system to the camera coordinate system by estimating the position and orientation. Of the matrices for converting the camera coordinate system and the world coordinate system, the rotation component matrix is referred to as a rotation matrix R, and the translation component matrix is referred to as a translation vector t.

  Here, R and t are each represented by the following formula (1).

なお、以下では、右側の右カメラ（ステレオカメラ５０の撮像方向と同じ向きを見た場合に右側）により撮像された画像を用いるものとして説明するが、左カメラの画像を用いてもよい。

In the following description, it is assumed that an image captured by the right camera on the right side (right side when viewed in the same direction as the imaging direction of the stereo camera 50) is used, but an image of the left camera may be used.

In order to obtain the rotation matrix R and the translation vector t, the three-dimensional coordinates X _i ^w of the feature points in the image in the world coordinate system and the perspective projection matrix of the stereo camera 50 are used. The following camera parameters 1003 are used for the perspective projection matrix.
-Focal length fx in the X direction
・ Y direction focal length fy
・ Image center ox in X direction
-Image center oy in the Y direction
The three-dimensional coordinates X _i ^w and the perspective projection matrix are expressed by the following expressions (2) and (3), respectively.

透視投影行列Ｐは、三次元点を撮像面に投影する行列である。三次元点が画像のどの位置に撮像されるかを算出することが可能になる。なお、透視投影行列Ｐは同次座標系で表されている。

The perspective projection matrix P is a matrix for projecting three-dimensional points onto the imaging surface. It is possible to calculate at which position in the image the three-dimensional point is imaged. The perspective projection matrix P is expressed in a homogeneous coordinate system.

  The three-dimensional coordinates of the feature points are registered in the environment map 1001 described above. Further, the focal length and the image center used in the perspective projection matrix P are registered in the camera parameter 1003. The camera parameter 1003 can be obtained in advance by a person in charge, for example, by camera calibration. For camera calibration, for example, Zhang's technique (Z. Zhang, “A flexible new technique for camera calibration”, IEEE Transactions on Pattern Analysis and Machine Intelligence, 22 (14): 1330-1334, 2000) may be used. Yes, but not limited to this.

  Moreover, in this embodiment, the mobile body position measurement part 16 may perform distortion correction of the stereo camera 50 other than the above-mentioned process. Note that the distortion parameters can also be obtained by the Zhang method. As for the distortion correction, the distortion of the image itself may be corrected before extracting the feature points, or the distortion may be corrected after extracting the feature points. The mobile body position measurement unit 16 can estimate the position and orientation of the stereo camera 50 or the flying object 100 with higher accuracy by performing the position and orientation estimation after performing any of the above-described processes. .

The moving body position measurement unit 16 uses the three-dimensional coordinates X _i ^w and the perspective projection matrix P to obtain R, t and three-dimensional point coordinates that minimize the reprojection error. The reprojection error is defined by a difference between a coordinate of a point obtained by projecting the three-dimensional coordinates of the feature point on the image (reprojection coordinate) and a coordinate actually observed by the stereo camera 50. The coordinates observed by the stereo camera 50 are the coordinates in the image of the feature points registered in the image feature point information 1002.

  The reprojection coordinates can be obtained by, for example, “conversion of a three-dimensional point to a camera coordinate system” and “projection onto a virtual screen”.

First, conversion of a three-dimensional point into a camera coordinate system can be obtained by the following equation (4). (4) In the formula, the coordinates of the three-dimensional point in the camera coordinate system and the X _i ^c.

上述した（４）式により、カメラの座標系での三次元点の座標を得ることができる。

The coordinates of the three-dimensional point in the camera coordinate system can be obtained by the above-described equation (4).

Next, the coordinates of the three-dimensional point re-projected on the virtual screen are obtained. Here, the coordinates of the reprojection point are set to x _i ^r and y _i ^r . x _i ^r is the x coordinate of the right camera image, and y _i ^r is the y coordinate of the right camera image. The coordinates x _i ^r and y _i ^r of the reprojection point can be obtained as follows.

（５）式は、（４）式で得られたカメラ座標系での三次元点の座標Ｘ_i ^cを透視投影行列Ｐで２次元の座標に変換する（投影する）処理を表している。また、同次座標系で表されている。

(5) represents the (4) for converting the coordinates X _i ^c of the three-dimensional point in two-dimensional coordinates in the perspective projection matrix P in the resulting camera coordinate system (projection) process. It is also expressed in a homogeneous coordinate system.

  Next, the reprojection error is shown as the sum of the square error of the coordinates of the reprojection points obtained by the equation (5) and the coordinates of the feature points actually observed by the stereo camera 50 (6) It is defined by an expression.

上述した（６）式において、ｎはあるフレームで撮像された三次元座標が既知の特徴点の点数である。位置と姿勢の推定は、再投影誤差を最小化するＲとｔを求める問題であり、以下に示す（７）式のように定式化することができる。

In the above equation (6), n is the number of feature points whose three-dimensional coordinates captured in a certain frame are known. The estimation of the position and orientation is a problem for obtaining R and t that minimize the reprojection error, and can be formulated as shown in the following equation (7).

この最小化問題は、ＰｎＰ問題として知られており、この式をそのまま解くと非線形最小化問題となる。ＰｎＰ問題は、ｎ点の対応から飛行物体１００の位置と姿勢を推定する問題である。

This minimization problem is known as a PnP problem, and if this equation is solved as it is, it becomes a non-linear minimization problem. The PnP problem is a problem of estimating the position and orientation of the flying object 100 from the correspondence of n points.

  In general, the nonlinear minimization problem has a problem that the calculation cost is high and an appropriate solution cannot be obtained unless an appropriate initial value is set. In order to solve the nonlinear minimization problem, a linearization method, a Levenberg-Marquardt method or the like is known.

  Further, since the stereo camera 50 is used, R and t that minimize the reprojection error may be obtained using the left and right images as follows.

（８）式は右カメラの画像と左カメラの画像のそれぞれで再投影誤差を求め、その合計が最も少なくなるＲとｔを求めることを示す。

Expression (8) indicates that the reprojection error is obtained for each of the right camera image and the left camera image, and R and t that minimize the sum are obtained.

  If the rotation matrix R and the translation vector t can be obtained as described above, the position and orientation of the flying object 100 can be obtained. Since the rotation matrix R is a rotation matrix with respect to the world coordinate system, the rotation matrix R is information indicating how much the flying object 100 integrated with the stereo camera 50 is inclined with respect to the world coordinate system.

（１２）式は、剛体がロール角φ、ピッチ角θ、ヨー角ψで回転する場合の回転行列を示す。r31の要素が-sinθであるためピッチ角θを求めることができる。ピッチ角θが分かるとr32又はr33からヨー角ψを求めることができる。同様に、r11又はr21からロール角φを求めることができる。したがって、飛行物体１００の姿勢を決定できる。

Equation (12) represents a rotation matrix in the case where the rigid body rotates with a roll angle φ, a pitch angle θ, and a yaw angle ψ. Since the element of r31 is −sin θ, the pitch angle θ can be obtained. If the pitch angle θ is known, the yaw angle ψ can be obtained from r32 or r33. Similarly, the roll angle φ can be obtained from r11 or r21. Therefore, the attitude of the flying object 100 can be determined.

For the position, since the translation vector t represents the translation amount from the world coordinate system, by applying the translation vector t to the three-dimensional coordinates X _i ^w in the world coordinate system of the feature points in the image, The position of the flying object 100 moving with the camera in the world coordinate system can be determined.

  In this embodiment, the rotation matrix R and the translation vector t that minimize the reprojection error are obtained. However, the rotation matrix R and the translation vector t that minimize the object space error are obtained. Also good. An object space error is an error in three-dimensional coordinates. In this case, the moving body position measurement unit 16 obtains the rotation matrix R and the translation vector t so that the object space error is minimized. That is, the perspective projection matrix P, the rotation matrix R, and the translation vector t are applied to the coordinates of the feature points of the image to obtain three-dimensional coordinates (transformed coordinates) so that the difference from the three-dimensional coordinates of the environment map is minimized. A rotation matrix R and a translation vector t are obtained. In this case, since the solution is obtained linearly, there is an advantage that the calculation cost can be reduced.

<< S60 Environmental Map Generation Processing >>
Next, the environment map generation processing in the environment map generation unit 15 described above will be described using a flowchart with reference to FIG. FIG. 8 is a flowchart illustrating an example of the environment map generation process.

In FIG. 8, the environment map generation unit 15 performs reprojection error minimization processing for the entire environment map (S601). This process of minimizing reprojection error is also called bundle adjustment. One posture (Rj, tj) is obtained by one imaging (j is the imaging order). In addition, the coordinates X _i of n three-dimensional points are obtained in each imaging (image). Therefore, in the process of step S601, for example, Nj positions and orientations (Rj, tj) obtained by past imaging and coordinates X _i of n three-dimensional points obtained by each imaging are used as parameters. This is a process for obtaining the position and orientation at which the projection error is minimized. This process can be expressed by equation (13).

（１３）式は、例えば非線形の最小二乗問題である。したがって、再投影誤差が最小となる位置と姿勢及び三次元点の座標Ｘ_iは、例えばＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法等を用いて求めることができるが、これに限定されるものではない。

Equation (13) is, for example, a nonlinear least square problem. Therefore, the position and orientation at which the reprojection error is minimized and the coordinate X _i of the three-dimensional point can be obtained by using, for example, the Levenberg-Marquardt method, but are not limited thereto.

Expression (13) expresses the position, orientation (Rj, tj), and coordinate X _i so that the reprojection error is minimized from the coordinates X _{i of} N _j- dimensional images and n three-dimensional points obtained by each imaging. Therefore, if the attitude (Rj, tj) that minimizes the expression (13) and the coordinates X _i of the three-dimensional point are known, the reprojection error of the entire environment map is minimized.

  Next, the environment map generation unit 15 registers the feature points that have been successfully tracked a plurality of times in the environment map 1001 (S602). The process in step S602 includes, for example, a three-dimensional reconstruction process and a reliability measurement process. The three-dimensional reconstruction process is a process for obtaining the three-dimensional coordinates of feature points, for example. The calculation of the three-dimensional coordinates can be obtained in the same manner as in FIG. Specifically, it calculates | requires by the following (14)-(16) Formula. Since the coordinates of the feature points in the image are known in the camera coordinate system, ranging information can be obtained from the parallax Z and the focal length f (Equation (16)). When the distance measurement information is obtained, the X coordinate and the Y coordinate of the camera coordinate system can be obtained by the pinhole camera model (Equations (14) and (15)).

（１４）〜（１６）式において、Ｚ、x_r、y_r、b、d、f、c_x、及び、c_yの意味は下記のようになる。
Ｚ：測距情報
x_r:特徴点の右カメラの画像上の観測座標[pixel]
y_r:特徴点の右カメラの画像上の観測座標[pixel]
b: ステレオカメラ５０のベースラインの距離[mm]
d: 視差[pixel]
f: 焦点距離 [mm]
c_x:画像のX方向の中心座標 [pixel]
c_y:画像のY方向の中心座標 [pixel]
なお、ステレオカメラ５０でなく単眼カメラである場合、２つの単眼カメラを用いてＤＬＴ（Direct Linear Transformation）法等を用いて三次元位置を求めることができる（ＤＬＴ法を用いた手法については非特許文献２を参照）。

(14) to _{(16), Z, x r, y r} , b, d, f, c x, and the meaning of c _y is as follows.
Z: Ranging information
x _r : Observation coordinates on the right camera image of the feature point [pixel]
y _r : Observation coordinates on the right camera image of the feature point [pixel]
b: Baseline distance of stereo camera 50 [mm]
d: Parallax [pixel]
f: Focal length [mm]
c _x : Center coordinate in the X direction of the image [pixel]
c _y : Center coordinates in the Y direction of the image [pixel]
If the camera is a monocular camera instead of the stereo camera 50, a three-dimensional position can be obtained by using a DLT (Direct Linear Transformation) method using two monocular cameras (a method using the DLT method is not patented). Reference 2).

  Next, the reliability measurement process described above is a process of registering in the environment map 1001 as a reliable point for a three-dimensional point that is higher than the threshold value, using the reciprocal of the Fisher information amount of the reprojection error as the reliability. . The Fisher information amount is the information amount that the random variable has regarding the parameter. Note that the above-described threshold value may be a preset value, or a predetermined number of points may be added to the environment map 1001 from the top among all the candidate points to be added to the environment map 1001. The extraction conditions for points to be added as described above may be set for each process. In other words, in the process of S602, the environment map generation unit 15 registers only the points that have been successfully subjected to the three-dimensional reconstruction process and have high reliability by the reliability measurement in the environment map 1001.

  Next, the environment map generation unit 15 deletes the 3D points of the outliers in the environment map 1001 (S603). Outliers are values that deviate significantly from other values in statistics. In the process of step S603, the reliability described above is calculated for all points on the environment map 1001, and points with the calculated reliability smaller than the threshold are deleted from the environment map 1001. This threshold value may also be set in advance, and a predetermined number of points may be deleted from the environment map 1001 from all the three-dimensional points on the environment map 1001. The point extraction conditions as described above may be set for each process.

<< S70 Definition of Allowable Motion >>
Next, the allowable motion definition will be described. The permissible motion definition is information describing which parameter set is permitted at each point in time for each of a plurality of parameter sets representing the motion of the flying object 100.

  FIG. 9 is an example of a flowchart illustrating a procedure in which the allowable motion definition unit 17 defines the allowable motion. The process of FIG. 9 is repeatedly executed for all the motion parameter IDs. Here, the number of exercise parameter IDs is n (0 to n-1). The exercise parameter ID is identification information for distinguishing the exercise parameter. Identification information refers to a name, a code, a character string, a numerical value, or a combination thereof used to uniquely distinguish a specific target from a plurality of targets.

  In the process of FIG. 9, first, the allowable motion definition unit 17 performs the allowable motion determination (S701). The allowable exercise determination refers to determining whether or not the exercise is allowable, and the details will be described with reference to FIG.

  If the motion is acceptable (Yes in S702), the motion related to the i-th motion parameter is registered as the permitted motion (S703).

  Since the allowable motion definition unit 17 selects a motion parameter capable of tracking a feature point from a finite number of motion parameters, an allowable motion can be determined within a practical time.

  FIG. 10 is an example of a flowchart for explaining the allowable motion determination process of FIG. In the allowable motion determination process, when the flying object 100 moves with a certain motion parameter, the feature point captured in the latest image among the three-dimensional points included in the environmental map is an image being captured (under exposure). This is a process for determining whether observation is possible. If the exposure time is long, observation on the image being picked up becomes difficult, and if the frame interval is long, the feature point moves greatly, so that observation on the image being picked up becomes difficult. Therefore, in the image being captured (exposure), by predicting how much the point captured in the latest image among the points included in the environment map moves in the image being captured (exposure), It is possible to determine whether or not the image can be captured with the (under exposure) image. In FIG. 10, a motion parameter (motion) that is expected to be able to observe a certain number or more of feature points is determined to be an allowable motion.

  First, the allowable motion definition unit 17 reads the environment map 1001 from the storage unit 1000 (S7701).

  Further, the allowable motion definition unit 17 reads the camera parameter 1003 from the storage unit 1000 (S7702). The camera parameter 1003 includes an exposure time and a frame interval.

  Next, the allowable motion definition unit 17 repeats the following process with M feature points captured in the latest image and included in the environment map.

  First, the allowable motion definition unit 17 predicts the moving distance of the feature point during exposure (S7703). When the flying object 100 rotates during the minute time as represented by the rotation matrix dR and moves as represented by the parallel movement vector dt, the movement amount v on the image is obtained by the following equation (17). Can do.

（１７）式の左辺は、微小時間が経過した後に露光中の特徴点が撮像される画像内の座標と、最新の画像で特徴点が撮像されている画像内の座標との差分を表している。（１７）式から、露光中の画像における移動量ｖを求めることができる。なお、移動距離は（１７）式のv_i ^xとv_i ^ｙの二乗和の平方根である。（１７）式では最新の画像で撮像されたカメラ座標系の特徴点の座標をＸ_i ^cとした。

The left side of the equation (17) represents the difference between the coordinates in the image where the feature point being exposed after the minute time has elapsed and the coordinates in the image where the feature point is captured in the latest image. Yes. From the equation (17), the movement amount v in the image being exposed can be obtained. The moving distance is the square root of the sum of squares of v _i ^x and v _i ^{y in} equation (17). (17) the coordinates of the feature points of the camera coordinate system captured at latest image was X _i ^c in formula.

  The rotation matrix dR is a minute amount of each element of the rotation matrix R that converts the three-dimensional coordinates of the feature point captured in the latest image into the camera coordinate system.

  First, the rotation matrix dR and the translation vector dt are obtained as follows. Since the moving body position measuring unit 16 measures the position and orientation, it calculates the moving speed and the rotating speed of each rotating shaft. Since the exposure time is known, the translation vector dt when the flying object 100 moves with the moving speed can be calculated. Similarly, the rotational speed of each rotating shaft is known to the moving body position measuring unit 16 because it is a temporal change in the yaw angle ψ, the pitch angle θ, and the roll angle φ. By multiplying the rotation speed of each rotation axis by the exposure time, the amount of rotation in the exposure time can be determined.

  Then, the allowable motion definition unit 17 reads the motion used with the motion parameter currently focused on from the motion parameter using motion 1006. Then, the elements of the rotation matrix dR and the translation vector dt corresponding to the motion not used are replaced with zero. For example, when it rises, each element of the rotation matrix dR may be set to zero. Further, in a posture change not accompanied by movement, each element of the parallel movement vector dt may be set to zero. Further, if the movement is in the X direction (only), the rotation angle other than the rotation angle based on the principle of movement does not change, so the elements that do not change among the elements of the rotation matrix dR may be set to zero. In this case, since the Y coordinate and the Z coordinate do not change, the change amount of the Y coordinate and the Z coordinate in the translation vector dt may be set to zero. In this way, the rotation matrix dR and the translation vector dt can be calculated according to the motion parameter.

  The allowable motion definition unit 17 can predict the movement amount v on the image of the flying object 100 during the exposure time by setting the rotation matrix dR and the parallel movement vector dt obtained in this way in the equation (17).

  Note that the movement amount v on the image of the flying object 100 may be predicted from only one of the rotation matrix dR and the parallel movement vector dt and the exposure time.

  Next, the allowable motion definition unit 17 determines whether or not the moving distance in the exposure time is smaller than the threshold value L1 [number of pixels] (S7014). When the moving distance during the exposure time is equal to or greater than the threshold value L1, it is determined that there is a high possibility of subject blurring (motion blur). That is, it is determined that the feature point is not observable. Theoretically, subject blur occurs when there is a movement of one pixel or more. For this reason, the threshold value L1 is 1 pixel (pixel to pixel pitch), and if the moving distance during the exposure time is less than 1 pixel, the determination in step S7014 is Yes. Some pixels may be allowed to allow slight subject blurring.

  Next, the motion constraint definition 17 unit predicts the moving distance of the flying object 100 on the image at the frame interval (S7015). What expresses a motion of a moving object on an image between frames as a vector is called an optical flow. In the present embodiment, the optical flow between frames is obtained in the same manner as in step S7013. That is, the frame interval may be used instead of the exposure time.

  The allowable motion definition unit 17 can predict the moving distance on the image of the flying object 100 in the frame interval by setting the rotation matrix dR and the translation vector dt obtained in this way in the equation (17).

  Next, in the motion constraint definition, it is determined whether or not the moving distance between frames is smaller than the threshold value L2 [number of pixels] (S7016). The threshold value L2 is determined in advance in consideration of the performance limit of the feature point tracking algorithm. Therefore, the threshold L2 is preferably different depending on the feature point tracking algorithm. For example, when the feature point tracking algorithm is block matching that specifies a search range in advance, the pixel size that specifies the search range is the threshold L2. The case where Pyramidal KLT (or Lucas-Kanade method etc.) is used for the feature point tracking algorithm will be described. “Pyramidal” of feature point tracking refers to creating a plurality of images with different resolutions from an original image and searching for feature points from each of them. Further, KLT refers to tracking feature points on the assumption that only parallel movement occurs in a minute time and there is no change in feature amount. When Pyramidal KLT (or Lucas-Kanade method or the like) is used, the size of the local window in which the cost of KLT is calculated in the highest level (highest resolution) pyramid image is the threshold L2.

  In the case of the technique using the feature description, the entire range of the image is the search range, and the range that does not exceed the imaging range of the stereo camera 50 is the threshold L2. By doing so, the flying object 100 can move with a motion that does not exceed the performance of tracking feature points.

  Even if the moving distance between frames is smaller than the threshold value L2, the feature point may move out of the image range in the next frame. For this reason, the allowable motion definition unit 17 determines whether the coordinates obtained by adding the optical flow movement vector to the position of the feature point in the latest image are larger than 0 both vertically and horizontally and do not exceed the width and height of the image. Determine. If it is not within this range, it is not registered as an observable feature point even if the moving distance between frames is smaller than the threshold L2.

  If the determination in step S7016 is Yes, the allowable motion definition unit 17 registers the feature point as an observable feature point (S7017).

  If it is determined whether M feature points are observable feature points, the process advances to step S7018.

  The allowable motion definition unit 17 determines whether or not there are K or more observable feature points (S7018).

  When the determination in step S7018 is Yes, the allowable motion definition unit 17 determines that the i-th motion parameter focused on in FIG. 9 is the allowable motion (S7019). The allowable motion definition unit 17 registers the motion parameter determined as the allowable motion among the motion parameters in the allowable motion map.

<Processing of moving body control unit 30>
Next, the process of the mobile body control part 30 is demonstrated using FIG. FIG. 11 is an example of a flowchart illustrating an operation procedure of the moving body control unit 30. The moving body control unit 30 employs the motion with the lowest path cost from the N allowable motion maps. The allowable motion map is a motion parameter registered as an allowable motion among the motion parameters. That is, when the flying object 100 performs the motion of the motion parameter of the allowable motion map, it is determined that N or more observable feature points are obtained in the next image.

  The mobile body control unit 30 calculates a route cost for reaching the planned destination for each motion parameter (S110). The route cost includes time, fuel consumption, and load on the aircraft. In the present embodiment, for convenience of explanation, it is assumed that the route cost is time.

  FIG. 12 is an example of a diagram illustrating a route cost calculation method. For example, when the A coordinate and the B coordinate are registered in the path along which the flying object 100 moves, the flying object 100 needs to move to the B coordinate that is the next scheduled movement destination after reaching the A coordinate. Here, there may be a plurality of motion parameters that move from the A coordinate to the B coordinate. Normally, a motion parameter that moves along a path c that can be reached in a straight line is employed. However, in the present embodiment, the allowable motion definition unit 17 may not allow a motion parameter that moves along the path c. Therefore, the flying object 100 of this embodiment can continue position estimation.

  In this case, since the moving body control unit 30 moves to the B coordinate, the moving body control unit 30 calculates the route cost using another motion parameter passing through the route a and the route b (S110).

  When the path cost is calculated with all the permitted motion parameters, the mobile control unit 30 adopts the motion parameter with the lowest path cost (S120).

  Therefore, the motion parameter that moves at the lowest path cost among the motion parameters of the allowable motion defined by the allowable motion definition unit 17 can be adopted. For example, an exercise parameter that moves in the shortest time can be employed.

  The route may be automatically generated using an A * algorithm or Dijkstra method, or may be recorded by a user on a moving body.

  As described above, the flying object 100 according to the present embodiment captures an image in which subject blurring does not occur and feature points that can be tracked between frames are greater than or equal to a threshold based on the image capturing conditions. Then, it is determined that the exercise with the exercise parameter of interest is possible. Therefore, the flying object 100 of the present embodiment is easy to detect the position and orientation continuously. In addition, since the motion parameter that moves with the lowest path cost is adopted among the motion parameters that are easy to detect the position and posture continuously, an increase in time required for the movement can be suppressed to the minimum.

<Other application examples>
The best mode for carrying out the present invention has been described above with reference to the embodiments. However, the present invention is not limited to these embodiments, and various modifications can be made without departing from the scope of the present invention. And substitutions can be added.

  In the present embodiment, the flying object 100 has been described as an example of the moving body. However, the present invention can be applied to various moving bodies such as a vehicle, a mobile robot, a ship, and a submarine. Further, the present invention can be applied to an information processing apparatus such as a smartphone mounted on a moving body.

  In addition, the configuration example in FIG. 10 and the like is divided according to main functions in order to facilitate understanding of processing by the flying object 100. The present invention is not limited by the way of dividing the processing unit or the name. The processing of the flying object 100 can be divided into more processing units depending on the processing content. Moreover, it can also divide | segment so that one process unit may contain many processes.

  Further, the order of the flowcharts described in the above embodiments may be changed as long as there is no contradiction.

  The stereo camera 50 is an example of an imaging device, the moving body position measurement unit 16 is an example of a position detection unit, and the camera parameter is an example of an imaging condition. The position measurement unit 10 is an example of a position estimation device, and the processes and methods performed by the position measurement unit 10 are examples of a position estimation method.

DESCRIPTION OF SYMBOLS 10 Position measurement part 11 Image feature point extraction part 12 Image feature point tracking part 13 Depth measurement part 14 Imaging part control part 15 Environment map generation part 16 Mobile body position measurement part 17 Permissible motion definition part 30 Mobile body control part 50 Stereo camera 100 Flying object

JP 2012-150655 A

Computer Vision and Pattern Recognition, 2005. CVPR 2005. IEEE Computer So-ciety Conference on, Vol. 2, pp.807-814, 2005. Tomokazu Sato, “Research on 3D modeling of outdoor environment based on camera parameter estimation using multiple moving images”, Nara Institute of Science and Technology, Doctoral Dissertation, March 24, 2003.

Claims (10)

A position estimation device that estimates a position of a moving body from a surrounding image captured by an imaging device capable of detecting distance information to a subject and creates a surrounding map,
An image feature point extraction unit that extracts a feature point from the image captured by the imaging device;
An image feature point tracking unit that tracks the feature point of the image in the image captured by the imaging device;
An environment map generation unit that generates the map including the three-dimensional coordinates of the feature points using the three-dimensional coordinates of the moving body, the distance information, and camera parameters of the imaging device;
An error between the projected coordinate and the feature point coordinate when the three-dimensional coordinate of the feature point is projected onto the image, or the converted coordinate and the feature point when the feature point coordinate is converted into the three-dimensional coordinate. A position detection unit for detecting the position and orientation of the moving body in which the error of the three-dimensional coordinates is smaller;
The feature point can be tracked based on at least one of the moving speed of the moving body detected based on the position or the rotational speed of the moving body detected based on the posture, and the imaging condition of the image. And a permissible motion defining unit that defines motion of the moving body capable of capturing the image. The imaging condition includes information on exposure time,
The allowable motion defining unit defines the motion that can track the feature point when the moving body moves at the moving speed and the rotational speed and the imaging apparatus captures the image during the exposure time. The position estimation apparatus according to claim 1.   The allowable motion defining unit is configured such that when the moving body moves at the moving speed and the rotational speed, and the imaging device captures the image during the exposure time, the moving amount of the feature point in the image is 1 pixel. The position estimation apparatus according to claim 2, wherein the movement is defined as being less than a value. The imaging condition includes information regarding an imaging interval,
The allowable motion defining unit is configured to perform the motion that can track the feature point when the moving body moves at the moving speed and the rotational speed and the imaging apparatus captures the next image at the imaging interval. The position estimation apparatus according to claim 1, wherein the position estimation apparatus is defined. The allowable motion definition unit calculates a movement vector of the feature point in the imaging interval;
The position estimation apparatus according to claim 4, wherein the feature point is determined to be traceable when a length of the movement vector is smaller than a threshold value. The image feature point tracking unit searches for the feature point only in a preset search range,
The position estimation apparatus according to claim 5, wherein the allowable motion definition unit determines that the feature point can be traced when the feature point moved by the movement vector is included in the search range. The image feature point tracking unit tracks the feature point using Pyramidal KLT,
The allowable motion defining unit determines that the feature point is traceable when the feature point moved by the movement vector is within a local window of the KLT in the highest level pyramid image used in Pyramidal KLT. 6. The position estimation device according to 6. The allowable motion definition unit reads a plurality of motion parameters that define motions that can be adopted by the mobile body and variable degrees of freedom associated with the motion parameters from the storage unit,
The permissible motion definition unit refers to the degree of freedom for each motion parameter, and determines whether the motion specified by the motion parameter is a motion that can capture the image capable of tracking the feature point. Item 8. The position estimation device according to any one of Items 1 to 7. An information processing apparatus that estimates a position of a moving body from a surrounding image captured by an imaging apparatus capable of detecting distance information to a subject and creates a surrounding map,
An image feature point extraction unit that extracts a feature point from the image captured by the imaging device;
An image feature point tracking unit that tracks the feature point of the image in the image captured by the imaging device;
An environment map generation unit that generates the map including the three-dimensional coordinates of the feature points using the three-dimensional coordinates of the moving body, the distance information, and camera parameters of the imaging device;
An error between the projected coordinates when the three-dimensional coordinates are projected onto the image and the coordinates of the feature points, or an error between the converted coordinates and the three-dimensional coordinates when the coordinates of the feature points are converted into the three-dimensional coordinates. A position detection unit for detecting the position and posture of the moving body that is smaller;
The feature point can be tracked based on at least one of the moving speed of the moving body detected based on the position or the rotational speed of the moving body detected based on the posture, and the imaging condition of the image. A program for functioning as an allowable motion definition unit that defines a motion capable of capturing the image. A position estimation method for a position estimation apparatus that estimates a position of a moving body from a surrounding image captured by an imaging apparatus capable of detecting distance information to a subject and creates a map of the surrounding area,
An image feature point extraction unit extracting a feature point from the image captured by the imaging device;
An image feature point tracking unit tracking the feature point of the image in the image captured by the imaging device;
An environment map generating unit generating the map including the three-dimensional coordinates of the feature points using the three-dimensional coordinates of the moving object, the distance information, and the camera parameters of the imaging device;
An error between the projection coordinates when the three-dimensional coordinates are projected on the image and the coordinates of the feature points, or the converted coordinates and the cubic when the coordinates of the feature points are converted into the three-dimensional coordinates. Detecting the position and orientation of the moving body in which the error of the original coordinates becomes smaller;
Based on at least one of the moving speed of the moving body detected by the position detecting unit based on the position or the rotational speed of the moving body detected based on the posture, and the imaging condition of the image, an allowable motion defining unit, Defining a motion capable of capturing the image capable of tracking the feature point.

Images