JP4196842B2 - Attitude angle detection method and attitude angle detection device - Google Patents

Description

  The present invention relates to a posture angle detection method and a posture angle detection device of a rotating body, and in particular, when detecting posture angles around three axes of roll, yaw, and pitch in a servo system such as a space stabilization mechanism, The present invention relates to a posture angle detection method and a posture angle detection device for a rotating body in which each axis of yaw and pitch is a pivot element having no mechanical constraint as a rotation center element.

The rotating body such as the payload of the space stabilization mechanism is rotated on each of its independent axes by mechanically constraining the rotation operation of the roll (around the x axis), the yaw (around the z axis), and the pitch (around the y axis). In general, a method of detecting an attitude angle by an angle detector such as an encoder or a resolver attached to each axis is generally used. However, in order to reduce the size and weight and to constrain the space, it is effective to use a pivot element such as a spherical bearing as the rotation center element instead of providing independent axes for the roll, yaw and pitch. . In this case, since the roll, yaw, and pitch axes do not exist mechanically, it is impossible to detect the attitude angle with the conventional angle detector attached to the mechanically constrained shaft. For example, Patent Document 1 discloses an attitude control device using a PSD (Position Sensitive Device). This is suitable for devices that require miniaturization, and eliminates unforeseen motor output associated with miniaturization, depending on the angle detected by the angle detector located at one end of the imaging unit. Depending on the relative relationship between an LED (Light-Emitting Diode) that selectively emits light and a PSD that receives light emitted from the LED and generates an output corresponding to the light receiving position, The posture angle is detected. Here, the LED and PSD serve as an auxiliary to an angle detector attached to a mechanically constrained gimbal shaft.
JP 2001-99652 A (pages 6-7, FIGS. 2-3)

  As described above, conventionally, the posture angle of a rotating body is detected by detecting an angle around a gimbal axis that is a mechanically constrained axis. However, when the rotation center element is a pivot element such as a spherical bearing, there is no mechanically constrained axis corresponding to the above-described gimbal axis, so the conventional method cannot be applied as it is. In order to cope with this, a method of adding position detection devices such as LEDs and PSDs by the number of degrees of freedom of rotation is conceivable. Further, the LED is not coherent light, and causes a problem in straightness, which is not preferable in position detection accuracy.

  The present invention relates to a posture angle detection method and posture angle detection device when a rotating body rotates in a three-dimensional space, in which the rotation center element is a pivot element and the roll, yaw, and pitch rotation axes are not mechanically constrained However, it is an object of the present invention to provide a posture angle detection method and a posture angle detection device that can efficiently detect the posture angle of a rotating body using a two-dimensional position detection element.

  In the first invention, the rotating body is attached to the support mechanism via the pivot element, and the laser light source is configured such that the optical axes of the two laser beams originating from the laser light source are orthogonal to the rotation center element of the rotating body. A two-dimensional position detection device that is fixed to the rotating body and receives the laser beam corresponding to each of the laser beams and detects a light receiving position, and the two-dimensional position detection device is the two-dimensional position detection device. The optical axis of the laser beam received at the center of the position detection surface is fixed to the support mechanism so as to be orthogonal to the position detection surface, and based on light reception position information detected by the two-dimensional position detection device The posture angle when the rotating body rotates around the rotation center element is calculated.

  According to the first invention, by using the two-dimensional position detection device provided corresponding to the laser beam, the pivot element is detected by detecting the light receiving position information of the laser beam rotating in conjunction with the rotation of the rotating body. The posture angle of the rotating body rotating in the three-dimensional space can be detected efficiently.

  A second invention is the position detection two-dimensional coordinate system defined in the first invention, wherein a two-dimensional coordinate system having the origin at the center of the position detection surface of the two-dimensional position detection device is defined on the position detection surface. A vector from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate system is set as the x axis, and the rotation center element is set as the start point and is parallel to the two axes defined by the position detection two-dimensional coordinate system. A position detection three-dimensional coordinate system is defined by defining a three-dimensional coordinate system having the vectors y and z, respectively, and based on the light receiving position information of the laser beam detected by the position detection two-dimensional coordinate system, Unit ray vector information indicating the posture of the rotator in the position detection three-dimensional coordinate system is generated, and based on the two unit ray vector information generated corresponding to each of the two-dimensional position detectors, Attitude angle Calculated for, configured as.

  According to the second invention, the light receiving position information of the two received laser beams is detected based on the two-dimensional coordinate system defined on the position detection surfaces of the two two-dimensional position detecting devices fixed to the support mechanism. In order to make it easy to calculate the attitude angle of a rotating body that rotates in a three-dimensional space, it is converted into unit ray vector information in a three-dimensional coordinate system, and light receiving position information in two two-dimensional coordinate systems It is possible to efficiently calculate the posture of the rotating body in the three-dimensional coordinate system simply by measuring.

  According to a third invention, with respect to the second invention, in the position detection three-dimensional coordinate system, the distance from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate is x, and the position detection two-dimensional Two coordinate values indicating the light receiving position when the laser beam is received in the coordinate system are y and z, respectively, and the unit beam vector information is a three-dimensional coordinate (x , Y, z) as vector information normalized to a unit vector.

  According to the third invention, the calculation of the posture angle of the rotating body in the three-dimensional space can be simplified to the calculation of the coordinate information of two unit vectors orthogonal to each other, and an efficient posture angle calculation is possible. become.

  According to a fourth aspect of the present invention, there is provided a support mechanism fixed coordinate system by defining a three-dimensional coordinate system composed of an x axis, a y axis, and a z axis fixed to the support mechanism, and the attitude angle of the rotating body is In the support mechanism fixed coordinate system, a roll angle indicating clockwise rotation in the positive direction of the x axis, a yaw angle indicating clockwise rotation in the positive direction of the z axis, and a clockwise rotation in the positive direction of the y axis. The pitch angle indicating rotation is expressed as a triaxial rotation angle that rotates three axes in order.

  According to the fourth invention, the posture of the rotating body in the three-dimensional space can be expressed as a combination of rotation angles around the three axes of the roll angle, the yaw angle, and the pitch angle. It becomes possible to express in an easy-to-understand manner, and more efficient posture angle calculation becomes possible.

  According to the present invention, even when the rotation center element that has no mechanical constraint is a pivot element such as a spherical bearing, the measurement of the light receiving position information by the two two-dimensional position detection devices enables the rotation of the rotating body in the three-dimensional space. The attitude angle can be detected. Also, the calculation of the attitude angle is based on the light receiving position information in the two two-dimensional position detection devices, and the unit ray vector that becomes two orthogonal unit vectors, and the three axes of roll angle, yaw angle, and pitch angle. This simplifies the calculation related to the rotation angle and enables efficient posture angle calculation.

  FIG. 1 shows the configuration (1) of the attitude angle detection device of the present invention.

  A support mechanism fixed coordinate system xyz fixed to the support mechanism includes an x axis 201, a y axis 202 (standing perpendicular to the paper surface), and a z axis 203.

  The laser light sources 4 and 5 are fixed to a rotating body that rotates around a rotation center element 1 that is a pivot element, and the optical axes of laser beams emitted from the laser light sources 4 and 5 are arranged to be orthogonal to the rotation center element 1.

  The two-dimensional position detection devices 6 and 7 are fixed to a support mechanism, receive laser beams emitted from the laser light sources 4 and 5 by their position information detection surfaces, detect the position information of the light reception points, and roll angles. A posture angle of the rotating body expressed by angles around three axes (around the x axis), yaw angle (around the z axis), and pitch angle (around the y axis) is calculated. Here, the roll angle is the positive rotation direction of the x-axis 201 of the coordinate system fixed to the support mechanism, and the yaw angle is the positive angle of the z-axis 203 of the coordinate system fixed to the support mechanism. The direction of rotation is a clockwise rotation angle, and the pitch angle is a clockwise rotation angle of the positive direction of the y-axis 202 of the coordinate system fixed to the support mechanism. In FIG. 1, the y-axis 202 stands perpendicular to the paper surface, and the vertical standing direction is a positive direction. As the two-dimensional position detection element, a PSD or a CCD (Charge Coupled Device) device can be used.

  The optical axes of the laser beams emitted from the laser light sources 4 and 5 that rotate together with the rotating body are orthogonal to each other at the rotation center element 1, and the optical axes of the two laser beams are, as an initial state, two-dimensional position detection device 6, respectively. 7 perpendicularly intersects the center of the position detection surface. Vectors 2 and 3 are vectors directed from the rotation center element 1 to the laser light sources 4 and 5 along the optical axis of the laser beam, respectively. The initial state shown in FIG. 1 is linked to the rotational motion of the rotating body. Move from. However, the state where the start point of the vector and the two vectors are orthogonal is maintained.

  FIG. 1 shows a case where the laser light sources 4 and 5 fixed to the rotating body are in an initial state, and the two-dimensional position detection devices 6 and 7 and the laser light sources 4 and 5 are on the xz plane in the three-dimensional space. Has been placed. The two-dimensional position detection devices 6 and 7 have two vectors (which coincide with the vectors 2 and 3 in the initial state of FIG. 1) from the rotation center element 1 toward the center of the position detection surface of the two-dimensional position detection devices 6 and 7. ) Are fixed to the support mechanism so as to have an angle of +45 [deg] and +135 [deg] with respect to the positive direction of the x-axis on the xz plane. Here, the symbol [deg] is a unit indicating an angle, and indicates a central angle of the circumference of one unit when the circumference is equally divided into 360 (the same applies hereinafter).

  The laser light sources 4 and 5 are fixed to a rotating body, and the three-dimensional rotation is performed around the rotation center element 1 while the optical axes thereof are orthogonal to each other at the rotation center element 1. It is assumed that the optical axes of the two laser beams rotate within a range that intersects the position detection surface of the two-dimensional position detection device 6 or 7.

  FIG. 2 shows a configuration example (2) of the attitude angle detection device according to the present invention, which shows a configuration example of the laser light sources 4 and 5 shown in FIG. 1, and the laser light emitted from the laser light emitting element 8 is condensed. A state in which light is condensed by the lens 9 is shown.

  In the configuration of FIG. 1 described above, for easy understanding, the optical axes of the two laser beams are on the xz coordinate plane in the support mechanism fixed coordinate system as an initial state, and the two two-dimensional position detection devices have a positive x-axis position. Although the case where the support mechanism is fixedly arranged at the angles of +45 [deg] and +135 [deg] with respect to the direction is shown, the present invention is not limited to this. In the following description, the principle of calculating the attitude angle of the rotating body will be described on the assumption that the optical axes orthogonal to each other in the rotation center element 1 are at arbitrary positions in the three-dimensional space as the initial state.

