HEADING INITIALIZATION METHOD FOR TILT RTK

Abstract

A method for calculating an INS initial heading angle error includes comparing an RTK trajectory with an INS trajectory under a tilt RTK application scenario, which may achieve heading angle initialization with an accuracy of 1 deg within 2 seconds. An INS trajectory estimation method eliminates accelerometers, and a large initial gyro bias is compensated by averaging the stationary gyro measurement at the beginning of the measurement to ensure the accuracy of the estimated INS trajectory. A rather short initialization duration also greatly improves the measurement efficiency. Compared with common heading initialization methods for the tilt RTK, the inventive method eliminates magnetometers and thus prevents interference from magnetic fields, obtaining stronger adaptability in complex environments.

Claims

1. A heading initialization method for a tilt RTK, comprising: step 1 of placing a measurement pole flatwise on a flat and clean ground, and remaining the measurement pole stationary for m minutes to determine a large gyro bias initial value; step 2 of raising the measurement pole to touch the ground with a pole tip of the measurement pole, and then titling the measurement pole to get ready for shaking, wherein the measurement pole remains stationary for m seconds before the shaking to determine an initial roll angle and an initial pitch angle; and step 3 of shaking a top end of the measurement pole while the measurement pole touches the ground and the pole tip remains stationary, to form a motion trajectory, which is called IMU trajectory, and then obtaining an initial heading error of an INS by following steps: 31) obtaining a position of an antenna phase center through RTK, and projecting the position to an IMU center to form a trajectory, which is called RTK trajectory; 32) calculating a position of the IMU center from a measurement value obtained by a positioning sensor to form a trajectory, which is called INS trajectory; and 33) comparing the RTK trajectory with the INS trajectory to calculate an initial heading angle error of the INS.

2. The heading initialization method for the tilt RTK according to claim 1, wherein the step 32) is specifically implemented by: describing a relationship between an IMU position of the tilt RTK and a position of the pole tip of the measurement pole by below Formula (1):
r.sub.T.sup.n=r.sub.IMU.sup.n+C.sub.b.sup.nl.sup.b  (1) wherein n represents a local coordinate system that is a frame n taking an IMU measurement center as an origin, in which an x-axis is parallel to a local horizontal plane and directed to due north, a y-axis is parallel to the local horizontal plane and directed to due east, and a z-axis is perpendicular to the local horizontal plane and directed downwards, so that the x-axis, y-axis and z-axis constitute a right-hand coordinate system; b represents a body coordinate system that is a frame b taking the IMU measurement center as an origin, in which an x-axis is directed to a forward direction of a carrier, a y-axis is perpendicular to the x-axis of the body coordinate system and directed to a right side of the carrier, and a z-axis is perpendicular to both of the x-axis and the y-axis of the body coordinate system to form a right-hand coordinate system; r.sub.T.sup.n represents a vector of the position of the pole tip, which is projected on the frame n, wherein T represents the pole tip; r.sub.IMU.sup.n represents a vector of the IMU position projected in the frame n; C.sub.b.sup.n represents an attitude matrix; and l.sup.b represents a lever arm vector under the frame b directing to the pole tip from the IMU center; further rewriting estimation formula of the IMU position as:
r.sub.IMU.sup.n(t)=r.sub.T.sup.n−C.sub.b.sup.n(t)l.sup.b  (2) wherein only the attitude matrix C.sub.b.sup.n at a right side of Formula (2) changes over time, and thereby estimation of the IMU position is completed as long as attitudes at each moment are obtained; wherein an attitude update algorithm is represented by Formula (3):
q.sub.b(k).sup.n(k)=q.sub.n(k-1).sup.n(k)q.sub.b(k-1).sup.n(k-1)q.sub.b(k).sup.b(k-1)  (3) wherein the attitudes are updated through an attitude quaternion q in Formula (3), where k−1 represents a previous moment and k represents a current moment, such that the quaternion q.sub.b(k).sup.n(k) of the frame b to the frame n at the current moment is further decomposed into three quaternions shown in Formula (3), wherein q.sub.n(k-1).sup.n(k) and q.sub.b(k).sup.b(k-1) represent attitude changes of the frame n and the frame b, respectively, and q.sub.b(k-1).sup.n(k-1) represents a quaternion at the previous moment; wherein updates of q.sub.n(k-1).sup.n(k) are ignored, and attitude changes of the frame b are simplified as: $\begin{array}{cc}{q}_{b\left(k\right)}^{b\left(k-1\right)}=\left[\begin{array}{c}\cos .Math..Math.0.5.Math..Math.{\phi }_k.Math.\\ \frac{\sin .Math..Math.0.5.Math..Math.{\phi }_k.Math.}{0.5.Math..Math.{\phi }_k.Math.}.Math.0.5.Math.{\phi }_k\end{array}\right]& \left(4\right)\\ {\phi }_k=\Delta .Math.{\theta }_k+\frac{1}{1.Math.2}.Math.\Delta .Math.{\theta }_{k-1}\times \Delta .Math.{\theta }_k& \left(5\right)\end{array}$ wherein Δθ.sub.k and Δθ.sub.k-1 represent gyro angle incremental output at the current moment and the previous moment, respectively.

