JP2006201092A - 6-DOF mobile body position / posture measurement method and position / posture measurement apparatus - Google Patents

Abstract

PROBLEM TO BE SOLVED: To easily and accurately measure the position / posture of a mobile body with 6 degrees of freedom using a plurality of sensors.
In a position / posture measuring apparatus for a six-degree-of-freedom moving body comprising a stator, a mover that is displaced by six degrees of freedom with respect to the stator, and a plurality of sensors, an appropriate moving coordinate system and fixed coordinates are provided. Set a system, perform coordinate transformation including rotational coordinate transformation in a fixed order, install a plurality of gap sensors 1, 2, 3, and two-dimensional sensors 5, 6 at appropriate locations, The position and orientation after the mover is displaced are calculated based on the given initial position information.
[Selection] Figure 1

Description

  The present invention relates to a measuring apparatus that measures the position and orientation of a 6-DOF moving body using a plurality of sensors.

A conventional 6-DOF moving body position / posture measuring apparatus has a measuring apparatus in which a total of six displacement sensors are installed in three directions of x, y, and z (see, for example, Patent Document 1). FIG. 9 is a block diagram showing a configuration of a conventional 6-DOF moving body position / posture measuring apparatus.
In FIG. 9, reference numeral 21 denotes a mover that is driven by an actuator (not shown) to be displaced in position / posture with 6 degrees of freedom. Reference numeral 31 denotes an x-direction displacement sensor that measures the amount of displacement in the x-direction. Reference numerals 32 and 33 denote y-direction displacement sensors, which measure a displacement amount in the y-direction and a rotation angle around the z-axis. Reference numerals 34, 35 and 36 denote z-direction displacement sensors which measure the displacement amount in the z direction and the rotation angle around the x and y axes.
When the rotational momentum around each axis is small, the influence of rotation on the measurement of the translational displacement is small, so that it can be ignored, and the position / posture after the moving element is displaced can be easily obtained.
As described above, the position / posture measurement method of the movable body with six degrees of freedom according to the prior art uses the displacement information from the six displacement sensors 31 to 36 installed in the three directions of x, y, and z. The position / posture after displacement is calculated.
JP 10-194450 A

However, the conventional position / posture measurement method for a 6-DOF moving body is difficult to accurately measure the position / posture after the mover is displaced when the rotational momentum is large or the measurement accuracy is required to be high. Therefore, there is a problem that the requested specification cannot be satisfied.
Therefore, the present invention has been made in view of such problems, and can easily and accurately move 6 degrees of freedom by installing various appropriate sensors at appropriate positions and performing appropriate coordinate conversion. 6-degree-of-freedom movement that can measure the position and posture of the body and can accurately measure the position and posture after displacement of the mover even when the rotational momentum is large or a high measurement system is required An object is to provide a body position / posture measurement method and position / posture measurement apparatus.

In order to solve the above problem, the present invention is configured as follows.
According to a first aspect of the present invention, there is provided a six-degree-of-freedom moving body position / posture measuring method comprising a stator, a mover that is displaced by six degrees of freedom with respect to the stator, and a plurality of sensors. A plurality of sensors are installed on the stator and the moving element based on a system and a fixed coordinate system, and the movement after the moving element is displaced based on detection information of the plurality of sensors and given initial position information When calculating the position and orientation of a child relative to the stator to obtain the position of an arbitrary point on the movable element, the fixed coordinate system xyzo is fixed to the stator, and the movable coordinate system XYZO is fixed to the movable element. The moving coordinate system and the fixed coordinate system are set so that the moving coordinate system and the fixed coordinate system are overlapped before the moving element is displaced, and the rotation angles of the moving elements around the x axis, the y axis, and the z axis are respectively set. Let α, β and γ be Absolute coordinates of the point _{(x o, y o, z} o) and then, the absolute coordinates (x _o of the origin of the absolute arbitrary fixed point coordinates and the movement coordinate system moving coordinates of any fixed point on the mover, y _o , z _o ) and rotation angles α, β, and γ, and the procedure of calculating the coordinates is calculated.
According to a second aspect of the present invention, the plurality of sensors includes a first gap sensor, a second gap sensor, and a third gap sensor that measure the distance between the head of the gap sensor and the target plane of the gap sensor. And rotating coordinate conversion in the order of the x-axis, y-axis, and z-axis of the fixed coordinate system, and all the heads with respect to the first gap sensor, the second gap sensor, and the third gap sensor. Are fixed to the stator and symmetrical with respect to the plane yoz or the plane xoz, and all the target planes are fixed to the movable element and parallel to the plane XOY, the first gap sensor, the second The gap sensor and the third gap sensor are installed.

The invention according to claim 3 is characterized in that the plurality of sensors include a first two-dimensional sensor and a second two-dimensional sensor for detecting a displacement amount in two orthogonal directions, and the first two-dimensional sensor and For the second two-dimensional sensor, all the heads are fixed to the stator, all the targets are fixed to the moving element, and the Z movement coordinates of the initial measurement points on all the target sides are the same, The first two-dimensional sensor and the second two-dimensional sensor and the second two-dimensional sensor have two measurement directions parallel to the x-axis or the y-axis. It features a procedure of installing a two-dimensional sensor.
According to a fourth aspect of the present invention, the plurality of sensors include a first one-dimensional sensor, a second one-dimensional sensor, and a third one-dimensional sensor that detect a displacement amount in a linear direction. With respect to one one-dimensional sensor, the second one-dimensional sensor, and the third one-dimensional sensor, the measurement directions of the first one-dimensional sensor and the second one-dimensional sensor are both y-direction, The measurement direction of the third one-dimensional sensor is the x direction, and a straight line passing through the initial measurement point on the target side of the first one-dimensional sensor and the initial measurement point on the target side of the second one-dimensional sensor is x The first one-dimensional sensor, the second one-dimensional sensor, and the third one-dimensional sensor are installed so as to be parallel to an axis.
In the invention according to claim 5, the plurality of sensors include a first one-dimensional sensor, a second one-dimensional sensor, and a third one-dimensional sensor that detect a displacement amount in a linear direction. With respect to one one-dimensional sensor, the second one-dimensional sensor, and the third one-dimensional sensor, the measurement direction of the first one-dimensional sensor is the y direction, and the measurement direction of the second one-dimensional sensor And the measurement direction of the third one-dimensional sensor is the x direction, and passes through the initial measurement point on the target side of the second one-dimensional sensor and the initial measurement point on the target side of the third one-dimensional sensor. The first one-dimensional sensor, the second one-dimensional sensor, and the third one-dimensional sensor are installed so that a straight line is parallel to the y-axis.

