JP7363225B2 - Measuring device, measuring system, measuring method and program - Google Patents

Description

The present invention relates to a technique for acquiring information on the position and posture of a moving body.

When controlling a moving body, the position and orientation of the moving body are important information. Furthermore, in order to avoid an increase in the size and weight of a moving object, it is desirable that a device for measuring the position and orientation of a moving object be suppressed from becoming larger and more complex.

Control devices for moving objects often acquire information about the surrounding environment and moving objects using a combination of millimeter wave radar, LIDAR (Light Detection and Ranging), acceleration sensors, gyro sensors, and cameras. . Among the above, millimeter wave radar and LIDAR are used to obtain information on distances to surrounding obstacles. Acceleration sensors and gyro sensors are used to obtain information about the direction and posture of a moving object. Furthermore, the camera is used to detect and recognize moving objects to be avoided. However, millimeter wave radars and LIDARs are expensive and require a combination of acceleration sensors and gyro sensors in order to determine the direction of motion, which can result in complex device configurations. Therefore, it is desirable to be able to acquire position and orientation information necessary for controlling a moving object while suppressing the increase in size and complexity of the measuring device. As a technique for obtaining position and orientation information for controlling a moving object while suppressing the increase in size and complexity of a measuring device, a technique such as that disclosed in Patent Document 1, for example, has been disclosed.

Patent Document 1 relates to a position/orientation estimation device that estimates the position and orientation of a camera from an acquired image. The position/orientation estimating device disclosed in Patent Document 1 calculates the error between a feature point on an image and a reference point registered in advance as a map point, and estimates the orientation and position of the camera.

Japanese Patent Application Publication No. 2016-91065

However, the technique of Patent Document 1 is not sufficient in the following points. In Patent Document 1, images captured by two cameras are required when estimating the position and orientation of the camera. Therefore, in the technique of Patent Document 1, it is necessary to mount a plurality of cameras on a moving object, which may lead to an increase in the size of the device. Therefore, the technique of Patent Document 1 is not sufficient as a technique for measuring the position and orientation of a moving object while suppressing the increase in size and complexity of the device.

An object of the present invention is to provide a measuring device that can measure the position and orientation of a moving object without increasing the size of the device.

In order to solve the above problems, the measuring device of the present invention includes an image acquiring means, a feature point extracting means, a specifying means, a distance calculating means, and an attitude determining means. The image acquisition means acquires an image of the surroundings of the moving body taken by a camera included in the moving body. The feature point extraction means extracts a plurality of feature points from the image. The identifying means identifies a structure corresponding to the feature point. The distance calculation means calculates the distance from the specified structure to the moving object based on data regarding the size of the structure stored in advance for each structure. The attitude identifying means determines the coordinates and attitude of the moving body based on the distances from the moving body to the plurality of structures and the coordinate data of each of the structures, and outputs information on the coordinates and attitude of the moving body.

The measurement method of the present invention acquires an image of the surroundings of the moving body taken by a camera included in the moving body, extracts a plurality of feature points from the image, and identifies structures corresponding to the feature points. The distance and orientation identification method of the present invention calculates the distance from the identified structure to the moving body based on data related to the size of the structure that is stored in advance for each structure. The distance/attitude identification method of the present invention determines the coordinates and attitude of a moving body based on the distance from the moving body to a plurality of structures and the coordinate data of each structure, and outputs information on the coordinates and attitude of the moving body. do.

According to the present invention, the position and orientation of the camera that captured the image can be measured without increasing the size of the device.

FIG. 1 is a diagram showing an outline of the configuration of a first embodiment of the present invention. It is a figure showing an outline of composition of a 2nd embodiment of the present invention. 7 is a flowchart showing the operation of the second embodiment of the present invention. FIG. 7 is a diagram schematically showing the camera of the imaging unit and the feature points of each structure in the second embodiment of the present invention. FIG. 6 is a diagram schematically showing the relationship between the size and distance between the camera of the imaging unit and the object to be photographed in the second embodiment of the present invention. FIG. 7 is a diagram schematically showing the positions of a camera of a photographing section and structures in a second embodiment of the present invention. It is a figure showing the example of other composition of the measurement device of a 2nd embodiment of the present invention.

