JP7225762B2 - Self-position estimation device, self-position estimation method and program - Google Patents

Description

The present invention relates to a self-position estimation device, a self-position estimation method, and a program suitable for a device worn on a human body to obtain movement history, for example.

Visual SLAM (Visual Simultaneous Localization And Mapping) is widely known as a technique for estimating the self-location of a device in a certain environment using an image.

A familiar example of visual SLAM use is a robot vacuum cleaner sold by an electrical company. For example, a technology has been proposed in which an optical travel distance sensor system mounted on a mobile robot captures an image of the tracking surface below the mobile body with a camera. (For example, Patent Document 1)

JP 2018-500624 A

There are roughly two methods of visual SLAM: a method using feature points in an image and a method using luminance/depth information in an image.
The method of using feature points in the image has the advantage that calculations can be done only around the feature points that are sparsely arranged in the image, and the amount of calculation is small. There is a problem that feature points cannot be calculated.
On the other hand, the method using brightness/depth information does not need to calculate feature points, so it has the advantage of being resistant to blurring. There is a problem that the amount of calculation is heavy.

Since the technology described in Patent Document 1 is basically applied to a robot that moves on a smooth road surface, there is no blurring in the captured image, and it is possible to extract feature points in the image. A method using the feature points of For this reason, the technology described in Patent Document 1 is applied to a device intended for a moving body that involves vigorous body movements, such as a device worn on the human body to analyze the movement history of a player in a ball game such as tennis or basketball. Even if one tries to do so, there is a high possibility that blurring will occur in the resulting image, and there is the problem that feature points cannot be extracted from the image.

On the other hand, if a method using luminance/depth information is applied to a device intended for a moving object that undergoes vigorous body movements, the problem of a heavy computational load arises as described above.

The present invention has been made in view of the above circumstances, and its object is to accurately estimate the self-position of a moving body including movements accompanied by violent body movements with an appropriate amount of calculation. A self-position estimation device, a self-position estimation method, and a program are provided.

According to one aspect of the present invention, a first acquiring unit acquires motion information about a motion of a moving object, and an imaging device provided in the moving object while the first acquiring unit acquires the motion information. a second acquisition means for acquiring image information captured by a device; a determination means for determining whether or not the movement of the moving object is rapid based on whether or not the motion information is equal to or less than a predetermined threshold; An estimation process using the feature points of the image for estimating the self-position of the moving body by executing a process for estimating the self-position based on the information, and an estimation process using the brightness and depth information of the image. and a control means for controlling switching between estimation processing using the feature points of the image and estimation processing using the brightness and depth information of the image according to the determination result of the determination means, wherein the control means determines that the movement of the moving object is rapid by the determining means, and the process immediately before estimating the self-position of the moving object is the estimation process using the feature points of the image. , converting the information obtained in the estimation process using the feature points of the image into information used in the estimation process using the brightness/depth information of the image, and controlling to switch to the estimation process using the brightness/depth information of the image. do .

According to the present invention, it is possible to accurately estimate the self-position of a moving body including movements accompanied by violent body movements with an appropriate amount of calculation.

1 is a block diagram showing the functional configuration of an electronic circuit of a position recorder according to one embodiment of the present invention; FIG. 4 is a flow chart showing the overall processing contents related to self-position estimation, motion trajectory estimation, and recording according to the embodiment; 4 is a flowchart of a subroutine showing details of motion intensity estimation processing according to the embodiment; 4 is a flowchart of a subroutine showing details of motion trajectory estimation processing from an image according to the embodiment; FIG. 4 is a diagram for explaining the concept of conversion between feature point information and luminance/depth information according to the embodiment;

An embodiment in which the present invention is applied to a position recorder 10 that can be worn on the player's own body in sports or leisure and can record the locus of movement of the player's own position will be described below with reference to the drawings. do.

