Mobile body position determination device, method and system

Abstract

The present invention provides a mobile body position determination device, a mobile body position determination method and a mobile body position determination system. The mobile body position determination device comprises a communication unit (203) and at least one processor, wherein the at least one processor performs the following processing: from a plurality of light-emitting devices whose coordinate positions in the three-dimensional space are known, including two light-emitting devices arranged at arbitrary positions with different heights in the three-dimensional space, an image including at least two light-emitting devices captured by a camera installed on the mobile body is obtained via the communication unit, the coordinate positions of the at least two light-emitting devices in the image are obtained based on the obtained image, and at least one of the coordinate position of the mobile body in the three-dimensional space and the direction the mobile body is facing is determined based on the coordinate position in the three-dimensional space and the coordinate position in the image.

Description

Technical Field

[0001] The present invention relates to a mobile body position determination device, a mobile body position determination method and a mobile body position determination system. Background Art

[0002] Conventionally, there is a technique in which an imaging device mounted on a moving object captures an image of a marker that is a light-emitting device in a space and calculates the position and orientation of the moving object in a three-dimensional space from the captured image (for example, Japanese Patent Application Laid-Open No. 2014-157051).

[0003] However, the above-mentioned technology has the restriction that the markers must be arranged on the same plane, and the degree of freedom in setting the markers and, therefore, the degree of freedom in calculating the positions and orientations is low. Summary of the invention

[0004] One advantage of the present invention is that the degree of freedom in calculating the position and orientation is improved.

[0005] The mobile body position determination device disclosed in the present invention performs the following processing: from a plurality of light-emitting devices whose coordinate positions in the three-dimensional space are known, including two light-emitting devices set at arbitrary positions with different heights in the three-dimensional space, the coordinate positions in the image of the at least two light-emitting devices are obtained based on an image including the at least two light-emitting devices captured by a camera device installed on the mobile body, and at least the coordinate position of the mobile body in the three-dimensional space and any one of the directions the mobile body is facing are determined based on the coordinate positions in the three-dimensional space and the coordinate positions in the image. BRIEF DESCRIPTION OF THE DRAWINGS

[0006] Figure 1 This is a diagram showing an example of a visible light communication system involved in an embodiment of the present invention.

[0007] Figure 2 This is a diagram showing an example of the structure of a server.

[0008] Figure 3 This is a diagram showing an example of the arrangement of cameras and markers in a space.

[0009] Figure 4 This is a diagram showing an example of a marker three-dimensional position table.

[0010] Figure 5 This is a diagram showing an example of camera internal parameters.

[0011] Figure 6 (a) is a diagram showing an example of an image before distortion correction. Figure 6 (b) is a diagram showing an example of an image after distortion correction.

[0012] Figure 7 This is a diagram showing an example of the image position and three-dimensional position of a marker.

[0013] Figure 8 This is a diagram showing an example of XY plane vectors.

[0014] Fig. 9 (a) is a diagram showing an example of an XY plane vector before adjustment. Fig. 9 (b) and (c) are diagrams showing an example of the adjusted XY plane vector.

[0015] Fig.10 This is a diagram showing an example of a camera position candidate line.

[0016] Fig.11 This is a diagram showing an example of obtaining the intersection point between the camera position candidate line and the extended vector.

[0017] Fig.12 This is the first flowchart showing an example of the operation of the server.

[0018] Fig.13 This is the second flowchart showing an example of the operation of the server.

[0019] Fig.14 This is the third flowchart showing an example of the operation of the server.

[0020] Fig.15 This is a diagram showing an example of XY plane vectors when there are two cameras. DETAILED DESCRIPTION

[0021] The following describes a visible light communication system as a mobile object position determination device involved in an embodiment of the present invention with reference to the accompanying drawings.

[0022] Figure 1 is a diagram showing the structure of a visible light communication system. Figure 1As shown, the visible light communication system 1 is composed of the following elements: a forklift 100 as a mobile body moving in a space 500; a camera 101a installed on the forklift 100; markers 102a, 102b, 102c as light sources installed on the ceiling, side, etc. of the space 500 (hereinafter, when the markers 102a, 102b, 102c are not limited respectively, they are appropriately referred to as "markers 102"); and a server 200 corresponding to the mobile body position determination device. The marker 102 includes an LED (Light Emitting Diode) not shown. The space 500 is defined by three-dimensional parameters consisting of the coordinates of the X axis (X coordinate), the coordinates of the Y axis (Y coordinate), and the coordinates of the Z axis (Z coordinate), thereby determining the position of an object existing in the space 500. Here, both the X axis and the Y axis are horizontal directions and are orthogonal to each other, and the Z axis is a vertical direction and is orthogonal to the X axis and the Y axis.