According to a sixth aspect of the present invention, the plurality of sensors includes a first gap sensor, a second gap sensor, and a third gap sensor that measure the distance between the head of the gap sensor and the target plane of the gap sensor. All the heads are fixed to the moving element and symmetrical with respect to the plane YOZ or the plane XOZ with respect to the first gap sensor, the second gap sensor, and the third gap sensor, The first gap sensor, the second gap sensor, and the third gap sensor are installed so that the target plane is fixed to the stator and parallel to the plane xoy.
According to a seventh aspect of the present invention, there is provided a six-degree-of-freedom moving body obtained by adding a fourth gap sensor to the six-degree-of-freedom moving body position / posture measuring apparatus according to any one of the second or sixth aspect. In the position and orientation measurement device,
The fourth gap sensor is installed such that the first gap sensor, the second gap sensor, the third gap sensor, and the fourth gap sensor are symmetrical with respect to a coordinate system, and The movable element is displaced based on detection information of any three gap sensors and given initial position information from one gap sensor, the second gap sensor, the third gap sensor, and the fourth gap sensor. After that, the position and orientation of the movable element with respect to the stator are calculated, and the procedure is to take an average value of the four sets of data thus obtained.
Further, the invention described in claim 8 is a moving element that is displaced by 6 degrees of freedom, a first three-dimensional sensor that detects a displacement amount in one direction and two rotation angles, a displacement amount in two orthogonal directions, and In a 6-DOF moving body position / posture measuring apparatus equipped with a second three-dimensional sensor that detects one rotation angle, the fixed coordinate system xyzo is fixed to the ground, and the moving coordinate system XYZO is fixed to the moving element. The moving coordinate system and the fixed coordinate system are set so that the moving coordinate system and the fixed coordinate system are overlapped before the moving element is displaced, and the rotation angles of the moving elements around the x axis, the y axis, and the z axis are respectively set. α, β, and γ, the absolute coordinates of the origin of the moving coordinate system are (x _o , _yo , z _o ), and the absolute coordinates of the arbitrary fixed point on the moving element are the moving coordinates and moving coordinates of the arbitrary fixed point. absolute coordinates of the origin of the system _{(x o, y o, z} o) and the rotation angle α The coordinate is transformed to be represented by β and γ, and the moving distance of the intersection of the fixed line that the first three-dimensional sensor is parallel to the z-axis and the target plane of the first three-dimensional sensor fixed to the moving element And the rotational angles α and β of the movable element around the x and y axes, and the displacement amount in the x direction and the displacement amount in the y direction at the target measurement point where the second three-dimensional sensor is fixed to the movable element The first three-dimensional sensor and the second three-dimensional sensor are installed so as to detect the rotation angle γ of the movable element around the z axis and the first three-dimensional sensor and the second tertiary It is characterized by the procedure of calculating the position and orientation of the moving element relative to the stator after the moving element is displaced based on detection information with the original sensor and given initial position information.

According to the present invention, various appropriate displacement sensors are installed at appropriate positions, and an appropriate coordinate system is set, so that the moving element can be controlled based on information on the installation positions of the sensors and signals detected by the sensors. The origin position and orientation of the fixed moving coordinate system can be obtained.
In addition, since the position of this point can be calculated after the mover is accurately displaced using the movement coordinates of the arbitrary point on the mover and the origin position and orientation after displacement of the movement coordinate system, the rotational momentum is large. In some cases, even when a high measurement system is required, it is possible to accurately measure the position / posture after the moving element is displaced.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

FIG. 1 is a diagram illustrating a configuration of a 6-DOF moving body position / posture measuring apparatus according to a first embodiment of the present invention.
In FIG. 1, reference numeral 21 denotes a mover that is driven by an actuator (not shown) to displace a position / posture of 6 degrees of freedom. 22 is a stator. Reference numerals 1, 2 and 3 denote gap sensors (optical, electrostatic, etc.), the head is fixed to the stator, the target is fixed to the movable element, and the distance between the head and the target plane is detected. Reference numerals 5 and 6 are two-dimensional sensors (surface detection is possible), which detect the amount of displacement in two vertical directions.
The six-degree-of-freedom moving body position / posture measuring apparatus of the present embodiment shown in FIG. 1 differs from the six-degree-of-freedom moving body position / posture measuring apparatus of Patent Document 1 shown in FIG. This is a part provided with two two-dimensional sensors 5 and 6 instead of three one-degree-of-freedom displacement sensors installed in the direction. Other configurations are the same.
Hereinafter, the measurement principle of the present embodiment will be described.
Here, in order to calculate the position and orientation precisely and clearly, a fixed coordinate system xyzo fixed to the stator and a moving coordinate system XYZO fixed to the mover and displaced together with the mover are introduced. Then, the moving coordinate system and the fixed coordinate system are set so that the fixed coordinate system xyzo overlaps the initial moving coordinate system XYZO (when the displacement of the moving element is 0). After the mover is displaced, the absolute coordinates of the origin O of the moving coordinate system are (x _O , y _O , z _O ), and in the order of the fixed coordinate system x-axis, y-axis, z-axis with respect to the moving coordinate system The absolute coordinates (x, y, z) of the point where the moving coordinates are (X, Y, Z), where (α, β, γ) is the orientation angle when rotating coordinate transformation is performed and G is the coordinate transformation matrix. Can be expressed by the following equation.

Where G ₁₁ = cos βcos γ,
G ₁₂ = cos βsin γ,
G ₁₃ = −sin β,
G ₂₁ = sin αsin βcosγ-cosαsinγ,
G ₂₂ = sinαsinβsinγ + cosαcosγ,
G ₂₃ = sin α cos β,
G ₃₁ = cosαsinβcosγ + sinαsinγ,
G ₃₂ = cosαsinβsinγ-sinαcosγ,
G ₃₃ = cos α cos β.
If the origin position (x _O , y _O , z _O ) and the posture (α, β, γ) of the moving coordinate system are obtained from the above equation, the position of an arbitrary point on the moving element can be obtained.

Based on the above coordinate system, the installation method of each sensor is demonstrated in detail.
The measurement points on the head side of the gap sensor 1, the gap sensor 2 and the gap sensor 3 are H ₁ , H ₂ and H ₃ , respectively, and the measurement points on the target side are T ₁ , T ₂ and T ₃ , respectively. In these three gap sensors, all target planes are parallel to the plane XOY, the points H ₁ and H ₂ are symmetric with respect to the plane yoz, and the points H ₂ and H ₃ are with respect to the plane xoz. And straight lines H ₁ T ₁ , H ₂ T ₂ and H ₃ T ₃ are all set parallel to the z-axis when there is no displacement of the slider. The measurement points on the target side of the two-dimensional sensor 5 and the two-dimensional sensor 6 are P and Q, respectively. The two two-dimensional sensors are installed such that the straight line PQ is parallel to the plane XOY and the two measurement directions of the sensor are parallel to the x-axis or y-axis.
Therefore, the absolute coordinates of the points H ₁ , H ₂ and H ₃ are (x _H1 , y _H1 , z _H1 ), (x _H2 , y _H2 , z _H2 ) and (x _H3 , y _H3 , z _H3 ), respectively. If the movement coordinates of point P and point Q are (X _P , Y _P , Z _P ) and (X _Q , Y _Q , Z _Q ), respectively,
x _H2 = x _H3 (2)
y _H1 = y _H2 (3)
And z _H1 = z _H2 = z _H3 , Z _P = Z _Q (4)
Holds.
Hereinafter, the origin position (x _O , y _O , z _O ) and posture (α, β,) of the moving coordinate system after the mover is displaced based on the detection information from each sensor and the given initial position of each sensor. γ) is obtained.
First, α and β are obtained based on detection information and initial position information of the three gap sensors.
When the initial z absolute coordinate of the measurement point T ₁ on the target side of the gap sensor 1 is z _T10 , the initial gap measured by the gap sensor 1 is d ₁₀ = z _T10 −z _H1 (5)
It becomes.
Since the target plane of the gap sensor is always parallel to the plane XOY, the normal vector of the target plane of the gap sensor after displacement is

It becomes. Further, since the moving coordinates of the intersection C1 between the target plane of the gap sensor 1 and the Z axis are (0, 0, z _T10 ), the absolute coordinates after the displacement are