FIG. 1 is a block diagram showing the functional configuration of the electronic circuit of the position recorder 10. As shown in FIG.
A processor 11 , an image processing section 12 , an inertial sensor section 13 , a memory interface (I/F) 14 , a wireless communication section 15 and a key input/indicator section 16 are connected to the bus B.

The processor 11 includes a CPU equipped with an RTC (Real Time Clock) 11A that keeps current time information regardless of whether the power is turned on, a RAM that serves as a work memory for the CPU, an operation program that is executed by the CPU, and fixed data. and a ROM for non-volatile storage of data, etc. The processor 11 controls the operation of the position recorder 10 as a whole.

The image processing unit 12 is composed of a dedicated DSP (Digital Signal Processor) that executes image processing operations under the control of the processor 11 . The image processing unit 12 causes the imaging unit 17 to execute an imaging operation at a constant time cycle through the control signal relating to imaging from the processor 11, and extracts three-dimensional feature point information from the obtained image data. , and extraction processing of luminance/depth information to obtain three-dimensional information of the shooting direction.

The imaging unit 17 includes an optical system lens unit, for example, a solid-state imaging device using a CMOS image sensor, an A/D conversion circuit, etc., and takes images at a constant time period under the control of the processor 11, and obtains a digital value image. Data is output to the image processing unit 12 .

The inertial sensor unit 13 has a 3-axis acceleration sensor and a 3-axis gyro sensor, and acquires acceleration and angular velocity corresponding to each of the 3 axes according to the behavior given to the position recorder 10 .

The memory interface 14 records data sent from the processor 11 via the bus B to the memory card 18 at any time while the memory card 18 is attached to the card slot CS of the position recorder 10 .

The data recorded on the memory card 18 includes, for example, the time information when the RTC 11A measures the time of shooting, the three-dimensional feature point information, or the brightness/depth information, and the data recorded at the previous timing and then recorded this time. This includes movement trajectory data associated with position estimation calculated from feature point information or brightness/depth information until data is recorded at the timing of .

The wireless communication unit 15 complies with, for example, the Bluetooth (registered trademark) LE (Low Energy) standard, which is one of short-range wireless communication standards, and is an external device (not shown) paired with the position recorder 10 in advance. and the antenna 19 to perform data transmission/reception.

The key input/indicator section 16 has a power key for turning on/off the power, and an indicator lamp for indicating the operating state by turning on/off.

Next, the operation of this embodiment will be described.
FIG. 2 shows the overall processing contents relating to self-position estimation, motion trajectory estimation, and recording, which are periodically executed by the position recorder 10 after the power is turned on. The processing in FIG. 2 is executed mainly based on an operation program pre-stored in the processor 11 .

At the beginning of processing, the processor 11 acquires the output of the inertial sensor section 13, and estimates whether or not the position recorder 10 is currently moving rapidly based on the acquired output of the inertial sensor section 13 (step S101).

FIG. 3 is a flowchart of a subroutine showing details of motion intensity estimation processing by the processor 11 . The processor 11 first determines whether or not the variance of acceleration obtained from the acceleration sensor of the inertial sensor unit 13 is smaller than a preset threshold AccVarTH for the same variance (step S201).

When determining that the variance value of the acceleration sensor is smaller than the preset threshold AccVarTH (Yes in step S201), the processor 11 next determines that the angular velocity obtained from the gyro sensor of the inertial sensor unit 13 is equal to the preset threshold value. It is determined whether or not the angular velocity is smaller than the threshold value GyrTH (step S202).

If it is determined that the angular velocity obtained from the gyro sensor is smaller than the preset threshold value GyrTH (Yes in step S202), the processor 11 treats the movement of the position recorder 10 at the present time as "not violent" in subsequent processing. (step S203), the process of FIG. 3 is terminated once, and the process returns to the process of FIG.

Further, in step S201, when it is determined that the variance of the acceleration sensor is equal to or greater than the preset threshold value AccVarTH (No in step S201), or in step S202, when the angular velocity obtained from the gyro sensor exceeds the preset threshold value GyrTH or more (No in step S202), the processor 11 assumes that the movement of the position recorder 10 at the present time is "strong" in the subsequent processing and executes the processing as an estimation result (step S204). ), once the process of FIG. 3 is terminated, the process returns to that of FIG.

