JP5761554B2 - Moving body posture determination method, moving body posture determination program, and moving body posture determination apparatus - Google Patents

Description

  The present invention relates to a method, a program, and an apparatus for determining the attitude of a moving body such as an aircraft moving in the air, an automobile moving on a road, a train moving on a track, a ship moving on the sea, and the like. The present invention relates to a method, a program, and an apparatus for performing positioning and image acquisition during movement and determining the moving body posture during movement based on these results.

  While the moving body is moving, the posture of the moving body changes, and techniques for detecting the posture are adopted in various fields. For example, in the case of an aircraft or a ship, the posture during movement affects the route and stability of the aircraft, so that the posture of the vehicle may be detected and grasped during movement. Alternatively, depending on the object to be transported, it is necessary to control the posture of the moving body, and thus the moving body posture during movement may be grasped. In particular, when measurement is performed by aircraft such as aerial photogrammetry and laser measurement (FIG. 7), the shooting direction and the laser irradiation direction are extremely important, and the attitude of the aircraft at the time of measurement is always acquired.

  In order to acquire the attitude of an aircraft in flight, as shown in Patent Document 1, GPS (Global Positioning System) and IMU (Internal Measurement Unit) are usually used. However, the IMU is expensive and requires a large amount of cost to adopt, for example, an impact-resistant mechanism is required when mounted on an aircraft. In the method using a combination of GPS and IMU, the calculation of the moving body posture is generally post-processing, and the posture cannot be acquired in real time during movement and reflected in posture control or the like.

  Therefore, Patent Document 2 proposes a method for acquiring posture angle information using a magnetic sensor and a gravitational acceleration sensor instead of the IMU. Further, Patent Literature 3 proposes a technique of “using an image including the sun” focused on in the present invention.

JP-A-2004-245741 JP 2004-264028 A JP 2010-173544 A

  Patent Document 1 shown here is a method for acquiring posture information by a combination of GPS and IMU as described above, and the calculation of the moving body posture is post-processing and real-time processing during movement cannot be performed. It has a problem of cost. Although Cited Document 2 does not use an IMU, there is no problem as in Cited Document 1, but the method using a magnetic sensor and a gravitational acceleration sensor is inferior in accuracy to obtain compared to a combination of GPS and IMU, and aerial photography. This technique is difficult to adopt for surveying or laser measurement. Although cited document 3 is paying attention to the point of "using the image containing the sun" like this application, it is for controlling the air conditioner for vehicles, and for determining the posture of a moving body. is not.

  SUMMARY OF THE INVENTION An object of the present invention is to solve the above problems, and to provide a method, a program, and an apparatus that can reduce the cost for adoption and can determine a moving body posture with high accuracy.

The determination method of the moving body posture is a method of determining the posture of the moving body that is moving, the position measuring means mounted on the moving body measures the position of the moving body twice or more during the movement, The image acquisition means mounted on the moving body acquires an image including the sun during movement, acquires an image acquisition time when the image is acquired, and based on the measured two or more moving body positions, The moving direction of the moving body is calculated, and based on the measured moving body position, an image acquisition position that is the position of the moving body when the image is acquired is obtained, and the solar position in the image based on the luminance of the image And an arbitrary coordinate system based on the calculated moving direction of the moving body is provided based on a coordinate axis adapted to the posture of the moving body, the image acquisition position being the origin, and based on the sun position in the image The task A measurement vector from the origin of the coordinate system to the sun is obtained, and an absolute coordinate system that is a coordinate axis composed of a horizontal plane and a vertical axis and that has the image acquisition position as an origin is provided, and based on the image acquisition position and the image acquisition time A reference vector heading from the origin of the absolute coordinate system toward the sun, combining the measurement vector and the reference vector, and calculating a posture of the moving object based on an angular difference between the arbitrary coordinate axis and the absolute coordinate axis. .

  In this case, the position measuring means measures the position of the moving body, acquires the measurement time, and calculates the image acquisition position based on the measured position of the moving body, the measurement time, and the image acquisition time. You can also

The moving body posture determination program is a program for determining the posture of a moving body that is moving, and a function of reading two or more moving body positions measured during movement by a position measuring unit mounted on the moving body. And a function of reading an image including the sun acquired during movement by the image acquisition means mounted on the mobile body and an image acquisition time when the image was acquired, and the two or more mobile body positions, A function for calculating the moving direction of the moving body, a function for obtaining an image acquisition position that is a position of the moving body when an image is acquired based on the position of the moving body, and a luminance in the image based on the brightness of the image a function of extracting the sun position, with a coordinate axis to match the orientation of the moving object as an origin the image acquisition position, provided any coordinate system based on the moving direction of the calculated moving body, the image Based on the position of the sun inside, a function for obtaining a measurement vector from the origin of the arbitrary coordinate system toward the sun, a coordinate axis composed of a horizontal plane and a vertical axis, and an absolute coordinate system with the image acquisition position as the origin, Based on the image acquisition position and the image acquisition time, a function for obtaining a reference vector from the origin of the absolute coordinate system toward the sun, and the measurement vector and the reference vector are matched, and the angle difference between the arbitrary coordinate axis and the absolute coordinate axis based on, but to execute the function of calculating the attitude of the moving body, to the computer.

  In this case, the computer has a function of reading the measurement time measured during the movement by the position measurement unit, and a function of obtaining the image acquisition position based on the moving body position, the measurement time, and the image acquisition time. It can also be executed.