3. The heading initialization method for the tilt RTK according to claim 1, wherein the step 33) is specifically implemented as by: denoting a coordinate system determined by the RTK trajectory as a reference coordinate system, which is a frame r; denoting a coordinate system determined by the INS trajectory as a calculation coordinate system, which is a frame c; simplifying the frame r and the frame c into two two-dimensional coordinate systems that are coplanar, an angle between the reference coordinate system and the calculation coordinate system being the initial heading angle error; and then selecting three parameters which are the initial heading angle error θ, a northward translation distance N, and an eastward translation distance E, to construct a coordinate transformation matrix (6): $\begin{array}{cc}{C}_c^r=\left[\begin{array}{ccc}\cos .Math..Math.\theta & \sin .Math..Math.\theta & N\\ -s.Math.\mathrm{in}.Math..Math.\theta & \cos .Math..Math.\theta & E\end{array}\right]& \left(6\right)\end{array}$ where C.sub.c.sup.r represents a coordinate transformation matrix that transforms vectors from the frame c to the frame r; establishing an observation equation (7) according to a coordinate transformation principle:
s.sub.i,r=C.sub.c.sup.rs.sub.i,c+Δ  (7) where s.sub.i,r and s.sub.i,c represent plane projections of coordinate sequences of the RTK trajectory and the INS trajectory, respectively; i represents an i.sup.th point on the RTK trajectory and the INS trajectory, where 1=1, 2, 3, 4 . . . n; and Δ represents an observation error; wherein although the coordinate transformation matrix C.sub.c.sup.r has 6 parameters, only three of which are independent parameters including the heading angle error θ, the northward translation distance N, and the eastward translation distance E, which are then taken as estimation parameters: $\begin{array}{cc}{X}^k=\left[\begin{array}{c}{\theta }^k\\ N^k\\ E^k\end{array}\right],x^k=\left[\begin{array}{c}{\theta }^k-{\theta }^{k-1}\\ N^k-N^{k-1}\\ E^k-E^{k-1}\end{array}\right]=\left[\begin{array}{c}\Delta .Math.\theta \\ \Delta .Math.N\\ \Delta .Math.E\end{array}\right]& \left(8\right)\end{array}$ where a superscript k represents the number of iterations; expanding the observation equation according to Taylor's formula and discarding second-order and higher-order terms to obtain: $\begin{array}{cc}{s}_{i,r}=C_c^r.Math.s_{i,c}+\Delta =C_c^2.Math.{{.Math.}_{X=X^{k-1}}.Math..Math.s_{i,c}+\left(\frac{\partial C_c^r}{\partial \theta }.Math.\mathrm{\Delta \theta }+\frac{\partial C_c^r}{\partial N}.Math.\Delta .Math..Math.N+\frac{\partial C_c^r}{\partial E}.Math.\Delta .Math..Math.E\right).Math.}_{X=X^{k-1}}.Math.s_{i,c}+\Delta .Math..Math.\mathrm{wherein}.Math..Math.f\left(X^{k-1}\right)=C_c^r.Math.{{.Math.}_{X=X^{k-1}}.Math.,H_i^k=\left[\frac{\partial C_c^r}{\partial \theta }.Math.s_{i,c}.Math.\frac{\partial C_c^r}{\partial N}.Math.s_{i,c}.Math.\frac{\partial C_c^r}{\partial E}.Math.s_{i,c}\right].Math.}_{X=X^{k-1}}& \left(9\right)\end{array}$ simplifying the Formula (9) as:
s.sub.i,r=f(X.sup.k-1).Math.s.sub.i,c+H.sub.i.sup.k.Math.x.sup.k+Δ  (10) wherein the Formula (9) is written in a residual equation form, which is: $\begin{array}{cc}{v=H.Math.x-z=.Math.\left[\begin{array}{ccc}\frac{\partial C_c^r}{\partial \theta }.Math.s_{1,c}& \frac{\partial C_c^r}{\partial N}.Math.s_{1,c}& \frac{\partial C_c^r}{\partial E}.Math.s_{1,c}\\ \frac{\partial C_c^r}{\partial \theta }.Math.s_{2,c}& \frac{\partial C_c^r}{\partial N}.Math.s_{2,c}& \frac{\partial C_c^r}{\partial E}.Math.s_{2,c}\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ \frac{\partial C_c^r}{\partial \theta }.Math.s_{n,c}& \frac{\partial C_c^r}{\partial N}.Math.s_{n,c}& \frac{\partial C_c^r}{\partial E}.Math.s_{n,c}\end{array}\right].Math.}_{X=X^{k-1}}.\left[\begin{array}{c}\mathrm{\Delta \theta }\\ \Delta .Math.N\\ \Delta .Math.E\end{array}\right]-.Math.\left[\begin{array}{c}{s}_{1,r}-f\left(X^{k-1}\right).Math.s_{1,c}\\ s_{2,r}-f\left(X^{k-1}\right).Math.s_{2,c}\\ \mathrm{.Math.}\\ s_{n,r}-f\left(X^{k-1}\right).Math.s_{n,c}\end{array}\right]& \left(11\right)\end{array}$ and obtaining solution results by a least square method:
x=(H.sup.TH).sup.−1H.sup.Tz  (12)
X.sup.k=X.sup.k-1+x.sup.k  (13) wherein an initial value of the least square method is obtained by a vector matching method.

