1. Introduction
Determining the attitude of underwater robots is one of the major research areas in the robotics field, because an underwater robot requires attitude information to estimate its location and control its motion [
1]. The rotation and attitude with respect to a reference can be described in terms of Euler angles, quaternions, rotation matrices, rotation vectors and angle-axis representations. This paper proposes an attitude estimation approach that uses a new method called the sine rotation vector to represent rotations. If exteroceptive measurements of range to beacons and communication between the robot and beacons are available [
2,
3], it is possible to estimate the location without the information on attitude. However, for long-range navigation, it is practically not feasible to implement the system for range measurements or a communication network. Therefore, most underwater navigation methods require attitude data.
A vast number of studies have focused on the attitude estimation and navigation of underwater robots. The major approaches employ the following methodologies: methods using range measurement from acoustic beacons, methods integrating velocity over time, methods using landmarks or features of the environment, methods using range and bearing measurements and cooperative navigation. With the development of sensor technologies and data-fusion theory, methods that fuse a variety of heterogeneous sensors and theories have recently been developed. The navigation methods for underwater robots comprise aspects that are similar to the methods used for ground-robot navigation, whereby the multiple measurements of different features are fused [
4,
5].
The Kalman filter approach is used in many methods designed for the attitude estimation and navigation of underwater robots. The nonlinear dynamics, forces, moments, position and velocity of the robot are fused for the estimation through a nonlinear observer and an extended Kalman filter (EKF) [
6]. A method that uses a robot kinetic model has also been proposed [
7]. This method, which can also be used to estimate sea currents, makes full use of the measurements from inertial navigation systems (INS) or attitude heading and reference systems (AHRS), ultrashort base line (USBL) systems and a Doppler velocity log (DVL) for the purpose of navigation. Another similar approach that uses robot dynamics for navigation, integrating low-cost INS and USBL systems, has also been reported [
8]. Notably, an adaptive dynamic model and an EKF are fused for the autonomous navigation of underwater robots [
9].
A navigation method that also estimates the sensor parameters has been proposed. The method uses an inertial measurement unit (IMU), a DVL, a magnetic compass and a depth sensor, all of which constitute a typical set of navigation sensors [
10]. Similar to the method described in [
7], a method that concurrently estimates the location of a robot and the sea level has also been proposed [
11]. Besides the popular Kalman filter-based approaches [
12], some methods adopt particle filters (PFs) for the estimation [
13,
14].
Interference of the magnetic field is one of the most critical issues when using a magnetometer for attitude estimation. Though many methods have investigated the estimation of the bias of the magnetic field measurement [
15], it is still hard to estimate and compensate for the bias because of the following reasons: the bias and interference depend on the environmental aspects, which are varying both with time and place; and it is in all directions, that is the bias is in the directions of north, east and Earth-center.
The proposed method does not explicitly estimate and compensate for the bias and interference of the magnetic field measurement. Nevertheless, the sine rotation vector method reduces the effect of bias and interference. The method uses only the northward direction of the magnetic field, and ignores the east and Earth-center direction components. Therefore, it is not affected by the bias and interference in these two directions. Likewise, the method uses only the Earth-center direction of the acceleration, thus eliminating the interference affecting the acceleration measurement toward the north and east.
This paper proposes a new approach called the sine rotation vector method, whereby an EKF is applied to estimate the attitude of an underwater robot. In some of the previous methods, the measurement innovation or residual is calculated by simply subtracting predicted Euler angles from the measured Euler angles in the correction step; however, it should be noted that physical rotation is not linearly related to Euler angles.
Euler angles can be used for the representation of both the rotation and attitude. However, the difference of two attitudes represented in Euler angles does not necessarily represent the Euler angles needed for the rotation between the attitudes. It is generally understood that the relationship does not hold, where is the rotation matrix with the roll φ, pitch θ and yaw ψ. To avoid the problem, the sine rotation vector is used to represent the rotation instead of the direct difference of Euler angles.
In general, correspondence between the Euler angle difference and the physical rotation is not uniquely determined. The difference between the Euler angles that represent two different attitudes does not indicate the rotation between the two attitudes. Let and indicate two attitudes that are represented by Euler angles. The rotation from attitude to attitude is not necessarily the rotation that is indicated by the Euler angle difference calculated by subtracting from . The difference between the Euler angles is therefore not a legitimate representation of the measurement innovation; that is, the difference between the Euler angles does not indicate the rotation needed to adjust the predicted attitude for the Kalman filter correction step. The difference between the Euler angles of the measured attitude and the predicted attitude does not represent the physical relationship between the measured attitude and the predicted attitude. To remove the ambiguity in the calculation of the measurement innovation, the proposed method computes the innovation by calculating the sine rotation vector from the predicted attitude to the measured attitude and then converting the sine rotation vector to Euler angles. This method produces a unique and physically-consistent measurement innovation that corresponds to the pair composed of the measured attitude and the predicted attitude.
Quaternions and SO(3) matrices have been used for attitude estimation [
16,
17,
18]. The quaternion and SO(3) matrix uniquely determine the rotation required to rotate the predicted attitude to the measured attitude as the sine rotation vector does. The problem to be addressed in using the quaternion is keeping the magnitude of the quaternion to one [
19,
20]. In the stage of prediction and correction, the magnitude of the quaternion deviates from one. Therefore, it is required to use some subterfuge to normalize the quaternion. Likewise, the SO(3) matrix should have the property of orthogonality. Therefore, the methods using the SO(3) matrix inevitably use some method to approximate the predicted and corrected matrix to some orthogonal matrix [
19,
20]. The use of the sine rotation vector does not require approximation for the normalization of vectors or the orthogonalization of matrices.
The attitude estimation problem studied in this paper is formulated in
Section 2.
Section 3,
Section 4 and
Section 5 describe the attitude estimation procedure.
Section 6 shows the results of experiments and simulations using the proposed method and the previously-reported methods and presents a comparison of these results.
Section 7 concludes the paper and suggests further research regarding the proposed method.
5. Innovation by Sine Rotation Vector
5.1. Calculation of Innovation by Sine Rotation Vector
The innovation
is given by Equation (
11), as follows:
The innovation is the difference between the measured attitude and the presumed measurement that is calculated under the assumption that the robot is in its predicted attitude . Many existing methods calculate the measurement from the acceleration and magnetic field , whereby the predicted attitude is used as the presumed measurement .
This paper proposes a new approach for finding the innovation, which is the difference between the measured attitude
and the presumed attitude
. The proposed method finds the sine rotation vector between the presumed attitude and the measured attitude. Because vehicle acceleration is negligible compared to gravity, the measured acceleration
is directed toward the
direction of the Earth-fixed coordinate frame; that is, the acceleration measurement
is the Earth-fixed
direction vector that is represented in the instrument coordinate frame. Let the presumed
direction represented in the vehicle coordinate frame be denoted by
. The presumed
direction vector
is calculated according to Equation (
12), as follows:
where
is the rotation matrix from the Earth-fixed coordinate frame to the vehicle coordinate frame at time
.
is calculated using the predicted attitude
; then, the cross product of
and
meets the sine rotation vector
. The sine rotation vector
represents the rotation from the presumed
direction to the measured
direction, as follows:
The direction of the sine rotation vector
represents the axis of rotation from the presumed
direction to the measured
direction. The length of the sine rotation vector
is the sine of the rotation angle from the presumed
direction to the measured
direction.
Figure 1a represents graphically how the
is calculated and represents the rotation.
Likewise, the measured magnetic field
directs the Earth-fixed
x direction that is represented in the instrument coordinate frame. The bias of the measured magnetic field
from the Earth-fixed
x direction should be adjusted by using the methods for bias estimation [
15,
21]. Let the presumed
x direction that is represented in the vehicle coordinate frame be denoted by
. Then, Equation (
14) provides the presumed
x direction vector
that is represented in the vehicle coordinate frame, as follows:
The cross product of
and
then becomes the sine rotation vector
. The sine rotation vector
represents the rotation from the presumed
x direction to the measured
x direction, as follows:
Figure 1b represents graphically how the
is calculated and represents the rotation.
Both of the sine rotation vectors
and
bear the rotation information from the presumed attitude to the measured attitude. These two vectors are the rotations determined by two different sensors (the accelerometer and the magnetometer) and are combined using Equation (
16), as follows:
and
in Equation (
16) are mixing parameters with a sum value of one, as follows:
The values of and need to be set based on the general rule: the larger value is assigned to the γ that corresponds to the more reliable measurement between the acceleration and magnetic field. If the acceleration measurement is more reliable than the magnetic field measurement, then will have a larger value, and vice versa. In the experiment, the values are determined through trial and error, as well as based on the general rule.
From the sine rotation vector, the rotation axis
and rotation angle
β are found by using Equations (
18) and (
19), as follows:
The rotation matrix
that is represented in terms of the rotation angle and rotation axis is described in Equation (
20), as follows:
where
is applied. The rotation matrix can also be represented in terms of the Euler angles
as the matrix
, which is shown in Equation (
21).
From the equivalence of the two rotation matrices
and
, the Euler angles are found by using Equation (
22), as follows.
The Euler angles in Equation (
22) represent the rotation from the presumed attitude to the measured attitude; consequently, it is concluded that the innovation
that was used for Equation (
9) is found by using Equation (
23), as follows:
A brief explanation regarding the calculation of the measurement
from the acceleration
and magnetic field
follows [
22]. First, the roll
φ and pitch
θ are calculated using the acceleration
in Equations (
24) and (
25), as follows:
φ and
θ are used to obtain the transformation matrix
through the use of Equation (
26), as follows:
Then, the yaw
ψ is calculated using Equation (
27), as follows:
5.2. Sine Rotation Vector
The sine rotation vector is defined by the following Equation (
28).
where
is the unit vector in the direction of the rotation axis and
β is the angle of rotation. The sine rotation vector is different from the rotation vector, which is defined by
[
20,
23]. The difference between the sine rotation vector and rotation vector is the magnitude of the vectors. The magnitude of the sine rotation vector is the sine of the rotation angle, while the magnitude of the rotation vector is the rotation angle itself.
The use of the sine rotation vector improves the attitude estimation in calculating the innovation vector in the EKF measurement update stage. First, the sine rotation vector uniquely determines the physical rotation regardless of the attitude to be rotated. In the case of the Euler angle difference between two attitudes represented in Euler angles, the rotation by the Euler angle difference does not necessarily correspond to the rotation between the attitudes. For example, the rotation of the attitude by the rotation does not result in the attitude . On the other hand, the rotation of the attitude by the rotation does result in the attitude . This example illustrates that the same rotation results in different physical rotation depending on the attitude to which the rotation is applied. On the contrary, the sine rotation vectors and uniquely rotate the predicted directions and to the measured directions and , regardless of the attitudes. In summary, the sine rotation vector uniquely determines the physical rotation regardless of the attitude to be rotated.
Second, the sine rotation vector relieves the effect of the bias and interference in measurement of the gravitational field and the magnetic field. The sine rotation vectors and deal only with x and directional measurements, respectively. is free from the bias in the direction of y and z. Likewise, is free from the bias in the direction of x and y. Therefore, use of the sine rotation vector mitigates the effect of bias in the field measurement, even though the method does not explicitly estimate and compensate the bias.
7. Conclusions
This paper proposes a new method for estimating the attitude of an underwater robot. The method uses the sine rotation vector to determine the rotational difference between the measured attitude and the presumed attitude. Compared to existing methods that directly calculate the Euler angle difference, the proposed method resolves the problem that arises from a non-unique mapping between the rotation and the Euler angle difference, when the attitude is described by the parameters of the Euler angles.
The experiments compared the proposed method to the existing methods that use EKFs, UKFs and CFs based on the differences of the Euler angles. The proposed method outperforms all of the other methods in terms of the location estimation of an underwater robot and the attitude estimation of a ground vehicle.
An investigation especially through underwater experiment to compare the properties of the sine rotation vector with other attitude representation methods, such as Euler angles, rotation matrices and quaternions with respect to attitude estimation, remains as the focus of further research. Furthermore, fusing more sensor measurements that provide directional information in the framework of the sine rotation vector is expected. Finally, it is expected that the proposed method will be implemented for navigation in a lake or ocean environment for the verification of the practical applicability, in a future study.