(First embodiment)
A first embodiment of the present invention will be described in detail with reference to the drawings. FIG. 1 is a diagram showing an outline of the configuration of a measuring device according to this embodiment. The measuring device of this embodiment includes an image acquisition section 1, a feature point extraction section 2, a specification section 3, a distance calculation section 4, and a posture determination section 5. The image acquisition unit 1 acquires an image of the surroundings of a moving body captured by a camera included in the moving body. The feature point extraction unit 2 extracts a plurality of feature points from the image. The identifying unit 3 identifies a structure corresponding to the extracted feature point. Here, identification corresponds to identification or estimation. The distance calculation unit 4 calculates the distance from the specified structure to the moving object based on data related to the size of the structure stored in advance for each structure. The attitude determining unit 5 determines the coordinates and attitude of the moving body based on the distances from the moving body to a plurality of structures and the coordinate data of each of the structures, and outputs information on the coordinates and attitude of the moving body.

In the measuring device of this embodiment, the feature point extraction section 2 extracts feature points from the image obtained by the image acquisition section 1, and the identification section 3 identifies structures corresponding to the feature points. In addition, in the measuring device of this embodiment, the distance calculating section 4 calculates the distance from the structure to the moving object, and the attitude determining section 5 determines the coordinates and attitude of the moving object. The measuring device of this embodiment can determine the coordinates and orientation of a moving body from an image acquired by one camera. Therefore, it is possible to suppress the increase in size of the device.

(Second embodiment)
A second embodiment of the present invention will be described in detail with reference to the drawings. FIG. 2 is a diagram showing an overview of the configuration of the measurement system of this embodiment. The measurement system of this embodiment includes an imaging section 10 and a distance/posture measurement section 20. The measurement system of this embodiment is a system that measures the position and orientation of a moving object and outputs information on the position and orientation of the moving object to a control unit that controls the operation of the moving object. The measurement system of this embodiment measures the position and orientation of a moving object based on images taken by the camera of the imaging unit 10. The measurement system of this embodiment can be used, for example, as a system that provides information on the position and attitude of a moving object to a control unit of a moving object such as an unmanned aircraft such as a drone, a flying object that flies under autonomous control, or a self-driving car. Can be done.

The photographing unit 10 has a function of photographing an image of the surrounding environment of the mobile body, which is used when measuring the position and orientation of the mobile body. The photographing unit 10 of this embodiment is configured using a visible light camera. The photographing unit 10 may include an infrared camera. The photographing section 10 sends data of the photographed image to the distance/posture measuring section 20. Further, the photographing section 10 and the distance/attitude measuring section 20 may be formed as an integrated device, or the photographing section 10 and the distance/attitude measuring section 20, which are formed separately, may communicate via a communication cable. Alternatively, the configuration may be such that communication is performed by wireless communication.

The configuration of the distance/posture measuring section 20 will be explained. The distance/posture measurement section 20 includes an image acquisition section 21 , a feature point extraction section 22 , a specification section 23 , a distance calculation section 24 , a posture determination section 25 , and a feature point registration section 26 .

The image acquisition unit 21 has a function as an interface for acquiring image data from the imaging unit 10. The image acquisition section 21 converts the image data acquired from the photographing section 10 into a data format used within the distance/posture measurement section 20 and outputs it to the feature point extraction section 22 .

The feature point extraction unit 22 has a function of extracting feature points from an image of the surroundings of the moving body photographed by the camera of the photographing unit 10. The feature point extraction unit 22 extracts points with large brightness changes in the image as feature points from images around the moving body, and calculates the brightness gradient and direction of brightness change between the extracted points and the surroundings. The point where the brightness change is large refers to a point where, for example, the brightness change per predetermined number of pixels is larger than the brightness change in the surrounding area by a preset value or more. The feature point extraction unit 22 extracts, for example, end points of each side of the structure, which are points with large brightness changes, as feature points. The feature point extraction unit 22 may also detect feature points using SIFT (Scale Invariant Feature Transform) and output feature point vectors as feature point information. The feature point extraction unit 22 outputs information on the extracted feature points to the identification unit 23.

The identifying unit 23 refers to the database of the feature point registering unit 26 and identifies the structure to which the feature point extracted by the feature point extracting unit 22 corresponds. The identifying unit 23 identifies structures that match the feature points extracted by the feature point extracting unit 22. When enlarging and reducing structures on the database, the identifying unit 23 identifies structures whose mutual positional relationship of feature points on the structure matches the mutual positional relationship of feature points extracted from the image. The feature points in the image are identified as corresponding structures. The identifying unit 23 outputs coordinate data of a structure identified as a structure with which the feature points correspond to the distance calculating unit 24 .

