CA2716597A1 - Apparatus and method for driftless attitude determination and reliable localization of vehicles - Google Patents

Abstract

In order to determine positional information, about a mobile robot, Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS) measurement data are obtained by at least two GNSS
receivers mounted on the mobile robot. Estimates of the covariance matrices of the measurement data are computed. The RTK GNSS measurement data are combined according to the covariance matrices to obtain enhanced positional information. The results may be fused with data from an IMU to obtain driftless attitude and/or localization information.

Description

r Y

Apparatus and Method for Driftless Attitude Determination and Reliable Localization of Vehicles Field of the Invention This invention relates to the field of robotics, and more particularly to the driftless attitude determination and reliable localization of vehicles, such as mobile robots, waking robots, or humanoid robots.

Background of the Invention Both position and attitude determination of a mobile robot are necessary for navigation, guidance and steering control of the robot. Dead-reckoning using vehicle kinematic model and incremental measurement of wheel encoders are the common techniques to determine the position and orientation of mobile robots for indoor applications. However, the application of this technique for localization of outdoor robots is limited, particularly when the robot has to traverse an uneven terrain or loose soils. This is because wheel slippage and wheel imperfection cause quick accumulation of the position and attitude errors.

The problem with inertial systems is that they require additional information about absolute position and orientation to overcome long-term drift. An attitude estimation system based on utilization of multiple inertial measurements of a mobile robots is known.
Only pitch and roll angles may be estimated in this method by using gravity components deduced from measurements of two accelerometer, but the yaw angle is not detectable. An integrated inertial platform consisting of three rate gyros, a triaxial accelerometer and two tilt sensors is known to minimize long-term drift.
Other research focuses on using vision system as the absolute sensing mechanism required to update the prediction position obtained by inertial measurements.

Attitude determination GPS (ADGPS) using a multi-GPS antenna technology, in which the receiver gives not only the position but also the velocity data, are used for airborne applications as well as marine vessels and ships. Integration of such a GPS system and Inertial Navigation System (INS) is straightforward because attitude measurements from the two systems is directly available. ADGPS
however need large antenna separation, e.g., 17 m, for offering competitive accuracy for azimuth and pitch determination of a ship. Multiple GPS antennas with separation distance of 11 m has been tried for attitude determination of aircraft. Methods for attitude estimation of UAVs based on position and velocity information obtained from a single GPS antenna have been disclosed. Since the velocity information is obtained from the Doppler shift measurement of the GPS
carrier signals, these methods are suitable for fast moving vehicles such as UAVs, but not for mobile robots or humanoid robots.

Nowadays, differential Global Navigation Satellite Systems (GNSSs) to centimeter-level accuracy are commercially available making them attractive for localization, guidance and control of outdoor mobile robots. GNSS is the generic term for a satellite-based navigation system, such as the Global Positioning System (GPS), the Russian GLONASS, or European Galileo systems, and the term will be used in this specification to encompass all such systems. It will also be understood that in some environments, such as on mars, a satellite-based system may not be available.
However, in such environments, it is possible to replace the satellites by transmitters placed at fixed locations that are functionally equivalent to satellites. Such transmitters are known as "pseudolites", and it will be understood that the expression GNSS as used herein will also encompass such pseudolite-based systems. However, for convenience the invention will be described specifically in the context of the GPS system, which is the most widely used system today.

In conventional GPS, the pseudo ranges to satellites are measured by aligning the locally generated pseudorandom signal to the received signal. In Real Time Kinematic (RTK) GPS, and more specifically RTK GPS, an attempt is made to align the carriers rather than the signals modulated on to the carriers. Since the carriers are about 1000 times faster than the signals, considerable improvements in accuracy over conventional GPS can be achieved.

One method of improving the accuracy of localization systems using RTK GPS in the presence of GPS
latency is to use a localization algorithm based on Kalman filtering that tries to fuse information coming from an inexpensive single GPS with inertial data and map-based data. A
decentralized data fusion algorithm for simultaneous position estimation of a land vehicle and building the map of the environment by incorporating data from inertial sensor, GPS, laser scanner, the wheel and steering encoders is described in J. Vaganay, M.J. Aldon and A. Fournier, "Mobile robot attitude estimation by fusion of intertial data", IEEE Int Conference on Robotics & Automation, Atlanta Georgia, May 1993, pp 277-282.

The use of vision systems has also been investigated. However, vision systems have proven to be sensitive to the lighting condition of the environment.

Summary of the Invention According to a first aspect of the present invention there is provided a method of determining positional information about a vehicle, comprising: computing estimates of the covariance matrices of Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS) measurement data obtained by at least two GNSS receivers mounted on the mobile vehicle; and fusing the RTK GNSS