It becomes. Therefore, the equation of the target plane of the gap sensor 1 after the movable element is displaced _{x-x O -G 13 z T10} ) G 13 + (y-y O -G 23 z T10) G 23
+ (Z−z _O −G ₃₃ z _T10 ) G ₃₃ = 0 (8)
It becomes.
In addition, when the above equation is arranged in consideration of G ₁₃ ² + G ₂₃ ² + G ₃₃ ² = 1,
_{x G13 + y G23 + z G33} = d O + z T10 ... (9)
It becomes. However,
d _O = x _O G ₁₃ + y _O G ₂₃ + z _O G ₃₃ (10)
It is.
Therefore, the distance between the point (x _H1 , y _H1 , z _H1 ) and the target plane of the gap sensor 1, that is, the gap measured by the gap sensor 1 after the mover is displaced is d ₁ = d _O + z _T10 −x _{H1 G 13 -y H1 G 23 -z H1} G 33 ... (11)
It becomes.
According to the equations (5) and (11), the amount of change of the first gap detected by the first gap sensor before and after the mover is displaced is Δd ₁ ≡d ₁ −d ₁₀ = d _O −x _H1 G ₁₃ −y _{H1 G 23 -z H1 G 33 -z} H1 ... (12)
Meet.
For the same reason, the change amounts Δd ₂ and Δd ₃ of the second and third gaps detected by the second and third gap sensors before and after the displacement of the moving element are respectively: Δd ₂ = d _O −x _H2 G ₁₃ −y _{H2 G 23 -z H2 G 33 -z H2} ... (13)
And _{Δd 3 = d O -x H3 G} 13 -y H3 G 23 -z H3 G 33 -z H3 ... (14)
Meet.
According to Formula (2) to Formula (4) and Formula (12) to Formula (14),
Δd ₁ −Δd ₂ = (x _H2 −x _H1 ) G ₁₃ (15)
And Δd ₂ −Δd ₃ = (y _H3 −y _H2 ) G ₂₃ (16)
It becomes.
Substituting G ₁₃ = −sin β and G ₂₃ = sin αcos β into the above two equations gives α and β as follows.
β = arcsin {(Δd ₁ −Δd ₂ ) / (x _H1 −x _H2 )} (17)
α = arcsin {(Δd ₂ −Δd ₃ ) / (y _H3 −y _H2 ) / cos β} (18)
Since H ₁ , H ₂ , and H ₃ are fixed points, α and β can be obtained from the above two equations by measuring x _H1 , x _H2 , y _H2, and y _H3 in advance.
Next, γ, x _O and y _O are obtained based on the obtained α and β values, detection information of the two two-dimensional sensors, and initial position information.
The absolute coordinates of point P and point Q are (x _P , y _P , z _P ) and (x _Q , y _Q , z _Q ), respectively, and according to equation (1),

and

It becomes.
Subtracting equation (20) from equation (19) taking Z _P = Z _Q into account,

It becomes.
Further, x _PQ = x _P −x _Q , X _PQ = X _P −X _Q , y _PQ = y _P −y _Q
And Y _PQ = Y _P −Y _Q
G ₁₁ = cos βcos γ,
G ₁₂ = cos βsin γ,
G ₂₁ = sin αsin βcosγ-cosαsinγ,
And G ₂₂ = sinαsinβsinγ + cosαcosγ
Substituting into the above two formulas and rearranging,

It becomes. Solving this equation gives

It becomes. Therefore, γ is given by the following equation.

In the above equation, x _PQ is the difference between x _P and x _Q, and y _PQ is the difference between y _P and y _Q.
Further, (x _P , y _P ) and (x _Q , y _Q ) are absolute coordinates after displacement of the points P and Q, respectively, and cannot be obtained directly from the sensor.
However, (X _P , Y _P ) and (X _Q , Y _Q ) are known, and the displacement of the point P measured by the two-dimensional sensor 5 is (S _Px , S _Py ), and the two-dimensional sensor 6 measures it. If the displacement amount of the point Q is (S _Qx , S _Qy ), the absolute coordinates after the displacement of the point P and the point Q are given as follows.
(X _P , y _P ) = (X _P , Y _P ) + (S _Px , S _Py ) (25)
(X _Q , y _Q ) = (X _Q , Y _Q ) + (S _Qx , S _Qy ) (26)
When the values of α, β, and γ are determined as described above, all elements G _ij (i = 1, 2, 3, j = 1, 2, 3) of the rotational coordinate transformation matrix G are obtained from the equation (1). Is decided. By substituting the moving coordinates (X _P , Y _P , Z _P ) of the point P and the xy absolute coordinates (x _P , y _P ) of the displaced point P into the equation (1), the origin of the displaced moving coordinates The xy absolute coordinates are given as follows:
_{x O = x P - (G} 11 X P + G 12 Y P + G 13 Z P) ... (27)
_{y O = y P - (G} 21 X P + G 22 Y P + G 23 Z P) ... (28)
For the same reason, the xy absolute coordinates of the origin of the movement coordinates after displacement are given as follows using the information of the point Q before and after displacement.
_{x O = x Q - (G} 11 X Q + G 12 Y Q + G 13 Z Q) ... (29)
_{y O = y Q - (G} 21 X Q + G 22 Y Q + G 23 Z Q) ... (30)
Measurement accuracy can be improved by using the average value of the two types of data obtained by using Expressions (27), (28) or Expressions (29), (30) as the xy absolute coordinates of the origin of the movement coordinates after displacement. . It is also possible to determine a detection error using these two types of data.

Finally, find z _O.
According to equations (10) and (11),
_{d 1 = z T10 + (x} O -x H1) G 13 + (y O -y H1) G 23 + (z O -z H1) G 33 ... (31)
It becomes.
From the above equation, z _O is given by the following equation.
_{z O = z H1 + {d} 1 -z T10 - (x O -x H1) G 13 - (y O -y H1) G 23} / G 33 ... (32)
In the above, although to calculate the z _O using information gap sensor 1, it is possible to calculate the z _O as follows by using the information of the gap sensor 2 or the gap sensor 3 for the same reason.
_{z O = z H2 + {d} 2 -z T20 - (x O -x H2) G 13 - (y O -y H2) G 23} / G 33 ... (33)
Alternatively _{z O = z H3 + {d} 3 -z T30 - (x O -x H3) G 13 - (y O -y H3) G 23} / G 33 ... (34)
The measurement accuracy can be improved by using the average value of the three kinds of data obtained by using the equation (32), the equation (33), or the equation (34) as the z absolute coordinate of the origin of the displacement coordinate after the displacement. It is also possible to determine a detection error using these three types of data.

Thus, the three gap sensors are such that all heads are fixed to the stator and symmetrical with respect to the plane yoz or the plane xoz, and all target planes are fixed to the movable element and parallel to the xy plane. The two two-dimensional sensors have all the heads fixed to the stator, all the targets fixed to the moving element, and the Z movement coordinates of the initial measurement points on all the target sides are the same. When installed in a certain manner, the origin position and orientation of the moving coordinate system fixed to the moving element can be obtained with high accuracy based on the information on the installation position of each sensor and the signal detected by each sensor.
Further, the position of this point can be calculated after the mover is accurately displaced using the moving coordinates of an arbitrary point on the mover and the origin position and orientation of the moving coordinate system after the mover is displaced.
As a result, the control of the displacement amount of the moving element as in the case of Patent Document 1 can be controlled by a more accurate calculation with respect to the method of comparing with the collected data before the operation of the apparatus by the approximate calculation. Even when the rotational momentum increases in the manufacture of large-sized and long-sized liquid crystal devices and semiconductor device substrates, etc., it is possible to prevent product deterioration and improve yield by responding with high-accuracy position control.