The distance calculation unit 24 calculates the distance between the structure in the image and the imaging unit 10 based on the coordinate data of the structure. Coordinates indicating the positions of structures and moving objects are set as a three-dimensional coordinate system with a preset reference point as the origin. The method of calculating the distance between the structure and the photographing section 10 in the distance calculation section 24 will be explained later. The distance calculation unit 24 outputs information on the calculated distance between the structure and the imaging unit 10 and the coordinate data of the structure to the attitude determination unit 25.

The posture determining section 25 determines the posture of the photographing section 10 based on information on the calculated distance between the structure and the photographing section 10 and the coordinate data of the structure. The posture determination unit 25 calculates the angle formed by the two structures when the imaging unit 10 is used as a reference. That is, the posture determining unit 25 calculates the angle formed by the two straight lines when straight lines are drawn from the imaging unit 10 to each of the two structures. For example, the attitude determination unit 25 determines the attitude of the imaging unit 10 by calculating the angles formed by two of the three structures by changing the combination of structures. The method of calculating the angle formed by the structure in the attitude determination unit 25 will be explained later. The image acquisition section 21, the feature point extraction section 22, the identification section 23, the distance calculation section 24, and the attitude determination section 25 are configured using, for example, a semiconductor device such as one or more FPGAs (Field Programmable Gate arrays). . The processing in each part of the image acquisition section 21, feature point extraction section 22, identification section 23, distance calculation section 24, and posture determination section 25 may be performed by a CPU (Central Processing Unit) executing a computer program. .

The feature point registration unit 26 has a database in which points on structures in the surrounding environment are registered as feature points. In the database stored in the feature point registration unit 26, coordinates of surrounding structures, information on feature points on the structures, and coordinates of feature points within the range in which the moving object is expected to move are stored in association with each other. ing. As structures to be registered as feature points, structures whose shapes and sizes are defined as standards or structures that are known are selected. Information on a feature point is obtained by calculating the brightness change of a structure in the image from the surrounding image acquired in advance, and calculating the brightness gradient and direction of brightness change near the feature point as the feature point information and the coordinates of the feature point. are associated and saved. The feature point registration unit 26 is configured by a nonvolatile semiconductor memory, a hard disk drive, or a combination thereof. In addition, the feature point registration unit 26 obtains data on feature points of structures existing in a range where a moving object is estimated to exist from a management server etc. via a wireless communication line, etc., and stores the obtained data as a database. The configuration may be such that

The operation of the measurement system of this embodiment will be explained. FIG. 3 shows an operation flow when specifying the position and orientation of the imaging unit 10 in the measurement system of this embodiment.

First, the surroundings are photographed by the camera of the photographing section 10. Photographing is performed, for example, in response to a request from a control unit of a moving body equipped with a measurement system. Furthermore, photographing may be performed at preset time intervals.

When photographing, the photographing section 10 sends data of the photographed image to the image acquisition section 21 of the distance/posture measuring section 20. After acquiring the image data from the imaging unit 10, the image acquisition unit 21 outputs the received image data to the feature point extraction unit 22 (step S101).

Upon receiving the image data, the feature point extraction unit 22 extracts feature points from the image (step S102). The feature point extraction unit 22 extracts, for example, points with large brightness changes in the image as feature points. When points with large brightness changes are extracted as feature points, the feature point extraction unit 22 calculates the brightness gradient and direction of brightness change of the feature points. After extracting the feature point information, the feature point extracting section 22 outputs the extracted feature point information to the specifying section 23 .

Upon receiving the feature point information, the identifying unit 23 refers to the database of the feature point registration unit 26 and identifies the structure to which the feature point extracted from the image corresponds (step S103). When enlarging and reducing structures on the database, the identifying unit 23 identifies structures whose mutual positional relationship of feature points on the structure matches the mutual positional relationship of feature points extracted from the image. The feature points in the image are identified as corresponding structures. After specifying the structure corresponding to the feature point, the specifying unit 23 searches the database for coordinate data of each feature point of the structure specified from the database.

After searching for the coordinate data of each feature point, the identification unit outputs the coordinate data (also referred to as coordinate information) of the feature point of each structure to the distance calculation unit 24 .

