Attitude self-compensation method to the transmitters of wMPS based on inclinometer

Abstract

The present invention discloses an attitude self-compensation method to the transmitters of wMPS based on inclinometer, including the following steps: step 1: arranging inclinometer-combined transmitters according to the mechanism structure of the transmitters; step 2: building a horizontal reference frame based on an automatic level and guide rail; step 3: calibrating rotation relationship between the inclinometer and transmitter coordinate systems by referring to the horizontal reference frame according to the measurement model of the inclinometer and rotation measurement model of the transmitter; step 4: updating the orientation parameters of the transmitters in real time according to the output values of the inclinometer and compensation algorithm for the orientation parameters. The method of the present invention aims at self-compensating the orientation parameters of transmitters in real time and increasing the stability of the system. By the attitude change of the inclinometer, this method can compensate the attitude change of transmitters in real time, improve the stability of the measurement system, and adapt to the harsh environment.

Claims

1. An attitude self-compensation method of transmitters of wMPS based on an inclinometer, comprising the following steps: step 1: arranging inclinometer-combined transmitters according to mechanism structures of the transmitters; step 2: building a horizontal reference frame based on an automatic level and a guide rail; step 3: calibrating a rotation relationship between an inclinometer and a transmitter coordinate system by referring to a horizontal reference frame according to a measurement model of the inclinometer and a rotation measurement model of the transmitter; step 4: updating orientation parameters of the transmitters in real time according to output values of the inclinometer and a compensation algorithm for the orientation parameters.

2. The attitude self-compensation method of a transmitter of wMPS based on an inclinometer according to claim 1, wherein the steps of “building a horizontal reference frame” in step 2 are as follows: step 2-1: arranging an automatic level, and adjusting it by an optical composite image level to parallel a horizontal plane; step 2-2: arranging a plurality of spherical receivers and corresponding spherical holders; step 2-3: arranging a guide rail which is fixed with the spherical holders, replacing the spherical receivers with spherically mounted retroreflectors (SMR) of a laser tracker on the corresponding spherical holders, adjusting the guide rail to ensure that a center of each SMR is in a same horizontal plane by using the automatic level, thus, finishing arrangements of target points on the horizontal plane; each SMR has a diameter of 1.5 inches; step 2-4: measuring the target points and other points by the laser tracker and establishing a horizontal reference frame O.sub.h-x.sub.hy.sub.hz.sub.h in accordance with the target points as xoy plane.

3. The attitude self-compensation method of transmitters of wMPS based on an inclinometer according to claim 2, wherein each of the spherical holders has a diameter of 1.5 inches.

