Information processor, information processing method and program

Abstract

To provide a positioning result of high accuracy, in positioning of an own position.SOLUTION: An information processor performs positioning processing of acquiring a first coordinate position in an image of a light source included in an image imaged by an imaging device (camera) among a plurality of light sources (position indices) where a coordinate position in a three-dimensional space is known, and of deriving at least one of a coordinate position and an azimuth of an own device in the three-dimensional space, on the basis of the coordinate position and the first coordinate position in the three-dimensional space of the light source included in the image. The information processor estimates the height of the imaging device in the three-dimensional space (step S2), performs positioning processing, using information on the estimated height of the imaging device (steps S6 to S8), and acquires a second coordinate position of the light source on the corresponding image, on the basis of a result of the positioning processing, and corrects the height of the imaging device, on the basis of the first coordinate position and the second coordinate position (step S11).SELECTED DRAWING: Figure 7

Description

[Technical field]

[0001] The present invention relates to an information processing device, an information processing method, and a program. [Background technology]

[0002] Conventionally, various positioning systems have been used to estimate a vehicle's own position. For example, a technology has been proposed that estimates the position of a moving object by capturing images of multiple light-emitting devices (position indicators) that transmit a unique ID using visible light communication (such as a three-color RGB pattern) with a camera mounted on the moving object (see Patent Document 1).

[0003] Specifically, the installation position (three-dimensional position) of each light-emitting device is measured in advance, and the coordinate position and ID of the light-emitting device in the image captured by the camera are detected. The position of the moving object is then determined based on the detected coordinate positions in the captured image and the three-dimensional position of each light-emitting device.

[0004] In positioning using the aforementioned visible light communication, for example, by limiting the space in which the vehicle can move as the target for self-positioning to a plane (planar constraint) and fixing the two degrees of freedom (pitch and roll) of height and camera attitude angle, if two position indicators (light source) can be detected from the image obtained by the camera (one point when the position indicators can be detected continuously over time), positioning with three degrees of freedom (horizontal position, horizontal orientation) becomes possible. [Prior art documents] [Patent documents]

[0005] [Patent Document 1] JP 2022-050929 A Summary of the Invention [Problem to be solved by the invention]

[0006] However, on the lunar surface, movement is not expected to occur within a fixed plane; rather, activity will be off-road. Therefore, when estimating self-location, more positioning (e.g., six degrees of freedom) will be required to accommodate variations in height and attitude angle.

[0007] On the other hand, when using visible light communication for positioning over a wide area such as the lunar surface, there is a demand for positioning with as high accuracy as possible while minimizing the burden of installing position indices. However, to stably perform six-degree-of-freedom positioning using known methods, it is necessary to place many position indices at high density so that at least six points are always visible. Given these demands, it has been difficult to arrange position indicators in a wide area so that many of them are always visible to the camera.

[0008] The present invention has been made in consideration of the above-mentioned problems in the conventional technology, and an object of the present invention is to provide highly accurate positioning results in determining a vehicle's own position. [Means for solving the problem]

[0009] In order to solve the above problems, the information processing device of the present invention comprises: Acquire a first coordinate position in an image of a light source included in an image captured by an imaging device, among a plurality of light sources whose coordinate positions in a three-dimensional space are known; a control unit that performs a positioning process to derive at least one of a coordinate position and an orientation of the device itself in the three-dimensional space based on a coordinate position in the three-dimensional space of a light source included in the image and the first coordinate position; The control unit is Estimating a height of the imaging device in the three-dimensional space; performing the positioning process using information on the estimated height of the imaging device; obtaining a second coordinate position of the light source on the image corresponding to the result of the positioning process; The height of the imaging device is corrected based on the first coordinate position and the second coordinate position. Effect of the Invention

[0010] According to the present invention, it is possible to provide highly accurate positioning results in determining the self-location. [Brief description of the drawings]

[0011] [Figure 1] 1 is a diagram showing a configuration of a positioning system according to an embodiment of the present invention. [Figure 2A] FIG. 2 is a diagram showing the attitude angle of a camera in a world coordinate system. [Figure 2B] FIG. 2 is a diagram for explaining the relative positional relationship between two cameras. [Diagram 3] FIG. 1 is a configuration diagram of a moving object. [Figure 4A] 11A and 11B are diagrams illustrating examples of how position indicators appear in a camera coordinate system. [Figure 4B] 11A and 11B are diagrams illustrating examples of how position indicators appear in a camera coordinate system. [Figure 4C] 11A and 11B are diagrams illustrating examples of how position indicators appear in a camera coordinate system. [Figure 5A] FIG. 13 is a diagram showing an example in which a camera coordinate system is transformed into a relative world coordinate system centered on the camera. [Figure 5B] FIG. 13 is a diagram showing an example in which a camera coordinate system is transformed into a relative world coordinate system centered on the camera. [Figure 6A] FIG. 13 is a diagram showing a three-dimensional unit direction vector from a camera to a position index in the camera coordinate system. [Figure 6B] 1 is a diagram showing the coordinate position of a camera and the coordinate positions of each position index in a world coordinate system. FIG. [Figure 7] 13 is a flowchart showing an overall process executed in a moving body. [Figure 8] 13 is a flowchart showing a world coordinate system direction vector calculation process. [Figure 9] 13 is a flowchart showing a camera height estimation process. [Figure 10] FIG. 11 is a diagram for explaining how to obtain the height of each camera. [Figure 11A] 4 is an example of an image captured by a camera. [Figure 11B] 4 is an example of an image captured by a camera. [Figure 12A] FIG. 2 is a diagram showing an example of the positional relationship between two cameras. [Figure 12B] FIG. 13 is a diagram showing a unit direction vector to a position index in the camera coordinate system. [Figure 12C] FIG. 13 is a diagram showing a unit direction vector to a position index in the camera coordinate system. [Figure 12D] 1A and 1B are diagrams showing the coordinate positions of each camera and each position index in the world coordinate system. [Figure 13] 13 is a flowchart showing a process for obtaining a circular arc from a lateral angle of view. [Figure 14] This is a diagram in which position indices corresponding to reception points in an image are projected onto the XY plane in the world coordinate system. [Figure 15] 13 is a flowchart showing a process for determining an azimuth angle from the XY coordinates of a camera. [Figure 16A] FIG. 2 is a diagram showing the positional relationship between a camera and a position indicator. [Figure 16B] FIG. 13 is a diagram showing a position indicator in an image captured by a camera. [Figure 17] 13 is a flowchart showing a second positioning calculation process when two points are captured by the same camera. [Figure 18A] 1 is an example of an image captured by a camera. [Figure 18B] This is a diagram of each position index and the camera projected onto the XY plane in the world coordinate system. [Figure 18C] FIG. 2 is a diagram showing a camera and a position indicator in a world coordinate system. [Figure 19] 13 is a flowchart showing a second positioning calculation process when one point is captured by each of two cameras. [Figure 20A] 4 is an example of an image captured by a camera. [Figure 20B] 4 is an example of an image captured by a camera. [Figure 20C]This is a diagram of each position index and each camera projected onto the XY plane in the world coordinate system. [Figure 21] 13 is a flowchart showing a third positioning calculation process when three points are captured by the same camera. [Figure 22A] 1 is an example of an image captured by a camera. [Figure 22B] This is a diagram of each position index and the camera projected onto the XY plane in the world coordinate system. [Figure 23] 13 is a flowchart showing a third positioning calculation process when two cameras capture two points and one point, respectively. [Figure 24A] 4 is an example of an image captured by a camera. [Figure 24B] 4 is an example of an image captured by a camera. [Figure 24C] This is a diagram of each position index and each camera projected onto the XY plane in the world coordinate system. [Diagram 25] 13 is a flowchart showing a weighted averaging process of positioning results. [Figure 26] FIG. 13 is an image diagram of a weighted average of positioning results. [Figure 27] 13 is a flowchart showing height correction processing by reprojection. [Figure 28A] 1 is a diagram showing actual reception points and reprojected points based on the current positioning result in an image captured by a certain camera; [Figure 28B] FIG. 2 is a diagram showing the positional relationship between a position indicator and a camera. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0012] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, an embodiment of an information processing device according to the present invention will be described with reference to the drawings. Note that the present invention is not limited to the illustrated examples.