Upon receiving the coordinate data of the feature points of each structure, the distance calculation unit 24 calculates the distance from each structure to the imaging unit 10 (step S104). A method of calculating the distance from each structure to the photographing section 10 in the distance calculation section 24 will be explained with reference to FIGS. 4 and 5. FIG. 4 is a diagram schematically showing the camera of the imaging unit 10 and the feature points of each structure in the surrounding environment to be photographed. Further, FIG. 5 is a diagram schematically showing the relationship between the size of the image sensor of the camera of the photographing unit 10, the size of the structural part that is the object to be photographed, and the distance between the camera and the object to be photographed.

The size of the structure is _L1 , the pixel pitch x the number of pixels of the structure, that is, the size on the image sensor, is _L2 , the distance from the lens of the imaging unit 10 to the structure is x, and the focal length, that is, the distance from the lens to the image sensor. Assuming that the known focal length is F, the focal length F is expressed as in the following equation (1).
F=xL ₂ /L ₁ ... (Formula 1)
Here, the size _L1 is known, and for example, the distance between the uppermost position and the lowermost position of the actual structure is used. Further, the size _L2 on the image sensor is calculated from the length between the uppermost feature point and the lowermost feature point of the image of the structure on the image sensor.

Further, when the above equation 1 is transformed, the following equation 2 is obtained.
x=FL ₁ /L ₂ (Formula 2)
Using Equation 2 above, the distance x from the lens to the object can be calculated. The distance x from the lens of the imaging unit 10 to the structure is calculated for each structure.

After calculating the distance from each structure to the imaging unit 10, the distance calculation unit 24 outputs information on the distance from each structure and the coordinates of each structure to the attitude determination unit 25.

Upon receiving the information on the distance and coordinates from each structure, the posture determining section 25 specifies the position and posture of the photographing section 10 from the distances to the plurality of structures (step S105). The posture determining section 25 first calculates the position of the photographing section 10. A method for calculating the position and orientation of the camera of the photographing unit 10 in the orientation determination unit 25 will be described with reference to FIG. FIG. 6 is a diagram schematically showing the positions of the camera of the photographing unit 10 and the structures. Below, as shown in FIG. 6, a method for specifying the posture of the imaging unit 10 based on information on the distances to three structures A, B, and C will be described as an example.

The coordinates of structure A are (x _i , y _i , z _i ), the coordinates of structure C are (x _j , y _j , z _j ), and the coordinates of structure C are (x _k , y _k , z _k ). Suppose that The coordinates of each structure are set in advance at representative points on the structure. For example, the coordinates of the center of gravity calculated from feature points on the structure are used as the coordinates of the representative points of the structure. Further, it is assumed that the coordinates of the photographing unit 10 (the coordinates of the focal position of the camera lens) are (x, y, z). At this time, if the distance between the camera of the imaging unit 10 and structure A is r ₁ , the distance between the camera of the imaging unit 10 and structure B is r ₂ , and the distance between the camera of the imaging unit 10 and structure C is r ₃ , then The distance between the camera of the imaging unit 10 and the structure can be expressed as follows.
(x−x _i ) ² + (y−y _i ) ² + (z−z _i ) ² = r ₁ ² (Formula 3)
(x−x _j ) ² + (y−y _j ) ² + (zz−z _j ) ² = r ₂ ² (Formula 4)
(x−x _k ) ² + (y−y _k ) ² + (zz−z _k ) ² = r ₃ ² (Formula 5)
When formula 3 is subtracted from formulas 4 and 5 and rearranged, the following is obtained.
(x _i −x _j )x+(y _i −y _j )y+(z _i −z _j )z=D (Formula 6)
(x _i −x _k )x+(y _i −y _k )y+(z _i −z _k )z=E (Formula 7)
D in Equation 6 and E in Equation 7 above are respectively D=((r ₁ ² −x _i ² −y _i ² −z _i ² )−(r ₂ ² −x _j ² −y _j ² −z _j ² ))/2
E=((r ₁ ² −x _i ² −y _i ² −z _i ² )−(r ₃ ² −x _k ² −y _k ² −z _k ² ))/2
It is said that

Here, x _ij = x _i - x _j , y _ij = y _i - y _j , z _ij = z _i - z _j , x _ik = x _i - x _k , y _ik = y _i - y _k , z _ik =z _i −z _k , then equations 6 and 7 can be expressed as follows, respectively.
x _ij x+y _ij y+z _ij z=D (Formula 8)
x _ik x+y _ik y+z _ik z=E...(Formula 9)
Transforming equations 8 and 9, we get
x=((Dy _ik -Ey _ij )+(y _ij z _ik -y _ik z _ij )z)/(x _ij y _ik -x _ik y _ij )
y=((Ex _ij -Dx _ik )+(x _ik z _ij -x _ij z _ik )z)/(x _ij y _ik -x _ik y _ij )
becomes.