The moving body posture determining device is a device that determines the posture of a moving moving body, is mounted on the moving body, acquires an image including the sun while moving, and acquires an image acquisition time when the image is acquired. An image acquisition means that can be acquired, a position measurement means that is mounted on the mobile body and that can measure the position of the mobile body during movement, the image, the image acquisition time, and the mobile body position are stored. Storage means, and arithmetic processing means for performing arithmetic processing by reading the image, the image acquisition time, and the moving body position from the storage means, and the arithmetic processing means comprises two or more of the movements Based on the body position, the moving direction of the moving body is calculated, the image acquisition position that is the position of the moving body when the image is acquired is calculated, and the sun position in the image is calculated based on the luminance of the image. Extract and move to the posture of the moving body With a Align was axis as an origin the image acquisition position, provided any coordinate system based on the moving direction of the calculated moving object, based on the sun position in the image, from the origin of the arbitrary coordinate system A measurement vector toward the sun is calculated, a coordinate axis composed of a horizontal plane and a vertical axis, and an absolute coordinate system having the image acquisition position as an origin is provided, and based on the image acquisition position and the image acquisition time, the absolute coordinate A reference vector from the origin of the system to the sun is calculated, and the measurement vector and the reference vector are matched, the angle difference between the arbitrary coordinate axis and the absolute coordinate axis, the calculated moving direction of the moving object, and the attitude of the moving object Is calculated.

  In this case, the position measurement unit acquires the measured measurement time, the storage unit stores the measurement time, and the arithmetic processing unit reads the measurement time from the storage unit to determine the position of the mobile object and the measurement The image acquisition position can also be calculated based on the time and the image acquisition time.

The mobile body posture determination method, the mobile body posture determination program, and the mobile body posture determination device of the present invention have the following effects.
(1) Since an IMU is not required and a relatively inexpensive product such as a GPS or a camera can be used, the cost can be reduced.
(2) Since only a small amount of equipment such as a GPS and a camera is installed, it can be installed relatively easily.
(3) The moving body posture can be acquired with higher accuracy than a magnetic sensor or the like.
(4) Since the moving body posture can be acquired in real time during movement, posture control can be performed while moving.
(5) Since complicated arithmetic processing is not required, a result can be acquired easily and quickly.

Explanatory drawing which shows the state which the mobile body 1 is moving. The block diagram which shows the component utilized in order to implement this invention. Explanatory drawing which shows the arbitrary coordinate system which is a coordinate system united with the attitude | position of the moving body. (A) is a model figure which shows the angle of the sun when seeing the sun from an image acquisition position in arbitrary coordinate systems, (b) is explanatory drawing which shows the sun position in an image. (A) is a model diagram showing a measurement vector in an arbitrary coordinate system, (b) is a model diagram showing a reference vector in an absolute coordinate system. (A) is a diagram in which the x-axis and y-axis of the arbitrary coordinate system are rotated by κ ′ around the z-axis, and (b) is the y-axis / z-axis of the arbitrary coordinate system rotated by φ ′ around the x-axis. FIG. 4C is a diagram in which the z-axis of the arbitrary coordinate system is rotated by ω ′ around the y-axis. Explanatory drawing which shows the laser measurement by an aircraft.

  An example of embodiments of a mobile body posture determination method, a mobile body posture determination program, and a mobile body posture determination apparatus according to the present invention will be described with reference to the drawings.