[0013] [Positioning system configuration] 1 is a diagram showing the configuration of a positioning system 1 in this embodiment. The positioning system 1 is composed of a moving body 100 and multiple position indices P1, P2, P3, P4,.... Hereinafter, when there is no need to distinguish between the position indices P1, P2, P3, P4,..., they will be referred to as "position index P". The positioning system 1 is assumed to be used on the surface of the moon, which is one of the most suitable application locations.

[0014] In this embodiment, the information processing device according to the present invention is installed in a moving body 100. Here, a lunar rover is assumed as the moving body 100, but it may also be an autonomous rover, a robot, a rover operated by a person (including an automatic driving mode), a mobile construction machine, or the like.

[0015] Cameras C1 and C2 used for receiving visible light communication are attached to the moving body 100. By capturing a position index P in images captured by the cameras C1 and C2, the three-dimensional position and orientation of the moving body 100 are derived.

[0016] In this embodiment, the moving body 100 acquires information related to visible light communication using two front and rear cameras, a camera C1 facing forward in the direction in which the moving body 100 moves, and a camera C2 facing backward. The reason for using two front and rear cameras is to ensure a detection range for visible light communication and to reduce correction errors of optical distortion of lenses, etc. Note that a single camera with a wide-angle lens may be used as long as distortion correction can be performed satisfactorily. Hereinafter, when there is no need to distinguish between the cameras C1 and C2, they will be referred to as "camera C."

[0017] The camera C1 is provided with an attitude angle sensor 40A. The attitude angle sensor 40A outputs the pitch angle and roll angle of the camera C1. The camera C2 is provided with an attitude angle sensor 40B. The attitude angle sensor 40B outputs the pitch angle and roll angle of the camera C2. Hereinafter, when there is no need to distinguish between the attitude angle sensors 40A and 40B, they will be referred to as "attitude angle sensor 40."

[0018] 2A shows the attitude angles θX, θY, and θZ of the cameras C1 and C2 in the world coordinate system. As an initial setting value, the installation yaw angle θZ is measured for each of the cameras C1 and C2 installed on the moving body 100, with the moving direction of the moving body 100 set as azimuth 0. In the case of a typical front and rear two-camera setup, the installation azimuth angle of the camera C1 is 0 degrees, and that of the camera C2 is 180 degrees. The cameras C1, C2, etc. may be referred to as camera Cn. The coordinate position of camera Cn may be referred to as CXn, CYn, CZn, and the attitude angle may be referred to as θXn, θYn, θZn. The three-dimensional coordinate position CZ1 of the camera C1 is the height of the camera C1.

[0019] As shown in FIG. 2B, regarding the relative positional relationship between the cameras C1 and C2, a setting vector V is set which indicates the position of the camera C2 with respect to the camera C1 (reference camera) which serves as the positioning reference. In this way, when multiple cameras C are installed, the relative difference in the spatial position and attitude angle for any two cameras C can be obtained. As a tentative camera height, the height of the camera C1 at the time of installation and the initial value of the ground height are set to 0 m. In order to improve accuracy from the initial state, if the conditions are known in advance, it is desirable to set the initial height.

[0020] On the surface of the moon, which is the operating area of ​​the moving body 100, position indicators P serving as a plurality of light sources are placed at extremely wide intervals. The position index P includes an LED (Light Emitting Diode). The position index P controls light emission or reflection by color modulation or luminance modulation in the wavelength region of visible light, and transmits information to be transmitted. For example, the position index P transmits at least identification information specific to the position index P (light source ID that can uniquely identify itself) by a three-color pattern of R (red), G (green), and B (blue). When each of the multiple position indexes P is installed, the installation position (three-dimensional coordinates) in the world coordinate system (Xw, Yw, Zw) is measured. For each position index P, the correspondence between the light source ID and the three-dimensional coordinates is stored in the light source ID-three-dimensional coordinate table 21 (see FIG. 3) in the moving body 100. Note that, when communication capacity is sufficient, the position index P may transmit its own installation position coordinate information, making it unnecessary to create and refer to the table.

[0021] The position indicator P is placed high enough or on a raised area (high altitude) so that it can be seen from a long distance over the lunar undulations. A solar cell or the like is used as the power source for the position index P. If the position index P is to be operated during the lunar night period (the period facing away from the sun), the position index P shall have a large-capacity charging mechanism corresponding to that period.

[0022] [Configuration of moving body] Fig. 3 is a configuration diagram of the moving body 100. As shown in Fig. 3, the moving body 100 includes a control unit 10, a storage unit 20, cameras C (C1, C2), attitude angle sensors 40 (40A, 40B), a traveling drive unit 50, and the like.

[0023] The control unit 10 is configured with a CPU (Central Processing Unit). The control unit 10 controls each part of the moving object 100 in accordance with a program stored in the storage unit 20.

[0024] The storage unit 20 stores programs executed by the control unit 10, various setting data, and the like. The programs are stored in the storage unit 20 in the form of computer-readable program codes. As the storage unit 20, a hard disk drive (HDD), a solid state drive (SSD), or the like is used.

[0025] The storage unit 20 stores a light source ID-three-dimensional coordinate table 21. In the light source ID-three-dimensional coordinate table 21, for each of a plurality of position indexes P, the light source ID of the position index P is associated with three-dimensional coordinates that are the installation position of the position index P. The light source ID-three-dimensional coordinate table 21 is used for positioning processing by visible light communication.

[0026] The camera C captures an optical image incident through a lens and generates two-dimensional image data. The camera C captures images continuously in time and outputs the continuous image data to the control unit 10.

[0027] The attitude angle sensor 40 is a known two-axis attitude angle sensor, and outputs the pitch angle, roll angle, etc. of the camera C corresponding to the attitude angle sensor 40 to the control unit 10.

[0028] The traveling drive unit 50 drives the wheels for traveling the moving body 100, changes the direction of travel, etc. The traveling drive unit 50 moves the moving body 100.

[0029] The control unit 10 analyzes image data continuously captured by the camera C, and detects the light source ID indicated by visible light communication from each light emitting point (position index P) included in the captured image. For each light emitting point on the captured image, the control unit 10 acquires a pair of the light source ID and the coordinate position of the light emitting point in the captured image (two-dimensional plane).

[0030] The control unit 10 performs a positioning process for the moving body 100 (camera C) based on the coordinate position on the captured image and the known three-dimensional position (the installation position of the position index P) for each light source ID received by visible light communication.

[0031] [Overview of positioning methods using camera visible light communication] The inventor has proposed a positioning method using camera visible light communication. In this method, among the six degrees of freedom (coordinates CX, CY, CZ, and attitude angles θX, θY, θZ) of the camera C, the attitude angles θX, θY (pitch and roll) and the camera height CZ of the camera are known (fixed or determined in advance). The remaining three degrees of freedom, the coordinates CX, CY and the orientation θZ of the camera C, are obtained as unknowns. The present invention also assumes the same. However, although the height CZ is determined prior to the positioning calculation, it is updated sequentially, so the output of the positioning result is four degrees of freedom (CX, CY, CZ, θZ). In this system, the camera attitude angles θX, θY are sequentially observed by the attitude angle sensor 40 and are known, so the output of the entire system is six degrees of freedom.

[0032] Figures 4A, 4B, and 4C show examples of how position indices P1 and P2 appear in the camera coordinate system. The focal position of the camera is the origin of the camera coordinate system, and the optical axis direction is the Z-axis direction. Consider that a perspective projection plane 60 of an erect image of the imaging plane (inverted image) on the negative side of the Z axis is located at a position of the camera's focal length f from the origin toward the positive side of the Z axis. Figure 4C is a projection of the camera coordinate system onto the XY plane as viewed from the negative side of the Z axis, and is equivalent to the image coordinate system.

