 Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 A Study on Heading and Attitude Estimation of Underwater Track Vehicle Dae-Hyeong Ji, Hyeung-Sik Cho * , Sang-Ki Jeong, Ji-Youn Oh, Seo-Kang Kim and Sam-Sang You Division of Mechanical Engineering, Korea Maritime and Ocean University, Busan, Korea Received 03 May 2018; received in revised form 05 August 2018; accepted 24 October 2018 Abstract In this paper, we studied the designing and manufacturing of an underwater track vehicle (UTV) that can be operated in the underwater environment. We designed the electrical and control system for precise operation of the UTV and conduct test operation by using the UTV. We developed an attitude reference system (ARS) that was composed of the ring laser gyro (RLG) sensor, a geomagnetic sensor, and USBL sensor to estimate precise heading and attitude of the UTV. An inertial navigation system (INS) is developed to be combined with the developed ARS. Also, the INS navigation is configured by supplementing the USBL sensor information. We design a controller for a precise trajectory and attitude tracking by installing the ARS on the UTV. In this paper, we used an extended Kalman filter in the INS to estimate the position and attitude of the UTV. The ARS is studied to obtain more precise sensor information in an uncertain environment underwater. Performance tests of the developed INS using the UTV are conducted and the results show that the system has the best performance. Keywords: ring laser gyro(RLG), ultra-short baseline(USBL), Attitude reference system(ARS), inertial navigation system(INS), extended kalman filter(EKF) 1. Introduction Many kinds of research have been conducted to develop an underwater platform and are capable of observing and moving the ocean with abundant resources and energy [1-2]. Fig. 1 is an example of an underwater platform being developed from observation and operation in the water. Motion modeling of such an underwater tracked vehicle has been carried out [3]. One of the critical technologies of this marine observation and work platforms is to measure their direction and location information accurately. Fig. 1 Underwater work platform * Corresponding author. E-mail address: hchoi@kmou.ac.kr Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 85 It is very important to obtain the heading information of the navigation sensor to measure its position and the travel direction. Among the navigation sensors, Ring laser gyro (RLG) is the key sensor to obtain the heading information. Attitude reference system (ARS) is a sensor system that obtains the heading and position information by using a gyro sensor and geomagnetic sensor [4-6]. [7] implemented MEMS inertial sensors, the error correction, and algorithms for high performance as the sensor needed to develop low cost and high-performance ARS. Also, to calculate the posture by integrating the gyro sensor value has a disadvantage is that it drifts due to sensor drift. So, [8] recently implemented ARS by using an acceleration sensor and gyro sensor to overcome this drawback of the combination of various sensors. [9] estimated the posture of MARG (Magnetic, Angular Rate, and Gravity) sensor information of EKF by using quaternion. [10] conducted a study to follow the position of ROV in USV by combining IMU and USBL. In this paper, we presented the underwater navigation of the underwater track vehicle (UTV) which can move on the sea floor. The ARS of the hull, which is an important factor of the underwater navigation, were constructed using RLG and a magnetometer. Also, a small UTV was designed and fabricated to verify the actual sea area of the constructed underwater navigation system. The UTV is designed to be able to move on land and sea and to mount various navigational sensors such as GPS, Depth, IMU, USBL on the hull. This paper is organized as follows. Section 2 of this paper describes the outline and specifications of the designed UTV. Section 3 describes the implementation of ARS using RLG and Magnetometer information. In Section 4, the implemented ARS was attached to the test-bed and the experiment was performed through the correct step. In Section 5, we concluded this paper. 2. Underwater Track Vehicle A small UTV was designed to verify the excellent performance of the developed underwater navigation system through experiments in the water tank and the sea area. The designed UTV is shown in Fig. 2(a). It is a structure equipped with an infinite orbit for movement on the land and seafloor. The external shape is designed to have a length of 1200mm, the endpoint distance of both endless tracks is 700mm and the distance from the bottom to the top is 800mm. Depends on the control method, a total of two motors control each caterpillar individually and can be turned forward / backward and left / right. The UTV also has various navigational sensors such as GPS, Depth, IMU, and USBL. The picture of the UTV is shown in Fig. 2(b) and the total weight is about 80kg. The UTV is transported to the experimental sea by the ship in the sea area test and is designed to be operated by the power supply and communicate by using the tether cable by descending to the sea floor through the crane in the target sea area. Route control and tracking operation are carried on by applying the designed underwater navigation system to UTV. (a) Design UTV (b) Produced UTV Fig. 2 Underwater Track Vehicle Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 86 3. Attitude Reference System ARS, which is an essential sensor in the underwater navigation of UTV, was constructed using RLG and a magnetometer. The development of the ARS is as follows. 3.1. Configuration of the ARS The ARS is composed of RLG and Magnetometer and the addition of the navigation algorithm. The ARS using RLG has a problem of integration error of data due to time increase. To reduce the accumulation of integration error, we constructed an extended Kalman filter algorithm using gyro data, acceleration data, and geomagnetism data. The block diagram that corrects the integration error accumulation of the sensor data using the extended Kalman filter is shown in Fig. 3. Fig. 3 Structure of ARS algorithm Fig. 3 is a method of calibrating using acceleration and external geomagnetism data. The angular velocity of RLG is integrated by using the relation between Euler angle and angular velocity, and the data of Roll, Pitch, and Yaw derived are corrected by the extended Kalman filter. The reason for using a similar sensor, gyro, and geomagnetism is to utilize the advantages of each sensor [10-11]. Yaw data from the geomagnetic sensor are sensitive to the surrounding magnetic field and has a large error. Also, it is greatly affected by the surrounding environment. To grasp this characteristic, we experimented with the characteristics of the geomagnetic sensor and constructed the experimental apparatus using the gyro and acceleration sensor to compare with the experimental data. 3.2. ARS testbed The testbed created for the performance test of the ARS consists of RLG, MEMS type Attitude, heading reference system (AHRS) that provides information on acceleration data and sensor module including the geomagnetic sensor. The storage and computation of the test device data were handled using an embedded Linux processor. Fig. 4 shows a diagram of the configuration of the ARS. Fig. 4 ARS diagram Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 87 In Fig. 4, the AHRS, GPS, and IMU sensor data is used for the algorithm that is ported to the embedded Linux. The computed data and the stored data are sent to the computer that stores the data in RF wireless communication. To extract the data from the sensors, a device that provides motion was created in the form of a rotating table. The developed equipment is shown in Fig. 5. The rotating table was constructed by applying a servo motor with a resolution of 0.088° and ARM-M4 series micro-controller was used to configure the system to control the rotation speed and the rotation angle of the motor precisely. The rotating table is shown in Fig. 5. The control system of the rotating table is shown in Fig. 6. Fig. 5 ARS test-bed(left) & rotating table(right) Fig. 6 Composition of turntable control system (a) Result of stop test (b) Result of exercise test Fig. 7 Analyze the geomagnetic sensor The performance of the ARS placed on the table using the rotating table was tested and the data of the geomagnetic sensor were analyzed. The data of the geomagnetic sensor are shown in Fig. 7 as a result of repeated rotation test at the stopping state and the constant angle. As shown in Fig. 7, the heading error in the static state of the geomagnetic sensor is shown in Fig. 7(a), Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 88 the minimum angle is 0.1° and the maximum angle is 0.3°. As shown in Fig. 7(b), the result of each repeated experiment (0°-20°) showed an error of 2.4° or more. Also, the overall error in 10 times independent geomagnetic sensor tests did not deviate significantly from the above graph, but the mean range of errors varied finely depending on the surrounding environment. Therefore, geo magnetic data in the absence of motion does not show large error, but the operation in the system cannot guarantee data in the system. In an environment that exerts a magnetic force on the periphery, the error value is not linear, so it is found difficult to use by itself. The RLG has a significant advantage that it is not affected by the surrounding magnetic environment. However, since the angular velocity information is provided, integration is required to obtain each information. At this time, the cumulative error occurs with time due to integration. In this experiment, the Euler angular data obtained by converting the RLG rotation experiment results into the Euler angles and integrating them are shown in Fig. 8. It shows the integration error of about 1.5 degrees due to the accumulation of the integration error when the angular velocity of the ring laser gyro is converted into the Euler angle and the result is tested for about 10 minutes. Fig. 8 Result of RLG sensor test Magnetic force profoundly influences the geomagnetic sensor. Therefore, we designed the system that uses only the sensor providing initial Yaw angle information, constructs the ARS using the RLG sensor, and processes the sensor data with the extended Kalman filter (EKF). The EKF algorithm is constructed as follows. In this system, the state variable of the EKF is denoted by x = [𝜙 𝜃 𝜓]. The system model shows the relationship between the gyro acceleration and the Euler angle as shown in Eq. (1) [12].   1 sin tan cos tan sin tan cos tan 0 cos sin cos sin 0 sin sec cos sec sin sec cos sec p p q r q w q r w f x w r q r                                                                        (1) Since the nonlinear function 𝑓(𝑥) cannot be applied to the EKF, the 𝑓(𝑥) of Eq. (1) is partially differentiated for each state variable to obtain the Jacobian as Eq. (2). 1 1 1 2 2 cos tan sin tan sin sec cos sec 0 2 2 2 sin cos 0 0 cos sec sin sec sin sec tan cos sec tan 0 3 3 3 f f f q r q r f f f A q r q r q r f f f                                                                              (2) The EKF is an algorithm for discrete-time. Therefore, when the Eq. (2) is discretized, the system matrix A is expressed by Eq. (3). Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 89 A I dt A   (3) In Eq. (3), dt is the sample time and I is the identity matrix. The acceleration and geomagnetism data are used as the correction values and are calculated as shown in Eq. (4). 0 sin 0 cos sin 0 cos cos fx u w v p g fy v w u q g fz w v u r g                                                             (4) about 1 1 sin sin cos fy fx mag kg g                    ， ， 1 mag mag mag k k   (5) In Eq. (4), ϕ and θ are calculated approximately by acceleration and the calculated ϕ and θ have large errors. The correction data ψ is obtained by using the geomagnetism data as shown in Eq. (5). By subtracting the first measured data from the measured geomagnetism data, a change in angle can be obtained. The data ψ including the integration error of the RLG is corrected using the angular variation of the geomagnetism data. Next, the measured model equation (z) can be calculated from the acceleration data and the geomagnetism data as shown in Eq. (6) to measure all three state variables. Therefore, it can be expressed by Eq. (6). 1 0 0 0 1 0 0 0 1 z v Hx v                           (6) Eq. (6) does not need to find a Jacobian in a linear form with measured values. Finally, the system noise covariance and the measurement noise covariance Q, R matrix can be measured close to the real one by having equipment that can accurately measure signal characteristics and RLG characteristics. However, since the measurement pieces of equipment are costly, the Q and R matrices are considered as design factors and the performance trends are observed and determined. The system design and matrices used for the EKF are obtained through Eqs. (1)-(6) and the calculation is performed by applying the EKF according to calculation order. 4. Experiments and Considerations of ARS 4.1. Calibration of the ARS The corrected data using the correction algorithm was verified by comparing the experimental data. The data was obtained at a sampling time of 100 Hz and data of 12, 6400 pieces in about 21 minutes. The measured data are shown in Fig. 9 as a graph. In Fig. 9, the three graphs located on the left side are RLG data alone and Euler angles are converted and integrated. In this case, an integration error of about 2.4° to 3.7° occurred for about 20 minutes. However, after applying the EKF, the integral error hardly occurred. The offsets are 0.3° and 1.2°, respectively, at the roll and pitch angles, are the error with the mechanism that fixes the RLG. Next, an experiment was conducted to evaluate the error of posture by rotating forward and backward by repeating the attitude constantly. Experiments were carried out in the range of -30° to 0° for a period using the experimental setup of Fig. 5. Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 90 About 500,000 data were received for about 83 minutes with a sampling time of 100Hz. The measured data are shown in Figs. 10-11 are shown. Fig. 9 Attitude correction in the stationary state In Fig. 10, the uncorrected yaw data exhibited an error of more than about 12° during rotation of about -30° to 0° for about 83 minutes. Fig. 11 is a graph of geomagnetism data used as correction data. Since there is no integration as shown in the graph, there is no integration error. However, it shows a measurement error of 6° to 7° by repeatedly rotating about -30° to 0° for about 83 minutes. Fig. 10 Yaw angle of Raw Gyro Fig. 11 Yaw angle of Raw Magnetometer The results are shown in Fig. 12 as a result of fusing data of Figs. 10-11 with an extended Kalman filter. In Fig. 12, it is shown that there is a slight error in the starting position and the final position due to the mechanical error of the rotating table Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 91 system. Therefore, as shown in Fig. 12, there is a slight error in the position of the start point and the end position. However, the results of the whole experiment are shown in Figs. 10-11 and it can be confirmed that the proposed algorithm does not diverge for about 83 minutes. Fig. 12 Yaw angle of Calibrated ARS 4.2. Test of platform The calibrated ARS was applied to the actual moving platform (UTV) to check the performance of the ARS under study. The turntable test showed satisfactory performance for the ARS under study. However, in application to an actual platform, the performance is affected by the actuating mechanism that generates many vibrations during movement is shown in Fig 13; it might cause a little more positioning and attitude errors. Fig. 13 Test-bed for ARS Fig. 14 Outdoor driving test Fig. 15 Result of Outdoor driving test Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 92 The configuration of the UTV consisted of the RLG constituting the ARS and the correction geomagnetic sensor TCM-3. The ARS was installed in the waterproof housing of the UTV. Also, a similar MEMS type IMU was installed and all the obtained data is recorded using RF wireless communication system in real time. The ARS has been developed for underwater usage. However, it is difficult to verify the performance under the water. Therefore, the land test was carried out to confirm the performance. The scene of the outdoor test is shown in Fig. 14 and the results are shown in Fig. 15. Fig. 15 shows the result of applying the algorithm and the result of non-using the algorithm. The graph without the algorithm (red color) diverges from the heading angle. During the 10-minute test time, the cumulative error occurred at about 2 °. Since the error due to integration increases rapidly with time, data that do not apply the algorithm will quickly diverge in the future. Therefore, the current test results show approximately 2° difference between using and non-using the algorithms. If the test is performed for a long time, the error due to integration will increase. Despite a lot of vibration while driving on uneven surfaces, the heading data of the ARS showed an error of about 0.02° to 0.5°, which is quite small. 5. Conclusions In this paper, ARS which can acquire the attitude of the underwater vehicle is an essential factor in underwater navigation and is designed by using RLG and Magnetometer. Also, a small UTV was designed and fabricated to verify the performance of the designed underwater navigation system through a water tank and sea area experiment. In the study, the extended Kalman filter was used to reduce the integration error of data over time, which is the biggest problem of ARS using RLG. Then, by using this, an algorithm that efficiently removes the integration error of the Yaw angle by correcting it through the fusion of the gyro, acceleration, and geomagnetism data was studied and was implemented in the system. Through the Turntable experiment, it was confirmed that the cumulative error accumulation amount before the correction was more than 12° in the result of about 83 minutes. The geomagnetism data to correct this error also showed measurement error more than 6° to 8°. As a result of calibrating the gyro data and geomagnetism data with the extended Kalman filter, the postural error of about 0.2° to 0.4° in the experiment over 1 hour occurred. This error is also assumed to be caused by the mechanical part of the experimental equipment. The algorithm of the ARS was verified by the ARS applied to the test-bed. It can be seen that only a slight error of about 0.02° to 0.4° occurred in the straight running of approximately 100m. However, due to the nature of the integral error that rapidly diverges instantaneously, many platform application tests in various situations are required. Acknowledgments This research is a part of the project National Research Foundation of Korea (NRF-2016R1A2B4011875) and a part of project titled “R&D center for underwater construction robotics”, South Korea (PJT200539), funded by the Ministry of Oceans and Fisheries (MOF). Conflicts of Interest The authors declare no conflict of interest. References [1] T. S. Kim, I. S. Jang, C. J. Shin, and M. K. Lee, “Underwater construction robot for rubble leveling on the seabed for port construction,” 14th International Conference on Control, Automation and Systems, 2014, pp. 1657-1661. [2] C. H. Kim, T. S. Kim, and M. K. Lee, “Study on the estimation of the cylinder displacement of an underwater robot for harbor construction using a pressure sensor,” Journal of Navigation and Port Research, vol. 36, no. 10, pp. 865-871, 2012. Advances in Technology Innovation, vol. 4, no. 2, 2019, pp. 84-93 93 [3] D. H. Choi, Y. J. Lee, S. M. Hong, H. S. Choi, and J. Y. Kim, “A study on dynamic modeling for underwater tracked vehicle,” Journal of Ocean Engineering and Technology, vol. 29, no. 5, pp. 386-391, 2015. [4] J. H. Kim, H. G. Min, J. D. Cho, J. H. Jang, S. H. Kwon, and E. T. Jeung, “Design of angular estimator of inertial sensor using the least square method, ” World Academy of Science, Engineering and Technology, vol. 60, pp. 1541-1544, 2009. [5] D. Jung and P. Tsiotras, “Inertial attitude and position reference system development for a small UAV,” In AIAA Infotech@ Aerospace 2007 Conference and Exhibit, May 2007, pp. 2763. [6] Y. C. Lai, S. S. Jan, and F. B. Hsiao, “Development of a low-cost attitude and heading reference system using a three-axis rotating platform,” Sensors, vol. 10, no. 4, pp. 2472-2491, 2010. [7] D. Gebre-Egziabher, R. C. Hayward, and J. D. Powell, “A low-cost GPS/inertial attitude heading reference system (AHRS) for general aviation applications,” Proc. of the 1998 IEEE Position Location and Navigation Symposium, January 1998, pp. 518-525. [8] W. Qin, W. Yuan, H. Chang, L. Xue, and G. Yuan, “Fuzzy adaptive extended kalman filter for miniature attitude and heading reference system,” In Proc. of the 2009 4th IEEE International Conference on Nano/Micro Engineered and Molecular Systems, January 2009, pp. 1026-1030. [9] J. L. Marins, X. Yun, E. R. Bachmann, R. B. McGhee, and M. J. Zyda, “An extended kalman filter for quaternion-based orientation estimation using MARG sensors,” Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, November 2001, vol. 4, pp. 2003-2011. [10] D. W. Jung, S. M. Hong, J. H. Lee, H. J. Cho, H. S. Choi, and M. T. Vu, “A study on unmanned surface vehicle combined with remotely operated vehicle system,” Proceedings of Engineering and Technology Innovation (2018), vol. 9, pp. 17-24. [11] H. S. Yu, C. J. Kim, C. K. Song, and H. W. Park, “An application of large heading attitude model for initial alignment of inertial navigation system,” The Korean Society for Aeronautical and Space Sciences, pp. 1983-1988, 2012. [12] H. S. Yu, S. W. Choi, and S. J. Lee, “Nonlinear filtering approaches to in-flight alignment of SDINS with large initial attitude error,” Journal of Institute of Control Robotics and Systems, pp. 468-473, 2014. [13] T. H. Chung, K. W. Song, and B. J. Chang, “Design of the kalman filter for transfer alignment of strapdown inertial navigation system,” Journal of Institute of Control, Robotics and Systems, 1991, pp. 142-146. Copyright© by the authors. Licensee TAETI, Taiwan. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY-NC) license (https://creativecommons.org/licenses/by-nc/4.0/).