In FIG. 2, the processor 11 estimates the current self-position of the user wearing the position recorder 10 from the image data captured by the imaging unit 17, and connects the self-positions together to estimate the motion trajectory. The image processing unit 12 is caused to execute the processing (step S102).

FIG. 4 is a flowchart of a subroutine showing the details of the process of estimating a motion trajectory from an image, which is executed by the image processing unit 12 mainly under the control of the processor 11. As shown in FIG. At the beginning of the process, the processor 11 determines whether or not the movement of the position recorder 10 is estimated to be "strong" in the previous step S101 (step S301).

If it is determined that it has been estimated to be “intense” (Yes in step S301), there is a high possibility that the feature point information of the image data is blurred, and it is possible to estimate the current self-position using the feature point information. Assuming that it is difficult, the processor 11 determines whether or not the brightness/depth information was used to estimate the self-position at the previous cycle timing (step S302).

If it is determined that the feature point information is used instead of the brightness/depth information for estimating the self-position at the previous cycle timing (No in step S302), the self-position estimation processing cannot be executed as it is. , the processor 11 causes the image processing unit 12 to convert the feature point information obtained at the previous timing into luminance/depth information (step S303).

FIG. 5 is a diagram for explaining the concept of mutual conversion between information on the positions of three-dimensional feature points and information on brightness/depth. FIG. 5A exemplifies the process of acquiring the data of the positions of the feature points in the three-dimensional space as the two-dimensional captured image IM when the image is captured by the imaging unit 17 of the position recorder 10. there is Regarding the extraction of the positions of feature points, image processing such as edge detection and contour extraction is performed according to the three primary color components and brightness of the image to extract feature points in the image, and their two-dimensional coordinate positions and Acquire distance (depth) information.

Regarding the positions of the feature points thus obtained, the known three-dimensional information of the feature points is converted into depth as shown in FIG. By calculating the initial value of the trajectory in the middle and estimating and updating the depth of the entire image so that the photometric error is reduced by repeated processing, the luminance and depth information as shown in FIG. get.

In step S302, if it is determined that the brightness/depth information was used to estimate the self-position at the previous cycle timing (Yes in step S302), and in step S303, the three-dimensional feature point information is converted to brightness/depth information. After executing the transforming process, the image processing unit 12, under the control of the processor 11, executes arithmetic processing for estimating the position and orientation of the position recorder 10 using the luminance/depth information (step S304).

Techniques for estimating the self-position and orientation from an image using brightness/depth information include, for example, "LSD-SLAM: Large-Scale Direct monocular SLAM, Jakob Engel, et al, ECCV 2014." Thus, the image processing unit 12 executes the arithmetic processing, the subroutine of FIG. 4 is completed, and the processing returns to that of FIG.

In step S301, when it is determined that the movement of the position recorder 10 is "not violent" (No in step S301), the processor 11 determines that there is a high possibility that the feature point information of the image data is not blurred. At the period timing before that, it is determined whether or not the feature point information is used for estimating the self-position (step S305).

If it is determined that the brightness/depth information is used instead of the feature point information to estimate the self-position at the previous period timing (No in step S305), the process of estimating the self-position based on the feature point information is executed as it is. Therefore, the processor 11 causes the image processing unit 12 to convert the brightness/depth information obtained at the previous timing into feature point information (step S306).

As shown in FIG. 5C, the image processing unit 12 uses a method that utilizes the brightness/depth information obtained at the previous cycle timing to reproduce the image at the current cycle timing. Depth information and motion trajectories are estimated. Next, from the image data actually acquired at the current cycle timing, the positions of the two-dimensional feature points are extracted as shown in FIG. Three-dimensional feature point information as shown in 5(A) is assumed.

