Self-location detection method for moving objects using one camera and multiple light sources

Recently, robots are widely used for transporting materials, parts and so on at manufacturing sites. It is predicted that they will be introduced to general facilities and homes in near future. For expanding the activity of robots, it is necessary to realize a robot capable of autonomous movement, and it is important to detect the self-position of robots in order to enable independent movement. As a general position detection method, there is a method in which an object or a pattern serving as a landmark is set as a landmark and its own position is detected by performing image processing. However, it takes much time to prepare and process, and it costs too much, so it is difficult to introduce it to general facilities and homes. Therefore, it is desired to realize a position detection method with easier preparation in advance and lower installation cost. In this presentation, we propose a simple method of location detection for a robot using several LED illumination and one camera.


Introduction
In recent years, robots have been widely used for transporting raw materials, parts, finished products, etc., mainly at the manufacturing site like the automobile manufacturing factory.In addition, it has been widely adopted not only at the manufacturing site but also in nonmanufacturing industry such as distribution center.For that reason, places of robotic activity will continue to increase and the market size of robots is expected to expand.Although the types of robots are diverse, among them, we focus on automatic guided vehicles, which is robots that move.The market size of automated guided vehicles in Japan is increasing year by year, and it is expected that market size will further expand by further developing new demand other than for FA.Examples of new demand include security robots that support security at public facilities and cleaning robots that rooms.In order to develop such new demand, it is necessary to realize an automatic guided vehicle that can autonomously move.And, in order to enable autonomous movement, the self-location detection and guidance method of the moving body becomes important.
In this paper, we aim a method that is easy to introduce to general facilities as well, and propose self-location detection method using one camera.

The principle of this research system 2.1 Outline of research system
The outline of this research system is shown in Figure 2.1.
For the illumination photographed by the camera, the nearest illumination and the position of the robot with respect to the illumination are detected.positional relationship between the LED illumination and the camera and the shot image of the camera.The center of gravity of the illumination can be calculated by performing image processing on the image captured by the camera and the light receiving point of the LED illumination in the camera sensor can be obtained from the coordinates of the center of gravity.When the number of LED illumination captured by the camera is three, it is possible to calculate the coordinates of the camera with respect to the nearest illumination, that is, the coordinates of the robot from the light receiving point in the camera sensor and the focal length of the camera and the interval of the LED illumination.

Extraction of light source area
In order to calculate the light receiving position of the illumination in the camera sensor based on the image captured by the camera, it is necessary to extract the light source region by illumination by image processing.In order to extract the light source area, at first binarization processing is performed.Since the pixel value of the light source area is sufficiently higher than that of other areas, it is not affected much by sunlight or the like.Therefore, the light source region can be stably extracted.
However, light other than the ceiling light source may be extracted due to reflection of the illumination or the like.Therefore, noise is removed by the opening process.Opening processing is one of expansion and contraction processing.The expansion process is a process of replacing white pixels with white even if there is one pixel around the pixel of interest.On the contrary, the contraction process is a process of replacing the pixel of interest with black if there is a black pixel even for one pixel around the pixel of interest.
Combining this is the opening process, and noise can be removed by expanding after contraction.
After extracting the light source region by the above processing, each region is distinguished by labeling processing and numerical values are assigned.If the number of extracted light source regions is less than three, change the camera direction to change the image capturing range and adjust the light source region so that there are three light source regions.In addition, it is assumed that the camera only performs panning (turning in the horizontal direction) and tilting (changing the direction in the vertical direction).Also, when the number of light source areas is three or more, use the fact that the closer the light source area is to the camera, the larger the light source area is selected to select three.

Calculation of light receiving position in camera sensor
When three light source regions can be extracted, the center of gravity of the light source region is obtained.

Coordinate calculation with the center of the camera sensor as the origin A) Coordinate system with the center of the camera sensor as the origin
Considering the coordinate system with the center of the camera sensor as the origin, the positional relationship between the light source and its light receiving point is as shown in Figure 2.6.Figures 2.7 (a) and 2.7 (b) show the positional relationship as viewed from the  ̅ −  ̅ plane and the positional relationship seen from the  ̅ −  ̅ plane respectively.Using this relationship, first, the coordinates of the light source A, the light source B, and the light source C are calculated with respect to the coordinate system with the center of the sensor of the camera as the origin.

