A dynamic model for GPS based attitude determination and testing using a serial robotic manipulator.

A computational algorithm is developed for estimating accurately the attitude of a robotic arm which moves along a predetermined path. This algorithm requires preliminary input data obtained in the static mode to yield phase observables for the precise, 3-axis attitude determination of a swinging manipulator in the dynamic mode. Measurements are recorded simultaneously by three GPS L1 receivers and then processed in several steps to accomplish this task. First, artkconv batch executable converts GPS receiver readings into RINEX format to generate GPS observables and ephemeris for multiple satellites. Then baseline vectors determination is carried out by baseline constrained Least-Squares Ambiguity Decorrelation (LAMBDA) method that uses double difference carrier phase estimates as input to calculate integer solution for each baseline. Finally, attitude determination is made by employing alternatively Least-squares attitude determination (LSAD) in the static mode and extended Kalman filter in the dynamic mode. The algorithm presented in this paper is applied to recorded data on Mitsubishi RV-M1 robotic arm in order to produce attitude estimates. These results are confirmed by another set of Euler angles independently evaluated from robotic arm postures obtained along the predefined trajectory.

Introduction

Double-differenced carrier phase measurements might be utilized for GPS-based high precision attitude determination of the object, and many techniques can be introduced by making use of integer ambiguity resolution. A lot of methods are proposed in this area of research and more recent ones make use of the LAMBDA, which is an abbreviation of Least-squares AMBiguity Decorrelation Adjustment method. Standard LAMBDA method can only be used for unconstrained and/or linearly constrained GPS models; however, a baseline constraint is nonlinear. So, most of the given methods take advantage of this additional information by optimizing a search space and checking whether or not the candidate ambiguities correspond to the given baselines. Although that kind of procedures improves the effectiveness of integer ambiguity resolution, the methods considered are still not able to achieve very high robustness, because formerly available information is not entirely brought into the ambiguity resolution procedure. Moreover, the success rate of ambiguity resolution diminishes significantly when implemented in a single frequency, epoch by epoch attitude determination systems [1]. For single epoch attitude determination, Park and Teunissen [2] and Teunissen [3] in their works propose two nonlinear constrained integer least square methods and relevant searching strategies. Both methods bring nonlinear constraints into an ambiguity objective function, the first method is related to LAMBDA ellipsoidal search area, while the second method is related with a more precise but complicated non-ellipsoidal search area. There are other more recently introduced methods, employed for constrained attitude determination solution, which are Least-Squares Ambiguity Solution Technique (LSAST) method [4], and Multivariate Constrained Approach [5], [6]. Furthermore, a comparative analysis between methods of baseline constrained Least-Squares Ambiguity Decorrelation (BC LAMBDA) and multivariate constrained Least-Squares Ambiguity Decorrelation (MC LAMBDA) is provided in Bing et al. [7]. Whilst they enhance the success rate to a higher level as compared to unconstrained models, it may be not sufficient for implementing in the competitive application. As the same with methods mentioned before, they mostly employ the baseline length as preliminary information. One of the approaches to fully integrate prior information is the addition of an appropriate dynamic model of the body that can be used to predict and correct ongoing angular estimates in the real time mode.

This study emphasizes the real-time requirement of attitude determination of the moving object and propose a reliable single frequency, epoch by epoch attitude determination algorithm constrained by baseline lengths and an auxiliary dynamic model of the platform. This algorithm utilizes prior information of baseline lengths and the predictive model of the rotating platform based on its potential trajectory and angular rates of rotation. This article is structured in the following way. Firstly, the paper discusses in detail the mathematical model and methods used to solve the given problem of attitude determination. Secondly, an experimental setup constructed for testing and verification of the developed algorithm is presented. Then, an overview of the corresponding software designed to implement the above mathematical model described. Finally, results of the static and dynamic tests that indicate good performance of the proposed mathematical algorithm are presented.

Methodology