FIG. 3 is a conceptual diagram of the coordinate system definition. The position detection two-dimensional coordinate system defined on the position detection surface of the two-dimensional position detection apparatus, and the position detection three-dimensional coordinate system ^A x ^A defined based on the position detection two-dimensional coordinate system. y ^A z is shown.

  The axes 101 and 102 are coordinate axes of a position detection two-dimensional coordinate system defined on the position detection surface of the two-dimensional position detection device 6.

The ^A x axis is defined as a vector having the rotation center element 1 as the origin and going toward the center of the position detection surface of the two-dimensional position detection device 6, and intersects the position detection surface perpendicularly. ^{The A} y axis and the ^A z axis are defined as vectors starting from the rotation center element 1 and having the same orientation as the axis 101 and the axis 102, respectively. ^A x-axis, which is defined as above, ^A y-axis is defined as ^A z position detecting 3-dimensional coordinate system composed of coordinate system axis ^A x ^A y ^A z.

  When the rotating body rotates three axes, the optical axis of the laser beam emitted from the laser light source 4, that is, the vector 2 from the rotation center element 1 toward the position detection surface of the two-dimensional position detection device 6 is displaced. It is detected as the displacement of the light receiving position in the position detection two-dimensional coordinate system defined on the position detection surface of the two-dimensional position detection device 6.

When the coordinates of the light receiving position in the position detection two-dimensional coordinate system are represented by P _y on the 101 axis and P _z on the 102 axis, the origin (0, 0) is ( P _y , P _z ), which is detected as a displacement to the position detection three-dimensional coordinate system ^A x ^A y ^A z, from the rotation center element 1 to the center of the position detection surface of the two-dimensional position detection device 6 Is a distance from the coordinates (d, 0, 0) to the coordinates (d, P _y , P _z ).

The position detecting 3-dimensional coordinate system ^A x ^A y ^A z, the origin (0,0,0) from the coordinates _{(d, P y, P z} ) unit vector were normalized to the unit vector of the ray vector ^s P (Corresponding to vector 2 in FIG. 3)

It is defined as Here, | ^S P | in = 1, ^S P _x is normalized so that ^A x component of the unit vector a distance d from the center of rotation element 1 to the center of the position detection surface of the two-dimensional position detecting apparatus 6 ^S P _y and ^S P _z are values obtained by normalizing the position detection amounts on the position detection surface of the two-dimensional position detection device 6 so as to become the ^A y component and the ^A z component of the unit vector, respectively. It is.