(Overview)
FIG. 1 is an explanatory diagram showing a state in which the moving body 1 is moving. The present invention determines the posture of the moving body 1 as shown in FIG. 1 and measures its own position (the position of the moving body 1 itself) while moving, and the sun S (FIG. 1). Is acquired, and the moving posture is determined based on the coordinates of the own position and the acquired image. Specifically, the moving direction (azimuth) of the moving body 1 is calculated from the coordinates measured during the movement, and an arbitrary coordinate system (a coordinate system that matches the attitude of the moving body 1) is obtained from the sun position in the acquired image. ), And the sun direction and altitude in the absolute coordinate system (a coordinate system consisting of a horizontal plane and a vertical axis) are calculated from the latitude, longitude, and date, and the sun direction and altitude in the arbitrary coordinate system and absolute coordinate system are obtained. As a result of combining the altitudes, the attitude of the moving body 1 is determined based on the angle difference between the coordinate axes generated in both coordinate systems and the moving direction (azimuth) of the moving body 1.

  The moving body 1 is not limited to an aircraft that moves in the air as shown in FIG. 1, but means any object that can move, such as an automobile that moves on the road, a train that moves on a track, and a ship that moves on the sea. Although it is a word, the mobile body 1 is demonstrated as an aircraft here for convenience.

  FIG. 2 is a block diagram showing components used for carrying out the present invention. As shown in this figure, position measuring means 2, image acquiring means 3, storage means 4, and arithmetic processing means 5 are used, and these are installed on the moving body 1 so as to be stable even during movement.

  FIG. 3 is an explanatory diagram showing an “arbitrary coordinate system” that is a coordinate system that matches the posture of the moving body 1. As shown in this figure, the arbitrary coordinate system is provided with an xn axis in the left and right direction (relative to the traveling direction) of the moving body 1, a yn axis in the traveling direction of the moving body 1, and a zn axis in the up and down direction of the moving body 1. The direction of the moving body 1 is changed according to the posture. The inclination of each coordinate axis determines the posture of the moving body 1. Specifically, this posture is obtained from an inclination with respect to an “absolute coordinate system” in which the horizontal plane is the eastward direction is the Xa axis, the horizontal plane is the northward direction is the Ya axis, and the vertical upward direction is the Za axis. , Rotation angle around the Xa axis (inclination of yn axis and Ya axis or zn axis and Za axis) pitch, rotation angle around Ya axis (inclination between zn axis and Za axis or xn axis and Xa axis) roll, Za The posture is determined from three elements where the rotation angle around the axis (inclination of the yn axis and the Ya axis or the inclination of the xn axis and the Xa axis) is yaw.

  Hereinafter, each component will be described in detail.

(Position measuring means)
The position measuring means 2 measures the position of the moving moving body 1 and can use GPS. As the GPS, in addition to the single positioning method, methods such as RTK-OTF and DGPS can also be adopted. When single positioning GPS is used as the position measuring means 2, as shown in FIG. 2, the position measuring means 2 includes an antenna portion 2a and a receiver portion 2b.

  The GPS is a system that specifies a position by receiving signals from a plurality of satellites, specifies the position of itself (position measuring means 2), and performs the measurement (hereinafter referred to as “measurement time”). ) Can also be acquired. The position measured here is in a commonly used coordinate system such as a mathematical coordinate system or a world geodetic system (or expressed in latitude, longitude, and altitude), and in FIG. 1, it is measured at the first point P1. The position (X1, Y1, Z1) and the position (X2, Y2, Z2) measured at the second point P2 after the movement are shown. Further, according to GPS, it is possible to measure at a relatively short interval, for example, it can be measured at an interval of once per second (1 Hz).

(Image acquisition means)
The image acquisition means 3 acquires an image including the sun S, and can use a digital camera or a digital video. A wide-angle lens (preferably having a field angle of 120 to 180 °) is suitable for the lens used in the image acquisition means 3, and it is desirable to employ a so-called zenith camera that employs a super-wide-angle lens or a circumferential fisheye lens. When a digital camera or digital video is used as the image acquisition unit 3, it is desirable that the number of pixels of the acquired image is about 3 million to 10 million and the frame rate is about 15 to 30 fps. It is not limited to such a standard.

  Since the image acquisition unit 3 needs to acquire an image including the sun S, the neutral density filter 6 can be installed in the lens portion of the image acquisition unit 3. By installing the neutral density filter 6, the outer shape of the sun S can be acquired more reliably. As the neutral density filter 6, it is possible to employ a neutral density black filter having a neutral density of about 1,000 to 10,000, and it is more preferable that the neutral density is appropriately selected according to weather conditions.

  The time at which the image is acquired by the image acquisition means 3 (hereinafter referred to as “image acquisition time”) can be measured with a separately prepared instrument, but is based on the position measurement means 2 performed immediately before or after the image acquisition. It is also possible to employ the measurement time, and more desirably, it may be obtained by calculation based on the frame rate (for example, 15 to 30 fps).

  The image acquisition means 3 can be mounted (installed) on the moving body 1 after being fixed to the platform 7 together with the antenna portion 2a of the position measurement means 2 (FIG. 2). In this case, it is desirable that the image acquisition unit 3 and the antenna unit 2a are close to each other and fixed to the platform 7. Moreover, although the image acquisition means 3 and the position measurement means 2 can be installed in the arbitrary positions of the mobile body 1, it is desirable to install the image acquisition means 3 in the position where the image of the sun S is easy to acquire. For example, when the moving body 1 is an aircraft, the image acquisition means 3 can be installed in the upper part of the aircraft.