This paper proposes an the algorithm for attitude determination by computing phase differences of the GPS L1 signal obtained at three antennas. It is known that carrier phase GPS readings are subject to the issue of undesirable cycle slips. Once there is a cycle slip in any receiver because of not sufficient signal to noise ratio or loss of track by the signal processing unit, the error will increase and diminish precision of the angular solution. So the double difference measurements are used to furnish this GPS-based attitude determination approach. In order to determine the attitude from GPS carrier phase measurements, ambiguities have to be resolved. This means that one should search for correct carrier phase integer ambiguity values. This procedure is the most important part of accurate GPS-based attitude determination.

In this article, an equation which outlines the relationship among integer ambiguity, the attitude, and double difference carrier phase observables is derived in the first step. Then integer ambiguity resolution by means of BC LAMBDA method is carried out to enhance the float solution obtained in the previous step. Finally, attitude determination is conducted by extended Kalman filter that uses integer solution for each baseline as input to calculate Euler angles of the body based on the dynamic model of its movements. The overall technique is configured to deal with integer ambiguities which can be computed incorrectly because of the scarcity of carrier phase data accumulated by single frequency GPS receivers.

Baseline constrained LAMBDA method

For the problem of GPS-based attitude determination, preliminary information such as the length of the baseline is known and does not change. Therefore the baseline constrained integer ambiguity resolution can take advantage of the typical GNSS model by incorporating the length constraint of the baseline where is known. Then observation equations are transformed into [8]:where y is the vector of observed minus estimated double differences of carrier phases of the order m, a is the unknown vector of ambiguities given in cycles rather than range to preserve their integer nature, b is the baseline vector, for which the length is considered to be known and constant in attitude determination problems, B is the geometry matrix consisting of normalized line-of-sight vectors, and A is a design matrix which bounds the measurement vector to the unknown vector a. The variance matrix of y is determined by the positive definite matrix which is supposed to be known, E is the mathematical average or the estimated value, and D is the variance of y.

Utilizing this transformation, the least squares principle states:where is the least squares solution for b, taking into account that a is known.

Numerous methods are already developed to solve this quadratically constrained least-squares problem. In this article, uses a method based on the repeatable computation of orthogonal projections onto an ellipsoid depicted in Teunissen [5]. Then the search for the integer least-squares ambiguity vector in the integer search area will be conducted:

It should be outlined here that this search area is not ellipsoidal anymore because of the inclusion of the residual baseline term.

To implement the baseline constrained technique, first the potential integer vectors within the search area is calculated using the typical LAMBDA method

This search area comprises of all integer vectors of and thereby the vector which is currently under consideration. In order to constrain the search area, it is necessary to employ only those integer ambiguities that meet the condition:

The search area should be as small as possible to be able to complete the process in an acceptable time which is essential for real-time applications. Nevertheless, in order to ensure that the search area can provide correct solutions, the search area should not be selected too small.

The computational process of can also be very time-consuming, in the case, it is required to be carried out for numerous integer candidates a. But if the search area is changed, it is possible to avoid this by employing the smallest and largest eigenvalues of . So that it can be proved that the initial search area should be smaller than a search area based on the largest eigenvalue and larger than a search area based on the smallest eigenvalue [9]. In this research, one will employ similar approach as was utilized in Park and Teunissen [10], where the search area is set as the smallest eigenvalue plus a value which is expanded until the search area can provide solutions.

Least-squares attitude determination (LSAD)

In the GPS-based attitude determination systems, Euler angles of the body represent the mismatch between the ABF (antenna body frame) and the LLF (local level frame), so Helmert formula might be stated as [11]:where is the rotation matrix from ABF frame to LLF frame; α implies the three-dimensional attitude angles; and are denoted to be baseline vectors from antenna n to antenna 1 (master antenna) referred to ABF and LLF, respectively. For instance, is denoted as the coordinate of antenna 2 in ABF in regard to the master antenna (that is the origin of ABF), that can be depicted as . A standard model of attitude determination should be firstly presented to compute a rotation matrix that provides minimization of least-squares residuals [12]:where is the weight of every baseline vector from the LLF to the ABF.

