JP2008281418A - Method for estimating position and attitude - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To improve detection precision of the position and the attitude of a model. <P>SOLUTION: In order to estimate the position of the model defined in X, Y and Z coordinate values of the model, levitated in the air by magnetic force and the attitude of the model defined in a roll angle around an X axis, a pitch angle around a Y axis and a yaw angle around a Z axis, a method for estimating the position and attitude includes a first observation device for observing a first mark for detecting the position and attitude described on the model from a Z-axis direction; and a second observation device for observing a second mark for detecting the position and attitude described on the model from a Y-axis direction. The estimating method includes a first process for estimating the deformation amount, the pitch angle and the yaw angle of the X, Y and Z-coordinate values of the model, by using detection data of the first mark and the second mark acquired by the first and second observation devices; and a second process for estimating the roll angle of the model, by using the deformation amount, the pitch angle and the yaw angle of the X, Y and Z coordinate values of the model estimated in the first process and a roll angle correction equation that have been registered previously. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a position and orientation estimation method for estimating the position and orientation of a model supported by magnetic force in the air.

As a device for supporting a model in an air current in a cavity, a magnetic suspension and balance system is known (see, for example, Patent Document 1).
This magnetic force support balance device is a device for stationary a model in the air by a magnetic force. Since the magnetic control parameters are determined based on the current position and orientation of the model, it is important to estimate the position and orientation of the model as accurately as possible in order to provide more stable magnetic support. .

As a model position / posture detection method, for example, there is one disclosed in Non-Patent Document 1. Non-Patent Document 1 is provided with an upper camera for observing a model arranged in an XYZ orthogonal coordinate system from the Z-axis direction (above) and a horizontal camera for observing from the Y-axis direction, and the model's knitting angle from the upper camera. By measuring the position in the Y-axis direction, the position in the X-axis direction, and the position in the Z-axis direction and the pitch angle (angle around the Y-axis) with a horizontal camera, Estimating position and orientation is disclosed.
JP 2003-344215 A Hideo Sawada and two others, "Preliminary trial results of a compact magnetically supported balance for high subsonic speeds", 29-31 October 2001, Proc. 39th Aircraft Symposium

  However, in the position and orientation estimation method disclosed in Non-Patent Document 1 as described above, the angle around the X axis (roll angle) is not measured, and the position and orientation of the model is estimated with high accuracy. There was a problem that I could not.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to provide a position / orientation estimation method capable of improving the detection accuracy of the position and orientation of a model.

In order to solve the above problems, the present invention employs the following means.
The present invention defines an XYZ orthogonal coordinate system as a reference for position detection for a model levitated in the air by magnetic force, and the position of the model defined by X, Y, and Z coordinate values, and the X axis A position / orientation estimation method for estimating an attitude of the model defined by a surrounding roll angle, a pitch angle around a Y-axis, and a yaw angle around a Z-axis. A first observation device for observing the mark from the Z-axis direction, and a second observation device for observing the second mark for position and orientation detection written on the model from the Y-axis direction, Displacement, pitch angle, and yaw angle of the X, Y, and Z coordinate values of the model using detection data of the first mark and the second mark acquired by the observation device and the second observation device A first step of estimating The roll angle of the model is estimated based on the displacement amount, the pitch angle, and the yaw angle of the X, Y, and Z coordinate values estimated in the process 1 and the roll angle correction formula registered in advance. A position and orientation estimation method comprising: a second step of:

When estimating the roll angle of the model, for example, the second mark written along the axial direction of the model is detected by the second observation device, and the roll angle is estimated based on the detection position of the second mark. It is possible to do. However, the position of the second mark written along the axial direction of the model is not only when the roll angle changes, but for example, when the model moves horizontally in the Z-axis direction or horizontally in the Y-axis direction. Therefore, the roll angle cannot be accurately detected only by detecting the second mark. That is, in order to estimate the roll angle, it is necessary to grasp the position of the Y coordinate value or Z coordinate value of the model and reflect this position.
On the other hand, parameters other than the roll angle, that is, the X, Y, Z coordinate values, the pitch angle, and the yaw angle are not dependent on other parameters, and thus are acquired by the first observation device and the second observation device. It is possible to perform highly accurate position / posture estimation by processing the detected data independently.
In consideration of such matters, in the present invention, five parameters including the displacement amount, pitch angle, and yaw angle of the X, Y, and Z coordinate values are used as a pre-stage for estimating the roll angle depending on other parameters. The roll angle was estimated based on the five parameters estimated by using the detection data of the first observation device and the second observation device. Thereby, the estimated roll angle becomes a value in consideration of a parameter that affects the roll angle. As a result, it is possible not only to estimate the roll angle but also to improve the estimation accuracy.

  In the position / orientation estimation method, the roll angle correction formula is a first relational expression representing a relationship between a position and orientation before and after displacement when the position and orientation of the model in the reference position and orientation are displaced in the XYZ orthogonal coordinate system. And the second relational expression relating to the resolution of the second mark of the detection data acquired by the second observation device when the model is horizontally moved in the Y-axis direction. It is good.

  In the position and orientation estimation method, the roll angle correction formula is expressed by, for example, the following formula (8).

  In the position / orientation estimation method, a model is set on a stage capable of moving the model to a desired position by inputting set values of at least three parameters including X, Y, and Z coordinate values, and a roll While the angle is kept constant at the reference angle, the setting values of each parameter are independently changed at predetermined intervals, whereby the position of the model is individually displaced at predetermined intervals, and the position at each position after displacement is changed. The model is observed by the first observation device and the second observation device, and the roll angle of the model is determined from the detection data of the first observation device and the second observation device and the roll angle correction formula. Calculating the miscalculation between the calculated roll angle of the model and the reference angle, and when the error exceeds a preset allowable range, It may be corrected degree correction equation.

  Thus, since the roll angle correction formula is corrected, it is possible to reduce the error of each parameter used in the roll angle correction formula. Thereby, it is possible to further improve the estimation accuracy of the roll angle.

  In the position / orientation estimation method, the second mark is detected by the first observation device, a roll angle is estimated using the detection data and the roll angle correction formula, and the roll angle is calculated by using the second angle. When the roll angle is smaller than the roll angle estimated using the detection data obtained by the observation device, the roll angle estimated using the detection data of the first observation device may be employed.

  For example, if the roll angle is greater than or equal to a predetermined value, the detection accuracy of the second mark by the second observation device may be reduced. On the other hand, in a state where the roll angle is equal to or greater than a predetermined value, the detection accuracy of the first observation device may be higher than that of the second observation device. Accordingly, the second observation device detects the second mark by each of the two observation devices, estimates the roll angle using the detected data, compares the estimated roll angles, and adopts the preferred one. The estimation accuracy can be improved as compared with the case where the roll angle is estimated using only the detection data obtained by the above.

  The present invention defines an XYZ orthogonal coordinate system as a reference for position detection for a model levitated in the air by magnetic force, and the position of the model defined by X, Y, and Z coordinate values, and the X axis A position / orientation estimation apparatus for estimating an attitude of the model defined by a surrounding roll angle, a pitch angle around a Y-axis, and a yaw angle around a Z-axis. A first observation device for observing the mark from the Z-axis direction, a second observation device for observing the second mark for position and orientation detection written on the model from the Y-axis direction, and the first observation And a processing device to which detection data of the first mark and the second mark acquired by the second observation device is input, the processing device including the first mark and the second mark Before using the mark detection data A first process for estimating the displacement, pitch angle and yaw angle of the XYZ coordinate value of the model, and the displacement, pitch angle and yaw angle of the XYZ coordinate value of the model estimated in the first process Provided is a position and orientation estimation device that executes a second process for estimating a roll angle of the model using a registered roll angle correction formula.

  The position / orientation estimation apparatus is suitable for use in, for example, a magnetic support balance apparatus, and by adopting the position / orientation estimation apparatus, the accuracy of current control to the coil for magnetic support is improved. It becomes possible to support the model in a desired position and orientation.

  The present invention defines an XYZ orthogonal coordinate system as a reference for position detection for a model levitated in the air by magnetic force, and the position of the model defined by X, Y, and Z coordinate values, and the X axis A position / orientation estimation method for estimating an attitude of the model defined by a surrounding roll angle, a pitch angle around a Y-axis, and a yaw angle around a Z-axis. A first observation device for observing the mark from the Z-axis direction, a second observation device for observing the second mark for position and orientation detection written on the model from the Y-axis direction, and the model. A third observation device for observing the third mark for roll angle detection from the X-axis direction, and the first mark and the second mark acquired by the first and second observation devices. The position of the model is determined using the mark detection data. A first process for estimating the pitch angle and the yaw angle, and a second process for estimating the roll angle of the model using the detection data of the third mark acquired by the third observation device. A position / orientation estimation method is provided.

  As described above, since the third observation device for observing the third mark for roll angle detection from the X-axis direction is provided, the roll angle can be easily estimated by using the data detected by the third observation device. It becomes possible.

  According to the present invention, it is possible to improve the detection accuracy of the position / posture of the model.

  Hereinafter, an embodiment of a position and orientation estimation method, a position and orientation estimation apparatus, and a magnetic force support balance apparatus including the position and orientation estimation apparatus according to the present invention will be described with reference to the drawings.

