JPH0611344A - Measuring method of position and attitude of moving body - Google Patents

Abstract

PURPOSE:To detect the position and attitude of a moving body with high precision and excellent efficiency by a method wherein the moving body provided with targets at three points serving as references of measurement is followed automatically by a distance-angle measuring apparatus and three-dimensional coordinate positions of the targets are determined, while the roll, yaw and pitch of the moving body are detected from the amounts of changes of coordinates. CONSTITUTION:In measurement of a position, rear reference points 2 of which the coordinates are known are used and the coordinates of an automatic tracking type distance and angle measuring apparatus 4 mounted on a following truck 3 of a shield excavator 1 are determined by measuring the rear reference points 2 by the apparatus 4. Based on the coordinates of the distance and angle measuring apparatus 4, subsequently, targets (reflective prisms) at three points provided for the excavator 1 are tracked automatically and measured and positions of the coordinates before and after movement are computed by a personal computer 6. Besides, amounts of the roll, yaw and pitch showing the attitude (tilt) of the excavator 1 are calculated from the amounts of changes of the coordinates before and after the movement.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method for measuring the position / orientation of a moving body. More specifically, the present invention relates to a new position / orientation surveying method capable of highly accurately and accurately surveying the position and posture of a moving body such as a shield machine.

[0002]

[Prior art and its problems] In recent years, along with technological advances in shield construction and propulsion construction, we have vigorously studied automatic control and automatic measurement means for advancing a shield machine along a predetermined planned line. Has been done. However,
These means for automation are still problematic and further improvements are needed for high efficiency and high precision shield excavation. Similarly, for construction machinery as a moving body, automated driving is expected from the viewpoint of reducing labor and improving safety, but it is difficult to realize it because the surveying system for automatic driving is not perfect. Has become.

For example, regarding the method of measuring the position / orientation of a shield machine, conventionally, pitching and rolling are detected by an inclinometer, and yawing is performed by a gyro. However, in the case of the attitude survey method of these conventional shield machines, errors due to restrictions such as temperature drift, linearity, and horizontal axis sensitivity (of the inclinometer) are unavoidable, affecting the survey accuracy. It was Therefore, there is a limit to improving the accuracy of the attitude survey.

In addition, it is also possible to grasp the position and direction of the shield plan center line and shield machine by manual work using a transit, a level, and steel tape based on a reference point installed in the mine. However, when this method is used, surveying is inefficient, labor saving and automation cannot be performed, and moreover, high-level experience and technology are required for surveying. Further, the smaller the cross section of the tunnel, the worse the accuracy becomes, and there is a drawback that there is a time lag in grasping the position and orientation of the shield machine.

Therefore, as a means for solving these drawbacks, achieving higher efficiency and saving labor, the shield machine has been developed.
A method in which bright points are installed and detected by a detection unit consisting of a TV camera and a light distance meter in the rear, and the position / orientation is calculated from the two-dimensional coordinate positions of the three points on the TV image and the distance measurement values. Is also proposed. However, in this method, the survey accuracy depends on the resolution of the television image, and the system is complicated. Also, since the device is expensive, the introduction effect may not be obtained depending on the site.

Therefore, a new surveying method is not limited to the shield machine, but is a common problem in the position / orientation surveying of a moving body, which is a system with higher efficiency, higher accuracy, and simple and low cost. The realization of was strongly desired. The present invention has been made in view of the above circumstances, and provides a new method capable of overcoming the drawbacks of the conventional methods and measuring the position / orientation of a moving body with high efficiency and high accuracy. Is intended.

[0007]

As a solution to the above-mentioned problems, the present invention provides an automatic tracking type distance-measuring finder.
The three-dimensional coordinate position of the target before the movement of the moving body on which the three targets serving as the surveying standards are set is obtained, and then the three-dimensional coordinate position of the target after the movement is obtained to detect the position of the moving body, and before and after the movement. The rolling, yawing and /
Alternatively, there is provided a method for measuring the position / orientation of a moving body, which is characterized by detecting a pitching inclination amount.

That is, the present invention is a method for automatically measuring the position, posture, etc. of a shield machine or the like as a moving body. An automatic tracking type rangefinder installed behind the body, that is, a target such as a reflecting prism is automatically searched and the distance and angle to this target are measured by a measuring device, and based on its own coordinates. The three-dimensional coordinates of this target can be obtained. Further, by using three automatic tracking type rangefinders and measuring three prisms at the same time, the position and orientation of the moving body can be measured in real time.

As a method of automatically obtaining the coordinates of the distance measuring and measuring instrument, triangulation or the like can be used. Such surveying is not limited to automatic surveying in shield work, but can be applied to surveying the position and orientation of various moving bodies, such as construction machinery. Hereinafter, the surveying method of the present invention will be described in more detail as an example.

[0010]

EXAMPLE As shown in FIG. 1, for example, the shield machine (1) is used as a moving body, and its position and attitude are obtained. First, in position surveying, two reflection prisms as rear reference points (2) whose coordinates are known are used, and they are attached to the trailing carriage (3) of the shield machine (1) or independent of this. The rear reference point (2) is measured by the automatic tracking type rangefinder (4) installed on the self-propelled cart. The coordinates of the range finder (4) are calculated from the measured values and the coordinates of the rear reference point (2) at this time.

Next, based on the coordinates of the distance measuring and angle measuring device (4), the three targets (5) installed on the shield machine (1) are measured. This makes the target (5)
Calculate the coordinates before moving. Next, after the shield machine (1) is moved, the same measurement is performed to calculate the coordinates of the moved target (5).