The above formula can be further organized as follows.
x=A ₀ +A ₁ z...(Formula 10)
y=B ₀ +B ₁ z...(Formula 11)
In equations 10 and 11, A ₀ = (Dy _ik - Ey _ij )/(x _ij y _ik - x _ik y _ij ), A ₁ = (y _ij z _ik - _{y ik} z _ij )/(x _ij y _ik -x _ik y _ij ), B ₀ = (Ex _ij -Dx _ik )/(x _ij y _ik -x _ik y _ij ), B ₁ = (x _ik z _ij -x _ij z _ik )/(x _ij y _ik -x _ik y _ij ).

Here, by substituting Equation 10 and Equation 11 into Equation 3, a quadratic equation regarding z is obtained.
Fz ² +2Gz+H=0...(Formula 12)
In equation 12, F=A ₁ ² +B ₁ ² +1, G=A ₁ (A ₀ −x _i )+B ₁ (B ₀ −y _i )−z _i , H=(A ₀ −x _i ) ² +( B ₀ −y _i ) ² −z _i ² −r ₁ ² .

Solving equation 12 results in the following.
z=(-G±(G ² -FH) ^0.5 )/F
By substituting the above z into Equations 10 and 11, x and y of the coordinates of the position of the imaging unit 10 are calculated.

After calculating the coordinates of the imaging unit 10, the attitude determination unit 25 calculates the angle between the two structures with respect to the imaging unit 10. The posture determining unit 25 calculates the angle between two structures for each combination of two structures among the plurality of structures.

The posture determination unit 25 calculates the angle θ _ij , the angle θ _ik , and the angle θ _kj as follows.
cosθ _ij = (r ₁ ² + r ₂ ² - d _ij ² )/2r ₁ r ₂
cosθ _ik = (r ₁ ² + r ₃ ² - d _ik ² )/2r ₁ r ₃
cosθ _kj = (r ₂ ² + r ₃ ² - d _kj ² )/2r ₂ r ₃
For d _ij , d _{ik ,} and d _kj indicating the actual distances between the structures, values registered in a database or values calculated from the coordinates of two measured points on the actual structures are used, respectively.

After calculating the angle between the structures, the attitude determining unit 25 searches the database for information on the attitude of the imaging unit 10 corresponding to the angle between each structure with the imaging unit 10 as a reference. Information indicating the relationship between the angle between each structure and the posture of the imaging unit 10 with respect to the imaging unit 10 is stored in a database.

After determining the orientation of the imaging unit 10, the orientation determining unit 25 outputs information on the coordinates and orientation of the imaging unit 10 to the control unit of the moving body equipped with the measurement system. A control unit of the mobile body equipped with the measurement system calculates the position and orientation of the mobile body based on the coordinates and orientation information of the imaging unit 10, and controls the operation of the mobile body. The control unit of the moving body holds in advance information about the attachment angle of the imaging unit 10 to the moving body. The distance/attitude measurement unit 20 converts the angle between each structure and the coordinates of the imaging unit 10 with respect to the imaging unit 10 into information on the position and orientation of the moving object, based on preset correction values. It may be converted and output to the control unit of the moving object.
In the measurement system of this embodiment, the distance/orientation measuring section 20 extracts feature points from an image photographed by the photographing section 10, and identifies a corresponding structure from the extracted feature points. Further, in the measurement system of the present embodiment, the distance/attitude measurement unit 20 calculates the distance between the imaging unit 10 and the structure, and based on the information on the distances to the plurality of structures, the position and orientation of the imaging unit 10 are calculated. is being calculated. Therefore, the measurement system of this embodiment can determine the position and orientation of the imaging unit 10, that is, the position and orientation of the moving body equipped with the imaging unit 10, using only one camera. As a result, the measurement system of this embodiment can measure the position and orientation of the moving object while suppressing the increase in size of the device.

Each process in the measuring device of the first embodiment and the distance/posture measuring unit 20 of the second embodiment may be performed by executing a computer program on a computer. FIG. 7 shows an example of the configuration of a computer 30 that executes a computer program that performs each process in the distance/orientation measuring section 20. The computer 30 includes a CPU 31, a memory 32, a storage device 33, and an I/F (Interface) section 34. Further, the measuring device of the first embodiment also has a similar configuration.