[0023] In this embodiment, the camera 101a installed on the forklift 100 captures the image of the sign 102. The sign 102 has a known three-dimensional position in the space 500, performs color modulation with information of various transmission objects, and transmits the information by changing the luminous color along the time series. In addition, the modulation method adopts the following method: the information of the transmission object is converted into bit data, the data has a given regularity, and is allocated in the order of the three primary colors of light (R (red) G (green) B (blue)).

[0024] On the other hand, the server 200 demodulates the changes in luminous color in the light image obtained by continuous photography in a time series by the camera 101a to obtain information emitted by the sign 102, and calculates the three-dimensional position of the camera 101a in the space 500 as the three-dimensional position of the forklift 100.

[0025] Figure 2 2 is a diagram showing an example of the structure of the server 200. Figure 2 As shown, the server 200 includes a control unit 202 , a wireless communication unit 203 , an image processing unit 204 , a memory 205 , an operation unit 206 , a display unit 207 , and a communication unit 208 .

[0026] Figure 3 1 is a diagram showing an example of the arrangement of the camera 101a and the marker 102 in the space 500. In the present embodiment, the server 200 calculates the unknown three-dimensional position of the camera 101a based on the known three-dimensional position of the marker 102 in the space 500.

[0027] The camera 101a captures (receives light) an optical image that enters through a lens (not shown), and converts the image signal within its camera angle of view into digital data to generate an image. In addition, the camera 101a continuously captures and generates images in time, and transmits the continuous images to the server 200 via wireless communication. The wireless communication unit 203 in the server 200 receives the image and outputs it to the image processing unit 204. The position of each pixel or image area in the image is defined by a two-dimensional coordinate plane composed of an X coordinate and a Y coordinate.

[0028] Return again Figure 2 The image processing unit 204 directly outputs the image (digital data) output from the wireless communication unit 203 to the control unit 202, adjusts the image quality and image size so that the image can be displayed on the display unit 207, and outputs it to the control unit 202. In addition, the image processing unit 204 has the function of encoding by compression encoding methods such as JPEG (Joint Photographic Experts Group) and MPEG (Moving Photographic Experts Group), and converting it into a still image file or a moving image file.