[First Embodiment]
FIG. 1 is a perspective view of a magnetic force support balance device according to this embodiment.
As shown in FIG. 1, the magnetic force support balance device 1 includes a plurality of coils 2 arranged on the outer periphery of the wind tunnel measuring unit, a power supply device (not shown) for supplying current and the like to these coils 2, A control device 3 that controls the position and posture of the model A housed in the wind tunnel measuring unit by controlling the supplied current and the like, and a position and posture estimation device that detects the position and posture of the model A in the wind tunnel measuring unit ( (Not shown).
The model A that is a target for wind tunnel measurement is formed into a desired shape using a nonmagnetic material such as aluminum, for example. Inside the model A, a magnetized material, a coil such as a superconducting coil that keeps a current flowing, or a magnet such as a permanent magnet is accommodated.

  In such a magnetic force support balance apparatus 1, a magnetic field is generated around the model A by passing a current through the coil 2. The permanent magnet housed in the model A generates a magnetic force by interfering with the magnetic field, and the magnetic force acts on the model A, so that the model A is levitated and supported in the wind tunnel measuring unit. The current flowing through the coil is controlled by the control device 3 so that the position and posture of the model A can be freely changed.

Next, the position / orientation estimation apparatus will be described with reference to FIG.
First, an XYZ orthogonal coordinate system (reference coordinate) serving as a reference for detecting the position of the model A is defined. As shown in FIG. 1, in this embodiment, the X axis is defined in the axial direction of the model A in the reference position and orientation, the axis perpendicular to the X axis and directed in the vertical direction is defined as the Z axis, An axis perpendicular to both the Z axis is defined as the Y axis. Further, the center of the model A when the model A is in the reference position and orientation coincides with the origin O of the XYZ orthogonal coordinate axes. Also, the roll around the X axis is defined as the roll, the pitch around the Y axis is defined as the yaw, and the displacement around the reference position and orientation is defined as the roll angle (φ), the pitch angle (θ), and the yaw angle (ψ), respectively. Show.

FIG. 2 is a perspective view showing main components related to the position / orientation estimation apparatus 10. FIG. 3 is a diagram showing the positional relationship between the axis mark (second mark) marked on the model A and the second observation apparatus that detects the axis mark.
As shown in FIG. 2 and FIG. 3, the position / orientation estimation device 10 observes a circumferential marker (first mark) drawn along the circumferential direction of the model A from the Z-axis direction. A second observation device 12 that detects an axial marker (second mark) composed of a front marker and a rear marker drawn along the axial direction of the model A, and the first observation device 11 and the second observation device. And a processing device 13 to which detection data of peripheral markers and axis markers respectively acquired by the device 12 is input.

The first observation device 11 and the second observation device 13 are movably installed with respect to the wind tunnel measurement unit, but once the arrangement position is determined in order to detect the position of the model A, Thereafter, the position is fixed at this position, and the positional relationship with the model A in the reference position / posture is fixed.
The processing device 13 is configured by a computer, for example.

As shown in FIG. 3, the axis marker is composed of two line segments, and the line segment written on the front side in the model A is the front marker, and the line segment written on the rear side is the rear marker. A circumferential marker is written between the front marker and the rear marker.
The first observation device 11 and the second observation device 12 are each constituted by three line sensors arranged in an H shape. That is, the position / orientation estimation apparatus 10 includes a total of six line sensors. The line sensor is configured by an image sensor such as a CCD camera, for example.

  In FIG. 3, the H-shaped broken line drawn on the upper surface of the model A (the side on which the first observation device 11 is arranged) is a photographing line by the first observation device 11, and the side surface of the model A ( The H-shaped broken line drawn on the side on which the second observation apparatus is arranged is a photographing line by the second observation apparatus 12.

  In this embodiment, the model A is colored white so that each marker can be easily detected, for example. Each marker is drawn in a color that is easy to detect, for example, black. Further, as shown in FIG. 2, illumination 15 is provided above and to the side of the model A so that these markers can be easily detected.

FIG. 4 shows an example of detection data acquired by the second observation device 12. Since the model A is colored with a bright color and the front marker and the rear marker are colored with a dark color, the marker can be detected by detecting the edge of the luminance data.
Here, there are a total of two edges on the upper side and the lower side of the marker, and either one of the edges is adopted. In the present embodiment, the upper edge E is adopted. In the detection data in FIG. 4, the edge e is an edge where a luminance difference between the model and the surroundings is detected.

Next, a position / posture detection method performed by the position / posture estimation apparatus 10 configured as described above will be described.
Here, first, referring to FIG. 5, a process of creating each correction equation used to estimate the X, Y, Z coordinate values, the pitch angle, and the yaw angle, which are performed as a preparation stage for detecting the position and orientation, respectively. I will explain.

First, the model A is placed on the calibration 6-axis stage (step SA1). This 6-axis stage for calibration inputs the coordinate values of X, Y, and Z, and the set values of six parameters of roll angle φ, pitch angle θ, and yaw angle ψ, so that the model A is in a desired position and orientation. It is a stage that can be displaced to
Next, one parameter to be moved is determined from the five parameters consisting of X, Y, Z, θ, and ψ (step SA2). Subsequently, the determined parameter is displaced by a predetermined amount. As a result, the stage moves and rotates according to the input parameter values (step SA3). The model A at the moved position or posture is photographed by the first observation device 11 and the second observation device 12, and the detection data at this time is sent to the processing device 13. The processing device 13 detects the edge of the peripheral marker, the upper edge of the front marker, and the upper edge E of the rear marker from the detection data from the first observation device 11 and the second observation device 12, respectively. The position of the edge E and the set input value of the parameter when the detection data is acquired are associated with each other and recorded in the storage device (step SA4). The storage device may be built in the processing device 13 or may be a storage medium that is detachably connected via an external terminal.

  Subsequently, it is determined whether or not data within the specified range has been acquired (step SA5). For example, if the parameters are X, Y, and Z, whether or not the data when moving a predetermined distance to the plus side and the minus side with respect to the origin O can be acquired, and if θ and ψ, the origin can be obtained. It is determined whether or not a predetermined angle has been rotated around the center. As a result, if data over a predetermined range cannot be acquired ("NO" in step SA5), the process returns to step SA3 to continue data acquisition, and if data can be acquired, the process proceeds to step SA6 and data for five parameters is obtained. Determine whether the acquisition is complete. As a result, if data has not been acquired for all five parameters ("NO" in step SA6), the process returns to step SA2, and one of the parameters for which data acquisition has not been completed is set. Data acquisition is performed by executing step SA5 from SA3.

  On the other hand, if data has been acquired for all five parameters, a correction expression for each parameter is created based on the acquired data (step SA7). This correction equation corresponds to a conversion equation for estimating the position and orientation of the model A from the position of the edge E in each captured image captured by the first observation device 11 and the second observation device 12 described above. . Specifically, if X, the detection data by the first observation device 11 when X is displaced is acquired, the position of the edge of the peripheral marker in each captured image in this detection data, and the X displacement amount A correction formula is obtained from the relationship with (ΔX). Similarly, for Y and ψ, correction equations are obtained independently using the detection data acquired by the first observation apparatus 11. For Z and θ, correction equations are obtained independently using detection data acquired by the second observation device 12.

  When the respective correction equations are created for the five parameters consisting of ΔX, ΔY, ΔZ, θ, and ψ, the processing device 13 records these correction equations in the storage device and ends the preparation process. As described above, the correction formula is recorded in the storage device, so that in the actual position / posture estimation processing, ΔX from the detection data from the first observation device 11 and the second observation device 12 and the correction formula. , ΔY, ΔZ, θ, and ψ can be estimated.

On the other hand, as described above, since the value of the roll angle φ changes due to the influence of the above five parameters, it cannot be handled by the correction formula creation method according to the same procedure as the above five parameters. Therefore, the roll angle correction formula is obtained by the following method.
Here, first, the concept for deriving the roll angle correction formula will be described.

For example, as shown in FIG. 6, arbitrary points (X ₁ , Y ₁ , Z ₁ ) are defined on the upper edge of the rear marker in the model A of the reference position / posture. Subsequently, the model A in the reference position and orientation is translated by a predetermined amount ΔX, ΔY, ΔZ in the X axis direction, the Y axis direction, and the Z axis direction, and further, a predetermined angle around the X axis, the Y axis, and the Z axis The quantities θ, φ, and ψ are rotated. Here, the coordinates (X ₂ , Y ₂ , Z ₂ ) corresponding to the point (X ₁ , Y ₁ , Z ₁ ) in the model A after movement / rotation are defined. The relationship shown in the following equation (1) is established between the point (X ₁ , Y ₁ , Z ₁ ) before the movement and the point (X ₂ , Y ₂ , Z ₂ ) after the movement.
In the following equation (1), the roll angle φ is a parameter to be obtained now.

  Further, when the above formula (1) is expanded, the following formula (2) is obtained from the following formula (2).

Here, there are 13 parameters X ₁ , Y ₁ , Z ₁ , X ₂ , Y ₂ , Z ₂ , ΔX, ΔY, ΔZ, θ in the above three expressions (2) to (4). , Ψ, and φ are included, and there are four unknown parameters among them as shown in Table 1 below. That is, the parameters Y ₁ , Z ₁ , X ₂ , ΔX, ΔY, ΔZ, θ, ψ are known parameters, and the parameters X ₁ , Y ₂ , Z ₂ , φ are unknown parameters.
Hereinafter, this reason will be briefly described.

First, when the axis marker is detected by the second observation device 12 when the model A is in the reference position and orientation, the axis marker is written along the axis direction of the model A. Among the data X ₁ , Y ₁ and Z ₁ detected by the above, the values of Y ₁ and Z ₁ are constant. On the other hand, for X ₁ , the value of X ₁ varies depending on which position of the marks written along the axis is detected by the second observation device 12. I can't. Therefore, X ₁ is an unknown parameter, and Y ₁ and Z ₁ are known parameters.

Next, considering the model A after movement, among the data X ₂ , Y ₂ , Z ₂ detected by the second observation device 12, X ₂ is the second observation from the origin O in the X-axis direction. It can be the distance to the scan position of the device 12. On the other hand, for Y ₂ , measurement is impossible because the Y axis corresponds to the depth direction when viewed from the second observation device 12.

For Z ₂ , the distance is usually determined based on the number of pixels from the image center point to the upper edge E of the rear marker in the captured image acquired by the second observation device 12. Here, the resolution of the rear mark in the captured image changes between when the model A is near the second observation device 12 in the Y-axis direction and when it is at a distant position. That is, when the model A is horizontally moved in the Y-axis direction while the position of the model A in the Z-axis direction is fixed, the size of the rear marker in the captured image changes according to the position of the Y-axis. Become. Therefore, the number of pixels from the center of the captured image to the upper edge E of the rear marker changes according to the Y coordinate value, and the detected Z coordinate even though the position in the Z-axis direction has not changed. The value will show a different value. Thus, for Z ₂ is a parameter that depends on Y _2, Y ₂ as described above or is unknown parameter, Z ₂ also becomes unknown parameters.
The roll angle φ is an unknown parameter because it is desired to obtain this value.
Further, the displacement amounts ΔX, ΔY, ΔZ, θ, and ψ can be obtained by calculation using the correction equations corresponding to the above-described parameters and are known.

As described above, since four unknown parameters exist in the three relational expressions, the unknown parameters X ₁ , Y ₂ , Z ₂ , and φ cannot be solved as they are. Therefore, a fourth relational expression is prepared.

Now, as shown in FIG. 7, a reference plane B passing through the origin O of the XYZ orthogonal coordinate system and parallel to the XZ plane is assumed for the model A after movement (see the lower part of FIG. 6). Here, the length of the reference plane B in the Z direction is S _B , and the distance between the second observation device 12 and the reference plane B is D _S. The coordinates of the upper edge E of the rear marker detected by the second observation device 12 are naturally (X ₂ , Y ₂ , Z ₂ ). Here, when considering a triangle formed when the reference plane B is the base and both ends thereof are connected to the center of the second observation device by a straight line, the base of the similar triangle at the upper edge E of the rear marker is observed. Let it be surface C.
From this relationship, the following equation (5) is obtained using the similarity of triangles.

S _B : S _C = D _S : D _S −Y ₂ (5)
Here, S _B and S _C represent the lengths of the reference plane B and the observation plane C, respectively.
Further, when the length (number of pixels) on the image of the reference plane B is P _H , the observation plane C is also photographed with the number of pixels of P _H , and the resolution on the observation plane C is the above equation (5). Is expressed by the following equation (6).

From this, from the pixel position of the upper edge E of the rear marker in the captured image and the resolution on the observation plane C expressed by the above equation (6), the coordinate value Z _{2 in} the Z-axis direction of the upper edge E of the rear marker is The following equation (7) can be used.

  When this equation (7) is added to the above equations (2) to (4) and the roll angle φ is solved, the following equation (8) is derived.

Next, actual processing based on the above-described concept will be described.
FIG. 8 is a flowchart showing a procedure for creating a correction formula used in the roll angle estimation process. This step is performed, for example, after the calculation processing of the correction formula relating to parameters other than the roll angle shown in FIG. 5, that is, X, Y, Z, θ, and ψ. Note that the above equation (8) is assumed to be registered in the storage device of the processing device 13 in advance. The processing procedure shown below is for acquiring a known parameter used in equation (8).

First, the model A is set as a reference position and orientation, and the distance in the X-axis direction from the origin O of the model A to the scan position of the rear mark by the second observation device 12 is measured. Processor 13 records in the storage device the distance as a parameter X ₂ (step SB1).
Subsequently, the second observation apparatus 12 captures the rear marker of the model A in the reference position and orientation, and acquires detection data. The processing device 13 acquires the Y coordinate value and the Z coordinate value of the upper edge E of the rear marker from this detection data, and records these respective coordinate values as parameters Y ₁ and Z ₁ in the storage device (step SB2). .

Next, the distance in the Y-axis direction from the origin O of the model A to the installation position of the second observation device 12 is measured. Processor 13 records in the storage device the distance as a parameter D _S (step SB3). Subsequently, a reference plane B passing through the origin O and parallel to the XZ plane is set. At this time, the length in the Z-axis direction of the reference plane B is a S _B (step SB4).
Then, the reference plane B taken by the second observation device 12, calculates the number of pixels P _H of S _B from the detected data at this time from the angle of view of the second observation device 12 (step SB5).
Thereby, in the correction formula for the roll angle φ expressed as the above equation (8), all necessary known parameters, that is, X ₂ , Y ₁ , Z ₁ , D _S , P _H and S _B are acquired. Will be.

Next, a case where the position and orientation of the model A is actually estimated using the correction formulas for the parameters created by the processing shown in FIG. 5 and the roll angle correction formula described above will be described with reference to FIG.
First, when the model A is photographed by the first observation device 11 and the second observation device 12, the circumferential marker and the axis marker are detected, and this detection data is sent to the processing device 13. The processing device 13 detects the positions of predetermined edges E of the circumferential marker and the axis marker in the captured image from each detection data, and the displacement amounts ΔX, ΔY, ΔZ of the X, Y, Z coordinate values, the pitch angle, and the low angle. The data necessary for estimating is acquired (step SC1).
Next, the processing device 13 uses the data acquired in step SC1 and the correction formula corresponding to each parameter registered in the storage device in advance, and parameters other than the roll angle, that is, ΔX, ΔY, ΔZ, θ. , Ψ are estimated (step SC2).
Next, the five position / posture parameters ΔX, ΔY, ΔZ, θ, and ψ estimated in step SC2 are substituted into the roll angle correction equation shown by equation (8) to estimate the roll angle φ (step SC3).

  As described above, according to the position / orientation estimation method, the position / orientation estimation apparatus, and the magnetic force support balance apparatus according to the present embodiment, it is possible to detect the roll angle and further influence the detection of the roll angle. Since the roll angle φ is estimated using the parameters ΔX, ΔY, ΔZ, θ, and ψ that affect the roll angle, it is possible to improve the estimation accuracy of the roll angle.

Incidentally, in the first embodiment described above, and such operator or the like the distance X ₂ and the like from the origin of the model to a second observation device 12 is measured by using a tape measure or the like, required for the roll angle estimation Get the known parameters. For this reason, an error is included in the acquired parameters, and there is a possibility that the estimation accuracy of the roll angle is lowered.
Therefore, it is possible to further provide a correction process for reducing an error included in each known parameter used in the above-described equation (8).
Hereinafter, the correction process will be described with reference to FIGS. 10 and 11.

First, one of the X, Y, and Z parameters is selected (step SD1). Subsequently, detection data when the selected parameter is changed at a predetermined interval is extracted from the storage device (step SD2). This detection data is the detection data acquired by performing the process shown in FIG.
Next, one of a plurality of extracted detection data is selected (step SD3), and each parameter ΔX, ΔY, ΔZ, θ, ψ is estimated using this detection data and each correction equation (step SD4). The roll angle is estimated by substituting these estimated parameters ΔX, ΔY, ΔZ, θ, and ψ into the roll angle correction equation (8) (step SD5).

  Next, an error between the estimated roll angle and the true roll angle when the detection data is obtained is calculated (step SD6). The detection data is detection data obtained when the model A in the reference position and orientation is moved only in the X-axis direction, only in the Y-axis direction, or only in the Z-axis direction. Therefore, the true value of the roll angle here is zero.

Subsequently, it is determined whether or not the error is within a preset allowable range (step SD7 in FIG. 11). As a result, if the error exceeds the allowable range (“YES” in step SD7), it is determined which parameter is selected in step SD1, and if X is the selected parameter. fix the X ₂ (step SD8,9), when Y is a selected parameter to correct the Y _1, D _S (step SD10,11), when was the parameter Z is selected Z ₁ is corrected (steps SD12 and 13).

  Next, the process proceeds to step SD14, and other detection data is selected from the plurality of detection data extracted in step SD2, and each parameter ΔX, ΔY, ΔZ, θ is used in the same manner as described above using this detection data. , Ψ is estimated, and the estimated roll angle is substituted into the corrected roll angle correction formula to estimate the roll angle (step SD15).

  Subsequently, returning to step SD6, an error between the roll angle estimated in step SD15 and the true roll angle is calculated. In subsequent step SD7, it is determined whether or not the error is within an allowable range. As a result, if the error is not within the allowable range (“YES” in step SD7), the processing after step SD8 is performed, and the roll angle correction formula is continuously corrected.

  On the other hand, when it is determined in step SD7 that the error is within the allowable range (“NO” in step SD7), the process proceeds to step SD16, and in step SD1 described above, all of X, Y, Z are selected. It is determined whether or not a parameter has been selected. As a result, when all the parameters have not been selected, the process returns to the above-described step SD1, selects other parameters, and continues the processes after step SD2. On the other hand, if all parameters have been selected, the roll angle correction formula recorded in the storage device is updated to the corrected roll angle correction formula, and the correction processing is terminated.

  In this way, the roll angle correction formula is corrected by comparing the roll angle estimated using the roll angle correction formula with the actual roll angle, thus reducing the error of each parameter included in the roll angle correction formula. It is possible to improve the estimation accuracy of the roll angle.

[Second Embodiment]
Next, a second embodiment of the present invention will be described.
In the first embodiment described above, the second observation device for detecting the peripheral marker of the model A by the first observation device 11 for observing the model A from the Z-axis direction and observing the model A from the Y-axis direction. 12, the axis marker was detected, and the position and orientation were estimated using these detection data. Here, if the roll angle φ is displaced beyond 45 °, the rate of change of the detection position of the axis marker with respect to the displacement amount of the roll angle φ is reduced. That is, the angular resolution is deteriorated.
Therefore, in the present embodiment, the axis mark is detected also by the first observation apparatus that observes the model A from the Z-axis direction, and the model A is also used by using information on the axis mark detected by the first observation apparatus. Estimate the position and orientation of

  Specifically, the processing device 13 uses the roll angle estimation processing using the information on the axis marker detected by the second observation device 12 and the information on the axis marker detected by the first observation device 11. The estimated roll angle is processed in parallel, and the smaller of the two estimated roll angles φ is adopted.

  As described above, the first observation device 11 also detects the axis marker and estimates the roll angle using the detection data of the axis marker, so that the detection accuracy can be improved even when the displacement of the roll angle is large. It is possible to suppress the decrease and maintain the accuracy above a certain value.

[Third Embodiment]
Next, a third embodiment of the present invention will be described.
In the first embodiment described above, the roll angle φ of the model A is estimated based on detection data from the second observation device 12 that observes the model A from the Y-axis direction. In the present embodiment, a third observation device that observes the model A from the X-axis direction is provided instead of the second observation device 12, and the roll angle φ is determined based on the mark detection data by the third observation device. Make an estimate.

  Specifically, as shown in FIGS. 12 and 13, a roll angle marker for detecting the roll angle φ is written on the front surface or the rear surface of the model A. This roll angle marker is, for example, a line segment that intersects the axis of the model A and is orthogonal to the axis. Around the model A, a third observation device 15 for observing the model A from the X-axis direction is provided.

  As shown in FIG. 12, the third observation device 15 includes two line sensors in order to detect the roll angle mark at two locations P1 and P2. The two line sensors are configured to observe the model A along the Z-axis direction.

  When the roll angle is estimated, the roll angle mark is photographed by the two line sensors, and the detection data is sent to the processing device. The processing device specifies the detection positions P1 and P2 of the roll angle marker in the detection data acquired by each line sensor, and estimates the roll angle φ from the positional relationship between P1 and P2. When the detection position of the upper edge of the roll angle marker in each line sensor is P1 and P2, the roll angle φ is given by the following equation (9).

  In the above equation (9), d is the distance between the line sensors.

  Thus, by providing the third observation device 15 for observing the model A from the X-axis direction and detecting the roll angle φ directly using the detection data detected by the third observation device 15, The roll angle detection accuracy can be further improved. In addition, since the detection is performed directly, it is possible to reduce the processing load and the processing time.

  As mentioned above, although embodiment of this invention was explained in full detail with reference to drawings, the specific structure is not restricted to this embodiment, The design change etc. of the range which does not deviate from the summary of this invention are included.

1 is a perspective view of a magnetic force support balance device according to a first embodiment of the present invention. It is the perspective view which showed the main components regarding the position and orientation estimation apparatus which concerns on the 1st Embodiment of this invention. It is the figure which showed the positional relationship of the mark described in the model A, and the line sensor which detects this mark. An example of the detection data acquired by the 2nd observation apparatus is shown. It is the flowchart shown about the preparation process of the correction formula performed as a preparatory stage which detects a position and attitude | position. It is a figure for demonstrating the positional relationship of the model of a reference | standard position and posture, and the model after a movement. It is a figure used for description about the relational expression used in order to create a roll angle correction formula. It is the flowchart which showed the preparation procedure of the correction formula used for the estimation process of a roll angle. 6 is a flowchart showing a processing procedure in the case of actually detecting the position and orientation of a model using a correction formula or the like created in the process shown in FIG. It is the flowchart which showed the procedure of the correction process for correcting a roll angle correction type | formula. It is the flowchart which showed the procedure of the correction process for correcting a roll angle correction type | formula. It is the figure which showed the 3rd observation apparatus with which the position and orientation estimation apparatus which concerns on the 3rd Embodiment of this invention is provided, and the mark for roll angles. It is a figure used in order to demonstrate the estimation method of the detection data and roll angle which are detected by the 3rd observation apparatus.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Magnetic support balance apparatus 2 Coil 3 Control apparatus 10 Position and orientation estimation apparatus 11 1st observation apparatus 12 2nd observation apparatus 13 Processing apparatus 15 3rd observation apparatus

Claims (8)

XYZ Cartesian coordinate system is defined as a reference for position detection for a model levitated in the air by magnetic force, and the position of the model defined by X, Y and Z coordinate values, and the roll angle around the X axis A position and orientation estimation method for estimating the orientation of the model defined by a pitch angle around the Y axis and a yaw angle around the Z axis,
A first observation device for observing the first mark for position and orientation detection written on the model from the Z-axis direction, and a second mark for position and orientation detection written on the model from the Y-axis direction And X and Y of the model using detection data of the first mark and the second mark acquired by the first observation device and the second observation device. , A first process of estimating the displacement of the Z coordinate value and the pitch angle and yaw angle;
Based on the displacement amount, pitch angle and yaw angle of the X, Y, and Z coordinate values of the model estimated in the first process, and the roll angle correction formula registered in advance, the roll angle of the model A position and orientation estimation method comprising: a second step of estimating.   The roll angle correction formula includes a first relational expression representing a relation between a position and orientation before and after displacement when the position and orientation of the model at the reference position and orientation in the XYZ orthogonal coordinate system are displaced, and the model 2. The position and orientation according to claim 1, wherein the position and orientation is derived using a second relational expression relating to the resolution of the second mark of detection data acquired by the second observation device when horizontally moved in the Y-axis direction. Estimation method. The position and orientation estimation method according to claim 1 or 2, wherein the roll angle correction formula is expressed by the following formula (8).

By inputting the set values of at least three parameters consisting of X, Y, and Z coordinate values, the model is placed on a stage that can move the model to a desired position,
While maintaining the roll angle constant at the reference angle, by changing the setting values of each parameter independently at predetermined intervals, the positions of the models are individually displaced at predetermined intervals, respectively.
The model at each position after displacement is observed by the first observation device and the second observation device,
Calculate the roll angle of the model from the detection data of the first observation device and the second observation device, and the roll angle correction formula, respectively.
Calculate the miscalculation between the calculated roll angle of the model and the reference angle,
The position and orientation estimation method according to any one of claims 1 to 3, wherein the roll angle correction formula is corrected when the error exceeds a preset allowable range. The second mark is detected by the first observation device, and using this detection data and the roll angle correction formula, a roll angle is estimated,
The roll angle estimated using the detection data of the first observation device is adopted when the roll angle is smaller than the roll angle estimated using the detection data of the second observation device. The position and orientation estimation method according to any one of claims 1 to 4. XYZ Cartesian coordinate system is defined as a reference for position detection for a model levitated in the air by magnetic force, and the position of the model defined by X, Y and Z coordinate values, and the roll angle around the X axis A position and orientation estimation device for estimating the orientation of the model defined by a pitch angle around the Y axis and a yaw angle around the Z axis,
A first observation device for observing the first mark for position and orientation detection written on the model from the Z-axis direction;
A second observation device for observing the second mark for position and orientation detection written on the model from the Y-axis direction;
A processing device to which detection data of the first mark and the second mark acquired by the first observation device and the second observation device is input, and
The processor is
A first process for estimating a displacement amount, a pitch angle, and a yaw angle of an XYZ coordinate value of the model using detection data of the first mark and the second mark;
A second estimation of the roll angle of the model using the displacement amount, pitch angle and yaw angle of the XYZ coordinate value of the model estimated in the first process and a roll angle correction formula registered in advance. A position and orientation estimation device that executes processing.   A magnetic force support balance apparatus comprising the position / orientation estimation apparatus according to claim 6. XYZ Cartesian coordinate system is defined as a reference for position detection for a model levitated in the air by magnetic force, and the position of the model defined by X, Y and Z coordinate values, and the roll angle around the X axis A position and orientation estimation method for estimating the orientation of the model defined by a pitch angle around the Y axis and a yaw angle around the Z axis,
A first observation device for observing the first mark for position and orientation detection written on the model from the Z-axis direction, and a second mark for position and orientation detection written on the model from the Y-axis direction And a third observation device for observing the third mark for roll angle detection marked on the model from the X-axis direction, and the first and second observation devices A first step of estimating the position, pitch angle and yaw angle of the model using the acquired detection data of the first mark and the second mark;
A position and orientation estimation method comprising: a second step of estimating a roll angle of the model using detection data of the third mark acquired by the third observation device.

Images