The CPU 31 reads computer programs for performing each process from the storage device 33 and executes them. The memory 32 is configured with a DRAM (Dynamic Random Access Memory) or the like, and temporarily stores computer programs executed by the CPU 31 and data being processed. The storage device 33 stores a computer program executed by the CPU 31 and a database of feature points of structural parts in the surrounding environment. The storage device 33 is configured by, for example, a nonvolatile semiconductor storage device. Other storage devices such as a hard disk drive may be used as the storage device 33. The I/F unit 34 is an interface that inputs and outputs data with the imaging unit 10, a control unit of a moving body equipped with a measurement system, and the like. The computer 30 may further include a communication module that communicates with other information processing devices via a communication network.

Further, the computer program for each process can be stored in a recording medium and distributed. As the recording medium, for example, a magnetic tape for data recording or a magnetic disk such as a hard disk can be used. Further, as the recording medium, an optical disc such as a CD-ROM (Compact Disc Read Only Memory) can also be used. A nonvolatile semiconductor memory may be used as the recording medium.

1 Image acquisition section 2 Feature point extraction section 3 Specification section 4 Distance calculation section 5 Posture determination section 10 Photography section 20 Distance/posture measurement section 21 Image acquisition section 22 Feature point extraction section 23 Specification section 24 Distance calculation section 25 Posture determination section 26 Feature point registration unit 30 Computer 31 CPU
32 Memory 33 Storage device 34 I/F section

Claims (10)

image acquisition means for acquiring an image of the surroundings of the mobile body taken by a camera included in the mobile body;
Feature point extraction means for extracting a plurality of feature points from the image;
identification means for identifying a structure corresponding to the feature point;
distance calculation means for calculating the distance from the specified structure to the moving body based on data regarding the size of the structure that is stored in advance for each structure;
Based on the distance from the moving body to the plurality of structures and the coordinate data of each of the structures , calculate the angle formed by the straight line between the moving body and each of the structures, and based on the calculation result, A measuring device comprising: attitude determining means for determining the coordinates and attitude of the moving body and outputting information on the coordinates and attitude of the moving body. Further comprising feature point registration means for registering information on structures around the moving object and feature points on the structures as a database,
The measuring device according to claim 1, wherein the specifying means specifies the structure by referring to the database. The distance calculation means calculates a distance from the structure to the moving body based on the size of the specified structure in the image and data regarding the size of the structure registered in the database. The measuring device according to claim 2, characterized by: 4. The measuring device according to claim 1, wherein the specifying means specifies at least three of the structures. 5. The measuring device according to claim 1, wherein the feature point extraction means extracts the feature points based on a change in brightness of the image. A camera that photographs the surroundings and outputs the data of the photographed image,
The measuring device according to any one of claims 1 to 5,
A measurement system, wherein the image acquisition means of the measurement device acquires the image from the camera. Obtaining an image of the surroundings of the moving body taken by a camera possessed by the moving body,
extracting a plurality of feature points from the image,
identifying a structure corresponding to the feature point;
Calculating the distance from the specified structure to the moving object based on data regarding the size of the structure stored in advance for each structure,
Calculating the angle formed by a straight line between the moving body and each of the structures based on the distance from the moving body to the plurality of structures and the coordinate data of each of the structures,
determining the coordinates and orientation of the moving body based on the calculation results ;
A measurement method characterized by outputting information on the coordinates and orientation of the moving body. retaining information on structures around the moving object and feature points on the structures as a database;
8. The measuring method according to claim 7, wherein the structure is identified by referring to the database. The measuring method according to claim 7 or 8, characterized in that at least three of the structures are specified . A process of acquiring an image of the surroundings of the moving body taken by a camera included in the moving body;
a process of extracting a plurality of feature points from the image;
a process of identifying a structure corresponding to the feature point;
a process of calculating a distance from the identified structure to the moving body based on data regarding the size of the structure that is stored in advance for each structure;
Based on the distance from the moving body to the plurality of structures and the coordinate data of each of the structures , calculate the angle formed by the straight line between the moving body and each of the structures, and based on the calculation result, A program that causes a computer to execute the following steps: determining the coordinates and orientation of the moving body, and outputting information on the coordinates and orientation of the moving body.

Images