4. The attitude self-compensation method of transmitter of wMPS based on an inclinometer according to claim 1, wherein the steps of “calibrating a rotation relationship between an inclinometer and transmitter coordinate system” in Step 3 are as follows: step 3-1: arranging transmitters in front of a horizontal reference frame according to a measurement range of the transmitters, and measuring total target points of the horizontal reference frame by the transmitters; step 3-2: defining a transmitter coordinate system O.sub.t-x.sub.ty.sub.tz.sub.t, calculating a rotation matrix from the transmitter coordinate system to the horizontal reference frame according to a measurement model of the transmitters, wherein a relationship between two coordinates is as follows: ${\left[\begin{array}{c}{a}_m\cos {\theta }_m+b_m\sin {\theta }_m\\ a_m\sin {\theta }_m-b_m\cos {\theta }_m\\ c_m\\ d_m\end{array}\right]}^T.Math.\left[\begin{array}{cc}{R}_T^H& T_T^H\\ 0& 1\end{array}\right].Math.\left[\begin{array}{c}{x}_h\\ y_h\\ z_h\\ 1\end{array}\right]=0$ where (a.sub.m, b.sub.m, c.sub.m, d.sub.m) m=1, 2 is an optical plane equation, m is a number of the optical plane equation, θ is a sweep angle of the transmitter, (x.sub.h, y.sub.h, z.sub.h) is coordinates of the horizontal reference frame; and R.sub.T.sup.H, T.sub.T.sup.H are a rotation matrix and a translation matrix from the transmitter coordinate system to the horizontal reference frame, respectively; step 3-3: obtaining the rotation matrix R.sub.T.sup.H from the transmitter coordinate system to the horizontal reference frame via a space-resection method; step 3-4: establishing an inclinometer measurement model, and defining an inclinometer coordinate system O.sub.i-x.sub.iy.sub.iz.sub.i; step 3-5: converting readings of the inclinometer to a rotation angle around a corresponding axis with respect to the inclinometer measurement model; $\{\begin{array}{c}{\theta }_i=a\\ {\gamma }_i=-\arcsin \left(\frac{\sin \beta }{\cos a}\right)\end{array}\hspace{1em}$ where α and β are the readings, θ.sub.i and γ.sub.i are rotation angles around x and y axes, respectively; and an equation of rotation matrix R.sub.H.sup.I from the horizontal reference frame to the inclinometer coordinate system is: $R_H^I=R_{\mathrm{yi}}.Math.R_{\mathrm{xi}}.Math.R_{\mathrm{zi}}=\left[\begin{array}{ccc}\cos {\gamma }_i& 0& \sin {\gamma }_i\\ 0& 1& 0\\ -\sin {\gamma }_i& 0& \cos {\gamma }_i\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\theta }_i& -\sin {\theta }_i\\ 0& \sin {\theta }_i& \cos {\theta }_i\end{array}\right]\left[\begin{array}{ccc}\cos {\psi }_i& -\sin {\psi }_i& 0\\ \sin {\psi }_i& \cos {\psi }_i& 0\\ 0& 0& 1\end{array}\right]$ where ψ.sub.i is an rotation angle around z-axis in the rotation matrix; step 3-6: defining a rotation matrix R.sub.T.sup.I from the transmitter coordinate system to the inclinometer coordinate system, and parameterizing the rotation matrix as follows: $R_H^T={\left(R_T^H\right)}^{-1}=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right]R_T^I=\left[\begin{array}{ccc}{r}_{i1}& r_{i2}& r_{i3}\\ r_{i4}& r_{i5}& r_{i6}\\ r_{i7}& r_{i8}& r_{i9}\end{array}\right]$ where r.sub.1 . . . r.sub.9 are known parameters, and ri.sub.1 . . . ri.sub.9 are parameters to be solved; step 3-7: obtaining an equation R.sub.H.sup.I=R.sub.T.sup.I.Math.R.sub.H.sup.T according to rigid transformation relations, and obtaining the following equations after mathematical processing: $\{\begin{array}{c}{r}_{i1}{r}_3+r_{i2}{r}_6+r_{i3}{r}_9=-\cos {\theta }_i\sin {\gamma }_i\\ r_{i4}{r}_3+r_{i5}{r}_6+r_{i6}{r}_9=\sin {\theta }_i\\ r_{i7}{r}_3+r_{i8}{r}_6+r_{i9}{r}_9=\cos {\theta }_i\cos {\gamma }_i\end{array}\hspace{1em}$ step 3-8: regulating at least three attitudes of the transmitters by mechanical adjustment, constructing a non-linear optimization objective equation combining orthogonal constraints of the rotation matrix and performing optimization solution via an iterative algorithm as $E=\underset{j=1}{\overset{n}{.Math.}}\left(F_{j1}^2+F_{j2}^2+F_{j3}^2\right)+M\underset{j=1}{\overset{6}{.Math.}}{f}_j^2$ $\mathrm{where}\{\begin{array}{c}{F}_{j1}=r_{i1}{r}_{j3}+r_{i2}{r}_{j6}+r_{i3}{r}_{j9}+\sin {\gamma }_i\cos {\theta }_{\mathrm{ji}}\\ F_{j2}=r_{i4}{r}_{j3}+r_{i5}{r}_{j6}+r_{i6}{r}_{j9}-\sin {\theta }_{\mathrm{ji}}\\ F_{j3}=r_{i7}{r}_{j3}+r_{i8}{r}_{j6}+r_{i9}{r}_{j9}-\cos {\theta }_{\mathrm{ji}}\cos {\gamma }_{\mathrm{ij}}\end{array}\{\begin{array}{c}{f}_1=r_{i1}^2+r_{i2}^2+r_{i3}^2-1=0\\ f_2=r_{i4}^2+r_{i5}^2+r_{i6}^2-1=0\\ f_3=r_{i7}^2+r_{i8}^2+r_{i9}^2-1=0\\ f_4=r_{i1i4}+r_{i2i5}+r_{i3i6}=0\\ f_5=r_{i1i7}+r_{i2i8}+r_{i3i9}=0\\ f_6=r_{i4i7}+r_{i5i8}+r_{i6i9}=0\end{array}\{\begin{array}{c}{f}_1=r_{i1}^2+r_{i2}^2+r_{i3}^2-1=0\\ f_2=r_{i4}^2+r_{i5}^2+r_{i6}^2-1=0\\ f_3=r_{i7}^2+r_{i8}^2+r_{i9}^2-1=0\\ f_4=r_{i1i4}+r_{i2i5}+r_{i3i6}=0\\ f_5=r_{i1i7}+r_{i2i8}+r_{i3i9}=0\\ f_6=r_{i4i7}+r_{i5i8}+r_{i6i9}=0\end{array},$ n is the number of attitudes of the transmitter by mechanical adjustment, n>=3.

5. The attitude self-compensation method of transmitters of wMPS based on an inclinometer according to claim 1, wherein the step of “updating orientation parameters of the transmitters in real time” of step 4 are as follows: step 4-1: obtaining readings of the inclinometer in real time, calculating a rotation matrix from a horizontal reference frame to an inclinometer coordinate system; step 4-2: calculating an updated attitude matrix of the transmitters according to a calibrated rotation matrix from the transmitter coordinate system to the inclinometer coordinate system, compensating a new attitude of the transmitters and obtaining an attitude compensation algorithm as follows:
(C.sub.T).sub.new=(R.sub.T.sup.I).sup.−1.Math.(R.sub.H.sup.I).sub.new.Math.((R.sub.H.sup.I).sub.old).sup.−1.Math.R.sub.T.sup.I.Math.(C.sub.T).sub.old Where (C.sub.T).sub.new, (C.sub.T).sub.old are a new transmitter coordinate system and an initial transmitter coordinate system, respectively; (R.sub.H.sup.I).sub.new, (R.sub.H.sup.I).sub.old are a new rotation matrix and an initial rotation matrix of the inclinometer, respectively.

6. The attitude self-compensation method of transmitters of wMPS based on an inclinometer according to claim 1, wherein a settling uncertainty accuracy of the automatic level is no more than 0.3″, a standard deviation over a 1 km double run is no more than ±1 mm.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a perspective cross-section diagram of an inclinometer-combined transmitter of the present invention;

(2) FIG. 2 is the establishment diagram of the horizontal reference frame of the present invention;

(3) FIG. 3 is the measurement model diagram of the inclinometer; and

(4) FIG. 4 is the measurement model diagram of the transmitter.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(5) The present invention will be further described accompanying with the figures.

(6) The present invention combines the high-accuracy two-axis inclinometer into the transmitter of the wMPS, and provides a method for self-compensating the orientation parameters of transmitters in real time, calibrating a transmitter coordinate system and inclinometer coordinate system by space resection measurement method, compensating attitude of the transmitters in real time according to the attitude change variation of the inclinometer, thus enhancing the system robustness in the harsh environment.

(7) The attitude self-compensation method to the transmitters based on inclinometer scanning of wMPS of the present invention is achieved by the following steps:

(8) Step 1: as shown in FIG. 1, fixing the inclinometer in the transmitters according the mechanism structure of the transmitter and characteristics of the inclinometer, wherein the angular accuracy of the inclinometer is less than 2″, and the cross coupling error is less than 0.72″.

(9) Step 2: as shown in FIG. 2, fixing an automatic level in the space and calibrating it by bubble level called “optical composite image level” to parallel to the horizontal plane; the automatic level in this embodiment is selected from DSZ2 produced by Suzhou FOIF Co., Ltd., which settling uncertainty accuracy is no more than 0.3″, the standard deviation over a 1 km double run is no more than ±1 mm;

(10) Arranging a plurality of spherical receivers having a diameter of 1.5 inches and the corresponding spherical holders; the dimension of the spherical receiver is same as that of SMR of the laser tracker so as to ensure the center of the receiver is at the same point after the replacement by the SMR; arranging a lifting guide rail which is fixed with said spherical holders, replacing the spherical receivers with SMRs of the laser tracker on the holders, wherein the center of the SMR is visible; adjusting the guide rail to ensure that the center of each SMR is in the same horizontal plane by using the automatic level, thus, finishing the arrangement of the target points on the horizontal reference plane;

(11) Measuring target points and other points by the laser tracker and establishing the horizontal reference frame O.sub.h-x.sub.hy.sub.hz.sub.h in accordance with the target points as xoy plane;

(12) As shown in FIG. 4, arranging transmitters in front of the horizontal reference frame according to the measurement range and measurement model of the transmitters, setting spherical receivers on the respective points on the horizontal reference frame, respectively; receiving scanning optical plane signal and synchronous marked optical signal of the transmitters, and obtaining rotation matrix R.sub.T.sup.H from the transmitter coordinate system to the horizontal reference frame via space-resection method.

(13) Step 3: as shown in FIG. 3, defining a inclinometer coordinate system O.sub.i-x.sub.iy.sub.iz.sub.i, and obtaining readings of two axes of the inclinometer, and then converting readings of the inclinometer to rotation angle around the corresponding axis with respect to the inclinometer measurement model;

(14) $\{\begin{array}{c}{\theta }_i=a\\ {\gamma }_i=-\arcsin \left(\frac{\sin \beta }{\cos a}\right)\end{array}\hspace{1em}$

(15) Where α and β are the readings, θ.sub.i and γ.sub.i are rotation angles around x and y axes, respectively;

(16) And the equation of rotation matrix R.sub.H.sup.I from the horizontal reference frame to the inclinometer coordinate system is:

(17) $R_H^I=R_{\mathrm{yi}}.Math.R_{\mathrm{xi}}.Math.R_{\mathrm{zi}}=\left[\begin{array}{ccc}\cos {\gamma }_i& 0& \sin {\gamma }_i\\ 0& 1& 0\\ -\sin {\gamma }_i& 0& \cos {\gamma }_i\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\theta }_i& -\sin {\theta }_i\\ 0& \sin {\theta }_i& \cos {\theta }_i\end{array}\right]\left[\begin{array}{ccc}\cos {\psi }_i& -\sin {\psi }_i& 0\\ \sin {\psi }_i& \cos {\psi }_i& 0\\ 0& 0& 1\end{array}\right]$

(18) Where ψi is the rotation angle around z-axis in the rotation matrix;

(19) Defining rotation matrix R.sub.T.sup.I from the transmitter coordinate system to the inclinometer coordinate system, and parameterizing the rotation matrix as follows:

(20) $R_H^T={\left(R_T^H\right)}^{-1}=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right]R_T^I=\left[\begin{array}{ccc}{r}_{i1}& r_{i2}& r_{i3}\\ r_{i4}& r_{i5}& r_{i6}\\ r_{i7}& r_{i8}& r_{i9}\end{array}\right]$

(21) Where r.sub.1 . . . r.sub.9 are known parameters, and r.sub.i1 . . . r.sub.i9 are parameters to be solved;

(22) Obtaining following equations according to rotation relationships among transmitter coordinate system, horizontal reference frame and inclinometer coordinate system:

(23) 0$\begin{array}{c}{R}_H^I=R_T^I.Math.R_H^T.Math.\left[\begin{array}{ccc}{r}_{i1}{r}_1+r_{i2}{r}_4+& r_{i1}{r}_2+r_{i2}{r}_5+& r_{i1}{r}_3+r_{i2}{r}_6+\\ r_{i3}{r}_7& r_{i3}{r}_8& r_{i3}{r}_9\\ r_{i4}{r}_1+r_{i5}{r}_4+& r_{i4}{r}_2+r_{i5}{r}_5+& r_{i4}{r}_3+r_{i5}{r}_6+\\ r_{i6}{r}_7& r_{i6}{r}_8& r_{i6}{r}_9\\ r_{i7}{r}_1+r_{i8}{r}_4+& r_{i7}{r}_2+r_{i8}{r}_5+& r_{i7}{r}_3+r_{i8}{r}_6+\\ r_{i9}{r}_7& r_{i9}{r}_8& r_{i9}{r}_9\end{array}\right]\\ =\left[\begin{array}{ccc}\cos {\gamma }_i\cos {\psi }_i-& \sin {\theta }_i\sin {\gamma }_i\cos {\psi }_i+& -\cos {\theta }_i\sin {\theta }_i\\ \sin {\theta }_i\sin {\gamma }_i\sin {\psi }_i& \cos {\gamma }_i\sin {\psi }_i& \\ -\cos {\theta }_i\sin {\psi }_i& \cos {\theta }_i\cos {\psi }_i& \sin {\theta }_i\\ \sin {\theta }_i\cos {\gamma }_i\sin {\psi }_i+& \sin {\psi }_i\sin {\gamma }_i-& \cos {\theta }_i\cos {\gamma }_i\\ \cos {\psi }_i\sin {\gamma }_i& \sin {\theta }_i\cos {\gamma }_i\cos {\psi }_i& \end{array}\right]\end{array}\hspace{1em}$

(24) Obtaining simultaneous equations via the above equations:

(25) $\{\begin{array}{c}{r}_{i1}{r}_3+r_{i2}{r}_6+r_{i3}{r}_9=-\cos {\theta }_i\sin {\gamma }_i\\ r_{i4}{r}_3+r_{i5}{r}_6+r_{i6}{r}_9=\sin {\theta }_i\\ r_{i7}{r}_3+r_{i8}{r}_6+r_{i9}{r}_9=\cos {\theta }_i\cos {\gamma }_i\end{array};$

(26) Regulating at least three attitudes of the transmitter by mechanical adjustment, solving the least squares solution of the linear equations to obtain r.sub.i1 . . . r.sub.i9, e.g. to obtain the rotation matrix from transmitter coordinate system to inclinometer coordinate system;

(27) Constructing a non-linear optimization objective equation combining the orthogonal constraints of the rotation matrix and performing optimization solution via iterative algorithm to obtain the exact solution of the rotation matrix from transmitter coordinate system to inclinometer coordinate system;

(28) $E=\underset{j=1}{\overset{n}{.Math.}}\left(F_{j1}^2+F_{j2}^2+F_{j3}^2\right)+M\underset{j=1}{\overset{6}{.Math.}}{f}_j^2$$\mathrm{Where}\{\begin{array}{c}{F}_{j1}=r_{i1}{r}_{j3}+r_{i2}{r}_{j6}+r_{i3}{r}_{j9}+\sin {\gamma }_{\mathrm{ji}}\cos {\theta }_{\mathrm{ji}}\\ F_{j2}=r_{i4}{r}_{j3}+r_{i5}{r}_{j6}+r_{i6}{r}_{j9}-\sin {\theta }_{\mathrm{ji}}\\ F_{j3}=r_{i7}{r}_{j3}+r_{i8}{r}_{j6}+r_{i9}{r}_{j9}-\cos {\theta }_{\mathrm{ji}}\cos {\gamma }_{\mathrm{ji}}\end{array}\{\begin{array}{c}{f}_1=r_{i1}^2+r_{i2}^2+r_{i3}^2-1=0\\ f_2=r_{i4}^2+r_{i5}^2+r_{i6}^2-1=0\\ f_3=r_{i7}^2+r_{i8}^2+r_{i9}^2-1=0\\ f_4=r_{i1i4}+r_{i2i5}+r_{i3i6}=0\\ f_5=r_{i1i7}+r_{i2i8}+r_{i3i9}=0\\ f_6=r_{i4i7}+r_{i5i8}+r_{i6i9}=0\end{array},$
where n is the number of attitudes of the transmitter by mechanical adjustment, n>=3.

(29) Step 4: Obtaining readings of the inclinometer in real time; according to the calibrated rotation matrix from transmitter coordinate system to inclinometer coordinate system, calculating the updated rotation matrix of the transmitter attitude, compensating the transmitter attitude in real time and obtaining the attitude compensation algorithm as follows:
(C.sub.T).sub.new=(R.sub.T.sup.I).sup.−1.Math.(R.sub.H.sup.I).sub.new.Math.((R.sub.H.sup.I).sub.old).sup.−1.Math.R.sub.T.sup.I.Math.(C.sub.T).sub.old.

(30) Consequently, the present invention achieves an attitude self-compensation method to the transmitters of wMPS based on inclinometer. By the high-accuracy attitude perception of the inclinometer, the method of the present invention compensates and updates the attitude of transmitters, which effectively avoids the attitude change of the transmitters caused by vibration or creep in harsh environments, and greatly improves the stability of the overall measurement field. The present invention satisfies the current high-accuracy and wide range measurement requirements and improves applicability of wMPS.

(31) The inclinometer measurement model involved in the present invention can refer to the reference of “Wen X., 2011 Research on error compensation of inclinometer in pose measurement of shield machine, Huazhong University of Science and Technology”; The transmitter measurement model involved in the present invention can refer to the reference of “Yang L., 2010 Research on large-scale space coordinate measurement location technology based on optical scanning, Tianjin University”;

(32) The space resection measurement method involved in the present invention can refer to the reference of “Liu X., 2014 Research on measurement and positioning method in large complex field, Tianjin University”.

(33) Although the functions and working processes of the present invention have been described above with reference to the accompanying drawings, the present invention is not limited thereto. The foregoing specific implementations are merely illustrative but not limiting. A person of ordinary skill in the art may make various forms under the teaching of the present invention without departing from the purpose of the present invention and the protection scope of the appended claims, and all the forms shall fall into the protection scope of the present invention.