[0029] The control unit 202 is composed of a CPU (Central Processing Unit) as a processor. The control unit 202 follows a program stored in the memory 205 (for example, for implementing the following Fig.12 , Fig.13 , Fig.14 The program for controlling the actions of the server 200 shown in the figure executes software processing to control various functions of the server 200.

[0030] The memory 205 is, for example, a RAM (Random Access Memory) or a ROM (Read Only Memory) and stores various information (programs, etc.) used for control, etc. in the server 200 .

[0031] In addition, the memory 205 stores Figure 4 The three-dimensional position table of the signs shown, Figure 5 The camera internal parameters are shown. Figure 4 The three-dimensional position table of the markers shown will be able to uniquely determine the ID (Identification) of each marker 102a, 102b, 102c and establish a correspondence between the three-dimensional position (X coordinate, Y coordinate, Z coordinate) of the markers 102a, 102b, 102c in the space 500.

[0032] in addition, Figure 5 The internal parameters of the camera shown include the following: image distortion correction data used to correct image distortion; the height (Z coordinate) of the camera 101a; a 3×3 matrix representing the rotation of the camera 101a with the Z axis as the rotation axis, which is posture data representing the direction in which the camera 101a is facing, namely the camera orientation matrix Rz(θ); and a 3×3 matrix representing the rotation of the camera 101a with the X axis as the rotation axis and the Y axis as the rotation axis, which is posture data representing the direction in which the camera 101a is facing, namely the camera setting matrix Rc.

[0033] Return again Figure 2 The operation unit 206 includes a keypad, function keys, etc., and is an interface for inputting the user's operation content. The display unit 207 includes, for example, an LCD (Liquid Crystal Display), a PDP (Plasma Display Panel), an EL (Electroluminescence) display, etc. The display unit 207 displays an image in accordance with the image signal output from the control unit 202. The communication unit 208 is, for example, a LAN (Local Area Network) card. The communication unit 208 communicates with an external communication device.

[0034] The control unit 202 includes functions of an image acquisition unit 230 , a marker identification unit 232 , an image position correction unit 234 , and a three-dimensional position acquisition unit 236 .

[0035] The image acquisition unit 230 acquires the image from the image processing unit 204. The marker identification unit 232 detects the position of the marker 102 in the image (image position: X coordinate and Y coordinate in the two-dimensional plane corresponding to the image). Here, the markers 102a, 102b, and 102c in the space 500 emit light modulated with an ID that can uniquely identify themselves and cyclically changes in a pattern of three colors of R (red), G (green), and B (blue).

[0036] The marker identification unit 232 detects the circling three-color pattern light contained in each image obtained by the camera 101a. Furthermore, the marker identification unit 232 attempts to detect the ID corresponding to the three-color light pattern, and further attempts to demodulate the ID. When the ID is detected, the marker identification unit 232 obtains the image position of the marker 102 corresponding to the ID in the image.

[0037] Furthermore, the marker identification unit 232 obtains the three-dimensional position in the space 500 of the marker 102 corresponding to the obtained ID by referring to the marker three-dimensional position table stored in the memory 205 .

[0038] The image position correction unit 234 corrects the image position of the mark 102. Specifically, Figure 6 As shown in (a) of FIG. 6 , a distortion occurs in the normal image 601a, and the longer the distance from the center of the image, the greater the distortion. For this purpose, the image position correction unit 234 uses Figure 5 The image distortion correction data of the camera internal parameters is used to correct the distortion. The image distortion correction data is data in which the correction amount increases as the distance from the center of the image increases. Figure 6 As shown in (b) of FIG. 6 , the distortion is corrected to form an image 601b in which the positions (image positions) of the markers 102a, 102b, and 102c in the image 601b are corrected.

[0039] In addition, the image position correction unit 234 is set so that the markers 102 existing at the four corners of the image after distortion correction are not used when acquiring the three-dimensional position of the camera 101a. Figure 6 As shown in (b), triangular invalid areas 603a, 603b, 603c, and 603d are set at the four corners of the image 601b. Since the mark 102c exists in the invalid area 603a, the mark 102c is not used when obtaining the three-dimensional position of the camera 101a, and only the marks 102a and 102b are used when obtaining the three-dimensional position of the camera 101a.

[0040] The three-dimensional position acquisition unit 236 acquires the three-dimensional position of the camera 101a in the space 500. Specifically, the three-dimensional position acquisition unit 236 uses Figure 7 The three-dimensional position C = (cx, cy, cz) of the camera 101a in the space 500 is calculated by a known method based on the image position 610a of the marker 102a and the image position of the marker 102b in the image area 700 shown, the three-dimensional position of the marker 102a and the three-dimensional position of the marker 102b obtained by referring to the three-dimensional marker table, and the posture data stored in the memory 205, namely the camera orientation matrix Rz(θ) and the camera setting matrix Rc.

[0041] Specifically, the three-dimensional position acquisition unit 236 calculates the three-dimensional direction vector (vx1, vy1, vz1) from the camera 101a to the marker 102a based on the three-dimensional position of the marker 102a and the internal parameters and posture data of the camera in the coordinate system of the three-dimensional space centered on the camera 101a, i.e., the camera reference relative system, and calculates the three-dimensional direction vector (vx2, vy2, vz2) from the camera 101b to the marker 102 based on the image position and posture data of the marker 102b.

[0042] Next, the three-dimensional position acquisition unit 236 calculates an XY plane vector V1 that projects the three-dimensional direction vector (vx1, vy1, vz1) onto the XY plane based on the height difference between the mark 102a and the camera 101a and the camera setting matrix Rc1 in order to apply the three-dimensional direction vector (vx1, vy1, vz1) from the camera 101a to the mark 102a in the three-dimensional space. Similarly, the three-dimensional position acquisition unit 236 calculates an XY plane vector V2 that projects the three-dimensional direction vector (vx2, vy2, vz2) onto the XY plane based on the height difference between the mark 102b and the camera 101a and the camera setting matrix Rc2 in order to apply the three-dimensional direction vector (vx2, vy2, vz2) from the camera 101a to the mark 102b in the three-dimensional space. Figure 8 It is a diagram showing an example of XY plane vectors V1 and V2.

[0043] Furthermore, the three-dimensional position acquisition unit 236 adjusts the XY plane vector V1 and the XY plane vector V2. This adjustment is to reduce the error in the calculated three-dimensional position C = (cx, cy, cz) of the camera 101a in the space 500 due to the error of the rotation angle θ around the Z axis represented by the camera orientation matrix Rz(θ).

[0044] For example, Fig. 9 As shown in (a), on the XY plane, when the positional relationship between the camera 101a and the markers 102a, 102b and the XY plane vector V1 and the XY plane vector V2 are inconsistent, the three-dimensional position acquisition unit 236 adjusts the XY plane vector V1 and the XY plane vector V2.

[0045] For example, the three-dimensional position acquisition unit 236 attempts to make the positional relationship between the camera 101a and the markers 102a and 102b consistent with the XY plane vector V1 and the XY plane vector V2 by adjusting the rotation angle θ around the Z axis represented by the camera orientation matrix Rz(θ).

[0046] In addition, due to the Fig. 9 In (a), the XY plane vector V1 is longer than the XY plane vector V2. Therefore, the three-dimensional position acquisition unit 236 regards that the error on the XY plane vector V1 is larger, as shown in FIG. Fig. 9 As shown in (b), the length and direction of the XY plane vector V2 are not adjusted to make the positional relationship between the camera 101a and the mark 102b consistent with the XY plane vector V2. On the other hand, the length and direction of the XY plane vector V1 are adjusted to make the positional relationship between the camera 101a and the mark 102b consistent with the XY plane vector V2.

[0047] In addition, the three-dimensional position acquisition unit 236 is as follows Fig. 9 As shown in (c), the lengths and directions of both XY plane vector V1 and XY plane vector V2 are adjusted by making the ratio of the adjustment amount of the length and direction of XY plane vector V1 and the ratio of the length of XY plane vector V1 and XY plane vector V2 consistent, so that the positional relationship between camera 101a and marker 102b is consistent with XY plane vector V2.

[0048] The three-dimensional position acquisition unit 236 uses the adjusted XY plane vectors V1, V2 and the camera orientation matrix Rz(θ) to calculate the three-dimensional direction vector (vx1, vy1, vz1), and then calculates the three-dimensional position C = (cx, cy, cz) of the camera 101a in the space 500, and calculates the orientation of the camera 101a based on the camera orientation matrix Rz(θ) and the camera setting matrix Rc.

[0049] Furthermore, the three-dimensional position acquisition unit 236 selects the best three-dimensional position C = (cx, cy, cz) among the three-dimensional positions C = (cx, cy, cz) of the camera 101a stored in the memory 205, and sets it as the final three-dimensional position of the camera 101a (the coordinate position of the moving object). The so-called best three-dimensional position is, for example, the position where the corresponding re-projection error is minimized among the three-dimensional positions C of the camera 101a, or the position calculated by weighted average of the inverse of the re-projection error. The three-dimensional position acquisition unit 236 stores the obtained three-dimensional position and orientation of the camera 101a in the memory 205 in association with the ID of the marker 102 included in the image.

[0050] In addition, when the camera 101a initially captures two or more marks 102 but then captures only one mark 102 due to the movement of the camera 101a, the final three-dimensional position and orientation of the camera 101a are calculated based on the image position of the one mark 102.

[0051] Specifically, the three-dimensional position acquisition unit 236 obtains a three-dimensional direction vector from the camera 101a to the marker 102 in a camera reference relative system, which is a coordinate system of a three-dimensional space centered on the camera 101a. The three-dimensional direction vector is obtained based on the image position and posture data of the marker 102. Fig.10 As shown, if the three-dimensional direction vector V100 is obtained, the position of the camera 101a in the three-dimensional space (three-dimensional position) is directly below the marker 102, and is an arbitrary position on the camera position candidate line 710, which is the outer edge of the circle with position C10 as the center on the same plane (XY plane) as the camera 101a.

[0052] Furthermore, the three-dimensional position acquisition unit 236 obtains the inclination of the three-dimensional direction vector with respect to the XY plane, and obtains the horizontal distance (distance on the XY plane) between the camera 101a and the marker 102 based on the inclination and the height difference between the camera 101a and the marker 102.

[0053] Next, the three-dimensional position acquisition unit 236 obtains the three-dimensional position of the camera 101a corresponding to the ID of the marker 102 stored in the memory 205 and the historical record of the orientation. Fig.11 As shown in the figure, a movement vector V110 is obtained with the three-dimensional position of the camera 101a obtained by the previous measurement (previous position) P1 as the starting point and the three-dimensional position of the camera 101a obtained by the previous measurement (previous position) as the end point, and an extension vector V120 is obtained by extending the movement vector V110. Furthermore, the three-dimensional position acquisition unit 236 uses the intersection point C20 of the extension vector V120 and the camera position candidate line 710 as the current three-dimensional position C20 of the camera 101a.

[0054] Next, the three-dimensional position acquisition unit 236 obtains the change in the orientation of the camera 101a from the lateral movement (movement on the XY plane) of the mark 102 in the image, and adds the change in the orientation to the orientation in the previous measurement to obtain the current orientation of the camera 101a. Furthermore, the three-dimensional position acquisition unit 236 stores the obtained three-dimensional position and orientation of the camera 101a in the memory 205 in association with the ID of the mark 102 contained in the image.

[0055] Next, the operation of the server 200 will be described with reference to the flowchart. Fig.12 , Fig.13 as well as Fig.14 This is a flowchart showing an example of the operation of the server 200 .

[0056] The image acquisition unit 230 in the control unit 202 acquires the image captured by the camera 101a from the image processing unit 204 (step S101). Next, the marker determination unit 232 analyzes the image and attempts to obtain the ID of the marker 102 to determine whether the ID is obtained (step S102). If the ID is not obtained (step S102 "No"), the series of operations ends.

[0057] On the other hand, when the ID is acquired (Yes in step S102 ), the marker identification unit 232 refers to the marker three-dimensional position table to acquire the three-dimensional position in the space 500 of the marker 102 corresponding to the acquired ID (step S103 ).

[0058] The image position correction unit 234 corrects the image distortion based on the image distortion correction data of the camera internal parameters (step S104). Furthermore, the image position correction unit 234 sets the image position so that the mark 102 existing in the invalid area 603a and the like in the distortion-corrected image is not used when acquiring the three-dimensional position of the camera 101a (step S105).

[0059] Next, the three-dimensional position acquisition unit 236 determines whether there are two or more marks 102 in the image (step S106). If there are two or more marks 102 in the image (step S106: Yes), the process proceeds to step S106. Fig.13 In the operation shown, the three-dimensional position acquisition unit 236 calculates a three-dimensional direction vector from the camera 101a to the marker 102 in the camera reference relative system based on the three-dimensional position of the marker 102a, the camera internal parameters, and the posture data (step S201).

[0060] Next, the three-dimensional position acquisition unit 236 calculates an XY plane vector that projects the three-dimensional direction vector onto the XY plane based on the height difference between the marker 102 and the camera 101 a and the camera installation matrix Rc (step S202 ).

[0061] Next, the three-dimensional position acquisition unit 236 selects two XY plane vectors having two markers 102 as end points among the markers 102 of two or more points (step S203 ).

[0062] Next, the three-dimensional position acquisition unit 236 adjusts the two XY plane vectors (step S204). Next, the three-dimensional position acquisition unit 236 uses the adjusted XY plane vectors and the camera orientation matrix Rz(θ) to calculate the three-dimensional position of the camera 101a in the space 500, and calculates the orientation of the camera 101a based on the camera orientation matrix Rz(θ) and the camera setting matrix Rc, and stores it in the memory 205 together with the ID of the marker 102 in the image (step S205).

[0063] Furthermore, the three-dimensional position acquisition unit 236 determines whether the processing of step S203 to step S205 is completed for all combinations of two XY plane vectors (step S206). If it is not completed (step S206 "No"), the operation after step S203 is repeated. On the other hand, if it is completed (step S206 "Yes"), the three-dimensional position acquisition unit 236 selects the best three-dimensional position and orientation of the camera 101a (step S207).

[0064] On the other hand, Fig.11In step S106, if there are no more than two marks 102 in the image (step S106 "No"), that is, if there is only one mark 102 in the image, the process proceeds to step S106. Fig.14 In the operation shown, the three-dimensional position acquisition unit 236 determines whether the ID of the marker 102 at this point has been acquired in the past when the three-dimensional position and orientation of the camera 101a were acquired (step S301). If the ID has not been acquired (step S301 "No"), a series of operations are terminated.

[0065] On the other hand, when the ID has been acquired (step S301: Yes), the three-dimensional position acquisition unit 236 obtains the three-dimensional direction vector from the camera 101a to the mark 102 in the camera reference relative system, which is a coordinate system of the three-dimensional space centered on the camera 101a. Furthermore, the three-dimensional position acquisition unit 236 obtains the inclination of the three-dimensional direction vector with respect to the XY plane, and obtains the horizontal distance (the distance on the XY plane) between the camera 101a and the mark 102 based on the inclination and the height difference between the camera 101a and the mark 102 (step S302).

[0066] Next, the three-dimensional position acquisition unit 236 obtains the movement vector from the three-dimensional position of the camera 101a obtained by the previous measurement (previous position) to the three-dimensional position of the camera 101a obtained by the previous measurement (previous position) based on the historical record of the three-dimensional position and orientation of the camera 101a corresponding to the ID of the mark 102 stored in the memory 205, and obtains the extended vector that extends the movement vector (step S303).

[0067] Furthermore, the three-dimensional position acquisition unit 236 sets the intersection point of the extended vector and the camera position candidate line as the current three-dimensional position of the camera 101 a (step S304 ).

[0068] Next, the three-dimensional position acquisition unit 236 obtains the change in the orientation of the camera 101a from the lateral movement of the mark 102 in the image, and adds the change in the orientation to the orientation in the previous measurement to set it as the current orientation of the camera 101a (step S305). Furthermore, the three-dimensional position acquisition unit 236 stores the obtained three-dimensional position and orientation of the camera 101a and the ID of the mark 102 contained in the image in the memory 205 in association with each other (step S306).

[0069] Thus, in this embodiment, when the server 200 obtains an image obtained by photographing the sign 102 by the camera 101a, the position of the sign 102 is corrected by correcting the distortion of the image, and the image position of the sign 102 in the corrected image is converted into the three-dimensional position of the camera 101a (forklift 100). In this embodiment, there is no restriction on the installation position of the sign 102, and the degree of freedom of calculation of the three-dimensional position and orientation of the forklift 100 can be increased.

[0070] In addition, in this embodiment, considering that the longer the distance from the center of the image, the greater the distortion of the image, the server 200 uses image distortion correction data with a larger correction amount as the distance from the center of the image is longer to perform image correction, and does not use the marker 102 such as the invalid area 603a existing at the four corners of the image when acquiring the three-dimensional position of the camera 101a. This can improve the calculation accuracy of the three-dimensional position and orientation of the camera 101a.

[0071] In addition, in this embodiment, the server 200 adjusts the XY plane vector to reduce the error in the three-dimensional position of the camera 101a due to an error in the rotation angle θ around the Z axis represented by the camera orientation matrix Rz(θ).

[0072] In addition, in the present embodiment, the best three-dimensional position and orientation of the camera 101a are selected or acquired based on the three-dimensional position and orientation acquired for each combination of the two markers 102, so that the calculation accuracy of the three-dimensional position and orientation of the camera 101a can be improved.

[0073] In addition, in this embodiment, even if there is only one mark 102 in the image, the current three-dimensional position and orientation of the camera 101a can be calculated when the three-dimensional position and orientation of the camera 101a were calculated in the past using the three-dimensional position of the mark 102. Therefore, the versatility of the calculation of the three-dimensional position and orientation of the camera 101a can be improved.

[0074] In addition, in this embodiment, since each marker 102 has an independent ID and the number of IDs of the markers 102 can be extremely large, a large number of markers 102 with different IDs can be arranged in a very wide range. Therefore, in the space 500, regardless of the previous state, the absolute distance of the marker 102 can be immediately obtained at the time when the ID of the marker 102 at two points is received.

[0075] In addition, the present invention is not limited to the description and drawings of the above-mentioned embodiments, and changes and the like can be appropriately added to the above-mentioned embodiments and drawings.

[0076] In the above embodiment, only the camera 101a is used, but multiple cameras may be used. For example Fig.12 As shown, when camera 101b is provided in addition to camera 101a, camera 101a photographs mark 102a, camera 101b photographs mark 102b, and an XY plane vector V3 from camera 101a to camera 101b in the XY plane is specified, thereby the three-dimensional position of the forklift 100 can be calculated with good accuracy.

[0077] In the above-mentioned embodiment, the sign 102 transmits the ID by changing the luminous color along the time series, but the three-dimensional position of the sign 102 may also be transmitted. In this case, the server 200 can detect the three-dimensional position of the sign 102 corresponding to the luminous pattern. Figure 4 The three-dimensional position table of the markers shown.

[0078] In addition, the symbol 102 is not limited to an LED, and the light source may be formed as a part of an LCD, a PDP, an EL display, or the like constituting a display device, for example.

[0079] In addition, the server 200 may be any device as long as it can obtain images from the cameras 101 a and 101 b.

[0080] In addition, in the above-mentioned embodiment, the executed program is stored in a computer-readable recording medium such as a floppy disk, CD-ROM (Compact Disc Read-Only Memory), DVD (Digital Versatile Disc), MO (Magneto-Optical Disc) and distributed, and a system for executing the above-mentioned processing can also be constructed by installing the program.

[0081] Alternatively, the program may be stored in a disk device or the like of a predetermined server on a network such as the Internet, and may be superimposed on a carrier wave and downloaded.

[0082] In addition, when the above functions are shared and implemented by an OS (Operating System) or when the above functions are implemented by cooperation between an OS and an application, only the portion other than the OS may be stored in a medium or downloaded.

[0083] Although preferred embodiments of the present invention have been described above, the present invention is not limited to the specific embodiments and is intended to include the inventions described in the claims and their equivalents.

Claims

1. A mobile body position determination device, characterized in that: It has a communication unit and at least one processor. The at least one processor performs the following processing: An image including at least two light emitting devices captured by a camera installed on a moving body is obtained through the communication unit from among a plurality of light emitting devices including two light emitting devices disposed at arbitrary positions at different heights in the three-dimensional space whose coordinate positions in the three-dimensional space are known, obtaining the coordinate positions of the at least two light emitting devices in the image based on the obtained image, determining at least one of the coordinate position of the moving object in the three-dimensional space and the direction the moving object is facing based on the coordinate position in the three-dimensional space and the coordinate position in the image, In order to determine the coordinate position or orientation of the moving body, control is performed so that the coordinate positions of at least two light-emitting devices existing in the image, excluding the coordinate positions of the light-emitting devices existing in the outer edge portion of the image, are selected from the coordinate positions of the plurality of light-emitting devices existing in the image, and the coordinate positions of the at least two selected light-emitting devices are used.

2. The mobile body position determination device according to claim 1, characterized in that: The at least one processor performs the following processing: The at least two light emitting devices that emit light with different light emission patterns are identified based on a plurality of images including the image and having different imaging timings.

3. The mobile body position determination device according to claim 1 or 2, characterized in that: The at least one processor performs the following processing: Acquire a plurality of three-dimensional direction vectors starting from the focus of the camera device and ending at the coordinate position in the image of the at least two light-emitting devices, and posture data indicating the direction in which the camera device is facing, Based on the obtained multiple three-dimensional direction vectors and the height difference of the at least two light-emitting devices in the three-dimensional space, the distances in the three-dimensional space from the camera device to the at least two light-emitting devices and the azimuth of the optical axis toward which the camera device is facing are obtained.

4. The mobile body position determination device according to claim 3, characterized in that: The at least one processor performs the following processing: The longer distance among the obtained distances is adjusted.

5. The mobile body position determination device according to claim 3, characterized in that: The at least one processor performs the following processing: The distances are adjusted in accordance with a ratio of a deviation from a coordinate position in the three-dimensional space calculated based on the plurality of three-dimensional direction vectors and the acquired distances.

6. The mobile body position determination device according to claim 1 or 2, characterized in that: The at least one processor performs the following processing: determining whether there are at least two light emitting devices other than the light emitting device existing in the outer edge portion in the image, When it is determined that there are at least two light-emitting devices other than the light-emitting device present at the outer edge, the coordinate position of the moving body in the three-dimensional space and the direction in which the moving body is facing are determined based on the coordinate position in the three-dimensional space and the internal parameters and posture data of the camera device, When it is determined that at least two light emitting devices other than the light emitting device present at the outer edge portion are not present, the coordinate position of the moving object is determined based on the calculated movement vector of the moving object, In a case where the image obtained after obtaining the first image representing an image including the first light-emitting device and other light-emitting devices is a second image including only the first light-emitting device and not any other light-emitting devices, the coordinate position of the moving body is determined based on the calculated movement vector of the moving body.

7. The mobile body position determination device according to claim 1 or 2, characterized in that: The at least one processor performs the following processing: The coordinate position of the moving object is determined based on the images acquired from the plurality of imaging devices mounted on the moving object.

8. A mobile position determination device, characterized in that: It has a communication unit and at least one processor. The at least one processor performs the following processing: From among a plurality of light emitting devices whose coordinate positions in the three-dimensional space are known, including two light emitting devices disposed at arbitrary positions in the three-dimensional space, a first image representing an image including a first light emitting device and other light emitting devices, captured by a camera device mounted on a mobile body, is obtained via the communication unit; determining at least one of a coordinate position of the moving object and a direction toward which the moving object is facing in the three-dimensional space based on the acquired first image, calculating a motion vector of the moving object based on a second image acquired after the first image is acquired, When the second image includes only the first light emitting device and does not include any other light emitting device, the coordinate position of the moving object is determined based on the calculated motion vector, In order to determine the coordinate position or orientation of the moving body, control is performed so that the coordinate positions of at least two light-emitting devices existing in the first image and the second image, except for the coordinate positions of the light-emitting devices existing in the outer edge portions of the first image and the second image, are selected, and the coordinate positions of the at least two selected light-emitting devices are used.

9. A method for determining the position of a moving object, characterized in that: The following processing is performed by at least one processor included in the mobile object position determination device: An image including at least two light emitting devices captured by a camera installed on a moving body is obtained through a communication unit from among a plurality of light emitting devices including two light emitting devices disposed at arbitrary positions at different heights in the three-dimensional space whose coordinate positions in the three-dimensional space are known, obtaining the coordinate positions of the at least two light emitting devices in the image based on the obtained image, determining at least one of the coordinate position of the moving object in the three-dimensional space and the direction the moving object is facing based on the coordinate position in the three-dimensional space and the coordinate position in the image, In order to determine the coordinate position or orientation of the moving body, control is performed so that the coordinate positions of at least two light-emitting devices existing in the image, excluding the coordinate positions of the light-emitting devices existing in the outer edge portion of the image, are selected from the coordinate positions of the plurality of light-emitting devices existing in the image, and the coordinate positions of the at least two selected light-emitting devices are used.

10. The method for determining the position of a moving object according to claim 9, wherein: The at least one processor performs the following processing: The at least two light emitting devices that emit light with different light emission patterns are identified based on a plurality of images including the image and having different imaging timings.

11. The method for determining the position of a moving object according to claim 9 or 10, characterized in that: The at least one processor performs the following processing: Acquire a plurality of three-dimensional direction vectors starting from the focus of the camera device and ending at the coordinate position in the image of the at least two light-emitting devices, and posture data indicating the direction in which the camera device is facing, Based on the obtained multiple three-dimensional direction vectors and the height difference of the at least two light-emitting devices in the three-dimensional space, the distances in the three-dimensional space from the camera device to the at least two light-emitting devices and the azimuth of the optical axis toward which the camera device is facing are obtained.

12. The method for determining the position of a moving object according to claim 11, wherein: The at least one processor performs the following processing: The longer distance among the obtained distances is adjusted.

13. The method for determining the position of a moving object according to claim 11, wherein: The at least one processor performs the following processing: The distances are adjusted in accordance with a ratio of a deviation from a coordinate position in the three-dimensional space calculated based on the plurality of three-dimensional direction vectors and the acquired distances.

14. The method for determining the position of a moving object according to claim 9 or 10, characterized in that: The at least one processor performs the following processing: In a case where the image obtained after obtaining the first image representing an image including the first light-emitting device and other light-emitting devices is a second image including only the first light-emitting device and not any other light-emitting devices, the coordinate position of the moving body is determined based on the calculated movement vector of the moving body.

15. The method for determining the position of a moving object according to claim 9 or 10, characterized in that: The at least one processor performs the following processing: The coordinate position of the moving object is determined based on the images acquired from the plurality of imaging devices mounted on the moving object.

16. A mobile body position determination system, characterized in that: have: Two light-emitting devices are arranged at arbitrary positions at different heights in three-dimensional space; A mobile object on which a camera device is mounted; and The moving body position determining device, The mobile body position determination device comprises a communication unit and at least one processor. The at least one processor performs the following processing: acquiring, via the communication unit, an image including at least two light emitting devices captured by the camera device from among a plurality of light emitting devices including the two light emitting devices whose coordinate positions in the three-dimensional space are known, obtaining the coordinate positions of the at least two light emitting devices in the image based on the obtained image, determining at least one of the coordinate position of the moving object in the three-dimensional space and the direction the moving object is facing based on the coordinate position in the three-dimensional space and the coordinate position in the image, In order to determine the coordinate position or orientation of the moving body, control is performed so that the coordinate positions of at least two light-emitting devices existing in the image, excluding the coordinate positions of the light-emitting devices existing in the outer edge portion of the image, are selected from the coordinate positions of the plurality of light-emitting devices existing in the image, and the coordinate positions of the at least two selected light-emitting devices are used.

Images