(Memory means)
The storage unit 4 stores the position information (moving body position data) of the moving body 1 measured by the position measuring unit 2 and the image information (image data) acquired by the image acquiring unit 3. The moving body position data is preferably stored in association with the measurement time information (measurement time data), and the image data is preferably stored in association with the image acquisition time information (image acquisition time data). An associated database can also be constructed in the storage means 4.

(Calculation processing means)
The arithmetic processing means 5 performs a series of arithmetic processing for determining the posture of the mobile body 1. Specifically, the arithmetic processing means 5 is a RAM (other storage medium) that stores a program for executing this arithmetic processing. Or a computer (electronic computer) equipped with a CPU for executing the program. When this program is executed, the moving body position data, measurement time data, image data, and image acquisition time data stored in the storage unit 4 are read from the storage unit 4 and calculation processing is executed using these data. Is done.

(Arbitrary coordinate system)
As described above, the arbitrary coordinate system referred to here is a coordinate system that matches the posture of the moving body 1 and is orthogonal to each other as shown in FIGS. 2 and 3 (the horizontal direction of the moving body 1). ), Yn axis (traveling direction of the moving body 1), and zn axis (up and down direction of the moving body 1). The origin of the arbitrary coordinate system (the point where the xn axis, yn axis, and zn axis intersect) is the position where the image acquisition unit 3 acquires an image (hereinafter referred to as “image acquisition position”), that is, the image acquisition unit 3. It is provided at the installation position. Thus, the origin of any coordinate system moves with the moving body 1, the coordinate axes are all SANYO changing its direction in accordance with the posture of the moving body 1, these changes when viewed from the absolute coordinate system discussed later The arbitrary coordinate system is a relative coordinate system.

(Measurement vector)
FIG. 4A is a model diagram showing the angle θ of the sun S when the sun S is viewed from the image acquisition position (coordinate origin O) in an arbitrary coordinate system, and FIG. 4B shows the sun position in the image. It is explanatory drawing shown. If the posture of the moving moving body 1 is unknown, the horizontal plane cannot be recognized on the moving body 1 and the altitude (angle formed with the horizontal plane) of the sun S cannot be grasped. Therefore, in order to grasp the elevation angle of the sun S on the moving body 1, a straight line connecting the origin and the sun S, a plane composed of the yn axis-zn axis of the arbitrary coordinate system, and the included angle are obtained. There are various means for obtaining the included angle. Here, as shown in FIG. 4A, an example of means for obtaining the included angle θ between the straight line connecting the origin and the sun S and the zn axis of the arbitrary coordinate system will be described. To do.

  When an image is taken with a zenith camera or the like using a wide-angle lens, a circular image as shown in FIG. 4B is acquired. Since the acquirable range is thus circular, this circle (the outermost circle) is called an image circle. In the image circle shown in FIG. 4B, an auxiliary circle corresponding to the size of the included angle θ is drawn. That is, the auxiliary circle away from the center point (coordinate origin O) has a larger included angle θ. Recognize. 4B employs equidistant projection in which an auxiliary circle is drawn with the product r · θ of the radius r of the image circle and the included angle θ as the radius. According to this method, since the size of the auxiliary circle is proportional to θ, there is a feature that it is intuitive and easy to understand. The method of drawing the auxiliary circle is not limited to the equidistant projection, and is given by an equal solid angle projection in which the auxiliary circle is drawn with a radius given by 2r · sin (θ / 2), or by r · sin (θ). It is also possible to adopt an orthogonal projection in which an auxiliary circle is drawn with a radius.

  The sun S contained in the image can be easily identified because its brightness is significantly higher than others. That is, if the conventional image recognition technique is used, the sun in the image can be easily extracted. Lightness can be used as the luminance. However, if the sun in the image can be identified, the saturation and hue of the three so-called color attributes can be used, and of course, these can be used in combination. .

  Extraction of the sun in the image is performed by the arithmetic processing means 5. Further, the arithmetic processing means 5 obtains the position of the center of gravity (centroid) from the extracted outline (contour) of the sun by calculation, and from the distance between the position of the center of gravity of the sun and the center point (coordinate origin O) in the image. Find the included angle θ. Further, as shown in FIG. 4B, the arithmetic processing means 5 obtains an included angle δ between a straight line connecting the center point (coordinate origin O) and the center of gravity position of the sun and the yn axis of the arbitrary coordinate system. In this way, from the image including the sun S, the nip axis between the yn axis of the arbitrary coordinate system and the sun with reference to the origin O (that is, the image acquisition position), and the xn axis of the arbitrary coordinate system from the aforementioned included angle θ− Since the included angle (π / 2−θ) with the yn-axis plane can be obtained, the direction of the sun S in the arbitrary coordinate system can be specified.

  FIG. 5A is a model diagram showing a measurement vector Vm in an arbitrary coordinate system. This measurement vector Vm represents the direction of the sun S in the arbitrary coordinate system by the included angle δ and the included angle θ (or π / 2−θ) obtained from the image, and starts from the origin O of the arbitrary coordinate system. And a vector with the sun S as the end point. Since it is a vector calculated | required based on the acquired image, it was set as measurement vector Vm. As shown in FIG. 5A, angles formed by the xn axis, yn axis, zn axis of the arbitrary coordinate system and the measurement vector Vm are αm, βm, and γm, respectively, and these cosines are defined as direction cosines for convenience. Call. That is, the direction cosine of the xn axis is cos (αm), the direction cosine of the yn axis is cos (βm), and the direction cosine of the zn axis is cos (γm). For convenience, it is assumed that the direction cosine cos (αm) of the xn axis is Lm, the direction cosine cos (βm) of the yn axis is Mm, and the direction cosine cos (γm) of the zn axis is Nm.