[0033] With the camera focal length f and the size of one pixel known, a three-dimensional unit direction vector uv (uv1, uv2) in the camera coordinate system directed from the camera focal point to one coordinate point on the image plane (the position of position indicators P1, P2) is found. This three-dimensional unit direction vector uv is converted into a direction vector to each position index P of the camera reference in the world coordinate system and a distance to each position index P.

[0034] Figures 5A and 5B show examples of converting the camera coordinate systems shown in Figures 4A, 4B, and 4C into a relative world coordinate system centered on the camera. Here, (θX, θY, θZ) = (-70 degrees, 0 degrees, 0 degrees). The distances on the XY plane from the origin (camera position) to position indicators P1 and P2 are d1 and d2, respectively. Figure 5A shows the ZY plane projection of the camera-centered relative world coordinate system as seen from the negative side of the X axis (d1 and d2 are ZY projected). A 3D rotation matrix determined from the attitude angles θX, θY (known) of camera C is applied to the 3D unit direction vector uv to obtain a unit vector uv' of the 3D direction vector in the world coordinate system (hereinafter also referred to as the "unit direction vector"). The unit direction vector uv' has a Z-axis rotation of 0 (orientation is undefined), but is a direction vector that reflects the pitch angle and roll angle of camera C.

[0035] FIG. 6A shows three-dimensional unit direction vectors uv1 and uv2 from camera C to position indices P1 and P2 in the camera coordinate system. FIG. 6B shows the coordinate position CamPos of the camera C in the world coordinate system, the coordinate position IDPos1 of the position indicator P1, and the coordinate position IDPos2 of the position indicator P2.

[0036] The three-dimensional unit direction vector uv is multiplied by a rotation matrix (Rzθ) that represents the desired azimuth angle θZ, and then multiplied by the distance d between camera C and position index P. This vector is translated (vector addition) to the coordinate position CamPos(CX,CY,CZ) (CZ is known) where camera C is located. The vector obtained as a result coincides with the vector from the coordinate position CamPos of the camera C to the coordinate position IDPos of the position index P. That is, for the reception result of one position index P, the following formula (1) is obtained. (Rzθ·uv)*d+CamPos=IDPos-CamPos…Equation (1)

[0037] When the matrix of Equation (1) is expanded, three linear equations are obtained. Three equations are obtained for one point (position index P), and the distance d between the camera C and the position index P is introduced as a new variable. Here, the azimuth angle θZ is expressed as a three-dimensional rotation matrix that has only the Z-axis rotation component that corresponds to the azimuth angle on the plane. This three-dimensional rotation matrix Rzθ contains the terms cosθZ and sinθZ. Once cosθZ and sinθZ are found, θZ can be found from the trigonometric formula. In equation (1), the unknown parameters are CX, CY, cosθZ, and sinθZ, as well as the distance d that appears anew for each equation (number of position indices).

[0038] The minimum condition for solving this simultaneous equation is that for N pieces of received position index P information, the number of equations (3 x N) is equal to or greater than the number of unknown parameters (4 + 1 x N). The smallest value of N that satisfies this condition is 2. In other words, when two position indexes P are visible, the following simultaneous equations with two determinants can be obtained. Rzθ·uv1*d1+CamPos=IDPos1-CamPos…Formula (2) Rzθ·uv2*d2+CamPos=IDPos2-CamPos…Equation (3)

[0039] Expanding the matrices of the above equations (2) and (3) results in six simultaneous equations. Since there are six unknown parameters, namely CX, CY, cosθZ, sinθZ, d1, and d2, it is guaranteed that they can be analytically solved as linear first-order equations. Since it is sufficient to simply solve the simultaneous equations, no external parameters such as previous observation data are required in this calculation, making global positioning (single-shot positioning) possible.

[0040] [Problems with previous positioning methods] As described above, the positioning method using camera visible light communication with some degrees of freedom known has the advantage that the number of observed position indices P can be minimized. However, in an off-road environment (where the height CZ of the camera C changes) where the position indicator P is installed over a wide area, positioning can be difficult.

[0041] In positioning of a wide area such as the surface of the moon, the position index P is often seen far away near the horizon, in which case the elevation and depression angles are extremely small. In other words, the position index P in the image captured by camera C may be widely scattered in the lateral direction (horizontal direction), but the scattering in the vertical direction (height and depth direction) is considered to be small. The elevation angle is the angle between the line of sight when looking up and the horizontal plane. The depression angle is the angle between the line of sight when looking down and the horizontal plane. Here, the elevation angle, which is the angle of view in the vertical direction (up and down direction), and the depression angle are collectively referred to as the "elevation and depression angles." When camera C is below the installation height of position index P, it is the elevation angle, and when camera C is above the installation height of position index P, it is the depression angle.

[0042] For example, even if the installation height of the position index P is increased assuming observation from a long distance, when a moving body 100 such as a rover climbs a hill on the lunar surface, the installation height of the position index P and the height of the moving body 100 (camera C) may become almost the same. In the above formula (1), when the Z coordinate of the coordinate position CamPos of the camera C and the coordinate position IDPos of the position index P are the same, the Z component of the direction vector from the camera C to the position index P is zero, which is the same as there being no information brought about by the inclination of the vector. In this case, algebraically speaking, the terms related to Z are simplified and the rank of the determinant is reduced by one, so the number of equations obtained by capturing an image of one position index P becomes two, and the general linear simultaneous equation shown in formula (1) cannot be solved with observation information from two points.

[0043] Furthermore, if the elevation / depression angles when the position index P is viewed from camera C are small, there will be a large error in the distance from camera C to the position index P and the positioning results of camera C. For example, if the difference in elevation between camera C and the position index P is 10 m and the distance between camera C and the position index P is 652 m, a difference of just 1 degree in the elevation / depression angles can result in an error of 300 m or more in the distance to the position index P. This can cause the positioning position of camera C to deviate by 400 m or more, and the orientation of camera C to also have an error of 10 degrees or more. In this way, when the elevation / depression angles relative to the position index P are small, using information on the Z direction of the vector to the position index P results in an increase in error in the positioning calculation, and there is a problem that the positioning calculation itself does not work when the elevation / depression angles are zero.

[0044] Summary of the Invention The control unit 10 acquires a first coordinate position (image coordinate system) within an image of a position index P (light source) included in an image captured by a camera C (imaging device) among multiple position indexes P (light sources) whose coordinate positions in three-dimensional space (world coordinate system) are known. The control unit 10 performs a positioning process to derive at least one of the coordinate position and orientation of the device (moving body 100) in three-dimensional space (world coordinate system) based on the coordinate position in three-dimensional space of the position index P included in the image and a first coordinate position in the image.

[0045] Here, the control unit 10 detects identification information (light source ID) of the position index P contained in the image captured by the camera C and a first coordinate position within the image, and acquires a known coordinate position in three-dimensional space (world coordinate system) corresponding to the detected identification information.

[0046] The control unit 10 estimates the height of the camera C in the three-dimensional space. Specifically, the control unit 10 sets the height of the camera C to a default value the first time, and estimates the height of the camera C from the previous value from the next time. The control unit 10 uses the information on the estimated height of the camera C to perform a positioning process for the camera C (the moving body 100). The control unit 10 obtains a second coordinate position of the position index P on the image corresponding to the result of the positioning process. The control unit 10 corrects the height of the camera C based on the first coordinate position and the second coordinate position.

[0047] Based on the result of the positioning process, the control unit 10 acquires a reprojected point obtained by reprojecting the position index P included in the image onto the image (perspective projection surface) as a second coordinate position. The control unit 10 corrects the height of the camera C based on the reprojection point and the first coordinate position of the position index P included in the image.

[0048] The control unit 10 obtains a first elevation / depression angle, which is an elevation / depression angle from the camera C to the position index P, from a first coordinate position of the position index P included in the image. The control unit 10 calculates a second elevation / depression angle, which is the elevation / depression angle from the camera C to the reprojection point. The control unit 10 obtains the distance between the position index P and the camera C in the horizontal direction based on the coordinate position of the position index P included in the image in the three-dimensional space and the positioning result of the camera C. The control unit 10 calculates a correction value for the height of the camera C based on the obtained distance, the first elevation / depression angle, and the second elevation / depression angle.