Next, a second embodiment of the present invention will be described with reference to the drawings.
FIG. 2 is a diagram illustrating a configuration of a 6-DOF moving body position / posture measuring apparatus according to the second embodiment of the present invention.
The six-degree-of-freedom moving body position / posture measuring apparatus of the present embodiment shown in FIG. 2 differs from the six-degree-of-freedom moving body position / posture measuring apparatus of the first embodiment shown in FIG. It is a part provided with three displacement sensors instead of the sensor. Other configurations are the same.
In FIG. 2, reference numerals 7, 8 and 9 denote displacement sensors (optical, electrostatic, magnetic, etc.), which detect the amount of displacement in one direction at a target measurement point fixed to the moving element. Further, the measurement direction of the displacement sensor 7 and the displacement sensor 8 is the y direction, the measurement direction of the displacement sensor 9 is the x direction, and passes through the target measurement point D of the displacement sensor 7 and the target measurement point E of the displacement sensor 8. These three sensors are installed so that the straight line is parallel to the x-axis. The movement coordinates of the target measurement points of the displacement sensor 7, the displacement sensor 8, and the displacement sensor 9 are respectively expressed as D (X _D , Y _D , Z _D ), E (X _E , Y _E , Z _E ), and F (X _F , Y _F , Z _F )
Y _D = Y _E , Z _D = Z _E (35)
Holds.
Hereinafter, based on the information from each sensor and the initial position of each sensor, the origin position (x _O , y _O , z _O ) and posture (α, β, γ) of the moving coordinate system after the mover is displaced are obtained. .

First, as in the first embodiment, α and β are obtained based on the detection information and initial position information of the three gap sensors 1, 2, and 3.
Next, the obtained alpha, the value of beta, based on the detected information and the initial position information of the three displacement sensors 7, 8, 9 gamma, seek x _O and y _O.
Assuming that the displacement amounts detected by the displacement sensor 7 and the displacement sensor 8 are SD and SE, respectively, the y absolute coordinates of the point D and the point E after the displacement of the moving element are y _D = Y _D + _SD and y _E = Y _{E, respectively.} + S _E (36)
Is required. Also, according to equation (1):
_{y D = y O + (G} 21 X D + G 22 Y D + G 23 Z D) ... (37)
And _{y E = y O + (G} 21 X E + G 22 Y E + G 23 Z E) ... (38)
Holds. Subtracting equation (38) from equation (37) considering equation (35)
_{y D -y E = G 21 (} X D -X E) ... (39)
It becomes. G ₂₁ = sin αsin βcos γ−cos αsin γ is substituted into the above equation to obtain γ,
γ = A−arcsin {k (y _D −y _E ) / (X _D −X _E )} (40)
It becomes.
Where A = arcsin (ksinαsinβ),
k = 1 / √ (sin2αsin2β + cos2α).
When the displacement amount detected by the displacement sensor 9 is SF, the x absolute coordinate of the point F after the displacement of the moving element is x _F = X _F + S _F (41)
Is required.
According to the equation (1), x _O is given by the following equation.
_{x O = x F - (G} 11 X F + G 12 Y F + G 13 Z F) ... (42)
Further, according to the equation (37), y _O is given by the following equation.
_{y O = y D - (G} 21 X D + G 22 Y D + G 23 Z D) ... (43)
Finally, z _O is calculated in the same manner as in the first embodiment.
Thus, by using the three gap sensors and the three displacement sensors, it is possible to accurately measure the position / posture after the moving element is displaced.
Compared with the first embodiment, this embodiment requires one more installation place because one sensor is added. However, since the displacement sensor is less expensive than the two-dimensional sensor, the cost can be reduced.

Next, Embodiment 3 of the present invention will be described with reference to the drawings.
FIG. 3 is a diagram illustrating a configuration of a 6-DOF moving body position / posture measuring apparatus according to the third embodiment of the present invention.
The position / posture measuring apparatus of the six-degree-of-freedom moving body of the present embodiment shown in FIG. 3 is different from the position / posture measuring apparatus of the six-degree-of-freedom moving body of the second embodiment shown in FIG. The installation method is different. Other configurations are the same.
In FIG. 3, the measurement direction of the displacement sensor 7 is the y direction, the measurement direction of the displacement sensor 8 and the displacement sensor 9 is the x direction, and the target measurement point E of the displacement sensor 8 and the target measurement point F of the displacement sensor 9 are Three one-dimensional sensors are installed so that the straight line passing through is parallel to the y-axis.
The movement coordinates of the target measurement points of the displacement sensor 7, the displacement sensor 8, and the displacement sensor 9 are respectively expressed as D (X _D , Y _D , Z _D ), E (X _E , Y _E , Z _E ), and F (X _F , Y _F , Z _F )
X _E = X _F , Z _E = Z _F (44)
Holds.
Hereinafter, based on the information from each sensor and the initial position of each sensor, the origin position (x _O , y _O , z _O ) and posture (α, β, γ) of the moving coordinate system after the mover is displaced are obtained. .

First, as in the first embodiment, α and β are obtained based on the detection information and initial position information of the three gap sensors 1, 2, and 3.
Next, the obtained alpha, the value of beta, based on the detected information and the initial position information of the three displacement sensors 7, 8, 9 gamma, seek x _O and y _O.
Assuming that the displacement amounts detected by the displacement sensor 8 and the displacement sensor 9 are S _E and S _F , respectively, the x absolute coordinates of the point E and the point F after the displacement are x _E = X _E + S _E and x _F = X _F + S, respectively. _F (45)
Is required.
According to equation (1):
_{x E = x O + (G} 11 X E + G 12 Y E + G 13 Z E) ... (46)
And _{x F = x O + (G} 11 X F + G 12 Y F + G 13 Z F) ... (47)
It becomes.
Subtracting equation (47) from equation (46) considering equation (44)
_{x E -x F = G 12 (} Y E -Y F) ... (48)
It becomes.
Substituting G ₁₂ = cos βsin γ into the above equation, γ is given by the following equation.
γ = arcsin {(x _E −x _F ) / (Y _E −Y _F ) / cos β} (49)
From the equation (46), x _O can be obtained as the following equation.
_{x O = x E - (G} 11 X E + G 12 Y E + G 13 Z E) ... (50)
When the displacement detected by the displacement sensor 7 is SD, the y absolute coordinate of the point D after the displacement of the moving element is y _D = Y _D + S _D (51)
Is required.
From the equation (1), y _O can be obtained as the following equation.
_{y O = y D - (G} 11 X D + G 12 Y D + G 13 Z D) ... (52)
Finally, z _O is calculated in the same manner as in the first embodiment.

  Thus, by using the three gap sensors and the three displacement sensors, it is possible to easily and accurately measure the post-mover position / posture. Compared with the second embodiment, the present embodiment has the same number and types of sensors, but when the movement range in the x direction is larger, the second embodiment uses less space for measurement, and the movement range in the y direction. In the case of a large value, the space used for measurement is smaller in this embodiment.