(Absolute coordinate system)
As described above, the absolute coordinate system here is composed of a horizontal plane and a vertical axis, and more specifically, is composed of three axes: an Xa axis and a Ya axis that constitute the horizontal plane, and a Za axis that is the vertical axis. The Xa axis is the east-west direction and the east direction is positive, the Ya axis is the north-south direction and the north direction is positive, and the Za axis is the positive direction. As in the arbitrary coordinate system, this absolute coordinate system also has its origin (the point where the Xa axis, Ya axis, and Za intersect) at the “image acquisition position” that is the position at which the image acquisition unit 3 has acquired the image. The position information (coordinates, latitude / longitude / altitude) of the moving body 1 acquired by the position measuring means 2 is set as the origin O. Thus, although the origin of the absolute coordinate system moves together with the moving body 1, each coordinate axis is always constant (east-west direction, north-south direction, vertical direction) regardless of the posture of the moving body 1.

(Reference vector)
Given that the earth is rotating and revolving around the sun, given the date and position (latitude, longitude, and coordinates), the solar altitude and its orientation at that position can be calculated theoretically. Various calculation formulas are known, and an example is shown below.

The solar azimuth ψ and altitude η at an arbitrary date and time at an arbitrary latitude φ and longitude λ can be obtained by the following equations. This calculation is performed by the arithmetic processing means 5.
A. Θo determined based on the number of days dn from New Year's Day
θo = 2π (dn−1) / 365
B. Solar declination δ of the desired day
δ = 0.006918−0.399912 cos (θo) +0.070257 sin (θo) −0.006758 cos (2θo) +0.000907 sin (2θo) −0.002697 cos (3θo) +0.001480 sin (3θo)
C. Geocentric solar distance r / r *
r / r * = 1 / {1.0010010 + 0.03421 cos (θo) +0.001280 sin (θo) +0.000719 cos (2θo) + 0.000077sin (2θo)} 0.5
D. Time difference Eq
Eq = 0.000075 + 0.001868 cos (θo) −0.032077 sin (θo) −0.014615 cos (2θo) −0.040849 sin (2θo)
E. Sun hour angle h
h = (JST-12) π / 12 + longitude difference from standard meridian + equal time difference (Eq)
However, JST: Japan Standard Time Solar direction ψ
ψ = arcsin {sin (φ) sin (δ) + cos (φ) cos (δ) cos (h)}
G. Solar altitude η
η = arctan [cos (φ) cos (δ) sin (h) / {sin (φ) sin (α) −sin (δ)}]

Calculation for obtaining the sun direction and altitude is performed by the arithmetic processing means 5. The solar altitude here is an included angle between a straight line connecting the origin of the absolute coordinate system and the sun S and a horizontal plane (that is, a plane composed of the Xa axis-Ya axis of the absolute coordinate system). The sun direction is an included angle between a straight line connecting the origin of the absolute coordinate system and the sun S and the north direction (that is, the positive direction of the Ya axis of the absolute coordinate system). Strictly speaking, the value of the solar altitude changes depending on the altitude (altitude), but considering the distance from the earth to the sun, the influence of the altitude is extremely small and can be ignored here.

  Here, the condition values necessary for calculation are date and position. Of course, the day (month and day) is the day when the measurement is performed, and the “measurement time” acquired by the position measuring means 2 can be used for the time. As for the position, the position information of the moving body 1 obtained by the position measuring means 2 can be used.

  FIG. 5B is a model diagram showing the reference vector Vc in the absolute coordinate system. This reference vector Vc represents the direction of the sun S in the absolute coordinate system according to the calculated sun azimuth and altitude, and is a vector starting from the origin O of the absolute coordinate system and starting from the sun S. . As shown in FIG. 5 (b), the angles formed by the Xa axis, Ya axis, Za axis of the absolute coordinate system and the reference vector Vc are αc, βc, and γc, respectively. Call. That is, the direction cosine of the Xa axis is cos (αc), the direction cosine of the Ya axis is cos (βc), and the direction cosine of the Za axis is cos (γc). For convenience, the direction cosine cos (αc) of the Xa axis is Lc, the direction cosine cos (βc) of the Ya axis is Mc, and the direction cosine cos (γc) of the Za axis is Nc.