4. The heading initialization method for the tilt RTK according to claim 3, wherein the initial value of the least square method is obtained by: selecting points on the RTK trajectory and the corresponding points on the INS trajectory under the same epoch to form multiple pairs of vectors, obtaining angles between the multiple pairs of vectors, and then assigning different weights to the angles between the multiple pairs of vectors to determine an angle value of the initial heading angle error.

Description

BRIEF DESCRIPTION OF THE SEVERAL VIEWS OF THE DRAWINGS

[0044] FIG. 1 is a schematic diagram of an IMU trajectory during initialization process for tilt RTK; and

[0045] FIG. 2 is a schematic diagram illustrating similarity between an INS trajectory and an GPS RTK trajectory.

DETAILED DESCRIPTION OF THE INVENTION

[0046] The technical solution of the present disclosure will be further described in detail below in conjunction with the accompanying drawings and embodiments.

[0047] In an application scenario of the tilt RTK, the heading initialization is performed according to the method of the present disclosure after the user powers on the equipment. The specific steps are as follows.

[0048] 1) A measurement pole is placed flatwise on a flat and clean ground after powering on the equipment, and remains stationary for 1 minute to determine a large gyro bias initial value.

[0049] 2) The measurement pole is raised to touch the ground with a pole tip and then tilted to get ready for shaking. The measurement pole remains stationary for 1 second before the shaking to determine an initial roll angle and an initial pitch angle.

