Attitude angle measuring device, attitude angle measuring method and program

Abstract

To provide a highly accurate positioning result in positioning of a self position.SOLUTION: A movement body 100 (a position acquisition device) comprises: a star tracker posture angle calculation unit 13 that, when a prescribed condition is met, can derive a posture angle of a self device on the basis of the number of a plurality of stars and position thereof in images acquired by a star tracker camera 40; and a positioning processing unit 12 that, when the posture angle of the self device is known, can derive a three-dimensional position of the self device on the basis of the number of two or more position indexes 3 and positions in images acquired by a visible light communication camera 30, and when six or more position indexes 3 are included in the image acquired by the visible light communication camera 30, can derive the three-dimensional position and the posture angle of the self device. The movement body is configured to integrate a derivation result by the star tracker posture angle calculation unit 13 and a derivation result by the positioning processing unit 12, and estimate the posture angle and three-dimensional position of the self device.SELECTED DRAWING: Figure 4

Description

[Technical field]

[0001] The present invention relates to Attitude angle measurement device, attitude angle measurement method and programs. [Background technology]

[0002] Conventionally, various positioning systems have been used to estimate a self-position. For example, a technology has been proposed in which a camera mounted on a moving object captures images of a plurality of light-emitting devices (position indicators) that transmit IDs that can uniquely identify the moving object by visible light communication (e.g., RGB three-color pattern) to estimate the position of the moving object (see Patent Document 1). Specifically, the installation positions (three-dimensional positions) of the light-emitting devices are measured in advance, and the coordinate positions and IDs of the light-emitting devices in the image captured by the camera are detected, and the position of the moving object is determined based on the detected coordinate positions on the captured image and the three-dimensional positions of the light-emitting devices.

[0003] 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]

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

[0005] However, on the surface of the moon, movement is not expected to occur within a fixed plane, and activities will be carried out off-road, so when estimating one's own position, six-degree-of-freedom positioning that can accommodate variations in height and attitude angle is essential. In other words, the positioning method using visible light communication mentioned above requires that six or more position indices are always included in the image captured by the camera.

[0006] On the other hand, when using visible light communication for positioning over a wide area, such as on the lunar surface, there is a demand for positioning with as high accuracy as possible while minimizing the installation load of position indicators. Given these requirements, it was difficult to arrange the position indicators so that all six indicators were always visible to the camera over a wide area on the order of kilometers.

[0007] The present invention has been made in consideration of the above problems in the conventional technology, The attitude angle of the moving body High accuracy To measurement Possible attitude angle measuring device, attitude angle measuring method and program The objective is to provide the following. [Means for solving the problem]

[0008] In order to solve the above problems, the present invention The attitude angle measurement device according to the present invention is an attitude angle measurement device provided on a moving body capable of moving in a space above the ground, and is characterized in that it comprises a correction means for correcting the first attitude angle or the second attitude angle based on a first likelihood for a first attitude angle of the moving body derived by detecting the number of celestial bodies that can be captured as images by an imaging means and their positions within an angle of view among a plurality of celestial bodies whose position information is associated with each other and which are registered in advance, and a second likelihood for a second attitude angle of the moving body derived by detecting the number of artificial light sources that can be captured as images by an imaging means and their positions within an angle of view among a plurality of artificial light sources that are arranged in advance as position indicators in the space above the ground. Moreover, an attitude angle measuring method according to the present invention is an attitude angle measuring method executed by an attitude angle measuring device provided on a moving body capable of moving in a space above the Earth's surface, and is characterized in that it includes a correction process for correcting the first attitude angle or the second attitude angle based on a first likelihood for a first attitude angle of the moving body derived by detecting the number of celestial bodies that can be captured as images by an imaging means and their positions within an angle of view among a plurality of celestial bodies whose position information is associated with each other and which are registered in advance, and a second likelihood for a second attitude angle of the moving body derived by detecting the number of artificial light sources that can be captured as images by an imaging means and their positions within an angle of view among a plurality of artificial light sources that are arranged in advance as position indicators in the space above the Earth's surface. Furthermore, the program of the present invention is characterized in that it causes a computer of an attitude angle measuring device provided on a moving body capable of moving in a space above the Earth's surface to function as a correction means for correcting the first attitude angle or the second attitude angle based on a first likelihood for a first attitude angle of the moving body derived by detecting the number of celestial bodies that can be captured as images by an imaging means and their positions within an angle of view among a plurality of celestial bodies whose position information is associated with each other and which are registered in advance, and a second likelihood for a second attitude angle of the moving body derived by detecting the number of artificial light sources that can be captured as images by an imaging means and their positions within an angle of view among a plurality of artificial light sources that are arranged in advance as position indicators in the space above the Earth's surface. Effect of the Invention

[0009] According to the present invention, The attitude angle of the moving body High accuracy To measurement Possible attitude angle measuring device, attitude angle measuring method and program can be provided. [Brief description of the drawings]

[0010] [Figure 1] 1 is a diagram showing a configuration of a positioning system according to an embodiment of the present invention. [Diagram 2] 11 is a top view of a positioning area showing an example of the arrangement of position indicators. FIG. [Diagram 3] FIG. 1 is a configuration diagram of a moving object. [Figure 4] FIG. 2 is a functional block diagram of a moving object. [Diagram 5] FIG. 2 is a diagram for explaining an outline of a positioning process. [Figure 6] 13 is an example of the likelihood of an attitude angle based on receiving six or more light source IDs. [Figure 7] This is an example of the likelihood of the attitude angle obtained by a 6-axis sensor. [Figure 8] 13 is a flowchart showing a three-axis attitude angle / likelihood determination process. [Figure 9] 13 is a flowchart showing an overall process. [Figure 10] This is an operation example showing how the positioning likelihood and the moving speed change with a change in the traveling conditions of the moving object. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0011] Hereinafter, an embodiment of a position acquisition 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.