Similarly for the other two-dimensional position detection device 7, a position detection three-dimensional coordinate system ^B x ^B y ^B z is defined. In the coordinate system ^B x ^B y ^B z, the rotation center element 1 shown in FIG. A unit beam vector ^s Q obtained by normalizing a vector from the rotation center element 1 toward the position detection surface of the two-dimensional position detection device 7 to a unit vector on the optical axis of the laser beam emitted from the passing laser light source 5;

It is defined as

Next, ^A x-axis of the position detection three-dimensional coordinate system ^{A x A y A z, A} y -axis, the unit vector in the support mechanism fixed coordinate system xyz of ^A z-axis, _{^{respectively, A e x, A e y}} , A _ez and position detection 3D coordinate system
^{If the} unit vectors in the support mechanism fixed coordinate system xyz of ^B x ^B y ^B z of ^B x axis, ^B y axis, and ^B z axis are ^B e _x , ^B e _y , and ^B e _z , respectively, position detection 3D The transformation matrices from the coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z to the support mechanism fixed coordinate system xyz are respectively:

Next, the unit ray vector ^S P expressed by the position detecting 3-dimensional coordinate system, ^S Q unit light vector P expressed in the support mechanism fixed coordinate system xyz, Q is

Thus, the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system
^{From the} coordinate axis component by ^B x ^B y ^B z, the coordinate axis component by the support mechanism fixed coordinate system xyz can be obtained.

FIG. 4 is a conceptual diagram of coordinate system conversion. The unit ray vectors P and Q, the position detection three-dimensional coordinate system ^A x ^A y ^A z, the position detection three-dimensional coordinate system ^B x ^B y ^B z, and the support mechanism The outline of the relationship with the fixed coordinate system xyz is shown.

For two-dimensional position detecting apparatus 6, a vector orthogonal to the center of the position detection surface center of rotation element 1 as the starting point and ^A x-axis, axis 101 and a two-dimensional coordinate system defined on the position detecting surface ^A position detection three-dimensional coordinate system ^A x ^A y ^A z is configured with vectors parallel to 102 and starting from the rotation center element 1 as the ^A y axis and the ^A z axis, respectively. Similarly, the position detection three-dimensional coordinate system ^B x ^B y ^B z is configured for the two-dimensional position detection device 7.

Next, the posture angles of the unit ray vectors P and Q orthogonal to each other are defined. In the support mechanism fixed coordinate system xyz system, two unit vectors [1 0 0] ^T and [0 0 1] ^{T which} are orthogonally rotated by three axes while maintaining the orthogonal relationship, are orthogonal to each other. , Q, a unit of orthogonally intersecting a set of roll angle (rotation angle around x axis) α, yaw angle (rotation angle around z axis) β, and pitch angle (rotation angle around y axis) γ It is defined as the attitude angle of ray vectors P and Q, and so on
It is written as “(α, β, γ)”.

At this time, the rotation matrix of the roll angle α around the x axis is R _x (α), the rotation matrix of the yaw angle β around the z axis is R _z (β), and the rotation matrix of the pitch angle γ around the y axis is R _y When expressed as (γ), the rotation matrix at the time of three-axis rotation is expressed as follows.

The unit vectors [1 0 0] ^T and [0 0 1] ^{T that} are orthogonal to the above equation (2) and the definition of the attitude angle rotate in three axes in the order of roll angle α, yaw angle β, and pitch angle γ, respectively. Since the orthogonal unit ray vectors P and Q become

Based on this, the following relational expressions for the coordinate axis component of the support mechanism fixed coordinate system xyz and the posture angle (α, β, γ) components are obtained for the unit light vectors P and Q.

    From the above equation (4), the posture angles (α, β, γ) of the unit light vectors P, Q can be obtained from the coordinate axis components of the support mechanism fixed coordinate system xyz of the unit light vectors P, Q.

Here, arctan2 (Y, X) is a function used in robotics or the like, and is an extension of tan ^-1 (Y / X). When the angle is displayed in radians, the range of tan ^-1 (Y / X) is

  Arctan2 (Y, X) specifies the quadrant from the signs of the arguments Y and X, while X = 0 is not supported.

  Even if X = 0, depending on the sign of Y,

  Is output.

Next, with the rotation center element 1 as the starting point, unit vectors that are orthogonal to each other and point in the center direction of the position detection surfaces of the two-dimensional position detection devices 6 and 7 fixed to the support mechanism, that is, the unit ray vector P _{0 in the} initial state. , Q ₀ becomes unit beam vectors P, Q orthogonal to each other by three-axis rotation of the rotating body. For example, a vector couple mutually orthogonal as shown in FIG. 1, respectively, corresponds to a unit of the initial state light vector P0, Q _0.

The attitude angle of the unit light vector P0, Q ₀ to the initial state orthogonal _{(α 0, β 0, γ} 0) and the unit light vector P orthogonal, the attitude angle of Q (α, β, γ) to. The components of the posture angle when the rotating body rotates three axes, that is, the change amounts of the roll angle, the yaw angle, and the pitch angle are expressed as Δα, Δβ, and Δγ, respectively. A rotation matrix for performing is expressed as ΔR (Δα, Δβ, Δγ),

Because

Furthermore, as a characteristic of the rotation matrix, the inverse matrix is equal to the transpose matrix, so

If the initial posture angles (α ₀ , β ₀ , γ ₀ ) of the orthogonal unit ray vectors of the rotator are known, the unit ray vectors of the rotated rotator after the rotation obtained by the above equation (4) are obtained. Based on the attitude angles (α, β, γ), a rotation matrix ΔR (Δα, Δβ, Δγ) can be obtained.

Next, in order to calculate the roll angle Δα, the yaw angle Δβ, and the pitch angle Δγ based on the rotation matrix ΔR (Δα, Δβ, Δγ) obtained above, orthogonal unit vectors [1 0 0] ^T , [0 0 1] ^T represents the posture angles (Δα, Δβ, Δγ) of orthogonal unit vectors e ₁ , e ₂ that are obtained by rotating three axes by the roll angle Δα, the yaw angle Δβ, and the pitch angle Δγ, respectively. What is necessary is just to obtain | require using (4).

That is, first, the unit vector e _x = [1 0 0] ^T in the x-axis direction on the support mechanism fixed coordinate system xyz and the unit vector e _z = [0 0 1] ^T in the _z -axis direction are converted into a rotation matrix ΔR. Convert with (Δα, Δβ, Δγ),