measurement data according to their corresponding covariance matrices to obtain enhanced positional information.

Embodiments of the invention provide a method for estimating vehicle attitude and position, in three dimensions, by optimally fusing two RTK GNSS measurements, accelerometric measurements of gravity from an accelerometer (or an inclinometer instrument) and angular rate measurements from a rate gyro. The relation between the GPS noises and the difference between the measured and actual antenna-to-antenna baselines is developed that allows to estimate the covariance matrix of the GNSS measurement noises.

First, the covariance matrices associated with the two RTK GNSS measurements are estimated based on the error on the magnitude of the antenna-to-antenna baseline measurement and the confidence of the GNSS receivers on the horizontal and vertical axes measurements. Then, the observation vector is constructed from the measurements of the onboard accelerometer and two RTK GNSSs. The measurement of the onboard rate gyro, the discrete-time observation as well as the estimated measurement covariance matrices are used by an Extended Kalman filter for driftless attitude determination of the mobile robot. Finally, the estimated attitude (in term of quaternion), RTK GPS measurements and the estimated covariance matrixes are used by another estimator to optimally localize the mobile robot by taking advantage of the redundancy in the GPS
measurements plus the knowledge of the GNSS noise characteristics. This allows enhancing the accuracy and robustness of the GNSS-based localization of vehicles.

Observability analysis shows that driftless attitude estimation requires the line connecting the antennas not to be collinear with the vector of gravity.

According to a second aspect of the invention there is provided an apparatus for determining positional information about a vehicle, comprising: at least two GNSS
receivers for mounting on the vehicle robot and defining an antenna-to-antenna baseline therebetween; and a processor configured to compute estimates of the covariance matrices of Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS) measurement data obtained by at least two GNSS receivers mounted on the vehicle, and to fuse the RTK GNSS measurement data according to their corresponding covariance matrices to obtain enhanced positional information.

A further aspect of the invention provides an apparatus for determining positional information about a vehicle, comprising at least two GNSS receivers for mounting on the vehicle and defining an antenna-to-antenna baseline therebetween; an inertial Measurement Unit (IMU) for obtaining data about the movement of the mobile robot; and a processor configured to and to fuse the RTK GNSS
measurement data and the data from the IMU to obtain enhanced positional information.

Brief Description of the Drawings The invention will now be described in more detail, by way of example only, with reference to the accompanying drawings, in which: -Figure 1 is a block diagram of an apparatus in accordance with one embodiment of the invention;
Figure 2 shows a mobile robot with two GPS antennas and an IMU mounted on the robot;

Figure 3 shows a mobile robot with three GPS antennas and an IMU mounted on the robot for experimental purposes;

Figure 4 shows the path taken by an experimental robot;
Figure 5 shows the IMU outputs;

Figure 6 shows the RTK GPS outputs;

Figure 7 shows the confidence on the RTK GPS measurements;
Figure 8 shows the estimated parameters;

Figure 9 shows the vehicle attitude;

Figure 10 shows the vehicle position;
Figure 11 shows the attitude errors; and Figure 12 shows the position errors.

Detailed description of Embodiments of the Invention The apparatus shown in Figure 1 comprises a vehicle 1 with wheels 2. A pair of RTK GPS antenna 3 coupled to respective GPS receivers 4, which may be differential receivers, are mounted a fixed distance apart on the vehicle 1 and separated by an antenna-to-antenna baseline 5. The baseline 5 should not be collinear with the vector of gravity for driftless attitude estimation. An accelerometer 6 and rate gyro 7 are mounted on the mobile robot 1. The embodiment will be described in connection with GPS, although as noted the invention applies more generally to any GNSS system, including systems using fixed pseudolites instead of satellites.

The mobile robot 1 also includes a processor 10, which may be a general-purpose computer, comprising a plurality of software/and or hardware modules, namely a covariance estimation module 11, a stochastic estimator module 12, an observation vector module 13, and an extended Kalman filter module 14, and an initializer 18.

The signals from the receivers 4 are subtracted from each other in subtraction module 15.

It will be understood that the various modules can be implemented in software, hardware or a combination of the two.

A base antenna 16 is also provided at a fixed location as is known in RTK GPS.

The method of estimating robot attitude and position, in three dimensions, involves optimally fusing two RTK GPS measurements, accelerometric measurements of gravity from an accelerometer (or an inclinometer) and angular rate measurements from a rate gyro in the Kalman filter 14 (KF). In addition to the pose, the KF 14 also estimates the gyro bias.

RTK GPS devices notoriously suffer from signal robustness issue as their signal can be easily disturbed by many factors such as satellite geometry, atmospheric condition and shadow. To deal with the uncertain GPS noise problem, the covariance matrix of the GPS noises is estimated in real-time so that the KF filter 14 is continually "tuned" as well as possible.