Next, Embodiment 3 of the present invention will be described with reference to the drawings.
In this embodiment, the gap sensor installation method is changed in the first embodiment.
FIG. 4 is a diagram illustrating a configuration of a 6-DOF moving body position / posture measuring apparatus according to a fourth embodiment of the present invention.
In FIG. 4, the installation method of the moving coordinate system and the fixed coordinate system is the same as that of the first embodiment. The three gap sensors 1, 2, and 3 are configured such that all the heads are fixed to the moving element and all the targets are fixed to the fixing element.
The target planes are all parallel to the plane xoy, the points H ₁ and H ₂ are symmetric with respect to the plane YOZ, and the points H ₂ and H ₃ are symmetric with respect to the plane XOZ. The vectors H ₁ T ₁ , H ₂ T ₂ and H ₃ T ₃ all coincide with the negative direction of the z axis. Therefore, if the movement coordinates of the points H ₁ , H ₂ and H ₃ are (X _H1 , Y _H1 , Z _H1 ), (X _H2 , Y _H2 , Z _H2 ) and (X _H3 , Y _H3 , Z _H3 ), respectively. ,
X _H2 = X _H3 (53)
Y _H1 = Y _H2 (54)
And Z _H1 = Z _H2 = Z _H3 (55)
Holds.
Rotation coordinate conversion is performed in the order of the z-axis, y-axis, and x-axis in the fixed coordinate system, the rotation angle of the corresponding mover is (γ, β, α), the coordinate conversion matrix is G, and the mover is displaced Assuming that the origin position (x _O , y _O , z _O ) of the moving coordinate system is after the absolute coordinates (x, y, z) of the point whose moving coordinates are (X, Y, Z) are be able to.

Where H ₁₁ = cos βcos γ,
H ₁₂ = sin αsin βcosγ-cosαsinγ,
H ₁₃ = cosαsinβcosγ + sinαsinγ,
H ₂₁ = cos βsin γ,
H ₂₂ = sinαsinβsinγ + cosαcosγ,
H ₂₃ = cosαsinβsinγ-sinαcosγ,
H ₃₁ = −sin β,
H ₃₂ = sin α cos β,
H ₃₃ = cos α cos β.
Hereinafter, the origin position (x _O , y _O , z _O ) and posture (α, β, γ) of the moving coordinate system after the moving element is displaced based on the detection information from each sensor and the initial position of each sensor will be described. Ask.
First, α and β are obtained based on detection information and initial position information of the three gap sensors.
Equations of target planes of the gap sensor 1, the gap sensor 2, and the gap sensor 3 are respectively z = z _T1 , z = z _T2 , z = z _T3 (57)
Assuming that the gap signals detected by the gap sensor 1, the gap sensor 2 and the gap sensor 3 are d ₁ , d ₂ and d ₃ , respectively, the z absolute coordinates of the post-displacement points H ₁ , H ₂ and H ₃ are respectively z _H1. = Z _T1 + d ₁ , z _H2 = z _T2 + d ₂ , z _H3 = z _T3 + d ₃ (58)
It becomes.
On the other hand, according to equation (56)
z _H1 = z _O + X _H1 H ₃₁ + Y _H1 H ₃₂ + Z _H1 H ₃₃ (59)
_{z H2 = z O + X H2} H 31 + Y H2 H 32 + Z H2 H 33 ... (60)
And _{z H3 = z O + X H3} H 31 + Y H3 H 32 + Z H3 H 33 ... (61)
Holds.
Substituting the equation (60) from the equation (59) in consideration of the equations (53) to (55) and subtracting the equation (61) from the equation (60),
_{z H1 -z H2 = (X H1} -X H2) H 31 ... (62)
And z _H2 −z _H3 = (Y _H2 −Y _H3 ) H ₃₂ (63)
It becomes.
H31 = -sinβ and Substituting H ₃₂ = sinαcosβ the two equations above, the α and β is given as follows.
β = arcsin {(z _H2 −z _H1 ) / (X _H1 −X _H2 )} (64)
α = arcsin {(z _H2 −z _H3 ) / (Y _H2 −Y _H3 ) / cos β} (65)
If the target planes of the gap sensor 1, the gap sensor 2 and the gap sensor 3 are all in the same plane,
z _T1 = z _T2 = z _T3
Therefore, according to the equation (58),
β = arcsin {(d ₂ −d ₁ ) / (X _H1 −X _H2 )} (66)
α = arcsin {(d ₂ −d ₃ ) / (Y _H2 −Y _H3 ) / cos β} (67)
It becomes.

Since z _T1 , z _T2 , z _T3 , X _H1 , X _H2, and X _H3 are constant, if measured beforehand, the gap sensor 1, the gap sensor 2, and the formulas (58) and (64) to (67) are obtained. Α and β can be obtained based on the gap signals d ₁ , d ₂ and d ₃ detected by the gap sensor 3.
Next, γ, x _O and y _O are obtained based on the obtained α and β values, detection information of the two two-dimensional sensors, and initial position information.
The absolute coordinates of the points P and Q are (x _P , y _P , z _P ) and (x _Q , y _Q , z _Q ), respectively, and according to the equation (56),

and

It becomes.
Subtracting equation (69) from equation (68) taking into account Z _P = Z _Q ,

It becomes.
Further, x _PQ = x _P −x _Q , X _PQ = X _P −X _Q , y _PQ = y _P −y _Q
And Y _PQ = Y _P −Y _Q
H ₁₁ = cos βcos γ,
H ₁₂ = sin αsin βcosγ-cosαsinγ,
H ₂₁ = cosβsinγ,
Substituting H ₂₂ = sin αsin βsin γ + cos α cos γ into the above two equations and rearranging,

It becomes. Solving this equation gives

It becomes. Therefore, γ is given by the following equation.

In the above equation, x _PQ is the difference between x _P and x _Q, and y _PQ is the difference between y _P and y _Q.
Further, (x _P , y _P ) and (x _Q , y _Q ) are absolute coordinates after displacement of the points P and Q, respectively, and cannot be obtained directly from the sensor.
However, (X _P , Y _P ) and (X _Q , Y _Q ) are known, and the displacement of the point P measured by the two-dimensional sensor 5 is (S _Px , S _Py ), and the two-dimensional sensor 6 measures it. If the displacement amount of the point Q is (S _Qx , S _Qy ), the absolute coordinates after the displacement of the point P and the point Q are given as follows.
(X _P , y _P ) = (X _P , Y _P ) + (S _Px , S _Py ) (74)
(X _Q , y _Q ) = (X _Q , Y _Q ) + (S _Qx , S _Qy ) (75)
When the values of α, β, and γ are determined as described above, all the elements H _ij (i = 1, 2, 3, j = 1, 2, 3) of the rotational coordinate transformation matrix H are obtained from the equation (56). Is decided.
When the movement coordinates (X _P , Y _P , Z _P ) of the point P and the xy absolute coordinates (x _P , y _P ) of the point P after displacement are substituted into the equation (56) and transformed, the origin of the movement coordinate after displacement is calculated. The xy absolute coordinates are given as follows:
_{x O = x P - (H} 11 X P + H 12 Y P + H 13 Z P) ... (76)
_{y O = y P - (H} 21 X P + H 22 Y P + H 23 Z P) ... (77)
Finally, z _O can be obtained by the following equation using equation (59).
_{z O = z H1 - (X} H1 H 31 + Y H1 H 32 + Z H1 H 33) ... (78)

  Thus, compared with the first embodiment, since the head of the gap sensor is fixed on the moving element, the signal detection wiring hinders the movement of the moving element, but the calculation is simplified.