(Direction of moving body)
As shown in FIG. 1, the azimuth of the moving moving body 1 can be calculated from the position information measured by the position measuring means 2. Specifically, the position information (X1, Y1, Z1) measured at the first point P1 shown in FIG. 1 and the position information (X2, Y2, Z2) measured at the second point P2 after movement are used. Then, the direction of the moving body 1 is calculated. Note that the coordinates acquired by the position measuring means 2 are not in an arbitrary coordinate system or an absolute coordinate system, and are in a commonly used coordinate system such as a mathematical coordinate system or a world geodetic system. Therefore, if (X1, Y1) and (X2, Y2) which are plane coordinates among position information are used, the azimuth | direction between two points on a horizontal surface can be calculated | required. In this way, if position information is acquired at two moving points, the traveling direction between them can be expressed as a bearing. However, not only two points but also three or more points can be acquired to calculate a moving bearing. it can. This azimuth calculation is performed by the arithmetic processing means 5.

(Image acquisition position)
As described above, the image acquisition position is a position at which an image is acquired by the image acquisition unit 3, and is an installation position of the image acquisition unit 3 at the time of image acquisition. The image acquisition position is calculated by the arithmetic processing unit 5 and is calculated based on the position information measured by the position measuring unit 2. Specifically, the image acquisition time data is read from the storage unit 4, the measurement time data before and after the image acquisition time is read, and the position information (moving body position data) measured at these measurement times is read, The position information at the image acquisition time, that is, the image acquisition position is calculated. Strictly speaking, what is calculated here is the position of the position measurement means 2 (antenna part 2a), but the image acquisition means 3 and the antenna part 2a are sufficiently close to each other, so these separations should be ignored. Can do.

  Note that the image acquisition position is read as the position information at the image acquisition time, that is, the image acquisition position for the sake of convenience, instead of the method obtained by the above calculation, by reading the measurement time data and the moving body position data immediately before the image acquisition time. You can also.

(Determination of posture)
The measurement vector Vm and the reference vector Vc are both vectors that connect from the image acquisition position to the sun S, and should be identical to each other. However, as shown in FIGS. The measured vector Vm and the reference vector Vc expressed in the absolute coordinate system have different directions. This is due to the posture of the moving body 1. That is, each coordinate axis of the arbitrary coordinate system corresponding to the posture of the moving body 1 is inclined with respect to each coordinate axis of the absolute coordinate system. In other words, the direction cosine Lm, This is because the direction cosines Lc, Mc, Nc of the reference vector Vc in the absolute coordinate system are different from Mm, Nm.

  If the deviation (angle difference) between each coordinate axis of the arbitrary coordinate system and each coordinate axis of the absolute coordinate system is obtained, the inclination of each coordinate axis of the arbitrary coordinate system, that is, the posture of the moving body 1 can be obtained. There are various methods for solving the angle difference between the coordinate axes of the arbitrary coordinate system and the absolute coordinate system. Here, the following method is exemplified and described.

  The origin of the arbitrary coordinate system shown in FIG. 5A is overlapped with the origin of the absolute coordinate system shown in FIG. 5B, and then the xn axis, yn axis, and zn axis of the arbitrary coordinate system are rotated. The directions of the measurement vector Vm and the reference vector Vc are matched. At this time, the amount (angle) obtained by rotating the xn axis, the yn axis, and the zn axis is calculated, and the posture of the moving body 1 is obtained based on these rotation amounts.

  A specific procedure for aligning the directions of the measurement vector Vm and the reference vector Vc while rotating the xn axis, the yn axis, and the zn axis of the arbitrary coordinate system will be described with reference to FIGS. 6A is a diagram obtained by rotating the x-axis and y-axis of the arbitrary coordinate system by κ ′ about the z-axis, and FIG. 6B is the y-axis of the arbitrary coordinate system about the x-axis. FIG. 6C is a diagram in which the z axis-x axis of the arbitrary coordinate system is rotated by ω ′ around the y axis.

First, as shown in FIG. 6A, the xn0 axis-yn0 axis of the arbitrary coordinate system is rotated by κ ′ around the zn0 axis. At this time, if the direction cosine of the measurement vector Vm in the first arbitrary coordinate system xn0 axis-yn0 axis-zn0 axis is Lm0, Mm0, Nm0, the direction cosine Lm1, Mm1 of the measurement vector Vm after being rotated around the zn0 axis , Nm1 is given by the following equation.
Lm1 = Lm0 · cosκ′−Mm0 · sinκ ′
Mm1 = Lm0 · sinκ ′ + Mm0 · cosκ ′
Nm1 = Nm0