If it is determined in step S305 that the feature point information was used to estimate the self-position at the previous cycle timing (Yes in step S305), and in step S306, the brightness/depth information is converted into three-dimensional feature point information. After executing the processing, the image processing unit 12 executes arithmetic processing for estimating the position and orientation of the position recorder 10 using the feature point information under the control of the processor 11 (step S307).

A technique for estimating the self-position and orientation from an image using feature point information is described, for example, in “ORB-SLAM: a Virtual and Accurate Monocular SLAM System, Raul Mur-Artal, et al, IEEE Transactions On Robotics, 18 Sep 2015. ”, the image processing unit 12 executes the arithmetic processing, the subroutine of FIG. 4 is finished, and the processing returns to that of FIG.

In FIG. 2, in step S102, after estimating the current self-position and estimating a motion trajectory connecting a plurality of self-positions from the image data, the processor 11 calculates the self-position and motion trajectory calculated from the image data. and the acceleration and angular velocity information obtained from the inertial sensor unit 13 are integrated and recorded in the memory card 18 by the memory interface 14 (step S103).

In this integration process, the processor 11 defines, for example, {position, velocity, and attitude} as a state space, {motion trajectory from an image} as an observed value having an error, and updates {inertial sensor acceleration, angular velocity} in the system. , can be realized by using a Kalman filter, which is a kind of infinite impulse response filter.

After that, the processor 11 determines whether or not to end the self-position estimation process depending on whether or not the power key of the key input/indicator section 16 has been operated (step S104).

If the power key is not operated and it is determined that the self-position estimation process is not to end (Yes in step S104), the processor 11 returns to the process from step S101, and repeats the same process as above at the timing of the next cycle. do.

In this way, the processing of steps S101 to S104 is repeatedly executed to integrate and record the estimation result of the self-position and the estimation result of the motion trajectory accompanying the movement of the user wearing the position recorder 10 and the output of the inertial sensor section 13. In the process of continuing, if the user operates the power key and determines to end the self-position estimation processing (Yes in step S104), the processor 11 stores the information in the memory card 18 via the memory interface 14 at that time. After the series of recorded contents that have been recorded are compiled into a predetermined data file and updated and recorded (step S105), the process of FIG. 2 is terminated.

Note that, in the present embodiment, the position recorder 10 periodically performs image capturing by the imaging unit 17 and motion detection by the inertial sensor unit 13 after the power is turned on. Although it is explained that self-position estimation and motion trajectory estimation and recording are executed in real time from various information obtained from the may be executed, or the recorded information may be transferred to another device and the processing may be executed by another device.

In addition, in the present embodiment, the self-position and motion trajectory information calculated from the image data and the acceleration and angular velocity information acquired from the inertial sensor unit 13 are integrated and recorded. Only the information on the motion trajectory may be recorded.

As described in detail above, according to the present embodiment, it is possible to accurately estimate the self-position of a moving body including movements accompanied by violent body movements with an appropriate amount of calculation.

In addition, in this embodiment, an image captured from the moving object itself is used, and the self-position is estimated by alternatively selecting calculation using feature point information or calculation using brightness/depth information from the movement situation. Therefore, it is possible to accurately estimate the self-position while reducing the amount of calculation of the circuit that performs the processing.

In this case, when it is determined that the movement of the moving object is large, calculation using the depth of light and darkness based on luminance information in the image information is executed, and when it is determined that the movement of the moving object is not large, feature point information is processed. Therefore, it is possible to reduce the amount of calculation of the circuit that performs the processing while maintaining the estimation accuracy of the self-position.

In particular, in this embodiment, the inertia sensor unit 13 including an acceleration sensor and a gyro sensor is used to detect the magnitude of the movement of the moving body. It can accurately determine the situation by following sudden changes.

In addition, in this embodiment, by connecting the estimation results of the self-position along the time series, it is possible to estimate the movement trajectory of the moving object together.