Next, a fifth embodiment of the present invention will be described with reference to the drawings.
In this embodiment, the gap sensor installation method is changed in the second embodiment.
FIG. 5 is a diagram illustrating the configuration of a 6-DOF moving body position / posture measuring apparatus according to a fifth embodiment of the present invention.
In FIG. 5, the installation method of the moving coordinate system and the fixed coordinate system and the installation method of the gap sensor are the same as those in the fourth embodiment.
On the other hand, the installation method of the three displacement sensors is the same as in the second embodiment. The coordinate conversion is performed in the same manner as in the fourth embodiment.
Hereinafter, based on the information from each sensor and the initial position of each sensor, the origin position (x _O , y _O , z _O ) and posture (α, β, γ) of the moving coordinate system after the mover is displaced are obtained. .
First, as in the fourth embodiment, α and β are obtained based on the detection information and initial position information of the three gap sensors 1, 2, and 3.
Next, γ, x _O and y _O are obtained based on the obtained α, β values, detection information of the three displacement sensors and initial position information.

Assuming that the displacement amounts detected by the displacement sensor 7 and the displacement sensor 8 are S _D and S _E , respectively, the y absolute coordinates of the point D and the point E after the displacement of the movable element are y _D = Y _D + _SD , y _E = Y _E + S _E (79)
Is required. Further, according to the equation (56),
_{y D = y O + (H} 21 X D + H 22 Y D + H 23 Z D) ... (80)
And _{y E = y O + (H} 21 X E + H 22 Y E + H 23 Z E) ... (81)
Holds. Subtracting equation (81) from equation (80) taking into account equation (35),
_{y D -y E = H 21 (} X D -X E) ... (82)
It becomes. Substituting H ₂₁ = cos βsin γ into the above equation to obtain γ,
γ = arcsin {(y _D −y _E ) / (X _D −X _E ) / cos β} (83)
It becomes.
Further, if the displacement amount detected by the displacement sensor 9 is SF, the x absolute coordinate of the point F after the displacement of the movable element is x _F = X _F + S _F (84)
Is required. According to the equation (56), x _O is given by the following equation.
_{x O = x F - (H} 11 X F + H 12 Y F + H 13 Z F) ... (85)
Further, according to the equation (80), y _O is given by the following equation.
_{y O = y D - (H} 21 X D + H 22 Y D + H 23 Z D) ... (86)
Finally, z _O is calculated in the same manner as in the fourth embodiment.

  Thus, as compared with the second embodiment, since the head of the gap sensor is fixed on the moving element, the signal detection wiring hinders the movement of the moving element, but the calculation is simplified. Further, compared to the fourth embodiment, since one sensor is added, it is necessary to take a large installation place. However, since the displacement sensor is less expensive than the two-dimensional sensor, the cost can be reduced.

Next, Embodiment 6 of the present invention will be described with reference to the drawings.
The sixth embodiment is different from the third embodiment in that the gap sensor installation method is changed.
FIG. 6 is a diagram illustrating a configuration of a position / posture measuring apparatus for a six-degree-of-freedom moving body according to a sixth embodiment of the present invention.
In FIG. 6, the installation method of the moving coordinate system and the fixed coordinate system and the installation method of the gap sensor are the same as those in the fourth embodiment.
On the other hand, the installation method of the three displacement sensors is the same as in the third embodiment. The coordinate conversion is performed in the same manner as in the fourth embodiment.
Hereinafter, based on the information from each sensor and the initial position of each sensor, the origin position (x _O , y _O , z _O ) and posture (α, β, γ) of the moving coordinate system after the mover is displaced are obtained. .
First, as in the fourth embodiment, α and β are obtained based on the detection information and initial position information of the three gap sensors 1, 2, and 3.
Next, γ, x _O and y _O are obtained based on the obtained α, β values, detection information of the three displacement sensors and initial position information.
Assuming that the displacement amounts detected by the displacement sensor 8 and the displacement sensor 9 are S _E and S _F , respectively, the x absolute coordinates of the point E and the point F after the displacement are x _E = X _E + S _E and x _F = X _F + S, respectively. _F ... (87)
Is required. According to equation (56):
_{x E = x O + (H} 11 X E + H 12 Y E + H 13 Z E) ... (88)
And _{x F = x O + (H} 11 X F + H 12 Y F + H 13 Z F) ... (89)
It becomes. Subtracting equation (89) from equation (88) taking into account equation (44),
_{x E -x F = H 12 (} Y E -Y F) ... (90)
It becomes.
H ₁₂ = sinαsinβcosγ-cosαsinγ
Substituting
γ = A−arcsin {k (y _E −y _F ) / (X _E −X _F )} (91)
It becomes.
Where A = arcsin (ksinαsinβ),
k = 1 / √ (sin2αsin2β + cos2α).

From Expression (88), x _O can be obtained as the following expression.
_{x O = x E - (H} 11 X E + H 12 Y E + H 13 Z E) ... (92)
If the displacement amount detected by the displacement sensor 7 is SD, the y absolute coordinate of the point D after the displacement of the moving element is y _D = Y _D + S _D (93)
Is required.
From the equation (56), y _O can be obtained as the following equation.
_{y O = y D - (H} 11 X D + H 12 Y D + H 13 Z D) ... (94)
Finally, z _O is calculated in the same manner as in the fourth embodiment.

As described above, by using the three gap sensors and the three displacement sensors, it is possible to easily and accurately measure the post-displacement position / posture.
Compared with the fifth embodiment, the present embodiment has exactly the same number and type of sensors, but when the moving range in the x direction is larger, the space used for the measurement in the fifth embodiment is smaller, and the moving range in the y direction. In the case of a large value, the space used for measurement is smaller in this embodiment.

Next, Embodiment 7 of the present invention will be described with reference to the drawings.
The seventh embodiment is an example in which one gap sensor is added to any one of the first embodiment, the second embodiment, the third embodiment, the fourth embodiment, the fifth embodiment, or the sixth embodiment.
FIG. 7 is a diagram showing a configuration of a 6-DOF moving body position / posture measuring apparatus according to Embodiment 7 of the present invention.
In FIG. 7, the gap sensor 1, the gap sensor 2, and the gap sensor 3 are installed in the same manner as any one of the first embodiment, the second embodiment, the third embodiment, the fourth embodiment, the fifth embodiment, or the sixth embodiment. 4 is installed so as to be symmetrical to the gap sensor 1, the gap sensor 2, and the gap sensor 3 with respect to the coordinate system.
In Example 1 or Example 2 or Example 3 or Example 4 or Example 5 or Example 6, α and β were obtained based on the information of the three gap sensors. Here, α and β are obtained by exactly the same method as in the first embodiment based on arbitrary three pieces of information from the four gap sensors. Thus, four types of data regarding α and β can be obtained. If the four gap sensors and all the actuators are symmetrical with respect to the structure of the moving element, the mechanical vibration component due to the bending mode of the moving element can be canceled by averaging the four kinds of data.

  In this way, this example increases one sensor compared to Example 1 or Example 2 or Example 3 or Example 4 or Example 5 or Example 6, but averages four types of data. Can increase the measurement accuracy of α and β. It is also possible to determine a detection error using these four types of data.