Next, the orthogonal unit vectors e ₁ = [e _1x , e _1y , e _1z ] ^T and e ₂ = [e _2x , e _2y , e _2z ] ^T and their attitude angles (Δα, Δβ, Applying equation (4) to the component of Δγ)

Based on this, the final displacement amounts Δα, Δβ, Δγ of the three-axis attitude angles can be calculated.

  The above is the principle of posture angle calculation according to the present invention.

  Next, a calculation procedure for obtaining the amount of change in the posture angle of the rotating body from the detection data of the light receiving position on the position detection surface of the two-dimensional position detection device using the above formulas (1) to (6). An example is shown.

  The following embodiment is intended to show the process of the calculation procedure using the above attitude angle calculation principle, and the calculated result includes a slight error because a rounding error is mixed in the calculation process. However, this does not affect the essence of the present invention.

  FIG. 5 is an overall view of the embodiment (1) of the present invention, and FIG. 6 is an enlarged view of the main part in the embodiment (1) of the present invention shown in FIG.

The laser light sources 301 and 302 are fixed to the rotating body 305 so that the optical axes of the laser beams emitted from the laser light sources 301 and 302 are orthogonal to each other by the rotation center element 1 (spherical bearing). Here, an example is shown in which a PSD is used as a two-dimensional position detection device. The PSDs 304 and 303 are fixed to the support mechanism 306 in such a manner that the perpendicular lines from the rotation center element 1 to the centers of their position detection surfaces are orthogonal to each other. Has been. The support mechanism fixed coordinate system xyz is defined for the support mechanism 306, the position detection three-dimensional coordinate system ^A x ^A y ^A z is defined for PSD 304, and the position detection three-dimensional coordinate system ^B x ^B y is defined for PSD 303. ^B z is defined.