[0050] 3) While the measurement pole touches the ground and the pole tip of the measurement pole remains stationary, a top end (i.e. IMU end) of the measurement pole is shaken to form a motion trajectory (called IMU trajectory), as shown in FIG. 1. At this time, the system performs real-time solution, and gives a prompt when the heading accuracy is reliable, so that the user may stop shaking. Generally, it only takes about 2 seconds of shaking to determine the initial heading of the INS.

[0051] The real-time solution of the system further includes the following sub-steps.

[0052] In sub-step 31), a position of an antenna phase center is obtained through RTK, and the position is projected to an IMU center to form a trajectory (called RTK trajectory).

[0053] In sub-step 32), a position of the IMU center is calculated from a measurement value obtained by a positioning sensor to form a trajectory (called INS trajectory). The present disclosure proposes the following solution method to determine the INS trajectory.

[0054] The relationship between an IMU position of the tilt RTK and a position of the pole tip of the measurement pole is described as follows:

r.sub.T.sup.n=r.sub.IMU.sup.n+C.sub.b.sup.nl.sup.b  (14)

[0055] In the formula, n represents a local coordinate system that is a frame n taking an IMU measurement center as an origin, in which an x-axis is parallel to a local horizontal plane and directed to due north, a y-axis is parallel to the local horizontal plane and directed to due east, and a z-axis is perpendicular to the local horizontal plane and directed downwards, so that the three axes constitute a right-hand coordinate system; b represents a body coordinate system that is a frame b taking the IMU measurement center as an origin, in which an x-axis is directed to a forward direction of a carrier, a y-axis is perpendicular to the x-axis and directed to a right side of the carrier, and a z-axis is perpendicular to both of the x-axis and the y-axis to form a right-hand coordinate system; r.sub.T.sup.n represents a vector of the position of the pole tip, which is projected on the frame n, and T represents the pole tip (pole bottom); r.sub.IMU.sup.n represents a vector of the IMU position projected in the frame n; C.sub.b.sup.n represents an attitude matrix; and l.sup.b represents a lever arm vector under the frame b directing to the pole tip from the IMU center. Wherein the position is in a form of north-east-down (planar coordinates and elevation).

[0056] In motions as designed in the initial alignment, r.sub.T.sup.n does not change over time, and as long as a rough initial value of the position is given, the lever arm l.sup.b in the formula may be accurately measured. Thus, the estimation formula of the IMU position may be rewritten as:

r.sub.IMU.sup.n(t)=r.sub.T.sup.n−C.sub.b.sup.n(t)l.sup.b  (15)

[0057] At a right side of the formula, only the attitude matrix C.sub.b.sup.n changes over time. Thus, the estimation of the IMU position is completed as long as the attitudes at each moment are obtained. An attitude update algorithm is as follows:

q.sub.b(k).sup.n(k)=q.sub.n(k-1).sup.n(k)q.sub.b(k-1).sup.n(k-1)q.sub.b(k).sup.b(k-1)  (16)

[0058] The attitudes are updated through an attitude quaternion q in Formula (3), where k−1 represents a previous moment and k represents a current moment, such that the quaternion q.sub.b(k).sup.n(k) of the frame b to the frame n at the current moment is further decomposed into three quaternions shown in Formula (3), wherein q.sub.n(k-1).sup.n(k) and q.sub.b(k).sup.b(k-1) represent attitude changes of the frame n and the frame b, respectively, and q.sub.b(k-1).sup.n(k-1) represents a quaternion at the previous moment.

[0059] The attitude changes in the frame b may be simplified as:

[00006]$\begin{array}{cc}{q}_{b\left(k\right)}^{b\left(k-1\right)}=\left[\begin{array}{c}\cos .Math..Math.0.5.Math..Math.{\phi }_k.Math.\\ \frac{\sin .Math..Math.0.5.Math..Math.{\phi }_k.Math.}{0.5.Math..Math.{\phi }_k.Math.}.Math.0.5.Math.{\phi }_k\end{array}\right]& \left(17\right)\\ {\phi }_k=\Delta .Math.{\theta }_k+\frac{1}{1.Math.2}.Math.\Delta .Math.{\theta }_{k-1}\times \Delta .Math.{\theta }_k& \left(18\right)\end{array}$