In order to employ the least-squares method to the model (2), this model firstly has to be transformed into linear form near the attitude parameters. Taking into consideration the measurement errors, Eq. (1) might be expressed as:where and represent the measurement errors of and , accordingly; the unknown attitude parameters estimates are integrated into the rotation matrix . Linearization of the right-hand side in regard to the attitude parameters produces:where y, r and p are short-hand representations for yaw, roll and pitch angles, accordingly.

Employing this equation, the least-squares adjustment can be carried out near the initial values of attitude parameters:where is an estimated Direction Cosine Matrix (DCM) defined by the initial values of Euler angles , and ; is the measurement vector; is the design matrix; is the correction values of the attitude parameters estimates, and is the measurement error vector. The equation specified in (5) is only true for a single baseline vector which consists of two antennas. Presuming that the system consists of n antennas, there are antenna baseline vectors with regard to the master antenna (antenna 1). Thus, (5) can be extended to all baseline vectors:where z is the measurement vector; A is the design matrix; V is the measurement error vector. The model (6) is based on the Least-Squares Attitude Determination approach (LSAD) [13]. The correction values for attitude parameters related to a rotation matrix are determined by:where the short-hand representation Cov(·) indicates the error covariance matrix; P is the weight matrix defined by the measurement errors. The least-squares adjustment continues until the correction values converge to a specific threshold or the maximal iteration parameter is reached.

The LSAD is targeted to compute misalignment of the ABF in regard with the LLF utilizing GPS carrier phase measurements. The measurement errors in both LLF and ABF are also taken into consideration. If the attitude dynamics may be characterized by an appropriate mathematic model or measured by other sensors, the LSAD model can be further incorporated with the attitude dynamics employing appropriate data combination methods, for example, Kalman filters, where the LSAD model acts as the measurement model.

An extended Kalman filter for dynamic attitude determination

If the object of attitude determination system is configured for repetitive rotational motion, then a constant angular rate model to approximate its attitude dynamics [14] can be used. According to this model, the state vector is composed of not only the attitude parameters (yaw, pitch, and roll) but also their respective angular rates:

The random-walk process based on discrete time can be written as:where x is the state vector to be evaluated; k is the time variable; F indicates the state transition function; G is the noise amplification function, and w indicates the operational noise. In order to estimate the operational noise, a one-dimensional relation with respect to the yaw angle can be considered to make it simple. When adopting a constant angular rate model, angular acceleration should be treated as the noise; therefore it is also referred to as the white noise acceleration model [15]. The angular acceleration is related to the angle and angular rate in a discrete-time variable by the following way:where is the sampling interval. Expanding (10) to three-dimensional attitude parameters and not taking into account the correlation between different angular accelerations of attitude parameters, following can be derived:where is a covariance matrix of the process noise . In order to determine Q, a priori knowledge of the standard deviation of each angular acceleration is required. The algorithm of an extended Kalman filter (EKF) begins from the linearization of the measurement and dynamic models. At epoch k, the dynamic model is made linear around posterior state estimates at last epoch (denoted by ) and the measurement model is made linear around prior state estimates at current epoch (denoted by ), so that the nonlinear equations f(·) and h(·) can be substituted by Jacobian matrices F and H, accordingly. The measurements update and time update procedures can be written in the following way [16]:

Initialization:

Time update:

Measurement update:where P indicates the covariance matrix of the given states; K is the Kalman amplification; R is the measurement error covariance matrix; s is the update vector. The relation (18) is the Joseph stabilized form of covariance measurement update. Furthermore, this equation can take simpler form in the following way:

Both relations are virtually interchangeable in the theory of mathematical analysis. Yet (18) ensures that is generally symmetric positive definite, as far as is symmetric positive definite [16]. In practical applications, the Kalman amplification may have a number of errors from the computation, for instance, because of the constrained computational compatibility of the processor. In (19), the calculation error of Kalman amplification may cause to be non-positive definite, even if the is symmetric positive definite. Due to these reasons, (18) is steadier to the Kalman amplification errors, so this relation can improve stability. While employing a triple-antenna system, the following parameters for the EKF can be used:where is the rotation matrix evaluated at the a priori estimates of attitude parameters. Here from and should be distinguished, defined in the previous section of the article. It can be noted from the form of H that the measurement model of the Kalman filter virtually is very similar to the LSAD approach presented earlier. The measurement errors are accumulated in both ABF and LLF. The antenna position in the ABF might be accurately defined during the calibration procedure, and then antenna position errors obtained in the LLF should be the dominant errors. The error covariance matrix of the antenna position in the LLF might be derived by taking into consideration the error propagation from the GPS measurement domain to the position domain. Assuming that the error covariance matrix of antenna 2 and antenna 3 are already derived, T can be written as:where and stand for LLF coordinates of both slave antennas, accordingly; is a multiplication factor which is a bit larger than 1, because R is usually computed in more direct way to counter the filter divergence; indicates that the computation is made at the first epoch. In the case of a short trajectory, the matrix R might be regarded as a constant value computed at the starting epoch. In the case of a long trajectory, the matrix R might be evaluated in a more exact way that is either epoch by epoch or by dividing the measurements into specific time intervals.

In comparison with the LSAD stated in the previous section, introducing the EKF with a dynamic model can improve the performance in at least the following cases. These cases may take place when the GPS observation is lost or cycle-slip occurs, while the phase ambiguity resolution or cycle-slip correction cannot be accomplished within one epoch. Consequently, the GPS phase measurements may not be used and the dynamic model is the only source to obtain the attitude information.

Experimental setup

In order to test the performance of the developed algorithm, a set of experiments were conducted in this study. Three GPS sensors U-blox AEK-4T single-frequency low-cost receivers (each sensor costs approximately US$350) were used in the experiments. Three corresponding antennas were mounted in a T-shape platform that was firmly attached to the gripper of a serial robotic arm Mitsubishi RV-M1 (see Fig. 1). Every side of this platform was approximately 1 m.

Fig. 1

Antenna platform attached to the robotic arm.

At the first stage of the experiments, GPS data were gathered in static mode for two hours for preliminary determination of the baseline lengths between antennas with millimeter-level precision. At the second stage, the manipulator’s end effector altered its attitude at a predefined and constant rotation rate and moved along certain trajectories. The rotations along yaw, roll, and pitch angles in the manipulator’s reference frame were modeled beforehand on Movemaster EX Simulator as shown in Fig. 2, Fig. 3, Fig. 4. Just before starting of these three dynamic tests, corresponding experiments in static modes were carried out to provide the initial attitude angles which were used in the verification analysis afterward.

Fig. 2

Simulation of the robotic arm movement along pitch angle.

Fig. 3

Simulation of the robotic arm movement along roll angle.

Fig. 4

Simulation of the robotic arm movement along yaw angle.

Based on the simulation models described above, and the dynamic experiments were conducted, such that the rotation of antennas was controlled by the robotic arm. In this experiment, the platform mounted with antennas rotated with a constant angular rate. The GPS data was collected at 1 Hz sampling rate. The LSAD and the EKF were used for static and dynamic modes accordingly to calculate the yaw, roll and pitch angles. Integer ambiguities were computed as a result of static experiments which were used as input to the algorithm for dynamic attitude determination.

The following parameters were used to tune the extended Kalman filter. The standard deviation of carrier phase noise was 2 mm. The matrix R was calculated based on the satellite geometry and the position of the master antenna at the starting epoch. The yaw, roll, and pitch estimates were initialized using the LSAD results at the first epoch. The initial angular rates were calculated by averaging the between-epoch variations of yaw, roll, and pitch angles during the entire static observation session.

Software design