In the embodiment (1) shown in FIGS. 5 and 6, assuming that the unit vectors from the rotation center element 1 to the centers of the position detection surfaces of the PSDs 304 and 303, that is, the unit ray vectors orthogonal to each other are P ₀ and Q ₀ , respectively. The posture angle is (0 [deg], 0 [deg], +45 [deg]). That is, the orthogonal unit ray vectors P ₀ and Q ₀ in the initial state are obtained by rotating unit vectors [1 0 0] ^T and [0 0 1] ^T orthogonal to each other around the y axis in the support mechanism fixed coordinate system xyz (y Same as rotating +45 [deg] clockwise (in the clockwise direction of the axis).

On the other hand, the position detection three-dimensional coordinate system ^A x ^A y ^A z of the PDS 304 is defined as a coordinate system that is coincident with the support mechanism fixed coordinate system xyz rotated by +45 [deg] around the y axis. In addition, the position detection three-dimensional coordinate system ^B x ^B y ^B z of the PDS 303 is obtained by rotating the coordinate system that coincides with the support mechanism fixed coordinate system xyz about −45 [deg] around the y axis. Define.

At this time, coordinate transformation matrices ^A T and ^B T respectively corresponding to the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z are as follows.

Further, the position detection value measured in the position detection two-dimensional coordinate system of the PSD 304 is represented as a displacement in the y-axis direction and the z-axis direction in the position detection three-dimensional coordinate system ^A x ^A y ^A z by definition, and PSD 303 The position detection values measured in the position detection two-dimensional coordinate system are expressed as displacements in the y-axis direction and the z-axis direction in the position detection three-dimensional coordinate system ^B x ^B y ^B z.

In the embodiment (1) shown in FIGS. 5 and 6, the value of the x-axis in both the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z That is, the distance from the rotation center element 1 to the position detection surfaces of PSD 304 and PSD 303 is fixed at d = 40 [mm]. Here, [mm] indicates that the length is displayed in millimeters (the same applies hereinafter).

Based on the condition of the above embodiment (1), the orthogonal unit ray vector of the rotator 305 is changed from the initial posture angle (0 [deg], 0 [deg], +45 [deg]) to the posture angle (α, β (1) and (2) below show simulation examples for calculating the displacement attitude angles (Δα, Δβ, Δγ) when rotated to (, γ) using the attitude angle calculation principle according to the present invention.
(1) Displacement posture angle calculation simulation 1
Assume that the position detection values in the position detection two-dimensional coordinate system defined on the PSD position detection surface as a result of the rotation of the rotator 305 about the pitch axis (y-axis 202) are as follows. Here, the position detection value is expressed in the form of “(value in the y-axis direction of the position detection three-dimensional coordinate system, value in the z-axis direction of the position detection three-dimensional coordinate system)”.

-PSD304: (0.00 [mm], + 3.50 [mm])
-PSD303: (0.00 [mm], + 3.50 [mm])
Here, the value of the x-axis in the position detection three-dimensional coordinate system, that is, the distance from the rotation center element 1 to the position detection surfaces of PSD 304 and PSD 303 is fixed at d = 40 [mm]. In the three-dimensional coordinate system ^A x ^A y ^A z, the vector indicating the detection point on the position detection surface of PSD304 is (+40 [mm] 0.00 [mm] +3.50 [mm]) ^T , which is normalized to a unit vector the unit light vector ^S P in the position detecting 3-dimensional coordinate system coordinate system ^A x ^A y ^A z that,

  It becomes.

Similarly, in the position detection three-dimensional coordinate system coordinate system ^B x ^B y ^B z, the vector indicating the detection point on the position detection surface of the PSD 303 is (+40 [mm] 0.00 [mm] +3.50 [mm]) ^T The unit ray vector ^S Q in the position detection three-dimensional coordinate system ^B x ^B y ^B z normalized to the unit vector is

It becomes.

Units from the applied formula (7), the unit ray vector ^S P and ^S Q are orthogonal, perpendicular to each other were represented by a supporting mechanism fixed coordinate system xyz on the equation (1) to (6) and Example (1) Ray vectors P and Q are

It becomes. When this result is applied to the above equation (4), the posture angles (α, β, γ) of the orthogonal unit light vectors P and Q that rotate in conjunction with the rotating body 305 are

From now on

Is obtained.

From the above results, the rotation matrices R _x (α), R _z (β), R _y (γ) around the three axes of the orthogonal unit light vectors P and Q rotating in conjunction with the rotating body 305 are:

It becomes.

Similarly, since the posture angles (α ₀ , β ₀ , γ ₀ ) in the initial state of the orthogonal unit ray vectors P and Q are (0 [deg], 0 [deg], +45 [deg]). , Rotation matrices R _x (α ₀ ), R _z (β ₀ ), R _y (γ ₀ ) around three axes of initial posture angles are

It becomes.

  By applying the above result to the equation (5), the rotation matrix ΔR (Δα, Δβ, Δγ) corresponding to the rotation of the change in the posture angle of the orthogonal unit ray vectors P and Q is

It becomes.

On the other hand, if the orthogonal unit vectors [1 0 0] ^T and [0 0 1] ^T are rotated by the posture angles (Δα, Δβ, Δγ) to become orthogonal vectors e ₁ and e ₂ ,

When this is applied to the equation (6),

Is obtained.

From the above, in conjunction with the rotation of the rotator 305, the orthogonal unit light vector of the rotator 305 represents the initial attitude angle (α ₀ , β ₀ , γ ₀ ) = (0 [deg], 0 [deg], + Position detection value at PSD304 and PSD303 when rotating from 45 [deg]) PSD304: (0.00 [mm], +3.50 [mm])
-PSD303: (0.00 [mm], + 3.50 [mm])
In this case, the displacement posture angle (Δα, Δβ, Δγ) of the unit light vector due to rotation can be calculated as (0.0 [deg], 0.0 [deg], −5.003 [deg]).

Next, a second displacement posture angle calculation example in the embodiment (1) is shown.
(2) Displacement attitude angle calculation simulation 2
In the above embodiment (1), it is assumed that the position detection values in the PSD as a result of the rotation of the rotator 305 about the pitch axis (y-axis 202) are as follows.

-PSD304: (+2.97 [mm],-2.15 [mm])
-PSD303: (0.002 [mm],-2.15 [mm])
^A vector indicating a detection point on the position detection surface of the PSD 304 in the position detection three-dimensional coordinate system ^A x ^A y ^A z defined in the PSD 304 is (40 [mm], +2.97 [mm], -2.15 [mm ]) T , and the unit light vector ^S P output which normalized to a unit vector and the position detected three-dimensional coordinate system ^a x ^a y ^a z is

It becomes. Similarly, vectors indicating detection points on the position detection surface of PSD 303 based on the position detection three-dimensional coordinate system ^B x ^B y ^B z defined in PSD 303 are (+40 [mm], +0.002 [mm]. , -2.15 [mm]) ^T , and the unit ray vector ^S Q of the position detection three-dimensional coordinate system ^B x ^B y ^B z normalized to the unit vector is

It becomes.

In addition, the rotation matrices ^A T and ^B T from the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z to the support mechanism fixed coordinate system xyz system are as described above. Same as displacement attitude angle calculation simulation 1,

From the above equation (1), the orthogonal unit ray vectors P and Q in the support mechanism fixed coordinate system xyz system are

It becomes. Applying the above result to the equation (4), the posture angles (α, β, γ) of the orthogonal unit ray vectors P and Q are

From now on

Is obtained.

From the above results, the rotation matrices R _x (α), R _z (β), R _y (γ) around the three axes of the unit ray vectors P and Q orthogonal to each other are

It becomes. Similarly, the posture angles (α ₀ , β ₀ , γ ₀ ) in the initial state of the unit ray vectors P and Q orthogonal to each other are (0 [deg], 0 [deg], +45 [deg]). From that

The rotation matrices R _x (α ₀ ), R _z (β ₀ ), R _y (γ ₀ ) around the three axes of the initial posture angle are

It becomes.

By applying the above result to the above equation (5), the orthogonal unit ray vectors P and Q have their initial attitude angles (α ₀ , β ₀ , γ ₀ ) = (0 [deg], 0 [deg] , +45 [deg]), the rotation matrix ΔR (Δα, Δβ, Δγ) when rotating the change in posture angle (Δα, Δβ, Δγ) is

It becomes. On the other hand, orthogonal unit vectors [1 0 0] ^T and [0 0 1] ^T
If it is rotated by (Δα, Δβ, Δγ) to become orthogonal vectors e ₁ and e ₂ ,

When this is applied to the equation (6),

Is obtained.

From the above, the orthogonal unit light vector rotating in conjunction with the rotating body 305 is obtained from the initial attitude angle (α ₀ , β ₀ , γ ₀ ) = (0 [deg], 0 [deg], +45 [deg]). Displacement posture angle (Δα, Δβ, Δγ) when rotating to the current posture angle (α, β, γ) is (+3.0037 [deg], + 2.9979 [deg], + 2.9916 [deg]) And can be calculated.

  The above is the embodiment of the displacement attitude angle calculation simulation for the embodiment (1) of the present invention shown in FIGS.

  Next, another embodiment (2) will be described.

  FIG. 7 shows an embodiment (2) of the present invention.

  The single laser light source 401, the half mirror 402, and the mirrors 403 to 407 are fixed to a rotating body that rotates around the rotation center element 1 (spherical bearing), and the PSDs 410 and 411 are fixed to the support mechanism 412. Yes. Compared with the embodiment (1) shown in FIGS. 5 and 6, this embodiment (2) has an advantage that a single laser light source can be used and a space can be saved by applying a small mirror. . The laser beam emitted from the laser light source 401 is divided into two laser beams 408 and 409 by the half mirror 402 and the mirrors 403 to 407 and received by the PSDs 410 and 411. Here, the optical axes of the laser beams 408 and 409 are orthogonal to each other in the rotation center element 1.

For the support mechanism 412, a support mechanism fixed coordinate system xyz consisting of three axes of x-axis 201, y-axis 202 (standing perpendicular to the paper surface), and z-axis 203 is defined. For PSD411, a position detection three-dimensional coordinate system is defined. ^A x ^A y ^A z is defined, and the position detection three-dimensional coordinate system for PSD 410
^B x ^B y ^B z is defined.

  The laser beams 408 and 409 rotate in conjunction with the rotating body, and FIG. 7 shows the initial state. That is, the vector indicating the laser beam 408 is a vector from the rotation center element 1 toward the center of the PSD 411 as an initial vector, and the vector indicating the laser beam 409 is a vector from the rotation center element 1 to the center of the PSD 410 as an initial vector. Rotate while maintaining an orthogonal relationship with each other while being fixed to the rotating body.

When unit vectors from the rotation center element 1 toward the centers of the PSDs 411 and 410 are expressed by P ₀ and Q ₀ , respectively, they are orthogonal to each other, and the roll angle (about the x axis around the support mechanism fixed coordinate system xyz) ), The posture angle indicated by the yaw angle (around the z-axis) and the pitch angle (around the y-axis) are expressed in the form of “(roll angle, yaw angle, pitch angle)”, and the vectors P ₀ and Q ₀ are expressed from FIG. Is a unit vector [1 0 0] ^T and [0 0 1] ^T that are orthogonal to each other in the support mechanism fixed coordinate system xyz by −45 [deg] around the y axis (clockwise in the positive direction of the y axis). ] From the definition of the attitude angle, it is assumed that the attitude angles of the unit vectors P ₀ and Q ₀ that are orthogonal to each other are (0 [deg], 0 [deg], -45 [deg]) Can do.