[0060] wherein Δθ.sub.k and Δθ.sub.k-1 are gyro angle incremental output at the current moment and the previous moment, respectively.

[0061] Since the initialization motion of tilt RTK has characteristics of small speed and limited position variation, the update of q.sub.n(k-1).sup.n(k) may be simplified or even ignored. In this way, the main error that affects the INS trajectory is only the constant gyro bias, and the large gyro bias initial value may be determined by calculating the initial value through the standstill at the beginning.

[0062] In sub-step 33), assuming that the INS trajectory is only affected by the initial heading error without considering errors of other sensors, the INS trajectory is highly similar to the RTK trajectory, which is represented by two trajectories having substantially the same shape with one merely rotated by an angle, as shown in FIG. 2. This angle is the initial heading angle error of the INS. Thus, the initial heading error of the INS may be calculated by comparing the RTK trajectory with the INS trajectory, and the trajectory comparing algorithm includes but is not limited to vector matching method, least square method, etc. The specific implementation method is as follows.

[0063] A coordinate system determined by the RTK trajectory is denoted as a reference coordinate system, namely a frame r. A coordinate system determined by the INS trajectory is denoted as a calculation coordinate system, namely a frame c. The frame r and the frame care simplified into two two-dimensional coordinate systems that are coplanar, and an angle between the two coordinate systems is the initial heading angle error. Then, three parameters (initial heading angle error θ, northward translation distance N, and eastward translation distance E) are selected to convert the model and thereby achieve conversion of the coordinate system. The following coordinate transformation matrix may be constructed:

[00007]$\begin{array}{cc}{C}_c^r=\left[\begin{array}{ccc}\cos .Math..Math.\theta & \sin .Math..Math.\theta & N\\ -s.Math.\mathrm{in}.Math..Math.\theta & \cos .Math..Math.\theta & E\end{array}\right]& \left(19\right)\end{array}$

[0064] where C.sub.c.sup.r represents a coordinate transformation matrix that transforms vectors from the frame c to the frame r. According to a coordinate transformation principle, an observation equation is established as follows:

s.sub.i,r=C.sub.c.sup.rs.sub.i,c+Δ  (20)

[0065] In the formula, s.sub.i,r and s.sub.i,c are plane projections of coordinate sequences of the RTK trajectory and the INS trajectory, respectively; i (i=1, 2, 3, 4 . . . n) represents an i.sup.th point on the RTK trajectory and the INS trajectory; and Δ is an observation error. It should be noted that since the coordinate transformation matrix C.sub.c.sup.r contains translation information, s.sub.i,c which is two-dimensional originally is expanded as a three-dimensional vector.

[0066] Although the coordinate conversion matrix C.sub.c.sup.r has 6 parameters, only three of which are independent parameters including initial heading angle error θ, northward translation distance N, and eastward translation distance E, which are then taken as estimation parameters:

[00008]$\begin{array}{cc}{X}^k=\left[\begin{array}{c}{\theta }^k\\ N^k\\ E^k\end{array}\right],x^k=\left[\begin{array}{c}{\theta }^k-{\theta }^{k-1}\\ N^k-N^{k-1}\\ E^k-E^{k-1}\end{array}\right]=\left[\begin{array}{c}\Delta .Math.\theta \\ \Delta .Math.N\\ \Delta .Math.E\end{array}\right]& \left(21\right)\end{array}$

[0067] In the formula, the superscript k represents the number of iterations.

[0068] A following equation is obtained by expanding the observation equation according to Taylor's formula and discarding second-order and higher-order terms:

[00009]$\begin{array}{cc}{s}_{i,r}=C_c^r.Math.s_{i,c}+\Delta =C_c^2.Math.{{.Math.}_{X=X^{k-1}}.Math..Math.s_{i,c}+\left(\frac{\partial C_c^r}{\partial \theta }.Math.\mathrm{\Delta \theta }+\frac{\partial C_c^r}{\partial N}.Math.\Delta .Math..Math.N+\frac{\partial C_c^r}{\partial E}.Math.\Delta .Math..Math.E\right).Math.}_{X=X^{k-1}}.Math.s_{i,c}+\Delta .Math..Math.f\left(X^{k-1}\right)=C_c^r.Math.{{.Math.}_{X=X^{k-1}}.Math.,H_i^k=\left[\frac{\partial C_c^r}{\partial \theta }.Math.s_{i,c}.Math.\frac{\partial C_c^r}{\partial N}.Math.s_{i,c}.Math.\frac{\partial C_c^r}{\partial E}.Math.s_{i,c}\right].Math.}_{X=X^{k-1}}& \left(22\right)\end{array}$

[0069] The aforesaid formula may be simplified as:

s.sub.i,r=f(X.sup.k-1).Math.s.sub.i,c+H.sub.i.sup.k.Math.x.sup.k+Δ  (23)

[0070] The formula in a residual equation form is:

[00010]$\begin{array}{cc}{v=H.Math.x-z=.Math.\left[\begin{array}{ccc}\frac{\partial C_c^r}{\partial \theta }.Math.s_{1,c}& \frac{\partial C_c^r}{\partial N}.Math.s_{1,c}& \frac{\partial C_c^r}{\partial E}.Math.s_{1,c}\\ \frac{\partial C_c^r}{\partial \theta }.Math.s_{2,c}& \frac{\partial C_c^r}{\partial N}.Math.s_{2,c}& \frac{\partial C_c^r}{\partial E}.Math.s_{2,c}\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ \frac{\partial C_c^r}{\partial \theta }.Math.s_{n,c}& \frac{\partial C_c^r}{\partial N}.Math.s_{n,c}& \frac{\partial C_c^r}{\partial E}.Math.s_{n,c}\end{array}\right].Math.}_{X=X^{k-1}}.\left[\begin{array}{c}\mathrm{\Delta \theta }\\ \Delta .Math.N\\ \Delta .Math.E\end{array}\right]-.Math.\left[\begin{array}{c}{s}_{1,r}-f\left(X^{k-1}\right).Math.s_{1,c}\\ s_{2,r}-f\left(X^{k-1}\right).Math.s_{2,c}\\ \mathrm{.Math.}\\ s_{n,r}-f\left(X^{k-1}\right).Math.s_{n,c}\end{array}\right]& \left(24\right)\end{array}$

[0071] Through the least square method, following solution results are obtained:

x=(H.sup.TH).sup.−1H.sup.Tz  (25)

X.sup.k=X.sup.k-1+x.sup.k  (26)

[0072] The initial value of the least square method may be, but not limited to, given by the vector matching method, and the specific steps are as follows. Multiple pairs of vectors are formed by the selected points on the RTK trajectory and the selected corresponding points on the INS trajectory under the same epoch. For example, assuming that the starting time of initial alignment is 9:00, coordinates of three time points, which are 9:00, 9:01 and 9:02, may be obtained after shaking for 2 seconds. The points on the RTK trajectory and the INS trajectory at the same time point are the corresponding points, and the three points on any trajectory may form 3 vectors in total, and the vectors on different trajectories may form 3 pairs of vectors. In theory, the angle between each pair of vectors is the angle between the two trajectories. After obtaining the angle between the multiple pairs of vectors, weights are assigned according to a certain criterion to determine an angle value of the initial heading angle error. The criterion here includes but is not limited to a vector length, time sequence, etc.

[0073] The specific embodiments described herein merely illustrate the spirit of the present invention exemplarily. Various modifications or additions may be made to the described embodiments, or alternatives may be employed, by those skilled in the art, without departing from the spirit of the present invention or going beyond the scope defined by the appended claims.