[0049] The control unit 10 performs positioning processing using a unit vector (uv') of a three-dimensional direction vector whose starting point is the focus of the camera C and whose ending point is the coordinate position in three-dimensional space of the position index P (light source) included in the image.

[0050] [Movement of moving object] Next, the operation of the moving body 100 will be described. FIG. 7 is a flowchart showing the overall process executed in the moving body 100.

[0051] First, the control unit 10 performs a world coordinate system direction vector calculation process (step S1). The world coordinate system direction vector calculation process is a process of calculating a direction vector in the world coordinate system with an orientation of 0 for each light-emitting point received by visible light communication in an image captured by the camera C.

[0052] Now, with reference to FIG. 8, the world coordinate system direction vector calculation process will be described. The control unit 10 judges whether or not the processing has been completed for all points included in the captured images captured by each camera C (step S21). If processing has not been completed for any point included in the captured image (step S21; NO), the control unit 10 detects a receiving point (light emitting point) corresponding to the unprocessed position index P from the captured image, and acquires the light source ID and coordinate position (image coordinate system) indicated by visible light communication (step S22).

[0053] Next, the control unit 10 corrects the coordinates of the reception point by correcting the distortion of the lens of the camera C (step S23).

[0054] Next, the control unit 10 obtains a unit direction vector uv (see FIG. 6A) toward the reception point in the camera coordinate system (step S24).

[0055] Next, the control unit 10 acquires the measurement results of the pitch angle and roll angle of the camera C from the attitude angle sensor 40 corresponding to the camera C that captured the reception point to be processed. Then, the control unit 10 obtains the current rotation matrix from the acquired pitch angle and roll angle and the installation yaw angle of the camera C (step S25).

[0056] Next, the control unit 10 multiplies the unit direction vector uv in the camera coordinate system by a rotation matrix to obtain a unit direction vector uv' in the world coordinate system (see FIG. 6B) (step S26). After step S26, the process returns to step S21.

[0057] In step S21, if the processing has been completed for all points included in the captured image (step S21; YES), the world coordinate system direction vector calculation processing ends.

[0058] Next, returning to FIG. 7, the control unit 10 performs a camera height estimation process (step S2). Since the fluctuation of height is not very frequent in positioning, an extremely simple prediction method is adopted in this embodiment. There are various known methods for predicting the next value of time-series data from the history of past values, so it is sufficient to combine these methods to appropriately improve the prediction method.

[0059] In reality, the height value is not required for the third positioning calculation process (a positioning method that uses three points included in the captured image) described below, but in order to accommodate the first positioning calculation process or the second positioning calculation process (a positioning method that uses two points included in the captured image) described below, the height of camera C is estimated using a camera height estimation process. Since the height cannot be expected to be accurate in the initial calculation, the first positioning calculation process or the second positioning calculation process is not performed until the corrected height is determined. Furthermore, even if the height is different in the initial state, a certain positioning result with a large error can be initially calculated in the third positioning calculation process, and a relatively accurate height can be obtained by subsequent correction.

[0060] Now, with reference to FIG. 9, the camera height estimation process will be described. The control unit 10 judges whether or not there is a history of positioning results in the storage unit 20 (step S31). If there is a history of positioning results (step S31; YES), the control unit 10 adds the amount of change in height between the previous time and the time before that to the previous height, and sets this as the height CZ1 of the camera C1 (step S32).

[0061] In step S31, if there is no history of positioning results (step S31; NO), the control unit 10 sets a prescribed initial value to the height CZ1 of the camera C1 (step S33).

[0062] After step S32 or step S33, the control unit 10 obtains the height CZn of each camera Cn from the height CZ1 of the camera C1 and the installation vector V for each camera Cn (step S34), as shown in Fig. 10. Specifically, the control unit 10 adds CZ1 to the relative height of C1 and Cn obtained by multiplying the installation vector V for the camera Cn by a rotation matrix indicating the attitude angle (pitch angle and roll angle) of the camera C1, and determines the height CZn of each camera Cn. This completes the camera height estimation process.

[0063] Next, returning to FIG. 7, the control unit 10 determines whether or not two or more light emitting points corresponding to the position index P have been captured from the captured image captured by the camera C (step S3).

[0064] If two or more light emitting points cannot be captured from the captured image (step S3; NO), that is, if only one point is received or there is no reception point, the process proceeds to step S4. The control unit 10 uses a known position estimation method or the like to estimate the coordinate position and orientation of the moving body 100 (camera C) from the history of previous positioning and the coordinate fluctuation of one receiving point (if there is one light emitting point corresponding to the position index P in the captured image) (step S4).

[0065] In step S3, if two or more light emitting points are captured from the captured image (step S3; YES), the control unit 10 creates a list of receiving points (combinations of two or three points) to be used for positioning and the calculation method to be applied (step S5).

[0066] For example, when the captured image includes two points whose elevation and depression angles are equal to or greater than a first threshold, the control unit 10 adds the combination of the two points that satisfy this condition to the list for performing the first positioning calculation process. For example, 7 degrees is used as the first threshold.

[0067] When the captured image includes a point whose elevation / depression angle is equal to or greater than a first threshold and a point whose horizontal angle of view between the point is equal to or greater than a second threshold, the control unit 10 adds the combination of two points that satisfy this condition to a list for performing the second positioning calculation process. For example, 7 degrees is used as the first threshold, and for example, 3 degrees is used as the second threshold. Hereinafter, the horizontal angle of view is referred to as the "lateral angle of view."

[0068] When the captured image includes three points where the lateral angle of view between two points is equal to or greater than the second threshold, the control unit 10 adds the combination of the three points that satisfies this condition to a list for performing the third positioning calculation process.

[0069] Fig. 11A is an example of a captured image 70A captured by the camera C1, and Fig. 11B is an example of a captured image 70B captured by the camera C2. The cameras C1 and C2 have a relative yaw angle of 180 degrees. The captured image 70A includes position indices Pa, Pb, and Pc. The elevation / depression angle of the position index Pb is equal to or greater than the first threshold, and the elevation / depression angles of the position indices Pa and Pc are less than the first threshold. The position indices Pd and Pe appear in the captured image 70B. The elevation and depression angles of the position indices Pd and Pe are less than the first threshold value. Further, the lateral angle of view between the position index Pb and the position index Pc is less than the second threshold value, but the lateral angles of view for the other combinations are equal to or greater than the second threshold value.

[0070] When the captured image 70A shown in FIG. 11A and the captured image 70B shown in FIG. 11B are obtained, there is no combination of two points whose elevation / depression angles are equal to or greater than the first threshold, and therefore there is no combination of reception points that are the subject of the first positioning calculation process.

[0071] For the second positioning calculation process, a combination of two position indices is used: a position index Pb whose elevation / depression angle is equal to or greater than a first threshold, and a position index whose lateral angle of view between the position index Pb and the position index Pb is equal to or greater than a second threshold. Specifically, the combinations (Pa, Pb), (Pb, Pd), and (Pb, Pe) are used as targets for the second positioning calculation process.

[0072] For the third positioning calculation process, a combination of three points whose lateral angle of view is equal to or greater than the second threshold is used. Specifically, the combinations of (Pa,Pb,Pd), (Pa,Pb,Pe), (Pa,Pc,Pd), (Pa,Pc,Pe), (Pa,Pd,Pe), (Pb,Pd,Pe), and (Pc,Pd,Pe) are used as targets for the third positioning calculation process.

[0073] If there are 10 or so receiving points in a captured image, the number of combinations for positioning processing will be enormous, and the effect of improving accuracy will be saturated. Therefore, if you want to keep the amount of calculations constant, it is a good idea to determine an upper limit on the number of "combinations of two or three points" used in positioning processing and set a predetermined priority to eliminate combinations that are unlikely to contribute to positioning. Examples of priorities include "prioritizing combinations of two points with a large elevation / depression angle," "prioritizing combinations of three points with a large horizontal angle of view," and "prioritizing combinations in which the positions of two points are symmetrical in the image (prioritizing two points that are seen equally on both sides of the horizontal center, rather than the center and the side, even if they have the same 45-degree angle of view)."

[0074] Generally, more accurate positioning results can be expected by performing positioning processing for each combination of two or three points and then weighting and averaging the positioning results later, rather than performing positioning processing using all receiving points (light emitting points) contained in the captured image. This is because, due to the nonlinearity of the lens of the real camera C, even if distortion correction is performed, the correction accuracy of the coordinates at the edge of the image is poor, so the influence of errors can be reduced by integrating the positioning results calculated individually for combinations of two or three points, rather than solving simultaneous equations for all points. When integrating the positioning results, methods such as lowering the weight when the reprojection error based on the image coordinates is large can be used.

[0075] After step S5, the control unit 10 performs positioning using the "reception points used for positioning" and the "applied calculation method" based on the created list (steps S6 to S8). Specifically, the control unit 10 performs a first positioning calculation process (step S6), a second positioning calculation process (step S7), and / or a third positioning calculation process (step S8).

[0076] When the position and attitude angle to be measured match between the reference camera C1 and the moving body 100, the positioning result of the reference camera C1 is used as the positioning result of the moving body 100. When the position and attitude angle to be measured do not match between the reference camera C1 and the moving body 100, the positioning result of the moving body 100 can be derived from the positioning result of the reference camera C1 based on the difference.

[0077] First, the first positioning calculation process (step S6) will be described. <First positioning calculation process when two points are captured by the same camera> When two points are captured by the same camera, a solution can be obtained by the linear simultaneous equations (2) and (3) above.

[0078] <First positioning calculation process when two cameras capture one point each> As shown in FIG. 12A, a three-dimensional installation vector from camera C1 to camera C2 is V, and a differential rotation matrix of the attitude angles between camera C1 and camera C2 is R2. FIG. 12B shows a unit direction vector uv1 toward a position index P1 in the camera coordinate system of the camera C1. FIG. 12C shows a unit direction vector uv2 toward the position index P2 in the camera coordinate system of the camera C2. 12D shows the coordinate position CamPos1 of the camera C1, the coordinate position CamPos2 of the camera C2, the coordinate position IDPos1 of the position index P1, and the coordinate position IDPos2 of the position index P2 in the world coordinate system. The distance between the camera C1 and the position index P1 is d1, and the distance between the camera C2 and the position index P2 is d2.

[0079] When the unit direction vector uv1 is multiplied by the rotation matrix (Rzθ) and then multiplied by the distance d1, the vector is translated to the coordinate position CamPos1, and coincides with the vector from CamPos1 to IDPos1, so that equation (4) is obtained. Rzθ·uv1*d1+CamPos1=IDPos1-CamPos1 ...Formula (4)

[0080] The position index P2 captured by the camera C2 is also multiplied by the differential rotation matrix R2 to obtain equation (5). Rzθ·R2·uv2*d2+CamPos2=IDPos2-CamPos2 …Formula (5)

[0081] Moreover, from the relationship between CamPos1 and CamPos2, equation (6) is obtained. CamPos2=CamPos1+(Rzθ·V) …Equation (6)

[0082] In addition, uv1' and uv2' in FIG. 12D are expressed by the following expressions (7) and (8). uv1'=Rzθ·uv1...Equation (7) uv2'=Rzθ·R2·uv2 …Equation (8)

[0083] The CamPos2 term can be eliminated from equations (4) to (6), and the six unknown parameters (CX1, CY1, cosθZ1, sinθZ1, d1, d2) can be solved from the six equations. Even when three or more cameras are used, the coordinate position and orientation of each camera can be obtained by the above method by considering the installation vector from the reference camera C1.

[0084] Here, a process commonly used in the second positioning calculation process and the third positioning calculation process will be described.

[0085] A process of obtaining a circular arc from the horizontal angle of view will be described with reference to Fig. 13. This process utilizes the theorem of circular angles to obtain a circular arc that is a set of positions where camera C can be located. Hereinafter, position indices P1, P2, etc. on the captured image or projection surface may also be referred to as points P1, P2, etc.

[0086] FIG. 14 is a diagram in which position indices P1 and P2 corresponding to reception points in an image are projected onto an XY plane in a world coordinate system. The control unit 10 acquires the lateral angle of view α between the two points P1 and P2 from the lateral azimuth angles within the angle of view of the points P1 and P2 in the captured image (step S41).

[0087] Next, the control unit 10 calculates the coordinates of the vertex E of an isosceles triangle on the XY plane, with the line segment connecting the points P1 and P2 as the base and the apex angle α (step S42). Here, the control unit 10 calculates the coordinates of the vertex E so that the points P1 and P2 satisfy the left-right relationship on the image.

[0088] Next, the control unit 10 obtains a circle that passes through three points P1, P2, and E on the XY plane, and selects an arc on the side of point E (step S43). This arc is an arc that satisfies the condition that point P1 is seen to the left of point P2 and the lateral angle of view between two points P1 and P2 is α. This completes the process of determining a circular arc from the lateral angle of view.

[0089] The process of determining the azimuth angle from the XY coordinates of the camera will be described with reference to FIG. The control unit 10 acquires the coordinates CX, CY of the camera C (step S51).

[0090] Next, the control unit 10 selects a position index P that is the reception point closest to the center in the horizontal direction within the camera's angle of view (step S52). FIG. 16A shows the positional relationship between the camera C and the position indicator P. 16B shows a position index P in an image captured by a camera C. A position index P that is closest to the center of the image in the horizontal direction (lateral direction) in the captured image is selected.

[0091] Next, the control unit 10 obtains an azimuth angle Ac of a vector from the camera C to the point P (step S53). Specifically, the control unit 10 obtains the azimuth angle Ac based on the coordinates (CX, CY) of the camera C acquired in step S51 and the known XY coordinates (PX, PY) of the position index P in the world coordinate system.

[0092] Next, the control unit 10 obtains the horizontal azimuth angle Ap within the field of view to the point P from the captured image (step S54). Note that Ap has a negative value on the left side of the center and a positive value on the right side in the horizontal direction within the field of view.

[0093] Next, the control unit 10 adds the horizontal azimuth angle Ap within the angle of view to the azimuth angle Ac to obtain the azimuth angle CθZ of the camera C (step S55).

[0094] Next, the control unit 10 obtains the installation yaw angle SθZ of the camera C (step S56).

[0095] Next, the control unit 10 adds the installation yaw angle SθZ to the azimuth angle CθZ of the camera C to obtain the azimuth angle θZ of the moving body 100 (step S57). This completes the process of determining the azimuth angle from the XY coordinates of the camera.

[0096] In addition, since the camera distortion correction error occurs toward the edge of the image, in step S52, the point P closest to the center is selected, but it is also possible to calculate all the azimuth angles for each point of the same camera using the above azimuth angle calculation method and take the average or weighted average. For the weighted average, for example, the weight is made smaller the farther from the image center coordinates.

[0097] Next, the second positioning calculation process (step S7) will be described. <Second positioning calculation process when two points are captured by the same camera> The second positioning calculation process (two points and one camera) will be described with reference to FIG. FIG. 18A shows an example of an image captured by a certain camera C. The control unit 10 determines a point where the elevation / depression angle is less than the first threshold as P1, and a point where the elevation / depression angle is equal to or greater than the first threshold as P2 (step S61).

[0098] Next, the control unit 10 acquires the three-dimensional coordinates of the installation positions of the points P1 and P2 (step S62). Specifically, the control unit 10 acquires the three-dimensional coordinates corresponding to the light source ID of each of the points P1 and P2 from the light source ID-three-dimensional coordinate table 21. FIG. 18B is a diagram in which the position indicators P1 and P2 and the camera C are projected onto the XY plane in the world coordinate system.

[0099] Next, the control unit 10 obtains a horizontal angle of view α between points P1 and P2 in the captured image. Then, the control unit 10 obtains an arc A in which the camera C projected onto the XY plane may exist from the XY coordinates of point P1, the XY coordinates of point P2, and the horizontal angle of view α (step S63). Details of the process are the same as those in FIG.

[0100] Next, the control unit 10 obtains the distance d between the camera C and the point P2 on the XY plane by trigonometry from the direction vector from the camera C to the point P2 (step S64).

[0101] 18C is a diagram showing camera C (CX, CY, CZ) and point P2 (P2X, P2Y, P2Z) in the world coordinate system. The vertical axis is the Z-axis direction, and the horizontal axis is the direction from point P2 to camera C. The distance d can be calculated by equation (9). The Z component of the unit direction vector uv2' directed from camera C to point P2 is defined as uv2'Z.

number

[0102] Next, the control unit 10 obtains a circle B having a radius d and centered on the point P2 on the XY plane (step S65).

[0103] Next, the control unit 10 obtains an intersection point between the arc A and the circle B, and sets the intersection point that satisfies the left-right relationship between the points P1 and P2 as the XY coordinates of the camera C (step S66).

[0104] Next, the control unit 10 obtains the azimuth angle θZ of the camera C from the XY coordinates of the camera C (step S67). Details of the process are the same as those in FIG.

[0105] Here, if the camera C is not the reference camera C1, the control unit 10 obtains the XY coordinates and azimuth angle θZ of the reference camera C1 based on the installation vector from the reference camera C1 to the camera C (step S68). This ends the second positioning calculation process (two points, one camera).

[0106] <Second positioning calculation process when two cameras capture one point each> The second positioning calculation process (two points and two cameras) will be described with reference to FIG. 20A and 20B show examples of images captured by the cameras Ca and Cb. The control unit 10 designates the camera capturing point P1 where the elevation / depression angle is equal to or greater than the first threshold as Ca, and the camera capturing point P2 where the elevation / depression angle is less than the first threshold as Cb (step S71).

[0107] Next, the control unit 10 acquires the three-dimensional coordinates of the installation positions of the points P1 and P2 (step S72). Specifically, the control unit 10 acquires the three-dimensional coordinates corresponding to the light source ID of each of the points P1 and P2 from the light source ID-three-dimensional coordinate table 21. FIG. 20C is a diagram in which the position indices P1 and P2 and the cameras Ca and Cb are projected onto the XY plane in the world coordinate system.

[0108] Next, the control unit 10 obtains the distance d between the camera Ca and the point P1 on the XY plane by trigonometry from the direction vector from the camera Ca to the point P1 (step S73).

[0109] Next, the control unit 10 obtains a circle A having a radius d and centered on the point P1 on the XY plane (step S74).

[0110] Next, the control unit 10 determines a provisional camera position Ca' on the circle A, and obtains a provisional azimuth angle θZa' of the camera Ca (step S75). Details of the process are the same as those in FIG. 15. Here, if there is no history of the previous state, the control unit 10 sets the location of the azimuth 0 as the provisional camera position Ca'. On the other hand, if there is a history of the previous state, the control unit 10 sets the point on the circumference closest to the previous position as the provisional camera position Ca'.

[0111] Next, the control unit 10 obtains a setting vector (relative positional relationship) from the camera Ca to the camera Cb based on the relationship of the initial setting vector. Then, the control unit 10 obtains a tentative camera position Cb' and a tentative azimuth angle θZb' of the camera Cb based on the setting vector from the camera Ca to the camera Cb and the azimuth angle θZa' (step S76). The details of the process are the same as those in FIG. 15.

[0112] Next, the control unit 10 obtains a direction vector from the virtual camera position Cb' to the point P2, and obtains the horizontal azimuth angle D' within the field of view (step S77). The details of the process of obtaining the horizontal azimuth angle D' within the field of view are the same as the process of step S53 in FIG.

[0113] Next, the control unit 10 calculates the difference between the lateral azimuth angle D within the field of view of the point P2 actually observed by the camera Cb and the lateral azimuth angle D' within the field of view calculated in step S77 (step S78).

[0114] Here, the control unit 10 judges whether the difference between D and D' is equal to or smaller than a threshold value (step S79). If the difference between D and D' is greater than the threshold (step S79; NO), the control unit 10 moves the tentative camera position Ca' on the circle A according to the difference in the azimuth angle, and updates Ca' and the azimuth angle θZa' (step S80). After step S80, the process returns to step S76, and is repeated in step S79 until the difference between D and D' becomes equal to or less than the threshold.

[0115] In step S79, if the difference between D and D' is equal to or smaller than the threshold value (step S79; YES), the control unit 10 determines the XY coordinates of the camera Ca and the azimuth angle θZa of the camera Ca (step S81).

[0116] Here, if the camera Ca is not the reference camera C1, the control unit 10 obtains the XY coordinates and azimuth angle θZ of the reference camera C1 based on the installation vector from the reference camera C1 to the camera Ca (step S82). This ends the second positioning calculation process (two points and two cameras).

[0117] Next, the third positioning calculation process (step S8) will be described. <Third positioning calculation process when three points are captured by the same camera> The third positioning calculation process (three points and one camera) will be described with reference to FIG. FIG. 22A shows an example of an image captured by a camera C. The control unit 10 designates the three points captured by the camera C as P1, P2, and P3, in that order from the left (step S91).

[0118] Next, the control unit 10 acquires the XY coordinates of the installation positions of the three points P1, P2, and P3 (step S92). Specifically, the control unit 10 acquires the three-dimensional coordinates corresponding to the light source ID of each of the points P1, P2, and P3 from the light source ID-three-dimensional coordinate table 21. FIG. 22B is a diagram in which the position indicators P1, P2, and P3 and the camera C are projected onto the XY plane in the world coordinate system.

[0119] Next, the control unit 10 obtains a horizontal angle of view α between points P1 and P2 in the captured image. Then, the control unit 10 obtains an arc A in which the camera C projected onto the XY plane may exist from the XY coordinates of point P1, the XY coordinates of point P2, and the horizontal angle of view α (step S93). Details of the process are the same as those in FIG.

[0120] Next, the control unit 10 obtains a horizontal angle of view β between points P2 and P3 in the captured image. The control unit 10 then obtains an arc B in which the camera C projected onto the XY plane may exist, based on the XY coordinates of point P2, the XY coordinates of point P3, and the horizontal angle of view β (step S94). Details of the process are the same as those in FIG.

[0121] Next, the control unit 10 obtains the intersection point between the arc A and the arc B, and sets the intersection point other than the point P2 as the XY coordinates of the camera C (step S95).

[0122] Next, the control unit 10 obtains the azimuth angle θZ of the camera C from the XY coordinates of the camera C (step S96). Details of the process are the same as those in FIG.

[0123] Here, if the camera C is not the reference camera C1, the control unit 10 obtains the XY coordinates and azimuth angle θZ of the reference camera C1 based on the installation vector from the reference camera C1 to the camera C (step S97). This ends the third positioning calculation process (three points and one camera).

[0124] <Third positioning calculation process when two cameras capture two points and one point respectively> The third positioning calculation process (three points and two cameras) will be described with reference to FIG. 24A and 24B show examples of images captured by the cameras Ca and Cb. The control unit 10 designates the camera capturing two points as Ca and the camera capturing one point as Cb (step S101).

[0125] Next, the control unit 10 designates the two points captured by the camera Ca as P1 and P2, from the left, and designates the point captured by the camera Cb as P3 (step S102).

[0126] Next, the control unit 10 acquires the XY coordinates of the installation positions of the three points P1, P2, and P3 (step S103). Specifically, the control unit 10 acquires the three-dimensional coordinates corresponding to the light source ID of each of the points P1, P2, and P3 from the light source ID-three-dimensional coordinate table 21. FIG. 24C is a diagram in which the position indices P1, P2, and P3 and the cameras Ca and Cb are projected onto the XY plane in the world coordinate system.

[0127] Next, the control unit 10 obtains a horizontal angle of view α between points P1 and P2 in the captured image. Then, the control unit 10 obtains an arc A in which the camera Ca projected on the XY plane may exist from the XY coordinates of point P1, the XY coordinates of point P2, and the horizontal angle of view α (step S104). The details of the process are the same as those in FIG.

[0128] Next, the control unit 10 determines a tentative camera position Ca' on the arc A, and obtains an azimuth angle θZa' (step S105). Details of the process are the same as those in FIG. 15. The initial position of the tentative camera position Ca' may be a point equivalent to the apex E of the isosceles triangle obtained in step S42. Furthermore, when the positioning state is continuous, it is even more preferable to use a point on the arc A that is closest to the estimated coordinates from the past positioning history as the initial position.

[0129] Next, the control unit 10 obtains a setting vector (relative positional relationship) from the camera Ca to the camera Cb based on the relationship of the initial setting vector. Then, the control unit 10 obtains a tentative camera position Cb' and a tentative azimuth angle θZb' of the camera Cb based on the setting vector from the camera Ca to the camera Cb and the azimuth angle θZa' (step S106). The details of the process are the same as those in FIG. 15.

[0130] Next, the control unit 10 obtains a direction vector from the virtual camera position Cb' to the point P3, and obtains the horizontal azimuth angle D' within the field of view (step S107). The details of the process of obtaining the horizontal azimuth angle D' within the field of view are the same as the process of step S53 in FIG.

[0131] Next, the control unit 10 calculates the difference between the lateral azimuth angle D within the field of view of the point P3 actually observed by the camera Cb and the lateral azimuth angle D' within the field of view calculated in step S107 (step S108).

[0132] Here, the control unit 10 judges whether the difference between D and D' is equal to or smaller than a threshold value (step S109). If the difference between D and D' is greater than the threshold (step S109; NO), the control unit 10 moves the tentative camera position Ca' on the arc A according to the difference in the azimuth angles, and updates Ca' and the azimuth angle θZa' (step S110). After step S110, the process returns to step S106, and step S109 is repeated until the difference between D and D' becomes equal to or less than the threshold.

[0133] In step S109, if the difference between D and D' is equal to or smaller than the threshold value (step S109; YES), the control unit 10 determines the XY coordinates of the camera Ca and the azimuth angle θZa of the camera Ca (step S111).

[0134] Here, if the camera Ca is not the reference camera C1, the control unit 10 obtains the XY coordinates and azimuth angle θZ of the reference camera C1 based on the installation vector from the reference camera C1 to the camera Ca (step S112). This ends the third positioning calculation process (three points and two cameras).

[0135] The first to third positioning calculation processes do not necessarily require parallel calculation, but rather, a combination of reception points to be used for each process is extracted according to the list created in step S5, and an appropriate positioning calculation process is performed using that combination.

[0136] Returning to FIG. 7, after steps S6 to S8, the control unit 10 determines whether or not there is a valid positioning result (step S9). If there is no valid positioning result (step S9; NO), the process proceeds to step S4.

[0137] In step S9, if there is a valid positioning result (step S9; YES), the control unit 10 performs a weighted average process on the positioning results (step S10).

[0138] The weighted averaging process of the positioning results will be described with reference to FIG. The control unit 10 calculates the reprojection error of each positioning result (step S121). Specifically, the control unit 10 calculates the reprojection error as an index indicating the difference (distance) between a point obtained by reprojecting the known installation position (three-dimensional coordinates) of each position index P onto the captured image (projection surface) of the camera C and a point on the captured image actually captured by the camera C. Note that the control unit 10 calculates the reprojection error for all points included in the captured image, not just for the points used for positioning, for each combination of the "applied calculation method" and the "reception points used for positioning."

[0139] Next, the control unit 10 calculates a weighted average of the positioning results, using the inverse of the reprojection error as the weight (step S122). Fig. 26 is an image diagram of the weighted average of the positioning results. Each point shown in Fig. 26 indicates the positioning result of camera C (moving body 100) in the world coordinate system and the weighted average of each positioning result. For each positioning result, the calculation method (second positioning calculation process / third positioning calculation process), the use point (combination of position index P used in the positioning process), and the reprojection error are shown. In Fig. 26, the second positioning calculation process is abbreviated to "second" and the third positioning calculation process is abbreviated to "third."

[0140] Next, the control unit 10 obtains the orientation of the reference camera C1 by using the horizontal azimuth angle within the angle of view at the average coordinates obtained by the weighted average (step S123). This completes the weighted averaging process of the positioning results.

[0141] 7, the control unit 10 performs height correction processing by reprojection (step S11). Assuming that the XY coordinates obtained as a result of positioning are accurate, if the initially estimated height is incorrect, there will be a shift in the vertical direction (image Y-axis direction) in the image coordinate system according to the distance from the camera C to the position index P. In this processing, the final height correction is performed using information on the shift in the captured image.

[0142] The height correction process by reprojection will be described with reference to FIG. Fig. 28A shows an actual reception point P0 in an image captured by a certain camera C, and a reprojected point Prep based on the current positioning result. The vertical direction in Fig. 28A is the Y-axis direction of the image coordinate system, which corresponds to the Z-axis direction of the world coordinate system. The "camera height plane" shown in Fig. 28A is the ideal horizon shifted by the camera height, and the imaging position changes depending on the pitch angle due to the installation of the camera and the inclination of the ground. The horizontal direction in Fig. 28A is the X-axis direction of the image coordinate system. Fig. 28B is a diagram showing the positional relationship between the position index P and camera C when the Z-axis direction of the world coordinate system is taken vertically and the direction from the position index P to camera C is taken horizontally. In Fig. 28B, the camera position corresponding to the current positioning result is indicated as Crep, and the actual camera position is indicated as C0.

[0143] First, the control unit 10 obtains the elevation / depression angle θ0 to the reception point P0 from the Y coordinate (the vertical position in the image coordinate system) of the reception point P0 in the actual image (step S131). The elevation / depression angle θ0 can be calculated using equation (10) where θX is the observed pitch angle of camera C, Y0 is the Y coordinate of the receiving point in the actual image, and f is the focal length of camera C divided by the side length of one pixel. θ0=θX+atan(Y0 / f) …Equation (10)

[0144] Next, the control unit 10 obtains the coordinates (Xrep, Yrep) of the reprojected point Prep obtained by reprojecting the position index P onto the captured image from the current coordinate position CX, CY, CZ and orientation θZ, and the world coordinates of the position index P (light source) (step S132).

[0145] Next, the control unit 10 obtains the elevation / depression angle θrep to the reprojection point Prep (step S133). The elevation / depression angle θrep is calculated by equation (11) using θX as the observed pitch angle of camera C, Yrep as the Y coordinate of the reprojection point Prep, and f as the focal length of camera C divided by the side length of one pixel. θrep=θX+atan(Yrep / f) …Equation (11)

[0146] Next, the control unit 10 obtains the distance d in the horizontal direction (XY plane of the world coordinate system) between the camera position Crep of the current positioning result and the position index P (step S134).

[0147] Next, the control unit 10 calculates the elevation difference ΔZ of the camera C from the elevation / depression angle θ0 corresponding to the actually observed reception point P0, the elevation / depression angle θrep corresponding to the reprojection point Prep, and the distance d calculated in step S134 (step S135). This elevation difference ΔZ becomes the correction value. The elevation difference ΔZ is calculated by equation (12). ΔZ=d*tan(θrep)-d*tan(θ0) …Equation (12)

[0148] Here, the control unit 10 judges whether or not the process has been completed for all reception points (step S136). If there is a reception point for which the processing has not been completed (step S136; NO), the processing returns to step S131. The control unit 10 repeats the processing for another reception point as a processing target.

[0149] In step S136, when the process is completed for all reception points (step S136; YES), the control unit 10 calculates the maximum likelihood average of all ΔZ (step S137). Here, when averaging the correction values ​​ΔZ indicated by each point, the control unit 10 increases the weighting in the average for a reception point corresponding to a position index P close to the camera C, for example, since the position in the vertical direction in the image may vary significantly.

[0150] Next, the control unit 10 adds the correction value ΔZ to the current estimated height CZ of the camera (the value used to calculate the reprojection point) to obtain the current final height (step S138). In reality, there will be some shift in the X-axis direction of the image due to errors in the positioning itself and errors in the distortion correction, but assuming that the difference in height is dominant, a correction value ΔZ is calculated for each reception point from the difference in coordinates in the up and down direction and the position of the target position index P. This completes the height correction process by reprojection.

[0151] Returning to FIG. 7, after step S11 or step S4, the control unit 10 performs final correction by filtering or the like, and estimates the coordinate position and orientation of the moving body 100 (camera C) (step S12). When the positioning history and the current positioning calculation result are available, a known state estimation method using a Kalman filter or the like that is effective under the system conditions can be used. After step S12, the process returns to step S1, and the same process is repeated.

[0152] 7, there is no combination to be used for the positioning process in the case where the combination conditions of the first to third positioning calculation processes are not met, for example, "when two points with elevation / depression angles less than the first threshold are received" or "when three points are received but the mutual lateral angle of view is not equal to or greater than the second threshold." In this case, it is determined in step S9 that there is no valid positioning result, and the process proceeds to step S4. Also, if the first to third positioning calculation processes have been performed but the calculations have diverged or become impossible, it may be determined in step S9 that there is no valid positioning result, and the process may proceed to step S4.

[0153] As described above, according to this embodiment, the control unit 10 performs positioning processing using information on the estimated height of the camera C (imaging device), obtains the second coordinate position of the position index P (light source) on the image corresponding to the result of the positioning processing, and corrects the height of the camera C based on the first coordinate position and the second coordinate position in the captured image of the position index P. This makes it possible to perform positioning in the height direction with high accuracy, and to provide a highly accurate positioning result in positioning the self-position.

[0154] Furthermore, the control unit 10 acquires the reprojection point of the position index P (light source) included in the image as a second coordinate position based on the result of the positioning process, and corrects the height of the camera C based on the reprojection point and the first coordinate position of the position index P. This allows accurate positioning in the height direction, and provides highly accurate positioning results in positioning the self-position. Specifically, the height estimated in advance can be corrected using the vertical deviation between the position of the position index P observed on the image captured by the camera C and the reprojection point.

[0155] Furthermore, the control unit 10 obtains the elevation / depression angle θ0 (first elevation / depression angle) to the position index P included in the captured image, the elevation / depression angle θrep (second elevation / depression angle) to the reprojection point, and the horizontal distance d between the position index P and camera C, and calculates a correction value ΔZ for the height of camera C based on the distance d, the elevation / depression angle θ0, and the elevation / depression angle θrep. In this manner, the height of camera C can be calculated by geometric calculation.

[0156] In addition, by using the unit direction vector uv' pointing from the focus of camera C toward the position index P, the control unit 10 can easily obtain the azimuth angle of camera C (moving body 100) based on the vector projected onto the XY plane.

[0157] In addition, the control unit 10 detects identification information of the position index P contained in the image captured by the camera C and obtains a known coordinate position in three-dimensional space corresponding to the detected identification information, thereby enabling the position index P on the image to be identified with high accuracy.

[0158] Furthermore, by using multiple cameras C1 and C2, the position index P can be captured efficiently, improving positioning accuracy. Due to the angle at which pixels are captured, it is easier to obtain distance performance and the like by viewing a wide range with multiple cameras C rather than using a single camera C with a wide-angle lens. Furthermore, by using multiple cameras C, the effects of correction errors due to lens distortion of the wide-angle lens can be avoided.

[0159] The above description of the embodiment is merely an example of the information processing device according to the present invention, and is not intended to limit the present invention. The detailed configuration and operation of each part constituting the device may be modified as appropriate without departing from the spirit and scope of the present invention.

[0160] For example, the case where two cameras, one front and one back, are used as the cameras C has been described as an example, but three or more cameras C may be attached to the moving body 100.

[0161] The computer-readable medium storing the program for executing each process is not limited to the above examples. As other computer-readable media, non-volatile memory such as flash memory and portable recording media such as CD-ROMs can be used. Furthermore, a carrier wave can be used as a medium for providing program data via a communication line. [Explanation of symbols]

[0162] 1 Positioning System 10 Control section 20 Memory section 21 Light source ID-3D coordinate table 40A, 40B Attitude angle sensor 50 Travel drive unit 100 Mobile C1, C2 Camera C Camera P1,P2,P3,P4,... position index P position index

Claims

1. acquiring a first coordinate position in an image of a light source included in an image captured by an imaging device, among a plurality of light sources whose coordinate positions in a three-dimensional space are known; a control unit that performs a positioning process to derive at least one of a coordinate position and an orientation of the device itself in the three-dimensional space based on a coordinate position in the three-dimensional space of a light source included in the image and the first coordinate position; The control unit is Estimating a height of the imaging device in the three-dimensional space; performing the positioning process using information on the estimated height of the imaging device; obtaining a second coordinate position of the light source on the image corresponding to the result of the positioning process; An information processing device that corrects a height of the imaging device based on the first coordinate position and the second coordinate position.

2. The control unit is acquiring, as the second coordinate position, a reprojected point obtained by reprojecting a light source included in the image onto the image based on a result of the positioning process; The information processing apparatus according to claim 1 , wherein a height of the imaging device is corrected based on the reprojection point and the first coordinate position of a light source included in the image.

3. The control unit is calculating a first elevation / depression angle, which is an elevation / depression angle from the imaging device to the light source included in the image, from the first coordinate position in the image of the light source; determining a second elevation / depression angle which is an elevation / depression angle from the imaging device to the reprojection point; calculating a horizontal distance between the light source and the imaging device based on a coordinate position in the three-dimensional space of a light source included in the image and a positioning result of the imaging device; The information processing apparatus according to claim 2 , further comprising: a correction value for a height of the imaging device calculated based on the distance, the first elevation / depression angle, and the second elevation / depression angle.

4. The information processing device according to claim 1 , wherein the control unit performs the positioning process using a unit vector of a three-dimensional direction vector having a focal point of the imaging device as a starting point and a coordinate position in the three-dimensional space of a light source included in the image as an end point.

5. each of the plurality of light sources modulates light to transmit identification information of the light source; The information processing device according to claim 1 , wherein the control unit detects identification information of a light source contained in an image captured by the imaging device and the first coordinate position from the image, and acquires a known coordinate position in the three-dimensional space corresponding to the detected identification information.

6. A plurality of the imaging devices are provided, The information processing apparatus according to claim 1 , wherein at least one of a coordinate position and a direction of the apparatus itself is derived based on images captured by a plurality of the imaging devices.

7. acquiring a first coordinate position in an image of a light source included in an image captured by an imaging device, among a plurality of light sources whose coordinate positions in a three-dimensional space are known; An information processing method for performing a positioning process to derive at least one of a coordinate position and an orientation of a host device in a three-dimensional space based on a coordinate position in the three-dimensional space of a light source included in the image and the first coordinate position, estimating a height of the imaging device in the three-dimensional space; performing the positioning process using information on the estimated height of the imaging device; acquiring a second coordinate position of the light source on the image corresponding to a result of the positioning process; correcting a height of the imaging device based on the first coordinate position and the second coordinate position; An information processing method comprising:

8. Computer, acquiring a first coordinate position in an image of a light source included in an image captured by an imaging device, among a plurality of light sources whose coordinate positions in a three-dimensional space are known; a program for causing the device to function as a control unit that performs a positioning process to derive at least one of a coordinate position and an orientation of the device itself in the three-dimensional space based on a coordinate position in the three-dimensional space of a light source included in the image and the first coordinate position, The control unit is Estimating a height of the imaging device in the three-dimensional space; performing the positioning process using information on the estimated height of the imaging device; obtaining a second coordinate position of the light source on the image corresponding to the result of the positioning process; A program for correcting a height of the imaging device based on the first coordinate position and the second coordinate position.

Images