Next, as shown in FIG. 6B, the yn1 axis-zn0 axis of the arbitrary coordinate system is rotated by φ ′ around the xn1 axis. At this time, direction cosines Lm2, Mm2, and Nm2 of the measurement vector Vm after being rotated around the xn1 axis are given by the following equations.
Lm2 = Lm1
Mm2 = Mm1 · cos φ′−Nm1 · sin φ ′
Nm2 = Mm1 · sinφ ′ + Nm1 · cosφ ′

Finally, as shown in FIG. 6C, the zn1 axis-xn1 axis of the arbitrary coordinate system is rotated by ω ′ around the yn2 axis. At this time, the direction cosines Lm3, Mm3, and Nm3 of the measurement vector Vm after being rotated around the yn2 axis are given by the following equations.
Lm3 = Lm2 · cosω′−Nm2 · sinω ′
Mm3 = Mm2
Nm3 = Lm2 · sin ω ′ + Nm2 · cos ω ′

That is, when Lm3, Mm3, and Nm3 are represented by Lm0, Mm0, and Nm0, the following equation is obtained.
Lm3 = [Lm0 · cosκ′−Mm0 · sinκ ′] · cosω ′ − [(Lm0 · sinκ ′ + Mm0 · cosκ ′) · sinφ ′ + Nm0 · cosφ ′] · sinω ′
Mm3 = [Lm0 · sinκ ′ + Mm0 · cosκ ′] · cosφ′−Nm0 · sinφ ′
Nm3 = [Lm0 · cosκ′−Mm0 · sinκ ′] · sinω ′ + [(Lm0 · sinκ ′ + Mm0 · cosκ ′) · sinφ ′ + Nm0 · cosφ ′] · cosω ′

In FIG. 6C, Lm3, Mm3, Nm3 of the measurement vector Vm in the arbitrary coordinate system xn2 axis-yn2-zn2 axis after rotation and the direction cosines Lc, Mc, Nc of the reference vector Vc in the absolute coordinate system are matched. The solutions κ ′, φ ′, ω ′ obtained in this way, that is, the angle difference between the coordinate axes of the arbitrary coordinate system and the absolute coordinate system. Then, an axis rotated by κ ′ from the Xa axis of the absolute coordinate system is taken as an xn axis, an axis rotated by φ ′ from the Ya axis is taken as an yn axis, and an axis rotated by ω ′ from the Za axis is taken as a zn axis. The xn axis−yn axis−zn axis is determined by matching the direction of the yn axis (the direction projected on the horizontal plane) with the orientation obtained from the position information measured at two or more points during the movement of the moving body 1. Xn axis−yn axis−zn axis determined here, and the angle φ, zn axis, and horizontal plane between the azimuth κ and yn axis of the moving body 1 and the horizontal plane (plane consisting of Xa axis−Ya axis of the absolute coordinate system) Of the moving body 1 is obtained based on the angle ω.

(Application examples)
The orientation of the moving body 1 is determined as described above, when carrying out the measurement by the aircraft, such as aerial photogrammetry and laser measurement, in addition to be able to be used for grasping the irradiation direction of the photographing direction and the laser, Figure 2 As shown, an external output terminal 8 can be provided to control an external device in real time. Here, the external device is, for example, a device that controls the operation posture of the mobile body 1, or a device that controls the posture of a load loaded on the mobile body 1.

  The moving object attitude determination method, moving object attitude determination program, and moving object attitude determination device of the subject feature are used for aircraft and ship operation control, measurement work such as aerial photogrammetry and laser measurement, etc. This invention can be applied in various fields such as transportation of chemicals and the like that need to maintain their posture.