Next, an eighth embodiment of the present invention will be described with reference to the drawings.
FIG. 8 is a diagram illustrating the configuration of a 6-DOF moving body position / posture measuring apparatus according to an eighth embodiment of the present invention.
In FIG. 8, reference numeral 21 denotes a mover which is driven by an actuator (not shown) and is displaced by 6 degrees of freedom. Reference numeral 10 denotes a three-dimensional sensor (first), which detects a displacement amount in one direction and two rotation angles. Reference numeral 11 denotes a three-dimensional sensor (second), which detects a displacement amount and two rotation angles in two orthogonal directions.
As in the first embodiment, an absolute coordinate system and a fixed coordinate system are set, and coordinate conversion is performed. Hereinafter, the installation method of the two three-dimensional sensors 10 and 11 and the position / posture measurement method of the six-degree-of-freedom moving body will be described in detail.
The three-dimensional sensor 10 is configured such that the target is fixed to the slider so that the target plane is parallel to the plane XOY, and the intersection of one fixed straight line (referred to as a measurement line here) always parallel to the z axis and the target plane. The displacement amount S _Mz of M in the z direction, the rotational angle α of the movable element around the x axis, and the rotational angle β of the movable element around the y axis are detected.
Further, the three-dimensional sensor 11 has a target fixed to the moving element, and detects an x-direction displacement amount S _Nx and a y-direction displacement amount S _{Ny of} the target measurement point N and a rotation angle γ of the moving element around the z axis.
As described above, the posture (α, β, γ) information is obtained directly from the two three-dimensional sensors 10 and 11.
Hereinafter, the origin position of the moving coordinate system after the movable element is displaced _{(x O, y O, z} O) Request.

First, the x _O and y _O.
From the displacement amount (S _Nx , S _Ny ) of the point N in the x direction and y direction and the initial absolute coordinate (X _N , Y _N ) of the point N in the x direction and y direction, the absolute coordinate of the point N after displacement is Given as follows:
(X _N , y _N ) = (X _N , Y _N ) + (S _Nx , S _Ny ) (95)
When the values of α, β, and γ are detected, all the elements G _ij (i = 1, 2, 3, j = 1, 2, 3) of the rotational coordinate transformation matrix G are obtained from the equation (1). Since it is determined and the moving coordinates (X _N , Y _N , Z _N ) of the point N are known, the xy absolute coordinates of the origin O of the moving coordinates after displacement are given by the following equation (1).
_{x O = x N - (G} 11 X N + G 12 Y N + G 13 Z N) ... (96)
_{y O = y N - (G} 21 X N + G 22 Y N + G 23 Z N) ... (97)
Next, z _O is obtained.
If the initial absolute coordinates of the measurement point M of the three-dimensional sensor 10 are (x _M0 , y _M0 , z _M0 ), the equation of the target plane of the three-dimensional sensor 10 before displacement is z = z _M0 (98)
It becomes.
The equation of the measurement line ML of the three-dimensional sensor 10 passing through the point M is x = x _M0 , y = y _M0 (99)
It becomes.
Further, the equation after the displacement of the target plane of the three-dimensional sensor 10 is given by the following equation in the same manner as the equation after the displacement of the target plane of the gap sensor 1 in the first embodiment is obtained.
_{xG 13 + yG 23 + zG 33} = d O + z M0 ... (100)
It becomes. However,
d _O = x _O G ₁₃ + y _O G ₂₃ + z _O G ₃₃ (101)
It is. Therefore, the z absolute coordinate of the point M after the displacement of the slider is
_{z M = (d O + z} M0 -x M0 G 13 -y M0 G 23) / G 33 ... (102)
It becomes.
Therefore, the displacement amount in the z direction of the point M detected by the three-dimensional sensor 10 is S _Mz ≡z _M −z _{M0
= (D O + z M0 -x} M0 G 13 -y M0 G 23) / G 33 -z M0 ... (103)
It becomes.
When determining the z _O by substituting equation (101) into equation (103),
_{z O = S Mz + z M0} - {z M0 + (x 0 -x M0) G 13 + (y 0 -y M0) G 23} / G 33 ... (104)
It becomes.

  As described above, the position / posture after the moving element is accurately displaced can be measured based on the detection information and the initial position information of the two three-dimensional sensors.

Next, a ninth embodiment of the present invention will be described with reference to the drawings.
Example 9 is an example in the case where Expression (56) is used instead of Expression (1) for coordinate conversion in the configuration of FIG. The posture (α, β, γ) and the origin position (x _O , y _O ) of the moving coordinate system after the mover is displaced are obtained in the same manner as in the eighth embodiment. Z _{O at} this time is obtained as follows.
Assume that the absolute coordinates of the measurement point M of the three-dimensional sensor 10 after the displacement of the slider is (x _M , y _M , z _M ), and the movement coordinates of the measurement point M are (X _M , Y _M , Z _M ).
Since x _M = x _M0 and y _M = y _M0 , the following equation is established from equation (56).
However, Z _M is known.

  From the two lines above equation (105)

It becomes. Solving this

It becomes. Z _O is obtained from the obtained X _M and Y _{M.
z O = S Mz + z M0} -H 31 X M -H 32 Y M -H 33 Z M ... (108)

Next, Embodiment 10 of the present invention will be described with reference to the drawings.
Example 10 is an example in which, in the configuration of FIG. 3 similar to Example 3, not Expression (1) but Expression (56) is used for coordinate conversion.
The configuration of the position / orientation measuring apparatus for a 6-degree-of-freedom moving body, the installation method of the moving coordinate system and the fixed coordinate system, the installation methods of the gap sensors 1 to 3 and the three displacement sensors 7 to 9 are the same as in the third embodiment. is there.
Hereinafter, based on the information from each sensor and the initial position of each sensor, the origin position (x _O , y _O , z _O ) and posture (α, β, γ) of the moving coordinate system after the mover is displaced are obtained. .
First, α, β, γ, x _O and y _O are obtained by the same method as in the sixth embodiment.
Z _{O at} this time is obtained as follows.
The absolute coordinates of the head position of the gap sensor 1 are (x _H1 , y _H1 , z _H1 ), the absolute coordinates of the measurement point T1 of the gap sensor 1 after the moving element is displaced (x _T1 , y _T1 , z _T1 ), The moving coordinates of the measurement point T ₁ are assumed to be (X _T1 , Y _T1 , Z _T1 ).
In the configuration of FIG. 3, the following equation is established.
x _T1 = x _H1 (109)
y _T1 = y _H1 (110)
z _T1 = z _H1 + d ₁ (111)
However, x _H1 , y _H1 and Z _T1 are known coordinates that do not move.
From the equation (56), the following equation is established.

From the two lines above formula (112)

It becomes. Solving this,

It becomes.
Using the obtained X _T1 and Y _T1 , z _O is obtained by the following equation.
z _O = z _H1 + d ₁ -H ₃₁ XT ₁ -H ₃₂ Y _T1 -H ₃₃ Z _T1 (115)

  Since the technology of the present invention can accurately measure the position / posture of a movable element displaced by six degrees of freedom based on the detection information and initial position information of a plurality of sensors, the position / posture control can be performed with a high degree of freedom and high accuracy. It can also be applied to the use of measuring a feedback signal of a control device that performs the above.

It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6-DOF moving body in Example 1 of this invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6-DOF moving body in Example 2 of this invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6-DOF moving body in Example 3 of this invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6 degrees-of-freedom moving body in Example 4 of the present invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6-DOF moving body in Example 5 of this invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6-DOF moving body in Example 6 of this invention. It is a figure which shows arrangement | positioning of the gap sensor of the position and attitude | position measuring apparatus of a 6-DOF moving body in Example 7 of this invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6 degrees-of-freedom moving body in Example 8 of the present invention. It is a figure which shows the structure of the position and attitude | position measuring apparatus of a 6 degree-of-freedom moving body of a prior art.