7, the position detection three-dimensional coordinate system ^A x ^A y ^A z defined in the PDS 411 is rotated by −45 [deg] degrees around the y axis in the coordinate system that coincides with the support mechanism fixed coordinate system xyz. The position detection three-dimensional coordinate system ^B x ^B y ^B z defined in the PDS 303 is defined as a coordinate system that coincides with the support mechanism fixed coordinate system xyz about −135 [deg ] Rotation is defined. At this time, the coordinate transformation matrices ^A T and ^B T respectively corresponding to the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z are as follows.

The position detection value of the laser beam 408 measured on the position detection surface of the PSD 411 is detected as a displacement in the y-axis direction and the z-axis direction in the position detection three-dimensional coordinate system ^A x ^A y ^A z, and the position detection surface of the PSD 410 The position detection value of the laser beam 409 measured at is detected as a displacement in the y-axis direction and the z-axis direction in the position detection three-dimensional coordinate system ^B x ^B y ^B z. Also, at any position detecting 3-dimensional coordinate system ^A x ^A y ^A z and the position detecting 3-dimensional coordinate system ^B x ^B y ^B z is also the position of the x-axis direction is fixed to d = 40 [mm].

Under the condition of the above embodiment (2), the orthogonal unit ray vector defined in the rotating body is changed from the initial posture angle (0 [deg], 0 [deg], +45 [deg]) to the posture angle (α, β , γ) displacement attitude angle when rotating to
Examples of simulation when calculating (Δα, Δβ, Δγ) using the attitude angle calculation principle according to the present invention are shown in the following (3) and (4).
(3) Displacement attitude angle calculation simulation 3
Assume that the position detection values on the position detection surface of the PSD as a result of the rotation of the rotating body around the pitch axis (y-axis 202) are as follows.

・ PSD411: (0.0 [mm], +3.50 [mm])
-PSD410: (0.0 [mm], +3.50 [mm])
Vector position detection points in the position detection surface of the rotating central element 1 (spherical bearing) based on a position detection three-dimensional coordinate system ^A x ^A y ^A z about the PSD411 is (+40 [mm] 0.00 [mm ] + 3.50 [mm]) ^T, and the unit light vector ^S P obtained by normalizing this unit vector,

It becomes. Similarly, the unit ray vector ^S Q in the position detection three-dimensional coordinate system ^B x ^B y ^B z for the PSD 410 is

It becomes.

Further, from the above equation (8), the transformation matrix ^A T from the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z to the support mechanism fixed coordinate system xyz. And ^B T are

Therefore, from the above equation (1), the orthogonal unit ray vectors P and Q in the support mechanism fixed coordinate system xyz are

It becomes. Applying the above result to the equation (4), the posture angles (α, β, γ) of the orthogonal unit ray vectors P and Q are

From now on

Is obtained.

From the above results, the rotation matrices R _x (α), R _z (β), R _y (γ) around the three axes of the orthogonal unit ray vectors P and Q that are fixed to the rotating body and rotate are:

It becomes. Similarly, the posture angles (α ₀ , β ₀ , γ ₀ ) in the initial state of the unit ray vectors P and Q orthogonal to each other are (0 [deg], 0 [deg], −45 [deg]). From

Is obtained, the initial attitude angle _{(α 0, β 0, γ} 0) of the three axes of rotation matrix _{R x (α 0), R} z (β 0), R y (γ 0) is

It becomes.

  By applying the above results to the above equation (5), a rotation matrix ΔR (Δα, Δβ, Δγ) that rotates the orthogonal unit ray vectors P and Q by the posture angle displacement is

It becomes. On the other hand, if the orthogonal unit vectors [1 0 0] ^T and [0 0 1] ^T are rotated by posture angles (Δα, Δβ, Δγ) to become orthogonal vectors e ₁ and e ₂ ,

When this is applied to the equation (6),

Is obtained. That is, the displacement attitude angles (Δα, Δβ, Δγ) of the orthogonal unit ray vectors P and Q are (0.0 [deg], 0.0 [deg], −5.0035 [deg]).
(4) Displacement posture angle calculation simulation 4
In the embodiment (2) of the present invention shown in FIG. 7, it is assumed that the position detection values in the PSD as a result of the rotation of the rotating body from the initial state are as follows.

PSD411: (0.002 [mm], -2.15 [mm])
PSD410: (-2.97 [mm], -2.15 [mm])
The vector indicated by the detected value in PSD 411 based on the position detection three-dimensional coordinate system ^A x ^A y ^A z around the rotation center element 1 (spherical bearing) is [+40 [mm] +0.002 [mm] -2.15 [mm ]] ^T, and the normalized unit light vector ^S P is the unit vector of this,

It becomes. Similarly, the unit ray vector ^S Q of the position detection three-dimensional coordinate system ^B x ^B y ^B z for the PSD 410 is

It becomes.

Further, the transformation matrices ^A T and ^B T from the position detection three-dimensional coordinate system ^A x ^A y ^A z and the position detection three-dimensional coordinate system ^B x ^B y ^B z system to the support mechanism fixed coordinate system xyz system are: Similar to the displacement attitude angle calculation simulations 1 and 2 above,

From the above equation (1), the orthogonal unit light vectors P and Q in the support mechanism fixed coordinate system xyz are

It becomes. Applying the above result to the equation (4), the posture angles (α, β, γ) of the orthogonal unit ray vectors P and Q are

From now on

Is obtained.

From the above results, the rotation matrices R _x (α), R _z (β), R _y (γ) around the three axes of the orthogonal unit ray vectors P and Q that rotate in conjunction with the rotating body 305 are:

It becomes. Similarly, the posture angles (α ₀ , β ₀ , γ ₀ ) in the initial state of the orthogonal unit ray vectors P and Q are (0 [deg], 0 [deg], −45 [deg]). From

The rotation matrices R _x (α ₀ ), R _z (β ₀ ), R _y (γ ₀ ) around the three axes of the initial posture angle are

It becomes.

  By applying the above result to the above equation (5), a rotation matrix ΔR (Δα, Δβ, Δγ) for performing rotation of the orthogonal unit ray vectors P and Q by the posture angle displacement is

