US2021199438A1PendingUtilityA1
Heading initialization method for tilt rtk
Est. expiryDec 27, 2039(~13.4 yrs left)· nominal 20-yr term from priority
G01S 19/49G01S 19/43G01S 19/14G01S 19/47G01C 21/20G01C 21/188G01C 21/165G01C 25/005G01C 21/183
44
PatentIndex Score
0
Cited by
0
References
0
Claims
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
exact text as granted — not AI-modifiedWhat is claimed is:
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 T n =r IMU n +C b n l 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 T 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 IMU n represents a vector of the IMU position projected in the frame n; C b n represents an attitude matrix; and l 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 IMU n ( t )= r T n −C b n ( t ) l b (2)
wherein only the attitude matrix C b 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 b(k) n(k) =q n(k-1) n(k) q b(k-1) n(k-1) q b(k) 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 b(k) 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 n(k-1) n(k) and q b(k) b(k-1) represent attitude changes of the frame n and the frame b, respectively, and q b(k-1) n(k-1) represents a quaternion at the previous moment; wherein updates of q n(k-1) n(k) are ignored, and attitude changes of the frame b are simplified as:
q
b
(
k
)
b
(
k
-
1
)
=
[
cos
0.5
φ
k
sin
0.5
φ
k
0.5
φ
k
0
.
5
φ
k
]
(
4
)
φ
k
=
Δ
θ
k
+
1
1
2
Δ
θ
k
-
1
×
Δ
θ
k
(
5
)
wherein Δθ k and Δθ 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):
C
c
r
=
[
cos
θ
sin
θ
N
-
s
in
θ
cos
θ
E
]
(
6
)
where C c 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 i,r =C c r s i,c +Δ (7)
where s i,r and s i,c represent plane projections of coordinate sequences of the RTK trajectory and the INS trajectory, respectively; i represents an i 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 c 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:
X
k
=
[
θ
k
N
k
E
k
]
,
x
k
=
[
θ
k
-
θ
k
-
1
N
k
-
N
k
-
1
E
k
-
E
k
-
1
]
=
[
Δ
θ
Δ
N
Δ
E
]
(
8
)
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:
s
i
,
r
=
C
c
r
s
i
,
c
+
Δ
=
C
c
2
X
=
X
k
-
1
·
s
i
,
c
+
(
∂
C
c
r
∂
θ
·
Δθ
+
∂
C
c
r
∂
N
·
Δ
N
+
∂
C
c
r
∂
E
·
Δ
E
)
X
=
X
k
-
1
·
s
i
,
c
+
Δ
wherein
f
(
X
k
-
1
)
=
C
c
r
X
=
X
k
-
1
,
H
i
k
=
[
∂
C
c
r
∂
θ
·
s
i
,
c
∂
C
c
r
∂
N
·
s
i
,
c
∂
C
c
r
∂
E
·
s
i
,
c
]
X
=
X
k
-
1
(
9
)
simplifying the Formula (9) as:
s i,r =f ( X k-1 )· s i,c +H i k ·x k +Δ (10)
wherein the Formula (9) is written in a residual equation form, which is:
v
=
H
·
x
-
z
=
[
∂
C
c
r
∂
θ
·
s
1
,
c
∂
C
c
r
∂
N
·
s
1
,
c
∂
C
c
r
∂
E
·
s
1
,
c
∂
C
c
r
∂
θ
·
s
2
,
c
∂
C
c
r
∂
N
·
s
2
,
c
∂
C
c
r
∂
E
·
s
2
,
c
⋮
⋮
⋮
∂
C
c
r
∂
θ
·
s
n
,
c
∂
C
c
r
∂
N
·
s
n
,
c
∂
C
c
r
∂
E
·
s
n
,
c
]
X
=
X
k
-
1
.
[
Δθ
Δ
N
Δ
E
]
-
[
s
1
,
r
-
f
(
X
k
-
1
)
·
s
1
,
c
s
2
,
r
-
f
(
X
k
-
1
)
·
s
2
,
c
⋮
s
n
,
r
-
f
(
X
k
-
1
)
·
s
n
,
c
]
(
11
)
and
obtaining solution results by a least square method:
x =( H T H ) −1 H T z (12)
X k =X k-1 +x 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.Join the waitlist — get patent alerts
Track US2021199438A1 — get alerts on status changes and closely related new filings.
We store only your email — no account needed. See our privacy policy.