Note that this embodiment has been described as an embodiment in the case of being applied to a dedicated device that can be worn on the player's own body in sports or leisure and can record the movement trajectory of the player's own position. is not limited to this, and can be applied to other devices in the same way, for example, in mobile information terminals such as smartphones, in which part of the data processing calculation is installed as an application program.

In addition, in the present embodiment, whether or not the position recorder 10 is moving rapidly is estimated based on the output result of the inertial sensor unit 13. Alternatively, the imaging unit 17 may be configured as a full-dome/half-dome camera, and the photographed vertically downward image may be analyzed to determine whether the vertically downward direction is a flat surface or an uneven surface. may be determined, and it may be estimated that the movement of the position recorder 10 is rapid in the case of an uneven surface, and that the movement is not rapid in the case of a flat surface.

In addition, the present invention is not limited to the above-described embodiments, and can be variously modified in the implementation stage without departing from the scope of the invention. Moreover, each embodiment may be implemented in combination as much as possible, and in that case, the combined effect can be obtained. Furthermore, the above-described embodiments include inventions at various stages, and various inventions can be extracted by appropriately combining a plurality of disclosed constituent elements. For example, even if some constituent elements are deleted from all the constituent elements shown in the embodiments, the problems described in the column of problems to be solved by the invention can be solved, and the effects described in the column of effects of the invention is obtained, the configuration from which this constituent element is deleted can be extracted as an invention.

The invention described in the original claims of the present application is appended below.
[Claim 1]
a first acquisition means for acquiring motion information about the motion of the moving object;
a second acquiring means for acquiring captured image information;
determining means for determining the magnitude of motion of the moving object based on the motion information;
self-position estimation means for estimating the self-position of the moving object based on the image information;
a control means for switching the processing content using the image information in the self-position estimation means according to the determination result of the determination means;
A self-localization device comprising:
[Claim 2]
2. The self-positioning device according to claim 1, wherein said self-position estimation means estimates the self-position of said moving object by either calculation using feature points of said image information or calculation using luminance/depth information of said image information. Position estimator.
[Claim 3]
When the determination means determines that the movement of the moving body is large, the control means causes the self-position estimation means to perform calculations using the brightness and depth information of the image information, and the determination means determines that the movement of the moving body is large. 3. The self-position estimation device according to claim 2, wherein when it is determined that the movement of the image is not large, the self-position estimation means executes calculation using the feature points of the image information.
[Claim 4]
The motion information includes information on dispersion of acceleration of the moving body and information on angular velocity,
The determining means determines that the movement of the moving body is large when at least one of the information about the variance of the acceleration of the moving body and the information about the angular velocity is greater than each preset threshold value.
The self-position estimation device according to any one of claims 1 to 3.
[Claim 5]
5. The self-position estimating device according to claim 1, further comprising trajectory estimating means for estimating a movement trajectory of said moving body from a time-series history of the self-position of said moving body estimated by said self-position estimating means. .
[Claim 6]
A self-position estimation method for a device comprising a first acquisition unit that acquires motion information related to the movement of a moving body and a second acquisition unit that acquires captured image information,
a determination step of determining the magnitude of motion of the moving object based on the motion information;
a self-position estimation step of estimating the self-position of the moving object based on the image information;
a control step of switching the processing content using the image information in the self-position estimation step according to the determination result of the determination step;
A self-localization method comprising:
[Claim 7]
A program executed by a computer contained in a device having a first acquisition unit for acquiring motion information related to the movement of a moving body and a second acquisition unit for acquiring captured image information, the program comprising:
determination means for determining the magnitude of motion of the moving object based on the motion information;
self-position estimation means for estimating the self-position of the moving object based on the image information;
Control means for switching processing content using the image information in the self-position estimation means according to the determination result of the determination means;
A program that acts as a

10... Position recorder 11... Processor 11A... RTC
12... Image processing unit 13... Inertial sensor unit 14... Memory interface (I/F)
DESCRIPTION OF SYMBOLS 15... Wireless communication part 16... Key input/indicator part 17... Imaging part 18... Memory card 19... Antenna B... Bus CS... Card slot

Claims (6)