It becomes. On the other hand, if the orthogonal unit vectors [1 0 0] ^T and [0 0 1] ^T are rotated by posture angles (Δα, Δβ, Δγ) to become orthogonal vectors e ₁ and e ₂ ,

When this is applied to the equation (6),

When the orthogonal unit ray vectors P and Q that rotate in conjunction with the rotating body rotate from their initial attitude angles (0 [deg], 0 [deg], -45 [deg]) (+3.000 [deg], +2.996 [deg], +3.003 [deg]) is obtained as the displacement posture angle from the posture angle.

  In the embodiment of the present invention described in detail above, the laser beam and the two-dimensional position detection device that receives the laser beam and detects the position are shown as an example. The present invention can be applied to any two-dimensional position detection apparatus that can detect the above without being limited to the laser beam.

In order to make the explanation easy to understand, in the simulation example of the displacement attitude angle calculation, the xz plane of the support mechanism fixed coordinate system xyz includes the center of the position detection surface and the rotation center element of the two two-dimensional position detection devices. Although explained, the principle of posture angle calculation of the present invention can be applied without being limited to this. That is, the orthogonal unit ray vectors P ₀ and Q ₀ indicating the initial posture of the rotating body can be arbitrarily set.

  In the above-described embodiment, an example using a PSD as a two-dimensional position detection device has been described. However, the present invention is not limited to this, and other means such as a CCD element may be used.

The embodiment of the present invention described in detail above is as follows.
(Supplementary Note 1) The rotating body is attached to the support mechanism via the pivot element,
A laser light source is fixed to the rotating body so that optical axes of two laser beams originating from the laser light source are orthogonal to each other at a rotation center element of the rotating body,
A two-dimensional position detection device for detecting the light receiving position by receiving the laser beam corresponding to each laser beam is provided,
The two-dimensional position detection device is fixed to the support mechanism so that the optical axis of the laser beam received at the center of the position detection surface of the two-dimensional position detection device is orthogonal to the position detection surface,
Based on light reception position information detected by the two-dimensional position detection device, a posture angle when the rotating body rotates around the rotation center element is calculated.
A posture angle detection method characterized by the above.
(Supplementary note 2) In the attitude angle detection method according to supplementary note 1,
Defining a two-dimensional coordinate system having the origin of the center of the position detection surface of the two-dimensional position detection device on the position detection surface to form a position detection two-dimensional coordinate system;
A vector from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate system is set as the x axis, and the rotation center element is set as the start point and is parallel to two axes defined by the position detection two-dimensional coordinate system. Define a three-dimensional coordinate system with vectors as y-axis and z-axis, respectively, to obtain a position detection three-dimensional coordinate system,
Based on light reception position information of the laser beam detected in the position detection two-dimensional coordinate system, unit beam vector information indicating the posture of the rotating body in the position detection three-dimensional coordinate system is generated,
Calculating the posture angle of the rotating body based on the two pieces of unit light vector information generated corresponding to each of the two-dimensional position detection devices;
A posture angle detection method characterized by the above.
(Appendix 3) In the attitude angle detection method according to Appendix 2,
In the position detection three-dimensional coordinate system, the distance from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate is x, and the light receiving position when the laser beam is received in the position detection two-dimensional coordinate system Let y and z be two coordinate values indicating
The unit ray vector information is vector information obtained by normalizing a vector starting from the origin of the position detection three-dimensional coordinate system and having the three-dimensional coordinates (x, y, z) as an end point into a unit vector.
A posture angle detection method characterized by the above.
(Appendix 4) In the attitude angle detection method according to any one of appendices 1 to 3,
Define a three-dimensional coordinate system consisting of an x-axis, a y-axis, and a z-axis fixed to the support mechanism to form a support mechanism fixed coordinate system,
In the support mechanism fixed coordinate system, the posture angle of the rotating body is a roll angle indicating clockwise rotation in the positive direction of the x axis, a yaw angle indicating clockwise rotation in the positive direction of the z axis, and a y axis. Expressed as a three-axis rotation angle that rotates three axes in the order of the pitch angle indicating clockwise rotation in the positive direction.
A posture angle detection method characterized by the above.
(Supplementary note 5) In the attitude angle detection method according to supplementary note 1,
The laser light source fixed to the rotating body is composed of two laser light sources,
The two laser beams emitted from the two laser light sources are arranged so that their optical axes are orthogonal to each other at the rotation center element of the rotating body,
A posture angle detection method characterized by the above.
(Supplementary note 6) In the attitude angle detection method according to supplementary note 1,
The laser light source fixed to the rotating body is composed of one laser light source,
The laser beam emitted from the one laser light source is split into two laser beams via a half mirror and a mirror, and the optical axes of the two laser beams branched are orthogonal to each other at the rotation center element of the rotating body. Arranged to
A posture angle detection method characterized by the above.
(Appendix 7) a rotating body attached to the support mechanism via a pivot element;
A laser light source fixed to the rotating body;
Means for arranging the optical axes of two laser beams originating from the laser light source so as to be perpendicular to the rotation center element of the rotating body;
A two-dimensional position detection device that is provided corresponding to each laser beam and receives the laser beam to detect light reception position information;
Means for fixing the two-dimensional position detection device to the support mechanism so that an optical axis of the laser beam received by the two-dimensional position detection device at the center of the position detection surface is orthogonal to the position detection surface;
Based on the light receiving position information detected by the two-dimensional position detection device, comprising means for calculating a posture angle when the rotating body rotates around the rotation center element,
A posture angle detection device characterized by the above.
(Supplementary note 8) In the posture angle detection device according to supplementary note 7,
A position detection two-dimensional coordinate system which is a two-dimensional coordinate system defined on the position detection surface with the center of the position detection surface of the two-dimensional position detection device as an origin;
A vector from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate system is set as the x axis, and the rotation center element is set as the start point and is parallel to two axes defined by the position detection two-dimensional coordinate system. A position detection three-dimensional coordinate system, which is a three-dimensional coordinate system in which vectors are defined as a y-axis and a z-axis, respectively;
Means for generating unit beam vector information indicating the posture of the rotating body in the position detection three-dimensional coordinate system based on the light-receiving position information of the laser beam detected in the position detection three-dimensional coordinate system;
Means for calculating a posture angle of the rotating body based on the two pieces of unit light vector information generated corresponding to each of the two-dimensional position detection devices;
A posture angle detection device characterized by the above.
(Supplementary note 9) In the posture angle detection device according to supplementary note 8,
In the position detection three-dimensional coordinate system, the distance from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate is x, and the light receiving position when the laser beam is received in the position detection two-dimensional coordinate system Vector information obtained by normalizing a vector starting from the origin of the position detection three-dimensional coordinate system and having the three-dimensional coordinates (x, y, z) as an end point to a unit vector. Means for providing the unit ray vector information;
A posture angle detection device characterized by the above.
(Supplementary Note 10) In the posture angle detection device according to any one of Supplementary Notes 7 to 9,
A support mechanism fixed coordinate system that is a three-dimensional coordinate system composed of an x-axis, a y-axis, and a z-axis that are defined fixedly to the support mechanism;
In the support mechanism fixed coordinate system, the posture angle of the rotating body is a roll angle indicating clockwise rotation in the positive direction of the x axis, a yaw angle indicating clockwise rotation in the positive direction of the z axis, and a y axis. Including a means for expressing a three-axis rotation angle that rotates three axes in the order of a pitch angle indicating clockwise rotation in the positive direction.
A posture angle detection device characterized by the above.
(Supplementary note 11) In the posture angle detection device according to supplementary note 7,
The laser light source fixed to the rotating body is composed of two laser light sources,
The two laser beams emitted from the two laser light sources are arranged so that their optical axes are orthogonal to each other at the rotation center element of the rotating body,
A posture angle detection device characterized by the above.
(Supplementary note 12) In the posture angle detection device according to supplementary note 7,
The laser light source fixed to the rotating body is composed of one laser light source,
The laser beam emitted from the one laser light source is split into two laser beams via a half mirror and a mirror, and the optical axes of the two laser beams branched are orthogonal to each other at the rotation center element of the rotating body. Arranged to
A posture angle detection device characterized by the above.
(Supplementary note 13) In the posture angle detection device according to supplementary note 7,
The two-dimensional position detection device includes a semiconductor position detection device (PSD).
A posture angle detection device characterized by the above.
(Supplementary note 14) In the posture angle detection method according to supplementary note 7,
The three-dimensional position detection device includes a charge coupled device (CCD).
A posture angle detection device characterized by the above.