A personal computer (6) is connected to the automatic tracking type range finder (4) through a control unit so that calculation can be performed. Next, as the inclination of the shield machine (1), the measurement calculation of rolling, pitching and yawing will be explained. The deviation of the coordinate due to the rotation from the coordinate of the target (5) before the movement and the coordinate after the movement. From this, the inclination of the shield machine (1) is calculated. That is, the amount of rolling, yawing, and pitching is calculated from the amount of change between the coordinates before movement and the coordinates after movement.

The relationship for this calculation is shown in FIG. 2. The center of gravity of P ₁ P ₂ P ₃ constituted by the three points of the target (5) is G (x _G , y _G , z _G ) age,
The three points before movement are P ₁ (x ₁ , y ₁ , z ₁ ) P ₂ (x ₂ , y ₂ , z ₂ ) P ₃ (x ₃ , y ₃ , z ₃ ) P ′ ₁ P ′ after movement Let the center of gravity of ₂ P ′ _{3 be} G ′ (x ′ _G ,
y ′ _G , z ′ _G ), and the three points after movement are P ′ ₁ (x ′ ₁ , y ′ ₁ , z ′ ₁ ) P ′ ₂ (x ′ ₂ , y ′ ₂ , z ′ ₂ ) P Assuming ′ ₃ (x ′ ₃ , y ′ ₃ , z ′ ₃ ), the amount of change with movement can be considered as shown in FIG. 2 as follows.

-Translation amount (ΔxΔyΔz) -Pitching amount ω-Rolling amount ψ-Yawing amount κ Therefore, the following calculation will be performed. (A) Correction of parallel movement amount In order to calculate the inclination (pitching, rolling, yawing) amount, it is necessary to first correct the parallel movement amount. Regarding this parallel movement amount, since (ΔxΔyΔz) = G (x _G , y _G , z _G ) -G ′ (x ′ _G , y ′ _G , z ′ _G ), the point P ′ ₁ P ′ ₂ P When correcting the translation amount of _'3,

[0015]

[Equation 1]

[0016] Here, as shown in FIG.
Square P₁P₂P₃And triangle P ″₁P ″₂P ″ ₃Is the center of gravity
Are the same, and the triangle P ″₁P ″₂P ″₃Is a triangle P
₁P₂P₃Around the center of gravity G around the xyz axes
The shape is rotated by ω ψ κ.(B) Calculation of rotation amount If the rotation matrix around the center of gravity G is R,

[0017]

[Equation 2]

[0018] For example, if the rotation matrix when rotated in the order of κψω is Rκψω, this Rκψω is as follows.

[0019]

[Equation 3]

The following equations are derived from the above equations (1), (2) and (3).

[0021]

[Equation 4]

Further, the following equations are derived from the equations (4), (5) and (6).

[0023]

[Equation 5]

From the equations (7), (8) and (9), a ₁₁ a ₁₂ a
_Ask for ₁₃ . Here, the matrix when rotated in the order of κψω is

[0025]

[Equation 6]

Therefore, this equation (10) (11) (1
From 2), obtain ψ and κ. ω is (4) (5)
It is obtained by using the equation (6). (C) Correction of Rotational Amount ωψκ is obtained as described above, but in the case of the adopted formula, when one of the three xyz axes is rotated, the other two axes also rotate. It will be affected by the quantity. That is, it becomes as shown in FIG.

Since the pitching, rolling, and yawing amounts of the shield machine are the inclination amounts from the reference coordinate system, respectively, when Rκψω is used in the above formula, only κ (the yawing amount) is the inclination amount from the reference coordinate system. Yes, ψ
ω is the tilt amount of the coordinate system after κ is corrected. Therefore,
For ψ and ω, the amount of inclination from the reference coordinate system can be obtained by replacing the rotation matrix R with Rκψω, in the following case: R = Rψωκ or Rψκω, · ω = Rωψκ or Rωκψ Further, as a case of obtaining κ, R = Rκωψ or Rκψω may be used. In the above example, the center of rotation is the center of gravity of the triangle before the movement, but it is also possible to set and calculate the center of rotation that suits the purpose without sticking to this center of gravity.

By the above calculation, in the surveying method of the present invention, by detecting the coordinates of the target,
Measurement can be performed with much higher accuracy than the conventional method using an inclinometer. Of course, in the present invention, various configurations are possible for the equipment configuration for measurement and surveying. The present invention includes any of these aspects.

[0029]

According to the surveying method of the present invention, it is possible to perform highly accurate surveying, and it is possible to use both the target of position surveying and the target of posture surveying efficiently, so that the surveying of position and posture can be efficiently performed. Then, if the arrangement of three points is optimized, the calculation formula is simplified and the accuracy is improved.

[Brief description of drawings]

FIG. 1 is a cross-sectional view showing a surveying method in a shield machine as an embodiment of the present invention.

FIG. 2 is a coordinate relationship diagram.

FIG. 3 is a coordinate system diagram after the parallel movement amount is corrected.

FIG. 4 is a coordinate system diagram showing an influence of a rotation amount.

[Explanation of symbols]

 1 Shield excavator 2 Rear reference point 3 Subsequent bogie 4 Distance measuring and angle measuring instrument 5 3 point target 6 Personal computer

Claims (2)

[Claims] 1. A three-dimensional coordinate position of a target before movement of a moving body on which three targets serving as a surveying standard are installed is obtained by an automatic tracking type range finder. A method for measuring the position / orientation of a moving body, wherein the position of the moving body is detected and the position of the moving body is detected, and the tilt amount of the moving body is detected from the amount of coordinate change of three targets before and after the movement. 2. The surveying method according to claim 1, wherein the moving body is a shield machine or another construction machine.