Kinematics Figure 2 schematically illustrates a vehicle 1 as a rigid body to which multiple differential GPS-antennas 3 and an IMU device are attached. Coordinate frame {A} is an inertial frame while (B) is a vehicle-fixed (body frame) coordinate system. The origin of frame {A}
coincides with that of the base antenna of the differential GPS system, i.e., the GPS measurements are expressed in this frame. On the other hand, the origin of {B} coincides with the IMU center and their axes are parallel, i.e., the IMU measurements are expressed in (B). The orientation of {B} with respect to ]T, {A} is represented by the unit quaternion q = C qqo where subscripts v and o denote the vector and scalar parts of the quaternion, respectively. The rotation matrix A representing the rotation of frame {B} with respect to frame {A) is related to the corresponding quaternion q by A(q) = (2q2 o -1)13 +2g0[gvx]+2q qv, (1) where [., ] denotes the matrix form of the cross-product. Consider quaternions and q3.
Then, A(q3) = A(q,)A(q,) and q3 = q2 q, are equivalent, where denotes the quaternion-product and the operators [q ] is defined, analogous to the cross-product matrix as [q ]= T - diag aq,, x],0) where =[ q0 q, -qv q The conjugate q' of a quaternion is defined such that q` q = q q* = [0 0 0 If.

Now, assume that vector r represents the location of the origin of frame {B}
that is expressed in coordinate frame {A}, and p; is the output of the Z th GPS measurement.
Apparently, from Fig. 2, we have p; = r+A(q)e; +vp1 . `di =1,2 (2) where constant vectors e,s are the locations of the GPS antennas in the vehicle-fixed frame and vpi s represent the GPS measurement noises, which are assumed random walk noises with covariance E[vp vT.] = Rp .

The IMU 20 is equipped with an accelerometer which can be used for accelerometric measurements of gravity. In general, accelerometer output contain components of the acceleration of gravity and the inertial acceleration. In mobile robots, the level of inertial acceleration is negligible compared to the acceleration of gravity; typically maximum inertial acceleration is about 0.02g. Therefore, despite the fact that estimation of the inertial acceleration of the robot can be obtained from the GPS data, it is sufficient to model the inertial acceleration as a measurement noise in the KF. Let assume that a be the accelerometer output. Then, the acceleration equation can be written as a Ilall = A T k + v, (3) where II=II denotes the Euclidean norm, and unit vector k is defined to be aligned with the gravity vector in frame {A}, while va captures the accelerometer noise and inertial acceleration all together. We treat va as a random walk noise with covariance E[vaVa VT] = 213' Observation Equations The objective of the extended Kalman filter (EKF) is to determine the vehicle attitude and position by fusing the IMU and GPS measurements.

In principle, the attitude of a rigid body can be determined from two non-collinear position vectors each of which expressed each in two coordinate systems. Equation (3) gives the gravity vector in both frames, while the baseline vector ApSp, -p2 is the rotated version of the vector from one antenna to the other, i.e., e, -e2 . Therefore, equations (2) and (3) suffice to obtain the attitude q and the position r. However, this will lead to an inaccurate estimate because of low signal-to-noise ratio of the Op measurement. Denoting vectors Ae = el -e2 and vp = Vp2 - Vp1 , one can infer from (2) that IlAell = IIAp+v,, 11 = IIApI +Ap Vp, (5) where p= ~p ~~ p11 and the RHS of (5) is obtained by the first-order approximation of the norm of summation of two vectors. Antenna-to-antenna baseline IlDell is approximately 1 m, whereas the GPS error isabout t5 Cm. This means that the error in measurement of orientation of vector op is about 10%, which is far from desired accuracy. The two GPS observations in conjunction with the measurements of the acceleration gravity are used as external updates in an elaborate Kalman filter integrating a rate gyro data with the observation data.

In the described embodiment, the IMU signals are given at the rate of 20 Hz, whereas the GPS data can be acquired at the rate of 1 Hz. Therefore, an average of the IMU signal can be obtained between two consecutive GPS data acquisitions. This tends to decrease the IMU
noise. Therefore, the discrete-time measurement of the acceleration can be obtained by averaging through integration of the IMU signals at that interval tk -te < t<- tk , where to denotes the GPS sampling rate, i.e., ak - t f la(~)dc (5) e Therefore, the discrete-time measurement vector is defined as zk k k (6) ak/llakll In view of (2), (3) and (6), the observation vector as a function of the discrete-time variables qk and kk can be written as h A(gk )(el - e2) (7) k AT(gk)kk r Thus, the observation equation is zk = hk + Vk, where Vk = [VP'k - vp2k Vak is the overall additive measurement noise. Assuming that the noises of the two GPS receivers are not mutually corrected and that they are also not corrected with the IMU
noise, the covariance matrix of the measurement noise takes the form R 'k 03 /
Rk = E[Vkvk ] = 2 ' (8) 03 a13 where R = R + R and R = E[v vT ] is the covariance matrix associated with the Pk Pik P2k Pik Pik Pik measurement nose of the : th GPS.

The discrete-time observation vector (7) is a nonlinear function of the quaternion. To linearize the observation vector, one also needs to derive the sensitivity of the nonlinear observation vector with respect to the system variables. To this end, consider small orientation perturbations &1=q qi= (9) around a nominal quaternion 4. Now, by virtue of A(q) = k(,5q (8> q), one can compute the observation vector (7) in terms of the perturbation 8q. Using the first order approximation of nonlinear matrix function AT (Sq) from expression (1) by assuming a small rotation sq, i.e., 1l8q = 1 and &7o =1, i.e., A(Sq) =13 +2[89,,x].

To take the decomposition rules of quaternion into account, the state vector to be estimated by the EKF is defined as x =col(Sgv,b) (10) where vector b is the corresponding gyro bias. Thus, the sensitivity matrix can be written as H _ ah _ -2A[(e1-e2)x] 03 11 ax 2AT [kx]A 03 , ( ) where A=A(4) Thus, the linearized observation equation is Zk =H%k+Vk (12) Covariance Estimation of the GPS Error Efficient implementation of the KF requires the statistical characteristics of the measurement noise.
The IMU noises are not usually characterized by time-invariant covariance.
Therefore, 6Q is a constant parameter, which can be either derived from the sensor specification or empirically tuned.
The GPS noise characteristics, however, are time-varying; the errors vary depending on many factors such as satellite geometry, atmospheric condition, multipath areas, and shadow.

The covariance of the GPS antenna-to-antenna measurements can be estimated from the difference between IIApII and IIAeII, which are both the antenna to antenna baselines expressed in two different frames. The magnitude of a vector is invariant under rotation transformation, i.e., ideally IIApII = IIieII if GPS data is noise-free. However, from the error equation (4), we can infer that E[(IIAPkII-IILeI)2]=ApkTRpkAPk= (13) If the covariance matrix is assumed diagonal as R Pk 13, (14) pk Pk then a'2 =E[(II/pII-IIAeI)2]. Moreover, since GPS receivers estimate the position via the averaging process over the sampling interval, we can assume E[IIAPII] =
IIAPkII = Therefore, knowing that Le is a deterministic variable, we can find the covariance from 6pk = (I kpk l -Del )2 .

Most Real Time Kinematic (RTK) GPS receivers can estimate the confidence on their position measurements in real-time that allows us to characterize the covariance matrix more accurately.
Typically, the positions of horizontal axes are measured with the same confidence, while the height measurements are given with less confidence. Therefore, for the i-th GPS, we can say C; =diag(ch 1 .,ch 1 .,c,,), where ch and cv. are the confidence on the horizontal and vertical measurement data that are provided by the receiver in real-time. We assume that Rpi = pC2 i di =1,2, (15) where the scaling factor p is yet to be found. Substituting (15) in the RHS of (13) yields (IILPk II - IIAeII)2 P OPT (C k +C2k )ePk Thus, the covariance matrix of the GPS noise can be computed from 1 = 4 R - _( epkll- jAell)2 C2; .
p~k 1X (C k +C2k )epk k Attitude Estimation The relation between the time derivative of the quaternion and the angular velocity w can be readily expressed by . (16) q= 2w q where w=
]
The angular rate obtained from the gyro measurement is cog =w-b-Eg (17) where b is the corresponding bias vector; Eg is the angular random walk noises with covariances E[EgE;] = a213 . The gyro bias is traditionally modeled as b = Eb, (18) where Eb is the random walk with covariances E[EbEb ] = 6b 13 . Then, adopting a linearization technique one can linearize (16) about the nominal quaternion q and nominal velocity w=cog+b, to obtain d8q =-wx8q +bb+IE . (19) dt v v 2 2 g Since &q,, is not an independent variable and it has variations of only the second order, its time derivative can be ignored. Setting the dynamics equations (19) and (18) in the standard state space form, gives -t x = Fx+GE, (20) where t =col(Eg,Eb); and F= -[6)X] 113 G= 213 03 Observabilty Analysis A successful use of Kalman filtering requires that the system be observable. A
linear time-invariant (LTI) systems is said to be globally observable if and only if its observability matrix is full rank. The observability matrix associated with linearized system (20) together with the observation model (11) is r O=[ HT (HF)T ... (HFS )T I .

The states of the system are assumed to be completely observable if and only if rank 0 = 6 (21) which is equivalent to 0 having 18 independent rows.

Although the described system is not time-invariant as F and H are defined on the nominal trajectory at a frozen instant in time, examination of the LTI observability measure can still provide a useful insight into the local observability of the system at that time. To this end, we construct the submatrices of the observability matrix 0, i.e., BF = [_A[Lex1[ix1 2 A[Dex]wx] 2 A[k'x] (22) where k' = ATk is the unit gravity vector expressed in the vehicle frame and we used the identity AT[kx]A = [k'x] to obtain (22). The observability matrix can be reduced to the following block-triangular matrix by few elementary Matrix Row Operations (MRO) MRo M 03 O - O'= x , (23) where M = [k'x] [Dex] - [iex] [k'x] (24) and hence rank 0 =rank 0'. It is clear from (23) that the full rankness of the block-triangular matrix rests on the invertibility of the square matrix M. In other words, if M
is invertible, then the system is observable.

Proposition 1 If a line connecting the two GPS antennas is not collinear with the gravity vector, then system (12)-(20) is observable.

Proof: In a proof by contradiction, we show that ME R3x3 must be a full-rank matrix. If M
is not full-rank, then there must exist a non-zero vector tl ~ 03x1 such that Mrs = 03x1 (25) In view of definition (24) and the property of the vector product of three vectors {a,b,c}, a x (b x c) =(a = c)b - (a = b)c, one can easily show that LHS of (25) is equivalent to Mn = k'x (Ae x q) - Ae x (k'x q) = (k'. i)Ae - (De = i)k' # 0, (26) in which, the inequality of (26) is inferred from the fact that vectors k' and De are not parallel and hence they can not annihilate one other. Therefore, it is not possible for (25) to be true meaning that matrix M must be full rank.

Discrete Time Model The equivalent discrete-time model of (20) is Xk+1 = (bkxk +Wk (27) where Fk = t1(tk +te,tk) is the state transition matrix over time interval t.
. In order to find a closed form solution for the state transition matrix, we need the nominal values. The nominal values of the relevant states at interval tk <T <tk +te are obtained from the latest estimate update, i.e., qk(T) = qk, "Jr) = bk. Similar to (5), the nominal value of the angular velocity is obtained by averaging through integration of the IMU signals at that interval between two consecutive GPS data acquisition, i.e., wk = bk + 1 jrk Wg(~)dd. (28) to 'k-1 Therefore, the nominal angular velocity remains constant in the interval.
Under these circumstances, the state transition matrix can be obtained in closed form for constant angular velocity or for varying angular velocity remains constant in direction. Let us define Ok(a) = WkZ and 9k = IIAkIl . Then, the state transition matrix Dk(T) = 4)(z,tk +T) takes on the form:

fik (Z) _ Tlk (Z) 2 T2k (Z) (29) where the elements of the above matrix are given as 17lk(te)=13-Sln9k [ekx]+1-COSOk [0kx]2 (30) 9k 9k T2k (te) = (13 + COS92 -1 [Okx]+ 9k -sin [ekX]2)to, 9k 9k Let E. =blockdiag(6g13,6b 13) be the continuous-time covariance matrix of the entire process noise. Then, the corresponding discrete-time covariance matrix is Qk = E[Wkwk] = 1`k+t,k~(t)GEG T(DT
(t)dt, (31) rk which has the following structure Qk = Xlk QA21 (32) b A 3 For small angle 9k, where SlnOk = 9k - 6 9k and cosOk = 1-29k , the elements of the state-transition matrix (30) can be effectively simplified. Then, upon integration of the substitution of the simplified state transition matrix into (31) we arrive at r -`

AI -(62to+6bt )13+6bt [ekx]2+(6gte+~btA)[0 ]4 k 4 12 240 80 1008 Azk = 6b t2 (113 - 1 [Okx] + 1 [OkX]2 ).

Estimator Design The state transition matrix (29) is used only for covariance propagation, while the system states have to be propagated separately by solving their own differential equations.
Let us compose the redundant state aX =col(q,b), which contains the full quaternion q and parameters b. Combining (16), (18), we then describe the state-space model of the system as az=f(az,E) The EKF-based observer for the associated noisy discrete system (27) is given in two steps: (i) estimate correction Kk = Pk HT (HkPk HT +Rk)-1 (33) 1k = 1k +K(Zk -h(xk)) (34) Pk = (112 -KkHk)Pk (35) and (ii) estimate propagation rr ai +I =a 1k + J k+rof (ax(t),O)dt (36) rk Pk+l - 0kPk(Dk + `tk (37) The vector of discrete-time states, %k, which is to be estimated by the KF, contains only the variations Sgvk not the quaternion qk. However, the quaternion can be obtained from the former variables if the value of the nominal quaternion 4k is given, that is 84k = qk qk (38) A natural choice for the nominal value of quaternion is its update estimate as 4(tk-1) = 4k-1 = Since the nominal angular velocity Wk is assumed constant at interval tk-1 <_ t:5 tk, then, in view of (16), the nominal quaternion evolves from its initial value 4(tk-1) to its final value 4k = q(tk) according to:

leek qk =e2 qk-1= (39) It can be shown that the above exponential matrix function has a closed-form expression so that the above equation can be written as qk = (Cos 0k + sm 9k )qk-1 + (? sin Bk - 1CosBk )6k (&k-1 (40) 2 2 9k 2 2 2 From (38), the KF step, i.e., (34), can be written in terms of the full quaternion, qk instead of its variation Sqvk , as SQ,k = [vec(fG k)]K(zk -hk) (41) Pk Pk where 4k is obtained from (40). Finally, assuming that IlSgvk I) < 1, a valid unit quaternion can be constructed from k k qk = [_11~'kI12 g(tk) (42) Substituting (40) into (42) gives "`lvk qk 1 - j Cos B+sin Bk )Qk-1 +e sin Bk - 2 Cos Bk )0k qk-1 Il8vk 112 k (Note the last 9 in the above equation should be in bold) Initialization of the Kalman Filter For the first iteration of the EKF, an adequate guess of the initial states is required. The best guess for the parameters at t = 0 s is = 03, while the initial orientation of the vehicle with respect to the inertial frame at t = 0 s is not known beforehand. It is that presumed IIBq,,II <_ 1. If this condition is violated, the error quaternion will not be unit norm. To prevent this from happening, it is important to keep the initial error in quaternion estimate small as much as possible.

Mathematically, the rotation matrix can be computed from two non-collinear unit vectors k and Op and their rotated versions a ma/IIalI and Ai 5. Similar to the methods for registration of 3-D
laser scanning data, let us form the following matrices from the units vectors as D. = [k Op kxAP] (43) Db = [a 05 "x051 (44) Then, in the absence of measurement noise, i.e., v = 0, the above matrices are related by Da = ADb (45) Matrices Da and Db are non singular as long as k and Op are not collinear, i.e., the line connecting the GPS antennas is not parallel to the gravity vector. Then, under this circumstance, the rotation matrix can be obtained from A = D,,Db' (46) Solution (46) yields a valid rotation matrix A so that ATA =13 , only if there is no error in the column vectors of matrices (45). However, due to the IMU and GPS noises, a valid rotation matrix may not be obtained from solution (46). To correct this problem, one may observe that all singular values of any orthogonal matrix must be one. This means that the singular value decomposition of the RHS of (33) yields DaDb' = UEVT , where U and V are orthogonal matrices and matrix E = 13 +A1 is expected to be close to the identity matrix, i.e., IIAEII < 1. Therefore, by ignoring small matrix A,:, a valid solution to the rotation matrix can be found as A = UVT, (47) which then can be used to obtain the equivalent quaternion at t = 0 s.

Position Estimation With the estimation of attitude in hand, one can use the two sets of position and orientation equations (4) to compute r. Despite an estimate of r can be obtained from one set of equations, a better estimation can be achieved by solving two equations together if the measurement noise characteristic is known.

Setting the two GPS equations (4) in the matrix form, we get y = Lr+np +nq (48) none where jpi_Aei , L= [13 , P2 -Ae2 13 while GPS measurement errors and attitude estimation errors are respectively represented by vectors v _ [elx]
n p = V PI
1 P2 PI nq = -2A I[e2X] Sqv Using the argument that mutual correlation between np and nq is negligible, we can write the covariance matrix of the entire noise in (48) as:

R, = E[npnp]+E[ngnT ] (49) Rpl +4A[elx]Pq[elx]AT 4A[elx]Pq[e2x]AT
4A[e2x]Pq[elx]AT Rp2 +4A[e2x]Pq[e2x]AT

where Pq = E[64,,64 ]E R'. Since the Kalman filer gives the covariance matrix of all estimated states including the quaternion, one can get Pq from the filter covariance matrix as E f~ p 6x6.
P = [Pq X
X x Then an optimal solution to (48) can be obtained by the weighted pseudo inverse where a suitable weighting matrix is the covariance matrix of the noise. That is (LTR;'L)-'LTR~y, P, -Aei (50) P2 - Ae2 Using (48) in (50) yields (LTRr'L)-'LTR,' If the orientation estimation error is sufficiently small, i.e., the second term in RHS of (49) is negligible, then (50) can be conveniently written as i = W,(P~ -Ae,)+W2(P2 -Ae2), where the weighting matrices are defined as W, LRp2 (Rpl +Rp2 )-' (52) W2SSRpl (Rpl +Rp2 )-'. (53) It is worth noting that W1 + W2 = 13. This means that the estimator (51) proportionally puts more weight on that GPS measurement with the smallest covariance matrix in order to estimate the position.

Experimental Verification The test vehicle was equipped with three RTK GPS antennas as shown in Figure 3. Although only two GPSs are required by the adaptive KF, having three GPSs allowed measurement of the vehicle attitude purely from a kinematic relation. Despite its poor accuracy, the three-GPS attitude determination method does not introduce any attitude drift. Therefore, it was possible to investigate whether or not the pose estimation method based on fusing two GPSs and an IMU
exhibits any drift.

Assume that p3 and e3 denote the third GPS measurement and the location of its antenna on the vehicle, respectively. Also, denote Op' = P1 _P3 and De'= el -e3 and N. = [Op Op' Ap x Op']

Nb = [De De' AexAe'] (54) where identity Na = ANb holds in the absence of GPS measurement noise.
Therefore, in a development similar to (40) -(47), one can calculate the rotation matrix from the above matrices.

A rover equipped with three RTK GPS receivers along with satellite antennas and radio modems (model Promark3RTK from Magellan Navigation Inc.) in addition to an INIU
device from Crossbow Technology, Inc. (model IMU300) was used for experiments. Experiments were conducted on the 30 x 60 m Mars Emulation Terrain (MET) of the Canadian Space Agency. The locations of the GPS
antennas expressed in the vehicle-frame are shown in the following table. The additional GPS
receiver was provided to permit estimation of the vehicle pose indpendently of the IMU
measurements.

Table x(m) y(m) z(m) el -0.452 0.604 -0.252 e2 -0.452 -0.616 -0.224 e3 -0.427 -0.003 -0.249 An operator sent a pre-scheduled sequence of primitive commands to the mobile robot-e.g., "move forward of a certain distance", "rotate clockwise by 45 ", etc-so that the robot follows a preplanned path going through some specified via points. Fig. 4 shows the 3-D path taken by the mobile robot.
The time-histories of the IMU measurements and GPS measurements are depicted in Figs. 5 and 6, respectively, while the time-history of the confidence on the GPS data is shown in Fig. 7.

The Two-GPS-IMU fusion method was compared with One-GPS-IMU method. Although it was not possible to obtain ground truth attitude and position information during the test, it was possible to obtain an estimate of the attitude without any drift and a fairly reliable estimate of position by processing the data acquired from the three GPS receivers.

Fig. 8 shows that the Kalman Filter can quickly identify the parameters, i.e., gyroscope bias and the direction cosines of the gravity vector in the inertial frame. Trajectories of the vehicle position and attitude based on the Two-GPS-IMU fusion method and One-GPS-IMU method versus the results of the geometric pose estimation process based on the data of the three GPS
receivers (used as the reference) are plotted in Figs. 9 and 10, respectively. The errors between the estimated and the reference pose trajectories are calculated by Position Error= rref II

Orientation Error = 2 sin' Ilvec (q of O 411 and the results are plotted in Figs. 11 and 12. It is clearly evident from Fig. 11 that the attitude estimation error using one GPS and IMU accumulates gradually due to gyro drift, whereas the Two GPS-IMU method exhibits no drift. As shown in Fig.12, the position estimation based on one GPS
also exhibits drift, which is the echoed attitude drift. Furthermore, it is apparent from the figure that the position estimation based on one GPS measurement is not accurate at the time around t = 400 s, because the error margin of the first GPS receivers during that period is incidentally high, as shown Fig. 6. Both drift and spike in the position errors are substantially reduced in the Two-GPS-IMU
estimation method.

The described system can be used for outdoor mobile robots for terrestial applications and to provide a ground-truth pose estimation of a vehicle during testing. The system is also applicable to applications on other worlds, such as the Moon or Mars, where low power pseudolites can be substituted for orbiting satellites.

Claims (33)

1. A method of determining positional information about a vehicle, comprising:

computing estimates of the covariance matrices of Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS) measurement data obtained by at least two GNSS receivers mounted on the vehicle; and fusing the RTK GNSS measurement data according to their corresponding covariance matrices to obtain enhanced positional information.

2. The method of claim 1, wherein an antenna-to-antenna baseline is defined between the GNSS receivers, and the estimates of the covariance matrices are based on the error in the magnitude of a measurement of the antenna-to-antenna baseline and the confidence in the GNSS
receiver measurements.

3. The method of claim 2, wherein the antenna-to-antenna baseline is not collinear with the vector of gravity.

4. The method of any one of claims 1 to 3, wherein the measurement data from the RTK GNSS
receivers is fused with measurement data from an inertial measurement unit (IMU).

5. The method of any one of claims 1 to 4, wherein the IMU includes an onboard accelerometer and rate gyro.

6. The method of any one of claims 1 to 4, wherein the IMU includes an onboard inclinometer and rate gyro.

7. The method of any one of claims 1 to 6, wherein the measurement data is fused in a Kalman filter.

8. The method of claim 7, wherein the positional information comprises attitude information obtained from the Kalman filter.

9. The method of claim 8, wherein the attitude information is derived from the quaternion

10. The method of claim 8, wherein the positional information also comprises localization information.

11. The method of claim 10, wherein the localization information is derived from the equation where the weighting matrix is defined as

12. The method of any one of claims 1 to 11, wherein the covariance matrix of the GNSS
measurement noise is derived from equation

13. The method of claim 7, wherein the initial states of the Kalman filter are determined from difference between the two GNSS measurements and the accelerometer measurement.

14. The method of claim 5, further comprising computing an observation vector from measurements obtained from the accelerometer and the GNSS receivers, and applying the observation vector, the estimates of the covariance matrices and measurements obtained from the onboard rate gyro as inputs to a Kalman filter to determine the attitude of the vehicle.

15. The method of any one of claims 1 to 14, wherein at least three GNSS
receivers are mounted on the vehicle and a pose estimation is obtained from measurements from the three GNSS
receivers.

16. The method of any one of claims 1 to 15, wherein the GNSS receivers compute their ranges with respect to a stationary GNSS base station.

17. An apparatus for determining positional information about a vehicle, comprising:

at least two GNSS receivers for mounting on the vehicle and defining an antenna-to-antenna baseline therebetween; and a processor configured to compute estimates of the covariance matrices of Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS) measurement data obtained by at least two GNSS receivers mounted on the mobile robot, and to fuse the RTK GNSS
measurement data according to their corresponding covariance matrices to obtain enhanced positional information.

18. The apparatus of claim 17, wherein an antenna-to-antenna baseline is defined between the GNSS receivers, and the estimates of the covariance matrices are based on the error in the magnitude of a measurement of the antenna-to-antenna baseline and the confidence in the GNSS
receiver measurements.

19. The apparatus of claim 17 or 18, wherein the processor is configured to fuse measurement data from the RTK GNSS receivers with measurement data from an inertial measurement unit (IMU).

20. The apparatus of claim 19, wherein the IMU includes an onboard accelerometer and rate gyro.

21. The apparatus of claim 19, wherein the IMU includes an onboard inclinometer and rate gyro

22. The apparatus of claim 19, wherein the processor comprises a Kalman filter.

23. The apparatus of claim 22, wherein the Kalman filter is configured to produce attitude information about the vehicle.

24. The apparatus of claim 23, wherein the attitude information is derived from the equation

25. The apparatus of claim 23, wherein the positional information also comprises localization information.

26. The apparatus of claim 25, wherein localization information is derived from the equation where the weighting matrix is defined as

27. The apparatus of claim 22, wherein the initial state of the Kalman filter is determined from the difference between GPS measurements and the accelerometer measurement.

28. The apparatus of claim 20, comprising a module for computing an observation vector from measurements obtained from the accelerometer and the GNSS receivers, and a Kalman filter receiving as inputs the observation vector, the estimates of the covariance matrices and measurements obtained from the onboard rate gyro and outputting the attitude of the mobile robot.

29. The apparatus of claim 17, comprising at least three GNSS receivers mounted on the mobile robot, and wherein the processor is configured to obtain a pose estimation from measurements from the two GPS receivers.

30. The apparatus of any one of claims 17 to 29, wherein the GNSS receivers are configured to compute their ranges with respect to a stationary GNSS base station.

31. An apparatus for determining positional information about a vehicle, comprising:

at least two GNSS receivers for mounting on the vehicle and defining an antenna-to-antenna baseline therebetween;

an inertial Measurement Unit (IMU) for obtaining data about the movement of the mobile robot; and a processor configured to and to fuse the RTK GNSS measurement data and the data from the IMU to obtain enhanced positional information.

32 32. The apparatus of claim 31, wherein the processor is configured to compute estimates of the covariance matrices of Real Time Kinematic (RTK) Global Satellite Navigation System (GNSS) measurement data obtained by at least two GNSS receivers mounted on the vehicle and to fuse the RTK GNSS measurement data according to their corresponding covariance matrices.

33. The apparatus of claims 31 or 32, wherein the vehicle is a mobile robot or a walking robot or a humanoid robot.