A piece of software on MATLAB has been implemented to process the data from the mounted GPS antennas. This software is used for post-processing of the RINEX data, which was obtained by converting corresponding navigational files output by the GPS receivers.

Attitude determination starts with the data synchronization. The GPS measurements from all three antennas at the same receipt epoch are processed to carry out the data synchronization. The position of the master antenna should be calculated prior to performing the differential positioning. If there is no available ground base station for differential correction, the position of the master antenna can be computed by single-point positioning (SPP). Although the related positioning accuracy can be around several tens of meters with Selective Availability turned off, it will only affect the positioning accuracy of slave antennas at millimeter level. It implies that the accuracy of the attitude parameters will not be significantly degraded due to the application of SPP to the master antenna.

Having known the position of the master antenna, the differential positioning can be performed in order to derive the baseline vectors between antennas by making use of the LSAD method. For this purpose, the integer phase ambiguities should be resolved first by employing the baseline constrained LAMBDA method. In order to obtain initial values as the input of the LSAD method, the Euler angles can be firstly computed by employing the direct attitude computation method.

In order to apply the extended Kalman filter, the antenna body frame (ABF) should be fixed at first. With two or three antennas, the ABF can be determined based on the simple geometric formulas, whereby the length of each master-slave antenna pair should be accurately measured. Note that the ABF needs to be fixed only one time before the rotation along any orientation angle starts.

The flowchart for the data processing in a GPS-based attitude determination system is illustrated below (see Fig. 5).

Fig. 5

Flowchart of the algorithm.

Experimental results

Good results were obtained at the first stage (static mode) in the computation of yaw and pitch angles, as shown in Fig. 6. Note that instead of extended Kalman filter, Least Squares Adjustment method was used for this case because it was not possible to utilize the dynamic model.

Fig. 6

Computation of yaw and pitch angles in static mode.

Fig. 7 presents the difference in dynamic mode between yaw angles calculated by the developed software and the corresponding verification model. The model benefited a lot from the nearly constant angular rate of the platform’s rotation.

Fig. 7

Verification of yaw angle determination in dynamic mode.

Fig. 8 shows a comparison between pitch angles computed by extended Kalman filter implemented in the algorithm and the verification model for the dynamic mode. An increased error in attitude determination for this case might be caused by slight bending of the antenna platform during the pitch dynamic test.

Fig. 8

Verification of pitch angle determination in dynamic mode.

Unfortunately, demonstrative results from the roll angle dynamic experiments were not acquired due to multiple data gaps in GPS readings from two antennas. These data interruptions took place because of carrier phase cycle slips caused by the particular rotation plane of the platform that partially prevented a line of sight with navigational satellites.

Conclusions

This paper has presented a way to resolve 3-axis attitude using three inexpensive single frequency GPS antennas, despite a dynamic environment and a low measurement sampling rate in signal reception which is 1 Hz. The 3-axis attitude estimation algorithm solves a mixed real/integer problem by taking advantage of given useful information such as baseline lengths and preliminary construction of the dynamic model. The efficiency of this paper’s algorithm is demonstrated by application to a set of data from a serial robotic arm experiment with mostly rotational motion. Despite some signal gaps, the raw GPS data is successfully processed by implementing baseline constrained LAMBDA method to produce accurate double differences of carrier phases and baseline vectors. These baseline vectors are input to the extended Kalman filter based on the constant angular rate model that solves for the real-valued attitude solution simultaneously. The results were reasonable when compared with those from a verification model constructed based on preliminarily assigned movements of the manipulator’s end effector, although there still looks to be some sort of time tagging or coordinate frame definition error.

The goal of this work was to develop GPS-based attitude determination algorithm based on the predefined dynamic model. The performance of this technique was tested in a series of experiments by using a serial robotic arm. Finally, the success rate and accuracy of this algorithm can be improved by incorporating appropriate cycle slip repair method for post-processing of the occurred data gaps.

Conflict of interest

The authors have declared no conflict of interest.

Compliance with Ethics Requirments

This article does not contain any studies with human or animal subjects.