DESCRIPTION OF SYMBOLS 1 Mobile body 2 Position measurement means 2a Antenna part 2b (of position measurement means) Receiver part 3b (of position measurement means) 3 Image acquisition means 4 Storage means 5 Arithmetic processing means 6 Neutral filter 7 Platform 8 External output terminal S Sun Xa ( X axis in absolute coordinate system Ya (in absolute coordinate system) y axis Za (in absolute coordinate system) x axis xn (in arbitrary coordinate system) x axis yn (in arbitrary coordinate system) y axis zn (in arbitrary coordinate system) x-axis Vc reference vector Lc (reference vector and Xa-axis) direction cosine Mc (reference vector and Ya-axis) direction cosine Nc (reference vector and Za-axis) direction cosine Vm measurement vector Lm (measurement vector and xn-axis) Direction cosine Mm Direction cosine (measurement vector and yn axis) Nm Direction cosine (measurement vector and zn axis)

Claims (6)

A method of determining the posture of a moving object that is moving,
By measuring the position of the moving body twice or more during movement by the position measuring means mounted on the moving body,
While acquiring the image including the sun during movement by the image acquisition means mounted on the moving body, acquire the image acquisition time when the image was acquired,
Based on the two or more measured positions of the moving body, the moving direction of the moving body is calculated,
Based on the measured said mobile location, it obtains the image acquisition position is a position of the moving body at the time of acquiring an image,
Extracting the sun position in the image based on the brightness of the image,
The coordinate axis is adapted to the posture of the moving body, the image acquisition position is the origin, and an arbitrary coordinate system is provided in which the traveling direction of the moving body is the yn axis, and based on the sun position in the image, Find the measurement vector from the origin of the arbitrary coordinate system toward the sun,
An absolute coordinate system having a coordinate plane composed of a horizontal plane and a vertical axis and having the image acquisition position as an origin is provided, and a reference vector directed from the origin of the absolute coordinate system to the sun based on the image acquisition position and the image acquisition time Seeking
Wherein with combining the measurement vector and the reference vector, calculates the angle difference of the arbitrary coordinate axis absolute coordinate, further based on the arbitrary coordinate axes the yn axis was determined according to the moving direction of said arbitrary coordinate axis, the A method for determining a posture of a moving body, comprising calculating the posture of the moving body. In the determination method of the mobile body posture of Claim 1,
The position measurement means measures the position of the moving body and acquires the measurement time,
A method of determining a mobile body posture, wherein an image acquisition position is calculated based on the measured mobile body position and the measurement time and image acquisition time. A program for determining the posture of a moving moving object,
A function of reading two or more moving body positions measured during movement by the position measuring means mounted on the moving body;
A function of reading an image including the sun acquired during movement by the image acquisition means mounted on the moving body, and an image acquisition time when the image was acquired;
Based on the two or more mobile location, a function of calculating a moving direction of the movable body,
On the basis of the moving body position, the function of obtaining the image acquisition position is a position of the moving body at the time of acquiring an image,
A function of extracting the sun position in the image based on the brightness of the image;
The coordinate axis is adapted to the posture of the moving body, the image acquisition position is the origin, and an arbitrary coordinate system is provided in which the traveling direction of the moving body is the yn axis, and based on the sun position in the image, A function for obtaining a measurement vector from the origin of an arbitrary coordinate system toward the sun,
Provided is an absolute coordinate system that is a coordinate axis composed of a horizontal plane and a vertical axis and that has the image acquisition position as an origin, and a reference vector from the origin of the absolute coordinate system toward the sun based on the image acquisition position and the image acquisition time The function to ask for
Matching the measurement vector and the reference vector , calculating an angle difference between the arbitrary coordinate axis and the absolute coordinate axis, and further, based on the arbitrary coordinate axis determined according to the movement direction, the yn axis among the arbitrary coordinate axes, A program for determining a moving body posture, which causes a computer to execute a function of calculating the posture of the moving body. In the moving body posture determination program according to claim 3,
A function to read the measurement time measured while moving by the position measurement means;
A program for determining a mobile body posture, which causes a computer to execute a function of obtaining an image acquisition position based on a mobile body position and a measurement time and an image acquisition time thereof. An apparatus for determining the posture of a moving body that is moving,
An image acquisition means mounted on the moving body, capable of acquiring an image including the sun during movement, and acquiring an image acquisition time at which the image was acquired;
Position measuring means mounted on the moving body and capable of measuring the moving body position during movement;
Storage means for storing the image, the image acquisition time, and the moving body position;
Arithmetic processing means for reading the image, the image acquisition time, and the moving body position from the storage means and performing arithmetic processing;
The arithmetic processing means includes:
2 or more on the basis of the moving body position of, as well as calculating a moving direction of the movable body, and calculates the image obtaining positions is the position of the moving body at the time of acquiring the image,
Extracting the sun position in the image based on the brightness of the image,
The coordinate axis is adapted to the posture of the moving body, the image acquisition position is the origin, and an arbitrary coordinate system is provided in which the traveling direction of the moving body is the yn axis, and based on the sun position in the image, Calculate the measurement vector from the origin of the arbitrary coordinate system toward the sun,
Provided is an absolute coordinate system that is a coordinate axis composed of a horizontal plane and a vertical axis and that has the image acquisition position as an origin, and a reference vector from the origin of the absolute coordinate system toward the sun based on the image acquisition position and the image acquisition time Calculate
Wherein with combining the measurement vector and the reference vector, calculates the angle difference of the arbitrary coordinate axis absolute coordinate, further based on the arbitrary coordinate axes the yn axis was determined according to the moving direction of said arbitrary coordinate axis, the A moving body posture determining apparatus, characterized by calculating a posture of a moving body. In the moving body attitude | position determination apparatus of Claim 5,
The position measuring means acquires the measured measurement time,
The storage means stores the measurement time,
The arithmetic processing means reads the measurement time from the storage means, and calculates the image acquisition position based on the mobile object position and the measurement time and image acquisition time. .

Images