a first acquisition means for acquiring motion information about the motion of the moving object;
a second acquiring means for acquiring image information captured by an imaging device provided in the moving body while the first acquiring means is acquiring the motion information;
determining means for determining whether or not the movement of the moving object is rapid based on whether or not the motion information is equal to or less than a predetermined threshold;
Estimation processing using the feature points of the image for estimating the self-position of the moving body by executing processing for estimating the self-position based on the image information, and estimation using brightness and depth information of the image. a self-localization means having a process ;
a control means for controlling switching between an estimation process using the feature points of the image and an estimation process using the brightness and depth information of the image according to the determination result of the determination means ;
If the determination means determines that the movement of the moving body is rapid and the process immediately before estimating the self-position of the moving body is the estimation process using the feature points of the image, the control means The information obtained by the estimation process using the feature points of is converted into information used in the estimation process using the brightness and depth information of the image, and the brightness and depth information of the image is controlled to switch to the estimation process.
A self-position estimation device characterized by: When the determining means determines that the movement of the moving body is not rapid and the process immediately before estimating the self-position of the moving body is an estimation process using the brightness and depth of the image, The information obtained by the estimation process using the brightness and depth of the image is converted into the information used in the estimation process using the feature points of the image, and control is performed to switch to the estimation process using the feature points of the image. 2. The self-position estimation device according to claim 1, wherein: The motion information includes information on dispersion of acceleration of the moving body and information on angular velocity,
The determining means determines whether or not the movement of the moving body is rapid, based on whether at least one of the information on the dispersion of the acceleration of the moving body and the information on the angular velocity is equal to or less than a preset threshold value. characterized by
The self-position estimation device according to claim 1 or 2 . 4. The apparatus according to any one of claims 1 to 3 , further comprising trajectory estimating means for estimating a movement trajectory of said moving body from a time-series history of the self-position of said moving body estimated by said self-position estimating means. self-localization device. a first acquiring unit that acquires motion information about motion of a moving body; and image information captured by an imaging device provided on the moving body while the first acquiring unit is acquiring the motion information. A self-position estimation method in a device comprising a second acquisition unit that acquires
a determination step of determining whether or not the movement of the moving body is rapid based on whether or not the motion information is equal to or less than a predetermined threshold;
Estimation processing using the feature points of the image for estimating the self-position of the moving body by executing processing for estimating the self-position based on the image information, and estimation using brightness and depth information of the image. a self-localization step comprising :
a control step of controlling to switch between estimation processing using the feature points of the image and estimation processing using brightness and depth information of the image according to the determination result in the determination step;
In the control step, when it is determined by the determination step that the movement of the moving object is rapid and the process immediately before estimating the self-position of the moving object is an estimation process using the feature points of the image, The information obtained by the estimation process using the feature points of is converted into information used in the estimation process using the brightness and depth information of the image, and the brightness and depth information of the image is controlled to switch to the estimation process.
A self-localization method characterized by: a first acquiring unit that acquires motion information about motion of a moving body; and image information captured by an imaging device provided on the moving body while the first acquiring unit is acquiring the motion information. A program executed by a computer installed in a device including a second acquisition unit for acquiring,
the above computer,
determination means for determining whether or not the movement of the moving object is rapid based on whether or not the motion information is equal to or less than a predetermined threshold;
Estimation processing using the feature points of the image for estimating the self-position of the moving body by executing processing for estimating the self-position based on the image information, and estimation using brightness and depth information of the image. a self-localization means having a process ;
Functioning as control means for controlling switching between estimation processing using the feature points of the image and estimation processing using brightness and depth information of the image according to the determination result of the determination means ,
If the determination means determines that the movement of the moving body is rapid and the process immediately before estimating the self-position of the moving body is the estimation process using the feature points of the image, the control means The information obtained by the estimation process using the feature points of is converted into information used in the estimation process using the brightness and depth information of the image, and the brightness and depth information of the image is controlled to switch to the estimation process.
program for.

Images