Configuration (1) of the posture angle detection device of the present invention Configuration (2) of the posture angle detection device of the present invention Conceptual diagram of coordinate system definition Conceptual diagram of coordinate system conversion Overall view of embodiment (1) of the present invention The enlarged view of Example (1) of this invention Embodiment (2) of the present invention

Explanation of symbols

1 Center of rotation (pivot element)
2,3 vector
4,5, laser
6,7 Two-dimensional position detector
8 Laser emitter
9 Condensing lens
101,102 Coordinate axes on the position detection surface of the two-dimensional position detection device
103 Position detection 3D coordinate system ^A x ^A y ^A z ^A x axis
104 position detection three-dimensional coordinate system ^A x ^A y ^{A A} y-axis and z
105 position detection three-dimensional coordinate system ^A x ^A y ^{A A} z-axis of the z
201 x axis of support mechanism fixed coordinate system xyz
202 y-axis of support mechanism fixed coordinate system xyz
203 Z axis of support mechanism fixed coordinate system xyz
301,302 Laser light source
303,304 PSD
401 Laser light source
402 Half Mirror
403,404,405,406,407 Mirror
408,409 Laser beam
410,411 PSD
412 Support mechanism

Claims (5)

The rotating body is attached to the support mechanism via a pivot element;
A laser light source is fixed to the rotating body so that optical axes of two laser beams originating from the laser light source are orthogonal to each other at a rotation center element of the rotating body,
A two-dimensional position detection device for detecting the light receiving position by receiving the laser beam corresponding to each laser beam is provided,
The two-dimensional position detection device is fixed to the support mechanism so that the optical axis of the laser beam received at the center of the position detection surface of the two-dimensional position detection device is orthogonal to the position detection surface,
Based on light reception position information detected by the two-dimensional position detection device, a posture angle when the rotating body rotates around the rotation center element is calculated.
A posture angle detection method characterized by the above. The attitude angle detection method according to claim 1,
Defining a two-dimensional coordinate system having the origin of the center of the position detection surface of the two-dimensional position detection device on the position detection surface to form a position detection two-dimensional coordinate system;
A vector from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate system is set as the x axis, and the rotation center element is set as the start point and is parallel to two axes defined by the position detection two-dimensional coordinate system. Define a three-dimensional coordinate system with vectors as y-axis and z-axis, respectively, to obtain a position detection three-dimensional coordinate system,
Based on light reception position information of the laser beam detected in the position detection two-dimensional coordinate system, unit beam vector information indicating the posture of the rotating body in the position detection three-dimensional coordinate system is generated,
Calculating the posture angle of the rotating body based on the two pieces of unit light vector information generated corresponding to each of the two-dimensional position detection devices;
A posture angle detection method characterized by the above. In the posture angle detection method according to claim 2,
In the position detection three-dimensional coordinate system, the distance from the rotation center element of the rotating body to the origin of the position detection two-dimensional coordinate is x, and the light receiving position when the laser beam is received in the position detection two-dimensional coordinate system Let y and z be two coordinate values indicating
The unit ray vector information is vector information obtained by normalizing a vector starting from the origin of the position detection three-dimensional coordinate system and having the three-dimensional coordinates (x, y, z) as an end point into a unit vector.
A posture angle detection method characterized by the above. In the attitude angle detection method according to any one of claims 1 to 3,
Define a three-dimensional coordinate system consisting of an x-axis, a y-axis, and a z-axis fixed to the support mechanism to form a support mechanism fixed coordinate system,
In the support mechanism fixed coordinate system, the posture angle of the rotating body is a roll angle indicating clockwise rotation in the positive direction of the x axis, a yaw angle indicating clockwise rotation in the positive direction of the z axis, and a y axis. Expressed as a three-axis rotation angle that rotates three axes in the order of the pitch angle indicating clockwise rotation in the positive direction.
A posture angle detection method characterized by the above. A rotating body attached to the support mechanism via a pivot element;
A laser light source fixed to the rotating body;
Means for arranging the optical axes of two laser beams originating from the laser light source so as to be perpendicular to the rotation center element of the rotating body;
A two-dimensional position detection device that is provided corresponding to each laser beam and receives the laser beam to detect light reception position information;
Means for fixing the two-dimensional position detection device to the support mechanism so that an optical axis of the laser beam received by the two-dimensional position detection device at the center of the position detection surface is orthogonal to the position detection surface;
Based on the light receiving position information detected by the two-dimensional position detection device, comprising means for calculating a posture angle when the rotating body rotates around the rotation center element,
A posture angle detection device characterized by the above.

Images