B) Use of Pythagorean theorem
Consider the light sources at both ends of the three light sources.As shown in Figure 2.8, it can be seen that the theorem of three squares holds for the light sources at both ends.Therefore, the following equation can be obtained.In the equation (2.1), ∆ is the distance between the light sources and is a known constant.Since   ，  ，   ，  ，  ，  are unknown variables, by solving the equations for these unknown variables, the coordinates of each light source with respect to the camera can be obtained.In this paper, we will solve the equation for the variable   .The process of finding each variable is shown below.

C) Use of similarity relationship of triangle
Paying attention to the positional relationship between the light source A and the corresponding light receiving point on the camera sensor, it can be seen that a similarity relationship of triangles is established as shown in Figure 2.9.Likewise, triangle similarity relationship holds for the light source B and the light source C. Therefore, the following expression holds.(2.4)

D) Utilization of position relation of light source
The variables   and   are obtained using the fact that the set of three light sources are arranged at regular intervals.Since the light source B is arranged at the center of a set of three light sources, the following equation can be obtained.

𝐿 𝐵 =
+   2 (2.5) ) When these equations are transformed, finally the equations necessary for the measurement are obtained as follows from (2.2) (2.3) (2.5) and as follows.Similarly, the distance   from the light source B and the distance   from the light source C can also be obtained.Then, it is understood that the one with the shortest distance is the lighting closest to the camera.

Calculation of the coordinates of the camera with the closest light source as the origin A) A coordinate system with the light source as the origin
In Section 2.2.3, we considered the coordinate system with the center of the camera sensor as the origin.However, in order to specify the position of the robot, it is necessary to obtain the position of the camera in the coordinate system with the light source closest to the camera as the origin.Therefore, it is necessary to convert from the coordinate system with the center of the camera sensor as the origin to the coordinate system with the light source as the origin.

B) Parallel displacement
To move in parallel as shown in Figure 2.11.At this time, let the center of the camera sensor be point P.As shown in Figure 2.11, the coordinates are converted as shown below.
( ′ ,  ′ ,  ′ ) = (0 −   , 0 −   , 0 −   ) (2.14) When these equations are deformed, they are finally expressed as follows.(2.34)Therefore, it is possible to obtain the coordinates of the center of the camera sensor when the nearest illumination is the origin, and the position of the robot can be detected.

Verification of accuracy when the camera position is changed
The camera angle and Z coordinate were fixed and the accuracy of position detection in the X axis direction and the Y axis direction was verified.Illumination was installed at 100 [cm] intervals as shown in According to Table 3.1, since the position detection error in each axis direction is 8.2 [cm] at the maximum, it is considered to be effective for self-position detection of the robot.As a cause of the error, image distortion due to the lens of the camera may be considered.As shown in Figure 3.3, in the case of x = 80 [cm] which was relatively small in error, the light source is shown in the center of the screen, but in the image with x = 140 [cm] where the error was large, the light source It is reflected toward the edge of the screen.Since the distortion caused by the lens of the camera becomes larger towards the edge of the screen, we believe that its influence leads to an error in the measurement result.In order to correct distortion, it is cited as a measure to correct distortion using camera calibration.Moreover, when changing the orientation of  the camera, it is thought that error can be reduced by controlling so that the light source appears in the center of the image.

Verification of accuracy when changing the orientation of the camera
We fixed the position of the camera and verified the accuracy of the position detection in the X axis direction and the Y axis direction when changing the shooting range by changing the direction.The camera was fixed at (x, y, z) = (70, 0, -261).The captured image is the six images shown in Figure 3.4.The results at this time are shown in Table 3.2 and Table 3.3.
From the table, the effectiveness of the position detection accuracy was shown even when changing the angle of the camera.

Conclusion
In this paper, we examined the position detection method by the camera and showed the effectiveness of the position detection accuracy by carrying out the experiment.However, the accuracy of the position detection by the camera is related to the distortion of the image by the lens of the camera.Therefore, in the future, in order to improve the accuracy of position detection, it is necessary to correct distortion.
Figure 2.1 Image diagram

Figure 2 .
4 shows an example of the imaging range of the camera.

Figure 2 .Figure 2 . 7
Figure 2.5 The center of gravity of the light source area and the corresponding light receiving point of the camera sensor (a) After extracting light source area

Figure 2
Figure 2.10 Distance from light source A to the origin Figure 3.1 (a).The height of the floor to the ceiling is 266[cm], and the camera moves the height of 5[cm] from the floor, so z = -261[cm].The measurement points are shown in Figure 3.1 (b).The results at this time are shown in Figure 3.2 and Table 3.1.The experimental results are shown in Figure 3.2.
Figure 2.13 Rotation around Z axis Figure 3.3 Comparison of images

Figure 3
Figure 3.4 Used image