[0012] [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, a plurality of position indices 3A, 3B, 3C, 3D,... (hereinafter, when there is no need to distinguish between them, they will be referred to as "position indices 3"), and a plurality of stars 4A, 4B, 4C, 4D, 4E, 4F,... (hereinafter, when there is no need to distinguish between them, they will be referred to as "stars 4"). The positioning system 1 is intended for use on the surface of the moon, which is one of the most suitable application locations.

[0013] In this embodiment, a position acquisition 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 be an autonomous rover, a robot, a rover operated by a person (including an automatic driving mode), or the like. Since the moving body 100 travels on the undulating and uneven surface of the moon, it measures its position along three axes and six degrees of freedom in three-dimensional coordinates.

[0014] The moving body 100 is equipped with visible light communication cameras 30A, 30B (second imaging means) used for receiving visible light communication, and a star tracker camera 40 (first imaging means) for determining the attitude angle based on the imaging state of stars 4. The stars 4, which are multiple light sources imaged by the star tracker camera 40 and used to derive the attitude angle described below, and the position indicators 3, which are multiple light sources imaged by the visible light communication cameras 30A, 30B and used to derive the attitude angle and three-dimensional position described below, are different.

[0015] In this embodiment, the mobile body 100 acquires information related to visible light communication using two front and rear cameras, a visible light communication camera 30A facing forward in the direction in which the mobile body 100 moves, and a visible light communication camera 30B facing backward. The reason for using two front and rear cameras is to ensure a detection range for visible light communication and to prevent correction errors of optical distortion of lenses, etc. Note that a single camera with a large 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 visible light communication cameras 30A and 30B, they will be referred to as "visible light communication cameras 30."

[0016] On the surface of the moon, which is the operating area of ​​the moving body 100, a plurality of position indicators 3 serving as light sources are installed at intervals. The position indicator 3 includes an LED (Light Emitting Diode). The position indicator 3 controls light emission or reflection by color modulation or brightness modulation in the wavelength region of visible light, and transmits information to be transmitted. For example, the position indicator 3 transmits at least identification information specific to the position indicator 3 (light source ID that can uniquely identify the position indicator 3) by a three-color pattern of R (red), G (green), and B (blue). When each of the multiple position indicators 3 is installed, the installation position (three-dimensional coordinates) in the world coordinate system (X, Y, Z) is measured. For each position indicator 3, the correspondence between the light source ID and the three-dimensional coordinates is stored in a light source ID-three-dimensional coordinate table 21 (see FIG. 3 and FIG. 4) in the moving body 100. If the position indicator 3 is capable of transmitting a sufficient amount of data, it may directly transmit its own position and height (three-dimensional coordinates) on the lunar surface.

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

[0018] The size of the light emitting portion of the position indicator 3 is determined based on the required communication distance, camera resolution, lens, etc. For example, when a 4K resolution camera with a lens having a horizontal angle of view of 100 degrees is used as the visible light communication camera 30, one pixel is approximately 0.025 degrees. Note that the visible light communication camera 30 may have a frame rate of, for example, 30 fps or 60 fps. In visible light communication, it is sufficient to be able to capture stable color or brightness modulation in the image of one pixel, but in reality, there is a limit to the spatial frequency due to the structure of the optical system and image sensor. Therefore, if we assume that stable image formation is possible with an area of ​​3 pixels square or more, for example, if the light source (position index 3) is 1m square, the distance at which this light source has an apparent angle of 0.075 degrees is 1 / tan(0.075 degrees) = approximately 763m, which is the communication limit distance. However, in reality, there is no air scattering on the surface of the moon, and the surroundings are dark even during the day, so the halation effect causes the light source to appear blurred and large against the dark background. This extends the communication limit distance by two to three times the theoretical value.

[0019] FIG. 2 is a top view of a positioning area showing an example of the arrangement of position indicators 3 (transmitting devices for visible light communication). Since the accuracy of positioning processing using visible light communication depends on the installation density of position indicators 3, position indicators 3 are arranged according to the required accuracy from high-precision areas 5 to general-precision areas 6. For example, in the vicinity of bases 7 that require high-precision positioning or in places where high-speed movement is desired, position indicators 3 are installed at high density to form high-precision areas 5. On the other hand, in general-precision areas 6, position indicators 3 may be arranged at a density equal to or higher than the minimum density so as to satisfy the positioning conditions.

[0020] As an example of a method for arranging position indicators 3 at the minimum density in the general accuracy area 6, the position indicators 3 are arranged so that the distance between any two points of the position indicators 3 is less than the communication limit distance D of visible light communication. If the communication limit distance D is 763 m, for example, the position indicators 3 may be arranged at each vertex of a tile of equilateral triangles with one side of length L, with the arrangement interval L of the position indicators 3 being less than 700 m.

[0021] As shown in Fig. 2, the visible light communication reception range is a sector-shaped area 8 formed with a horizontal angle of view of 100 degrees and a radius of communication limit distance D (763 m) from visible light communication cameras 30A, 30B in the front-rear direction of moving body 100. If position indicators 3 are placed at the vertices of an equilateral triangle, it is possible to receive two or more light source IDs by combining the two front and rear cameras at any position within general accuracy area 6, regardless of the direction in which visible light communication cameras 30A, 30B are facing. In most cases, three or more light source IDs will be received.

[0022] It is not necessary for the position indicators 3 to be arranged so as to form an exact equilateral triangle. If there are restrictions on the installation positions and directions of the position indicators 3, the position indicators 3 may be arranged in a more optimized manner. In areas with topographical problems, the position indicators 3 may be individually arranged according to the topography.

[0023] [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, a visible light communication camera 30 (30A, 30B), a star tracker camera 40, a six-axis sensor 50, a traveling drive unit 60, and the like.

[0024] 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. The control unit 10 includes a visible light communication processing unit 11, a positioning processing unit 12, a star tracker attitude angle calculation unit 13, a three-axis attitude angle / likelihood determination processing unit 14, a positioning likelihood evaluation unit 15, and a movement speed control unit 16. Each unit constituting the control unit 10 is realized by software processing through cooperation between a CPU and a program stored in a storage unit 20. Details of each unit constituting the control unit 10 will be described later.

[0025] 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.

[0026] The storage unit 20 stores a light source ID-three-dimensional coordinate table 21, star map data 22, and a likelihood-speed control table 23. In the light source ID-three-dimensional coordinate table 21, for each of a plurality of position indicators 3, the light source ID of the position indicator 3 is associated with three-dimensional coordinates that are the installation position of the position indicator 3. The light source ID-three-dimensional coordinate table 21 is used for positioning processing by visible light communication.

[0027] The star chart data 22 is data in which information regarding the positions of multiple stars 4 in the sky and the movements of celestial bodies is recorded, and is used to calculate the absolute attitude angle by the star tracker.

[0028] In the likelihood-speed control table 23, the positioning likelihood is associated with the maximum speed allowed for the positioning likelihood. The positioning likelihood is the likelihood for the entire positioning. The maximum speed includes the speed in the traveling direction and the angular speed in turning. Note that instead of the likelihood-speed control table 23, a relational expression for deriving the maximum speed allowed when the likelihood is input may be stored in the storage unit 20.

[0029] The visible light communication camera 30 captures an optical image incident through a lens and generates two-dimensional image data. The visible light communication camera 30 captures images continuously in time and outputs the continuous image data to the control unit 10 (visible light communication processing unit 11). The visible light communication camera 30 captures an image including the position index 3.

[0030] The star tracker camera 40 captures an optical image received through a lens and generates image data. The star tracker camera 40 outputs the generated image data to the control unit 10 (star tracker attitude angle calculation unit 13). The star tracker camera 40 captures an image including the fixed star 4.

[0031] The six-axis sensor 50 includes a known three-axis acceleration sensor and a three-axis angular velocity sensor (gyro sensor). The six-axis sensor 50 outputs the acceleration in three-axis directions, the angular velocity around three axes, and the like to the control unit 10 (three-axis attitude angle / likelihood determination processing unit 14). The six-axis sensor 50 generally has a fast response speed, with a response frequency of several tens to several hundreds of Hz. When calculating the three-axis attitude angle from the six-axis data of the six-axis sensor 50 (acceleration in three-axis directions, angular velocity around three axes), an integral error generally increases over time due to accumulation of drift error in the gyro sensor.

[0032] The traveling drive unit 60 drives the wheels for traveling the moving body 100, changes the direction of travel, etc. The traveling drive unit 60 moves its own device (the moving body 100).

[0033] Fig. 4 is a functional block diagram of the moving body 100. As shown in Fig. 4, the moving body 100 includes a visible light communication processing unit 11, a positioning processing unit 12, a star tracker attitude angle calculation unit 13, a three-axis attitude angle / likelihood determination processing unit 14, a positioning likelihood evaluation unit 15, a moving speed control unit 16, a visible light communication camera 30, a star tracker camera 40, a six-axis sensor 50, and the like.

[0034] A visible light communication positioning function is realized by the visible light communication camera 30, the visible light communication processing unit 11, and the positioning processing unit 12. The visible light communication positioning function is a function related to visible light communication and geometric positioning calculation by visible light communication (PnP problem). The PnP problem is a problem of estimating the position and orientation of the camera from the correspondence between the three-dimensional coordinates of n points in the world coordinate system and the image coordinates of each point projected onto the image plane. An attitude angle measurement function is realized by the star tracker camera 40, the star tracker attitude angle calculation unit 13, the three-axis attitude angle / likelihood determination processing unit 14, and the six-axis sensor 50. The attitude angle measurement function is a function related to three-axis attitude angle measurement.

[0035] In the following description, it is assumed that the three-axis attitude angles obtained by the star tracker, the six-axis sensor 50, and the PnP positioning coincide with the three-axis attitude angles of the vehicle body (mobile body 100).

[0036] The visible light communication processing unit 11 analyzes image data continuously captured by the visible light communication camera 30, and detects a light source ID indicated by visible light communication from each light emitting point (position indicator 3) included in the captured image. For each light emitting point on the captured image, the visible light communication processing unit 11 obtains a pair of the light source ID and the coordinate position of the light emitting point in the captured image (two-dimensional plane).

[0037] The positioning processor 12 performs positioning processing of the moving body 100 (the visible light communication camera 30) based on the coordinate position on the captured image and the known three-dimensional position (the installation position of the position indicator 3) for each light source ID received by visible light communication. The positioning processor 12 outputs a positioning result with a total of six degrees of freedom including the three-axis attitude angle of the visible light communication camera 30 and the three-dimensional coordinate in the world coordinate system (the lunar plane coordinate and the height from the plane, etc.). When referring to the "three-axis attitude angle of the visible light communication camera 30," the two visible light communication cameras 30A, 30B may be treated as a set. In other words, processing is performed such that an upward change in the pitch angle of the front camera (the visible light communication camera 30A) results in a downward change in the pitch angle of the rear camera (the visible light communication camera 30B). In this embodiment, the positioning processing unit 12 acquires the three-axis attitude angles, among the six degrees of freedom, from the three-axis attitude angle / likelihood determination processing unit 14 .

[0038] When the three-axis attitude angles are known, if the visible light communication processing unit 11 can receive light source IDs of at least two points through visible light communication, the positioning processing unit 12 can calculate the positioning result.

[0039] An overview of the positioning process will be described with reference to Fig. 5. Here, it is assumed that two position indexes 3A and 3B are within the field of view of the visible light communication camera 30. A light-emitting point 2A corresponding to the position index 3A and a light-emitting point 2B corresponding to the position index 3B are detected from an image plane 70 captured by the visible light communication camera 30. The coordinate positions of the light-emitting points 2A and 2B on the image plane 70 are (P1, Q1) and (P2, Q2), respectively. On the other hand, the three-dimensional coordinate positions of the position indicators 3A and 3B in the world coordinate system are known from prior measurement and are (X1, Y1, Z1) and (X2, Y2, Z2), respectively. The positioning processing unit 12 calculates the three-dimensional coordinate position (X0, Y0, Z0) of the visible light communication camera 30 in the world coordinate system from the correspondence between the coordinate positions (P1, Q1), (P2, Q2) on the captured image (image plane 70) and the three-dimensional positions (X1, Y1, Z1), (X2, Y2, Z2).

[0040] When a larger number of light source IDs are received in visible light communication, the amount of data used for positioning processing in the positioning processing unit 12 increases, and thus the positioning accuracy increases.

[0041] Furthermore, when six or more light source IDs are received by visible light communication, the positioning processor 12 also performs processing to solve all six degrees of freedom (three-axis attitude angles, three-dimensional coordinates) using only the coordinate information of the received light source ID group in parallel. The positioning processor 12 outputs the three-axis attitude angles obtained only from the information of the visible light communication camera 30 (the three-axis attitude angles based on the reception of six or more light source IDs) to the three-axis attitude angle / likelihood determination processor 14.

[0042] When the three-axis attitude angle of the own device (moving body 100) is known, the positioning processing unit 12 derives at least the three-dimensional position of the own device (moving body 100) based on the number and positions of two or more light sources (position indexes 3) in the image acquired by the visible light communication camera 30. Even when the three-axis attitude angle of the own device (moving body 100) is not known, the positioning processing unit 12 derives the three-dimensional position and attitude angle of the own device when a predetermined number or more of light sources (position indexes 3) are included in the image acquired by the visible light communication camera 30. That is, the positioning processing unit 12 functions as a second derivation means. Here, "6" is used as the "predetermined number". Note that the positioning accuracy increases as the number of light sources (position indexes 3) included in the image acquired by the visible light communication camera 30 increases.

[0043] The positioning processing unit 12 detects the light source ID and coordinate position of the position indicator 3 included in the image acquired by the visible light communication camera 30, acquires a known three-dimensional position in the world coordinate system corresponding to the detected light source ID, and derives the three-dimensional position and attitude angle of the device itself based on the coordinate position in the image acquired by the visible light communication camera 30 and the three-dimensional position in the world coordinate system.

[0044] The positioning processing unit 12 calculates a reprojection error as a self-evaluation index of the positioning result. Specifically, the positioning processing unit 12 calculates the reprojection error as an index indicating the difference (distance) between a point obtained by reprojecting a known installation position (three-dimensional coordinates) of each position indicator 3 onto the captured image plane of the visible light communication camera 30 based on the positioning result and a projected point on the captured image plane actually captured by the visible light communication camera 30. In addition, an internal index of a filter calculation when performing positioning calculation may be used as a self-evaluation index of the positioning result.

[0045] When a predetermined condition is satisfied, the star tracker attitude angle calculation unit 13 derives the attitude angle of the own device (moving body 100) based on the number and positions of multiple light sources (fixed stars 4) in the image acquired by the star tracker camera 40. That is, the star tracker attitude angle calculation unit 13 functions as a first derivation means. Here, the predetermined condition refers to the moving speed of the own device (moving body 100) caused by the traveling drive unit 60 being equal to or lower than a predetermined speed, or the own device being in a stopped state. In general, the greater the number of light sources (fixed stars 4) included in the image acquired by the star tracker camera 40, the higher the positioning accuracy.

[0046] The star tracker attitude angle calculation unit 13 derives the three-axis attitude angle of the device by pattern matching between the positional relationship of light sources (stars 4) included in the image acquired by the star tracker camera 40 and pre-prepared light source arrangement data (star chart data 22). Particularly in space, the star tracker achieves the most accurate angle measurement. The likelihood of the attitude angle derived by the star tracker attitude angle calculation unit 13 is higher than the likelihood of the attitude angle derived by the positioning processing unit 12 based on receiving six or more light source IDs.

[0047] However, the star tracker requires a long exposure time with high sensitivity to capture the fixed stars 4, and also imposes a large load on the matching process with the star chart data 22. For this reason, there are technical constraints such as insufficient exposure and failure to capture stars when the moving body 100 is moving faster than a predetermined speed (for example, in the case of linear movement, as the speed increases, it becomes impossible to capture images from the periphery of the angle of view of the star tracker camera 40), and a long processing response time (for example, 0.1 Hz to several Hz).

[0048] Furthermore, when the moving speed of the own device (moving body 100) becomes equal to or lower than a predetermined speed, the star tracker attitude angle calculation unit 13 becomes able to re-execute the derivation of the attitude angle of the own device using the star tracker camera 40. As described later, when the positioning likelihood of the moving body 100 decreases, the traveling drive unit 60 is controlled so as to reduce the moving speed of the moving body 100. Therefore, when the positioning likelihood decreases, the moving speed of the moving body 100 becomes equal to or lower than a predetermined speed, and it becomes possible to re-execute the derivation of the attitude angle of the moving body 100 using the star tracker camera 40.

[0049] The six-axis sensor 50 derives the attitude angle of the own device (moving body 100) based on the output values ​​of the three-axis acceleration sensor and the three-axis angular velocity sensor. That is, the six-axis sensor 50 functions as a third derivation means.

[0050] The three-axis attitude angle / likelihood determination processing unit 14 determines the attitude angle of the host apparatus using the derivation result by the six-axis sensor 50 (third derivation means) instead of or together with the derivation result by the star tracker attitude angle calculation unit 13. That is, the three-axis attitude angle / likelihood determination processing unit 14 functions as an attitude angle determination means.

[0051] The three-axis attitude angle / likelihood determination processing unit 14 integrates the three-axis attitude angle obtained by the star tracker and the three-axis attitude angle obtained by the six-axis sensor 50 to determine the current three-axis attitude angle. Specifically, the three-axis attitude angle / likelihood determination processing unit 14 gives priority to the attitude angle obtained by the star tracker, and if the star tracker does not function, successively estimates the angle state from the six-axis sensor 50. In addition, when the number of reception points (light source IDs) by visible light communication is six or more, the three-axis attitude angle can be obtained only from the information of the visible light communication camera 30, so the three-axis attitude angle / likelihood determination processing unit 14 takes into account the three-axis attitude angle obtained only from the information of the visible light communication camera 30, if any.

[0052] The three-axis attitude angle likelihood determination processing unit 14 calculates the likelihood of the three-axis attitude angle (attitude angle likelihood). The attitude angle likelihood is an index showing the plausibility of the three-axis attitude angle. There is no particular limitation on the method of calculating the likelihood. The attitude angle likelihood can rise suddenly depending on the method of deriving the attitude angle, but when it falls, it gradually fluctuates over time due to the relationship with the previous state.

[0053] When the moving body 100 is stopped or moving at a low speed, the star tracker functions to maximize the likelihood of the attitude angle. The likelihood of the three-axis attitude angle obtained by the star tracker is set as likelihood A (1.0).

[0054] The likelihood B of the attitude angle obtained by PnP positioning based on receiving six or more light source IDs is determined by the number of receptions N. The likelihood B is lower than the likelihood A. The likelihood B is represented by a characteristic curve that gradually saturates to 1, with a value of about 0.5 when the number of light source IDs (number of receptions) detected by the visible light communication processing unit 11 is the minimum of six.

[0055] Fig. 6 shows an example of the likelihood B of an attitude angle based on receiving six or more light source IDs. In Fig. 6, the horizontal axis represents the number N of received light source IDs, and the vertical axis represents the likelihood B. The likelihood B of an attitude angle increases as the number of light sources (position indexes 3) in an image captured by the visible light communications camera 30 increases.

[0056] The likelihood B is expressed, for example, by the following formula (1).

number

[0057] The positioning processing unit 12 can derive the attitude angle of the own device using the likelihood B even when the moving speed of the own device (moving body 100) caused by the traveling drive unit 60 is faster than a predetermined speed.

[0058] When only the six-axis sensor 50 is used to calculate the attitude angle, the likelihood C of the attitude angle decreases as the accumulation of angle fluctuations over time increases. After the six-axis sensor 50 is calibrated, the likelihood C decreases gently at first as the six-axis sensor 50 is used, but then decreases rapidly toward a certain limit. For example, when the moving body 100 continues to change (turn) its moving direction, the likelihood C of the attitude angle obtained by the six-axis sensor 50 decreases. As for the behavior of the six-axis sensor 50, due to factors such as response frequency, strong fluctuations tend to result in errors, but slow movement allows for more accurate angle estimation.

[0059] Fig. 7 shows an example of the likelihood C of the attitude angle obtained by the six-axis sensor 50. In Fig. 7, the horizontal axis represents the integrated value E of the angular displacement amount obtained by the six-axis sensor 50, and the vertical axis represents the likelihood C. Note that when E is greater than a threshold value E0, the likelihood C is set to 0. The amount of angular displacement is represented, for example, by the absolute value of the distance between the end points of three-dimensional vectors indicating the attitude angle when the start points of the vectors are aligned before and after the change.

[0060] The likelihood C is expressed, for example, by the following formula (2).

number

[0061] However, there is no mathematical rigor in formula (2). In practice, the optimum is selected based on the statistical characteristics of the error and the overall model design of the entire system.

[0062] The positioning processing unit 12 integrates the derivation result (attitude angle) by the star tracker attitude angle calculation unit 13 (first derivation means) and the derivation result (three-dimensional position / attitude angle) by the positioning processing unit 12 (second derivation means) to estimate the attitude angle and three-dimensional position of the own device. That is, the positioning processing unit 12 functions as an estimation unit. In this embodiment, the result integrated by the three-axis attitude angle / likelihood determination processing unit 14 is used as the derivation result of the attitude angle.

[0063] Specifically, the positioning processing unit 12 weights the derivation results by the star tracker attitude angle calculation unit 13 having a high likelihood of the derived attitude angle more heavily than the derivation results by the positioning processing unit 12, and integrates the derivation results by the star tracker attitude angle calculation unit 13 and the derivation results by the positioning processing unit 12.

[0064] The positioning likelihood evaluation unit 15 evaluates the likelihood of final positioning. Specifically, the positioning likelihood evaluation unit 15 judges the likelihood of the attitude angle and three-dimensional position of the own device estimated by the positioning processing unit 12 (estimation means). That is, the positioning likelihood evaluation unit 15 functions as a judgment means. For example, the positioning likelihood evaluation unit 15 performs a weighted average of the reprojection error and the attitude angle likelihood to calculate the likelihood of the overall positioning.

[0065] In this embodiment, the reprojection error and the attitude angle likelihood are combined to determine the evaluation value of the positioning. Although the error likelihood can be estimated by the reprojection error, due to the nature of the reprojection, the reprojection result may be good with a combination of three-axis attitude angles different from the true value (even with different locations and attitude angles, the light emitting point corresponding to the position index 3 may appear the same), so the reprojection error alone cannot be used as the self-evaluation value of the positioning. In particular, in a positioning system 1 that uses stars 4 and position indicators 3, the attitude angle likelihood is very important.

[0066] The moving speed control unit 16 controls the moving speed of the own device (moving body 100) by the traveling drive unit 60 according to the likelihood (likelihood of overall positioning) of the attitude angle and three-dimensional position of the own device determined by the positioning likelihood evaluation unit 15. Specifically, the moving speed control unit 16 refers to the likelihood-speed control table 23 in the storage unit 20 to acquire the maximum speed (speed, angular velocity of turning) associated with the positioning likelihood (likelihood of the attitude angle and three-dimensional position), and controls the traveling drive unit 60 to adjust the speed of the moving body 100 so that the moving speed of the own device is equal to or lower than the maximum speed according to the positioning likelihood at each time point.

[0067] If the reprojection error becomes an obviously abnormal value, it is necessary to stop the movement of the moving body 100 once and redo all the positioning. Therefore, it is desirable to evaluate not only the attitude angle likelihood but also the reprojection error. In addition, even if the reprojection error of the positioning likelihood does not deteriorate significantly, if the attitude angle likelihood decreases, the speed or direction change is suppressed.

[0068] [Movement of moving object] Next, the operation of the moving body 100 will be described. FIG. 8 is a flowchart showing a three-axis attitude angle / likelihood determination process executed in the moving body 100. As shown in FIG.

[0069] First, the star tracker attitude angle calculation unit 13 attempts to calculate the three-axis attitude angle by the star tracker (step S1). The star tracker attitude angle calculation unit 13 extracts point sources corresponding to the fixed stars 4 from the image data captured by the star tracker camera 40, collates the extracted point sources with the star chart data 22, and performs pattern matching to identify which fixed star 4 each point source corresponds to, and calculates the three-axis attitude angle.

[0070] Next, the six-axis sensor 50 calculates the three-axis attitude angle from the sensor values ​​of the three-axis acceleration sensor and the three-axis angular velocity sensor and the previous state (step S2). Here, sequential drift correction using a Kalman filter, a complementary filter, etc. is also included.

[0071] Furthermore, the three-axis attitude angle / likelihood determination processor 14 obtains the likelihood C of the three-axis attitude angle obtained by the six-axis sensor 50 (step S3). As shown in Fig. 7, the likelihood C decreases as the integrated value E of the angular displacement increases. The three-axis attitude angle / likelihood determination processor 14 determines the likelihood C of the three-axis attitude angle according to the integrated value E of the angular displacement for the six-axis sensor 50 after calibration.

[0072] Next, the three-axis attitude angle / likelihood determination processor 14 judges whether the measurement by the star tracker is successful (step S4). Specifically, the three-axis attitude angle / likelihood determination processor 14 judges whether the measurement result is acquired from the star tracker attitude angle calculation unit 13. If the measurement by the star tracker is valid (step S4; YES), the three-axis attitude angle / likelihood determination processing unit 14 sets the likelihood A of the three-axis attitude angle obtained by the star tracker to "1.0" (step S5).

[0073] In step S4, if the measurement by the star tracker is not successful (step S4; NO), the 3-axis attitude angle / likelihood determination processing unit 14 determines whether or not 6-degree-of-freedom PnP positioning is successful in the positioning processing unit 12, that is, whether or not six or more light source IDs are detected by the visible light communication processing unit 11 (step S6).

[0074] If six-degree-of-freedom PnP positioning is established (step S6; YES), the three-axis attitude angle / likelihood determination processing unit 14 acquires the three-axis attitude angles based on the reception of six or more light source IDs from the positioning processing unit 12 (step S7).

[0075] In addition, the three-axis attitude angle / likelihood determination processing unit 14 determines the likelihood B of the three-axis attitude angle based on receiving six or more light source IDs (step S8). As shown in Fig. 6, the likelihood B becomes larger (approaching 1) as the number N of received light source IDs increases. The three-axis attitude angle / likelihood determination processing unit 14 determines the likelihood B of the three-axis attitude angle according to the number N of received light source IDs.

[0076] After step S5 or step S8, the three-axis attitude angle / likelihood determination processing unit 14 (control unit 10) calibrates the six-axis sensor 50 (step S9). Specifically, if there is a three-axis attitude angle obtained by the star tracker, the three-axis attitude angle / likelihood determination processing unit 14 uses the three-axis attitude angle obtained by the star tracker to calibrate the six-axis sensor 50. On the other hand, if there is no three-axis attitude angle obtained by the star tracker, the three-axis attitude angle / likelihood determination processing unit 14 calibrates the six-axis sensor 50 using the three-axis attitude angle based on the reception of six or more light source IDs.

[0077] After step S9, or if 6-DOF PnP positioning is not established in step S6 (step S6; NO), the 3-axis attitude angle / likelihood determination processing unit 14 integrates the 3-axis attitude angles and integrates the likelihoods of the attitude angles (step S10).

[0078] For example, when there are three-axis attitude angles obtained by a star tracker, the three-axis attitude angle / likelihood determination processing unit 14 may use the three-axis attitude angles obtained by the star tracker as they are.

[0079] In FIG. 8, if the measurement by the star tracker is established (step S4; YES), the process of acquiring the three-axis attitude angle based on the reception of six or more light source IDs (step S7) is not performed. However, regardless of whether the measurement by the star tracker is established or not, if the PnP positioning is established based only on the information obtained from the visible light communication camera 30, the three-axis attitude angle based on the reception of six or more light source IDs may be acquired and used.

[0080] Then, the 3-axis attitude angle / likelihood determination processor 14 integrates the 3-axis attitude angle obtained by the star tracker, the 3-axis attitude angle based on the reception of six or more light source IDs, and the 3-axis attitude angle obtained by the 6-axis sensor 50 by weighting and averaging according to the likelihood, to calculate the current 3-axis attitude angle. Note that the 3-axis attitude angle / likelihood determination processor 14 may appropriately combine and integrate the acquired 3-axis attitude angles.

[0081] Furthermore, the three-axis attitude angle / likelihood determination processing unit 14 may select one of the three types of three-axis attitude angles that has the highest likelihood. In addition, the three-axis attitude angle / likelihood determination processing unit 14 may reject the calculated attitude angle when the amount of variation from the previous state is large, when it is outside the attitude angle constraint range, or when it is considered to be an outlier, regardless of the likelihood of each three-axis attitude angle. In addition, the three-axis attitude angle / likelihood determination processing unit 14 may integrate the three-axis attitude angles by weighting the three-axis attitude angles obtained by the star tracker, the three-axis attitude angles based on reception of six or more light source IDs, and the three-axis attitude angles obtained by the six-axis sensor 50 in that order.

[0082] For example, if the measurement by the star tracker is established (step S4; YES) and the 6-degree-of-freedom PnP positioning is established (step S6; YES), the 3-axis attitude angle obtained by the star tracker may be multiplied by 0.7, the 3-axis attitude angle obtained by the 6-degree-of-freedom PnP positioning may be multiplied by 0.2, and the 3-axis attitude angle obtained by the 6-axis sensor 50 may be multiplied by 0.1, and the respective values ​​may be integrated to calculate the current 3-axis attitude angle. Also, if the measurement by the star tracker is not established (step S4; NO) but the 6-degree-of-freedom PnP positioning is established (step S6; YES), the 3-axis attitude angle obtained by the 6-degree-of-freedom PnP positioning may be multiplied by 0.8, and the 3-axis attitude angle obtained by the 6-axis sensor 50 may be multiplied by 0.2, and the respective values ​​may be integrated to calculate the current 3-axis attitude angle. In addition, when the measurement by the star tracker is established (step S4; YES), but the 6-degree-of-freedom PnP positioning is not established (step S6; NO), the 3-axis attitude angle obtained by the star tracker may be multiplied by 0.9, and the 3-axis attitude angle obtained by the 6-axis sensor 50 may be multiplied by 0.1, and the respective values ​​may be integrated to calculate the current 3-axis attitude angle.

[0083] In addition, the three-axis attitude angle / likelihood determination processing unit 14 integrates the likelihood of the integrated attitude angle based on the likelihood corresponding to each attitude angle used to integrate the three-axis attitude angles. For example, the three-axis attitude angle / likelihood determination processing unit 14 calculates the likelihood of the attitude angle by averaging or weighting the likelihood of the integrated attitude angles. This completes the three-axis attitude angle / likelihood determination process.

[0084] In the above description, the three-axis attitude angles obtained by the star tracker, the six-axis sensor 50, and the PnP positioning are each identical to the three-axis attitude angles of the vehicle body (moving body 100). However, if the three-axis attitude angles obtained by the star tracker, the six-axis sensor 50, and the PnP positioning do not each match the three-axis attitude angles of the vehicle body (moving body 100), the following processing may be performed. That is, after step S10, the three-axis attitude angle / likelihood determination processing unit 14 may determine the attitude angle variation from the integrated three-axis attitude angle at the time of installation for the integrated three-axis attitude angle. Here, the integrated three-axis attitude angle at the time of installation is stored in the storage unit 20.

[0085] The three-axis attitude angle / likelihood determination processing unit 14 may add the attitude angle variation to the three-axis attitude angle at the time when the visible light communication camera 30 (moving body 100) was installed, and calculate the current three-axis attitude angle of the visible light communication camera 30. The three-axis attitude angle at the time when the visible light communication camera 30 was installed is stored in the storage unit 20. Furthermore, the likelihood of the three-axis attitude angle of the visible light communication camera 30 calculated in this manner is the same as the likelihood of the three-axis attitude angle integrated in step S10.

[0086] 9 is a flowchart showing the overall process executed in the moving body 100. In the overall process, the process other than that related to the attitude angle will be mainly explained.

[0087] First, the visible light communication processing unit 11 acquires image data of a captured image obtained by capturing an image using the visible light communication camera 30, and performs visible light communication reception processing (step S11). The visible light communication processing unit 11 detects a light emitting point corresponding to the position index 3 from the captured image, and acquires a pair of a light source ID and a coordinate position indicated by visible light communication from each light emitting point on the captured image.

[0088] Next, the positioning processing unit 12 determines whether or not two or more light source IDs have been received through visible light communication (step S12). If the number of light source IDs received through visible light communication is less than two (step S12; NO), the process returns to step S11.

[0089] In step S12, if two or more light source IDs are received through visible light communication (step S12; YES), the positioning processing unit 12 determines whether six or more light source IDs are received through visible light communication (step S13).

[0090] When six or more light source IDs are received by visible light communication (step S13; YES), the positioning processor 12 calculates six degrees of freedom by PnP (n≧6) calculation based on each light source ID (step S14). Specifically, the positioning processor 12 acquires three-dimensional coordinates corresponding to each detected light source ID from the light source ID-three-dimensional coordinate table 21, and calculates the three-axis attitude angles and three-dimensional position of the own device (mobile body 100) based on the coordinate position on the captured image and the three-dimensional coordinate in the world coordinate system.

[0091] Next, the positioning processor 12 provides the three-axis attitude angle based on the reception of six or more light source IDs to the three-axis attitude angle / likelihood determination processor 14 (step S15). This "three-axis attitude angle based on the reception of six or more light source IDs" is the three-axis attitude angle acquired in step S7 of the three-axis attitude angle / likelihood determination process (see FIG. 8).

[0092] In step S13, if the number of light source IDs received by visible light communication is less than 6 (step S13; NO), or after step S15, the positioning processing unit 12 acquires the integrated three-axis attitude angle and attitude angle likelihood from the three-axis attitude angle / likelihood determination processing unit 14 (step S16). The three-axis attitude angle and attitude angle likelihood acquired here are the values ​​integrated in step S10 of the three-axis attitude angle / likelihood determination processing.

[0093] Next, the positioning processing unit 12 acquires, for each of the light source IDs of the two or more position indicators 3 detected by visible light communication, three-dimensional coordinates corresponding to the light source ID from the light source ID-three-dimensional coordinate table 21, and performs positioning calculations (calculation of the three-axis attitude angle and the three-dimensional position) based on the pair of the coordinate position on the captured image and the three-dimensional coordinates in the world coordinate system for each light source ID, and the integrated three-axis attitude angle acquired in step S16 (step S17).

[0094] In addition, when there is a positioning result in which PnP positioning is established based only on the information obtained from the visible light communication camera 30 (six or more light source IDs), this positioning result (the positioning result of step S14) may be integrated with the positioning result based on the integrated three-axis attitude angle and two or more light source IDs obtained from the three-axis attitude angle / likelihood determination processing unit 14 (the positioning result of step S17).

[0095] Next, the positioning processing unit 12 calculates a reprojection error from the positioning result of step S17 (step S18). The reprojection error is a value indicating the likelihood of the coordinates.

[0096] Next, the positioning likelihood evaluation unit 15 calculates the positioning likelihood from the reprojection error calculated in step S18 and the attitude angle likelihood acquired in step S16 (step S19). Here, the positioning likelihood evaluation unit 15 integrates the reprojection error and the attitude angle likelihood to calculate the likelihood of the entire positioning.

[0097] Next, the moving speed control unit 16 refers to the likelihood-speed control table 23 to obtain the maximum speed (speed, turning angular velocity) corresponding to the positioning likelihood, and sets the current allowable maximum speed (step S20).

[0098] Next, the moving speed control unit 16 judges whether or not the speed of the moving body 100 exceeds the maximum allowable speed (step S21). Specifically, the moving speed control unit 16 judges whether or not the speed of the moving body 100 in the traveling direction exceeds the set maximum allowable speed. The moving speed control unit 16 also judges whether or not the angular velocity of the moving body 100 during the turn exceeds the set maximum allowable angular velocity. If the speed of the moving body 100 does not exceed the maximum allowable speed (step S21; NO), the process returns to step S11.

[0099] In step S21, if the speed of the moving body 100 exceeds the maximum allowable speed (step S21; YES), the moving speed control unit 16 suppresses the speed (velocity, angular velocity of turning) of the moving body 100 to the maximum allowable speed (step S22). That is, the moving speed control unit 16 controls the traveling drive unit 60 to adjust the speed of the moving body 100 so that the speed is equal to or lower than the maximum speed. After step S22, the process returns to step S11.

[0100] In the three-axis attitude angle / likelihood determination process (see FIG. 8), if a predetermined condition for the measurement by the star tracker to be established is not satisfied (step S4; NO), and six or more light sources (position indicators 3) are included in the image acquired by the visible light communication camera 30, and six degrees of freedom PnP positioning is established (step S6; YES), the positioning processing unit 12 (estimation means) may estimate the attitude angle and three-dimensional position of the own device based on the attitude angle and three-dimensional position of the own device derived by the six degrees of freedom PnP positioning. That is, the six degrees of freedom calculated by the PnP calculation in step S14 of the overall process (see FIG. 9) may be the attitude angle and three-dimensional position of the own device finally estimated. When the results derived by the star tracker cannot be used, the positioning process can be performed using only the results derived by visible light communication.

[0101] FIG. 10 is an operation example showing how the positioning likelihood and the moving speed of the moving body 100 change with a change in the traveling situation of the moving body 100. In FIG. In Fig. 10, the horizontal axis indicates the passage of time t, and the vertical axis indicates the positioning likelihood and the moving speed. In Fig. 10, the positioning likelihood is indicated by a solid line, and the moving speed is indicated by a dashed line. The scale of "1.0" indicated on the vertical axis is for the positioning likelihood. The positioning likelihood is a value calculated by integrating the reprojection error and the attitude angle likelihood.

[0102] First, in time period T1 immediately after the moving body 100 starts moving, the three-axis attitude angles are determined mainly by the star tracker, and the attitude angle likelihood (likelihood A) becomes 1.0. Therefore, the integrated positioning likelihood also becomes approximately 1.0, and the moving body 100 can accelerate. The moving speed of the moving body 100 gradually increases.

[0103] In time period T2, measurements using the star tracker become difficult, and the three-axis attitude angle is determined from information only from the six-axis sensor 50. However, since the road surface variation level is small, there is no sudden drop in the attitude angle likelihood (likelihood C). As a result, the integrated positioning likelihood also drops gradually. "Small road surface variation level" refers to a situation in which there is a lot of straight driving and the road surface is not very rough. When the positioning likelihood is relatively stable, the moving speed of the moving body 100 is also maintained at a certain level or higher.

[0104] In time period T3, the three-axis attitude angle continues to be determined based only on information from the six-axis sensor 50, but as the road surface variation level increases, the attitude angle likelihood (likelihood C) drops sharply. As a result, the integrated positioning likelihood also drops sharply. A "large road surface variation level" refers to a situation in which there is a large variation in the traveling direction or the road surface is very rough. As the positioning likelihood decreases, the moving speed of the moving body 100 is restricted.

[0105] In time period T4, the speed limit for the moving object 100 continues, and the moving object 100 eventually reaches a speed that can be measured by the star tracker, and the positioning likelihood rapidly recovers. As the positioning likelihood recovers, the moving object 100 becomes able to accelerate, and the moving speed of the moving object 100 increases.

[0106] In time period T5, the three-axis attitude angle is determined only by the information from the six-axis sensor 50, and since the road surface variation level is large, the attitude angle likelihood (likelihood C) drops rapidly. Therefore, the integrated positioning likelihood also drops rapidly. As the positioning likelihood drops, the moving speed of the moving body 100 is restricted.

[0107] In time period T6, the road surface variation level is large, but since the moving body 100 often enters a high-precision area where six or more light source IDs can be detected from the image captured by the visible light communication camera 30, the attitude angle likelihood (mainly due to the influence of likelihood B) becomes high when positioning based on receiving six or more light source IDs is established. As the attitude angle likelihood increases, the integrated positioning likelihood also increases. As the positioning likelihood increases, the moving speed of the moving body 100 also increases. In likelihood integration, when the likelihood increases, it becomes the best integrated value, but when it decreases, a filter is applied, so even if the number of receiving points in visible light communication suddenly decreases, the likelihood does not decrease instantaneously.

[0108] In the above example, a case was described in which a series of speed restrictions are imposed on the moving body 100, eventually resulting in a speed at which measurement can be performed by a star tracker, and the positioning likelihood rapidly recovers, allowing the moving body 100 to accelerate. However, it is also possible to set a threshold value for the positioning likelihood, and stop the movement of the moving body 100 when the positioning likelihood falls below the threshold, thereby enabling measurement by the star tracker.

[0109] As described above, according to this embodiment, the attitude angle obtained by the star tracker and the positioning result obtained by visible light communication are integrated to estimate the attitude angle and three-dimensional position of the moving body 100, so that it is possible to provide highly accurate positioning results in determining the self-position. Furthermore, the positioning likelihood evaluation unit 15 integrates the attitude angle likelihood and the reprojection error to grasp the likelihood of the entire positioning, thereby obtaining a highly accurate positioning result.

[0110] In positioning using visible light communication, the positioning processor 12 uses values ​​acquired from the star tracker or six-axis sensor 50 for the three degrees of freedom of the attitude angle, and obtains a three-dimensional position as the remaining three degrees of freedom. The positioning processor 12 estimates the attitude angle and three-dimensional position of the moving body 100 based on the position of the position indicator 3 in the image acquired by the visible light communication camera 30, using the attitude angle acquired by the three-axis attitude angle / likelihood determination processor 14. This makes it possible to reduce the number of light emitting points (light source IDs) required to be detected from the captured image for positioning suitable for off-road situations where there are fluctuations in height and attitude angle. Positioning is possible if two or more light-emitting points (light source IDs) can be detected from the captured image. Therefore, compared to conventional positioning methods that required six or more position indicators 3 to be captured by the camera at all times, the number and installation density of position indicators 3 can be reduced.

[0111] In particular, for positioning on the lunar surface, etc., a star tracker that is effective for estimating a three-axis attitude angle in space and a six-axis sensor 50 to supplement the star tracker that is not suitable for high-speed response are used, and if six or more light source IDs can be received by visible light communication, the results can also be used in combination to estimate the attitude angle by integrating them.

[0112] In addition, since the likelihood of the attitude angle derived by the star tracker is higher than the likelihood of the attitude angle derived by the positioning processing unit 12 based on receiving six or more light source IDs, the positioning accuracy is improved by using the attitude angle derived by the star tracker.

[0113] In addition, the positioning processing unit 12 weights the results derived by the star tracker attitude angle calculation unit 13, which have a high likelihood of deriving an attitude angle, more heavily than the results derived by the positioning processing unit 12, and combines the results derived by the star tracker attitude angle calculation unit 13 and the results derived by the positioning processing unit 12 to obtain highly accurate positioning results.

[0114] In addition, the star tracker attitude angle calculation unit 13 derives the three-axis attitude angle of the moving body 100 by pattern matching the positional relationship of the stars 4 contained in the image acquired by the star tracker camera 40 with the star chart data 22, so that the absolute attitude angle of the moving body 100 can be obtained with high accuracy. In particular, on the surface of the Moon, even on the side facing the Sun, Star 4 is visible in the sky, making it possible to obtain the attitude angle using a star tracker.

[0115] In addition, in the positioning process related to visible light communication, the three-dimensional position and attitude angle of the moving body 100 can be derived based on the coordinate position of the position indicator 3 in the image acquired by the visible light communication camera 30 and the three-dimensional position of the position indicator 3 in the world coordinate system.

[0116] Furthermore, the likelihood B of the attitude angle based on receiving six or more light source IDs becomes higher as the number of light sources (position indicators 3) in the image acquired by the visible light communication camera 30 increases. Therefore, in the positioning process related to visible light communication, the more light source IDs are detected in the image, the more accurate the positioning results that can be obtained.

[0117] In addition, by using the attitude angle obtained by the six-axis sensor 50, which has a fast response speed, instead of or in addition to the attitude angle obtained by the star tracker, it is possible to provide processing results that cannot be obtained by the star tracker alone.

[0118] Furthermore, by controlling the speed of the moving body 100 based on the likelihood of the overall positioning, the positioning likelihood is maintained at a certain value or more at any time during the movement, and the positioning accuracy can be prevented from decreasing.Finally, by controlling the speed until the moving body stops or moves at a very slow speed, the attitude angle can be measured by the star tracker, which enables the positioning with the highest likelihood.

[0119] The above description of the embodiment is an example of the position acquisition device according to the present invention, and is not limited thereto. The detailed configuration and detailed operation of each part constituting the device can be appropriately changed without departing from the spirit of the present invention.

[0120] [Variation 1] In the first modification, the light sources imaged by the visible light communication camera 30 are not limited to those capable of transmitting a light source ID. In the above embodiment, it is sufficient to arrange the position indicators 3 so that two or more points are visible from the visible light communication camera 30 in a wide area where positioning is required. However, the fewer the number of position indicators 3 required and the lower the installation cost per unit, the more practical the system will be.

[0121] Therefore, it is not necessary for all light sources captured by the visible light communication camera 30 to have position indicators 3 capable of transmitting light source IDs, and some of them may simply be high-luminance auxiliary light sources. However, the three-dimensional coordinates of the installation positions of the auxiliary light sources are known, as are the position indicators 3. Furthermore, in the captured image obtained by the visible light communication camera 30, the auxiliary light sources can also be individually identified based on their positional relationship with the position indicators 3 that transmit the surrounding light source IDs, and the coordinate positions of the auxiliary light sources in the captured image can be obtained. Since the auxiliary light sources do not need the function of transmitting light source IDs, the structure can be simplified and costs can be reduced.

[0122] In addition, since the lunar space is a vacuum with no atmospheric scattering and the background is always dark, it is acceptable to use a supplementary light source that uses sunlight, assuming that it will be used during the day. For example, if a supplementary light source that appropriately reflects and diffuses sunlight is installed in a high position with the sky as the background, a high-brightness indicator without a battery can be obtained.

[0123] The structure of the non-powered auxiliary light source is a rectangular parallelepiped, sphere, square tube, cylinder, etc., formed by a semi-transparent diffuser plate with a transmittance of about 50 to 70%. When the sun is on the same side of the auxiliary light source as the visible light communication camera 30, the semi-transparent diffuser plate mainly diffuses the sunlight, making the auxiliary light source appear to shine with high luminance from the visible light communication camera 30. On the other hand, when the sun is on the opposite side of the auxiliary light source to the visible light communication camera 30, the semi-transparent diffuser plate mainly transmits the sunlight, making the auxiliary light source appear to shine with high luminance from the visible light communication camera 30.

[0124] In this way, by using a non-powered position indicator as an auxiliary light source that is installed to fill in the gaps between position indicators 3 that transmit light source IDs, it is possible to significantly reduce both the cost of the light source itself and the cost of installing the light source over an entire wide area.

[0125] [Modification 2] In the second modification, a star tracker is not used to obtain the attitude angle. In the above embodiment, a case has been described in which a star tracker is used as a means for acquiring the absolute attitude angle of the moving body 100, but the present invention can also be used in situations other than space, such as on the surface of the moon. Even on Earth, the present invention is effective in wide indoor and underground areas where satellite position information cannot be used, and in areas where the geomagnetism is unstable.

[0126] For example, in a wide underground space, a known lighting device or the like is used instead of a star 4 as a light source to be imaged by an imaging means (first imaging means) separate from the visible light communication camera 30. However, to treat it exactly like a star tracker, the arrangement of the lighting devices is made non-uniform so that each lighting device can be identified. Specifically, the attitude angle of the device itself is derived by pattern matching between the positional relationship of the multiple lighting devices included in the image acquired by the first imaging means and pre-prepared light source arrangement data (data recording the arrangement of the lighting devices). Other processing is the same as that in the above embodiment.

[0127] In the above embodiment, an example was described in which two front and rear cameras were used as the visible light communication camera 30, but the visible light communication camera 30 may be composed of three or more cameras. Positioning processing using a series of visible light communication cameras 30 can be performed using one camera, two cameras, or three or more cameras, but the most recommended configuration is a two-camera configuration (front and rear) to achieve high-precision positioning obtained from the camera's detection range and resolution.

[0128] In addition, when allocating the same total number of pixels, it is possible to capture each pixel in detail (making it possible to identify more distant objects) by limiting the angle of view by cutting out only the front and rear directions rather than taking the angle of view over the entire circumference. Therefore, in most cases, a better balance between detection and accuracy of light source ID in visible light communication is achieved by mounting two cameras with the same angle of view on the front and rear, rather than a single camera with double the angle of view. When capturing images at two angles of view, the mathematical processing model is the same even if two cameras are installed, one facing forward and the other 90 degrees to the side. However, since the accuracy when receiving light source IDs from only two points depends on the apparent horizontal angle, it is best to use a positional relationship that allows images to be captured at the greatest angle (for example, front and rear).

[0129] In addition, 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 also be applied. In addition, a carrier wave can be applied as a medium for providing program data via a communication line. [Explanation of symbols]

[0130] 1 Positioning System 3A,3B,3C,3D,... position index 4A,4B,4C,4D,4E,4F,... Star 10 Control section 11 Visible light communication processing section 12 Positioning processing section 13 Star tracker attitude angle calculation unit 14 3-axis attitude angle / likelihood determination processing unit 15 Positioning likelihood evaluation unit 16 Movement speed control section 20 Memory section 21 Light source ID-3D coordinate table 22 Star Chart Data 23 Likelihood-Rate Control Table 30 (30A, 30B) Visible light communication camera 40 Star Tracker Camera 50 6-axis sensor 60 Travel drive unit 100 Mobile

Claims

1. An attitude angle measuring device provided on a moving body capable of moving in space above the ground surface, comprising: a correction means for correcting the first attitude angle or the second attitude angle based on a first likelihood for a first attitude angle of the moving body derived by detecting the number of celestial bodies that can be captured as images by the imaging means and their positions within an angle of view among a plurality of celestial bodies whose position information is associated with each other and registered in advance, and a second likelihood for a second attitude angle of the moving body derived by detecting the number of artificial light sources that can be captured as images by the imaging means and their positions within an angle of view among a plurality of artificial light sources that are previously arranged in a space on the ground surface as position indicators, An attitude angle measuring device comprising:

2. The correction means corrects the first attitude angle or the second attitude angle based on a third likelihood for a third attitude angle of the moving body derived by detecting acceleration in three axial directions and angular velocity about three axes.

2. The attitude angle measuring device according to claim 1 .

3. An attitude angle measuring method performed by an attitude angle measuring device provided on a moving body capable of moving in a space above the ground surface, comprising: a correction process for correcting the first attitude angle or the second attitude angle based on a first likelihood for a first attitude angle of the moving body derived by detecting the number of celestial bodies that can be captured as images by the imaging means and their positions within an angle of view among a plurality of celestial bodies whose position information is associated with each other and registered in advance, and a second likelihood for a second attitude angle of the moving body derived by detecting the number of artificial light sources that can be captured as images by the imaging means and their positions within an angle of view among a plurality of artificial light sources that are arranged in advance as position indicators in a space on the ground surface, 2. An attitude angle measuring method comprising:

4. A computer of an attitude angle measuring device provided on a mobile body capable of moving in space above the surface of the earth, and functioning as a correction means for correcting the first attitude angle or the second attitude angle based on a first likelihood for a first attitude angle of the moving body derived by detecting the number of celestial bodies that can be captured as images by the imaging means and their positions within the angle of view among a plurality of celestial bodies whose position information is associated with each other and registered in advance, and a second likelihood for a second attitude angle of the moving body derived by detecting the number of artificial light sources that can be captured as images by the imaging means and their positions within the angle of view among a plurality of artificial light sources that are arranged in advance as position indicators in the space above the earth's surface. A program characterized by:

Images