Explanation of symbols

1, 2, 3, 4 Gap sensor 5, 6 Two-dimensional sensor 7, 8, 9 Displacement sensor 10, 11 Three-dimensional sensor 21 Mover 22 Stator

Claims (8)

In a position / posture measurement method for a 6-degree-of-freedom moving body comprising a stator, a mover that is displaced by 6 degrees of freedom relative to the stator, and a plurality of sensors
A plurality of sensors are installed on the stator and the moving element based on a moving coordinate system and a fixed coordinate system, and the moving element is displaced based on detection information of the plurality of sensors and given initial position information. When calculating the position and orientation of the mover relative to the stator to obtain the position of an arbitrary point on the mover, the fixed coordinate system xyzo is fixed to the stator, and the move coordinate system XYZO is set to the mover. Fixed, set the moving coordinate system and the fixed coordinate system to overlap the moving coordinate system and the fixed coordinate system before the mover is displaced,
The rotation angles of the mover around the x-axis, y-axis, and z-axis are α, β, and γ, respectively, and the absolute coordinates of the origin of the moving coordinate system are (x _o , _yo , z _o ), and the mover The absolute coordinate of the arbitrary fixed point above is transformed so that it can be expressed by the moving coordinate of this arbitrary fixed point and the absolute coordinate (x _o , _yo , z _o ) of the origin of the moving coordinate system and the rotation angles α, β and γ. A 6-DOF moving body position / posture measurement method, characterized in that processing is performed in accordance with a procedure of calculating by   The plurality of sensors includes a first gap sensor, a second gap sensor, and a third gap sensor that measure the distance between the head of the gap sensor and a target plane of the gap sensor. The x-axis, y-axis, and z-axis are used in this order. With respect to the first gap sensor, the second gap sensor, and the third gap sensor, all the heads are fixed to the stator so that the plane yoz Or the first gap sensor, the second gap sensor 2 and the third gap so that they are symmetrical with respect to the plane xoz and all target planes are fixed to the slider and parallel to the plane XOY. The position / posture measurement method for a six-degree-of-freedom moving body according to claim 1, wherein a sensor 3 is installed.   The plurality of sensors include a first two-dimensional sensor and a second two-dimensional sensor that detect a displacement amount in two orthogonal directions, and with respect to the first two-dimensional sensor and the second two-dimensional sensor, All the heads are fixed to the stator, all the targets are fixed to the moving element, the Z movement coordinates of the initial measurement points on all the target sides are the same, and the two measurements of the first two-dimensional sensor The first two-dimensional sensor and the second two-dimensional sensor are installed such that the direction and the two measurement directions of the second two-dimensional sensor are both parallel to the x-axis or the y-axis. The position / attitude measurement method for a six-degree-of-freedom moving body according to claim 1.   The plurality of sensors include a first one-dimensional sensor, a second one-dimensional sensor, and a third one-dimensional sensor that detect a displacement amount in a linear direction, and the first one-dimensional sensor and the second primary sensor For the original sensor and the third one-dimensional sensor, the measurement directions of the first one-dimensional sensor and the second one-dimensional sensor are both y-direction, and the measurement direction of the third one-dimensional sensor is x And the first straight line passing through the target-side initial measurement point of the first one-dimensional sensor and the target-side initial measurement point of the second one-dimensional sensor is parallel to the x-axis. The six-degree-of-freedom moving body position / posture measurement method according to claim 1, wherein a one-dimensional sensor, the second one-dimensional sensor, and the third one-dimensional sensor are installed.   The plurality of sensors include a first one-dimensional sensor, a second one-dimensional sensor, and a third one-dimensional sensor that detect a displacement amount in a linear direction, and the first one-dimensional sensor and the second primary sensor With respect to the original sensor and the third one-dimensional sensor, the measurement direction of the first one-dimensional sensor is the y direction, the measurement direction of the second one-dimensional sensor and the measurement direction of the third one-dimensional sensor Are both in the x direction, and the straight line passing through the target-side initial measurement point of the second one-dimensional sensor and the target-side initial measurement point of the third one-dimensional sensor is parallel to the y-axis. The 6-degree-of-freedom moving body position / posture measurement method according to claim 1, wherein the first one-dimensional sensor, the second one-dimensional sensor, and the third one-dimensional sensor are installed.   The plurality of sensors includes a first gap sensor, a second gap sensor, and a third gap sensor 3 that measure a distance between a head of the gap sensor and a target plane of the gap sensor, and the first gap sensor, With respect to the second gap sensor and the third gap sensor, all heads are fixed to the moving element and symmetrical with respect to the plane YOZ or the plane XOZ, and all target planes are fixed to the stator. The position of the six-degree-of-freedom movable body according to claim 1, wherein the first gap sensor, the second gap sensor, and the third gap sensor are installed so as to be parallel to the plane xoy. Posture measurement method. In the 6-DOF moving body position / posture measuring device according to claim 2 or 6, wherein a fourth gap sensor is added to the 6-DOF moving body position / posture measuring apparatus.
The fourth gap sensor is installed such that the first gap sensor, the second gap sensor, the third gap sensor, and the fourth gap sensor are symmetrical with respect to a coordinate system, and The movable element is displaced based on detection information of any three gap sensors and given initial position information from one gap sensor, the second gap sensor, the third gap sensor, and the fourth gap sensor. A position / posture measurement method for a six-degree-of-freedom moving body, characterized in that the position and posture of the movable body relative to the stator after calculation are calculated and an average value of the four sets of data thus obtained is taken. A moving element that is displaced by six degrees of freedom, a first three-dimensional sensor that detects a displacement amount and two rotation angles in one direction, and a second cubic that detects a displacement amount and one rotation angle in two orthogonal directions In the 6-DOF moving body position / posture measurement method equipped with the original sensor,
The fixed coordinate system xyzo is fixed to the ground, the moving coordinate system XYZO is fixed to the moving element, and the moving coordinate system and the fixed coordinate system are set so that the moving coordinate system and the fixed coordinate system are overlapped before the moving element is displaced. , X, y, and z, the rotation angles of the mover are α, β, and γ, respectively, and the absolute coordinates of the origin of the moving coordinate system are (x _o , _yo , z _o ), and the movement Coordinate transformation so that the absolute coordinates of the arbitrary fixed point on the child are expressed by the moving coordinates of this arbitrary fixed point, the absolute coordinates of the origin of the moving coordinate system (x _o , _yo , z _o ), and the rotation angles α, β, and γ The moving distance of the intersection of the fixed line in which the first three-dimensional sensor is parallel to the z-axis and the target plane of the first three-dimensional sensor fixed to the mover, and the x and y axes around the axis Rotation angles α and β of the slider are detected, and the second three-dimensional sensor The first three-dimensional sensor and the second three-dimensional so as to detect the amount of displacement in the x direction and the amount of displacement in the y direction at the fixed target measurement point and the rotation angle γ of the movable element around the z axis. A sensor is installed, and the movable element is displaced with respect to the stator after the movable element is displaced based on detection information of the first three-dimensional sensor and the second three-dimensional sensor and given initial position information. A 6-DOF moving body position / posture measuring method, wherein processing is performed by a procedure of calculating a position and a posture.

Images