Inertial Sensor Self-Calibration in a Visually-Aided Navigation Approach for a Micro-AUV

2.1. Hardware

The Fugu-C structure consists of a cylindrical, transparent, sealed housing containing all of the hardware and four propellers, two horizontal and two vertical. The dimensions of the cylinder are ø212 × 209 mm, and the motor supports are 120 mm long. Despite the fact that the propeller configuration could provide the vehicle with four DOFs, surge, heave, roll and yaw, due to the vehicle dynamics, it is almost passively stable in roll and pitch. Consequently, the robot has three practical DOFs, surge, heave and yaw: (x, z, yaw).

Figure 1a shows the 3D CAD model design of the vehicle, Figure 1b shows the hardware and the supporting structure, Figure 1c,d show two views of the final result.

Figure 1. Fugu-C. (a) The CAD model of the vehicle; (b) the hardware and structure; (c,d) two different views of the robot.

Fugu-C can operate as an AUV, but it can also be teleoperated as a remotely operated vehicle (ROV) with a low-cost joystick. Its hardware includes (see Figure 2):

Figure 2. The schematic of the Fugu-C hardware connection.

• Two IEEE-1394a stereo cameras, one of them oriented toward the bottom, whose lenses provide a 97° horizontal field of view (HFOV), and another one oriented toward the forward direction with a 66° HFOV.

• A 95-Wh polymer Li-Ion battery pack. With this battery, Fugu-C has an autonomy for about 3 h, depending on how much processing is done during the mission. For an autonomous mission with self-localization, the autonomy is reduced to two hours.

• A PC104 board based on an Atom N450 at 1.66 GHz with a 128-GB SSD 2.5-inch hard drive.

• A three port 800 FireWire PC104 card to connect the cameras to the main computer board.

• A nano IMU from Memsense, which provides triaxial acceleration, angular rate (gyro) and magnetic field data [28].

• A power supply management card, which permits turning on/off the system, charging the internal battery and the selection between the external power supply or the internal battery.

• A microcontroller-based serial motor board that manages some water leak detectors, a pressure sensor and four DC motor drivers.

• An external buoy containing a USB WiFi antenna for wireless access from external computers.

• A set of DC-DC converters to supply independent power to the different robot modules.

• Four geared, brushed DC motors with 1170 rpm of nominal speed and 0.54 kg·cm torque.

A 16-pin watertight socket provides the connectivity for wired Ethernet, external power, battery changing and a WiFi buoy. Note that not all connections are needed during an experiment. If the wired Ethernet is used (tethered ROV in use), then the buoy is not connected. Alternatively, if the buoy is connected (wireless ROV/AUV), then the wired Ethernet remains disconnected.

The autonomy of the robot when operating in AUV mode with the current type of battery has been calculated taking into account that its average power consumption at maximum processing (all sensors on, including both cameras, and all of the electronics and drivers operative) and with the motors engaged is around 44.5 W, so the battery would last (96 W·h)/(44.5 W) = 2.15 h at full charge. If it was necessary to include two LED bulbs (10 W) attached to the vehicle enclosure to operate in environments with poor illumination conditions, the internal power consumption would increase to 64.5 W and the battery would last (96 W·h)/(64.5 W) = 1.49 h.

The robot can operate in shallow waters up to 10 m in depth. However, another version for deeper waters could be made by thickening the acrylic cylinder.

Experiments showed an average speed in surge of 0.5 m/s and an average speed in heave of 1 m/s.

2.2. Software

All of the software was developed and run using ROS middleware [25] under GNU/Linux. Using ROS makes the software integration and interaction easier, improves their maintainability and sharing and strengthens the data integrity of the whole system. The ROS packages installed in Fugu-C can be classified into three categories: internal state management functions, vision functions and navigation functions.

The internal state management function continuously checks the state of the water leak sensors, the temperature sensor and the power level. The activation of any of these three alarms induces the system to publish a ROS topic-type alarm, to terminate all running software and to shutdown the horizontal motors, while it activates the vertical ones to launch the vehicle quickly up to the surface.

The visual functions include image grabbing, stereo odometry, 3D point cloud computation and environment reconstruction, which is explained in Section 3.

The navigation functions include the MESKF for sensor fusion and pose estimation and all of the modules for twist and depth control, and it is detailed in Section 4.

3.1. Image Grabbing and Processing

In order to save transmission bandwidth, raw images provided by any of the stereo cameras are encoded using a Bayer filter and sent directly to the computer, where they are processed as follows:

(1)

First, RGB and gray-scale stereo images are recovered by applying a debayer interpolation.

(2)

Then, these images are rectified to compensate for the stereo camera misalignment and the lens and cover distortion; the intrinsic and extrinsic camera parameters for the rectification are obtained in a previous calibration process.

(3)

Optionally, they are downscaled from the original resolution (1024 × 768) to a parameterizable lower rate (power of two divisions); this downsampling is done only if there is a need to monitor the images from remote computers without saturating the Ethernet communications. Normally, a compressed 512 × 384 px image is enough to pilot the vehicle as an ROV without compromising the network capacity. If the vehicle is being operated autonomously, this last step is not performed.

(4)

Following the rectification, disparity calculation is done using OpenCV's block matching algorithm to finally project these disparity images as 3D points.

(5)

Odometry is calculated from the bottom-looking camera with the LibViso2 library. In our experiments, the forward-looking camera has been used only for monitoring and obstacle detection, although it could also be used to compute the frontal 3D structure.

Figure 3 shows the image processing task sequence performed in Fugu-C.

Figure 3. The successive steps of the on-line image processing task.

3.2. Visual Odometry

Stereo visual odometry is used to estimate the displacement of Fugu-C and its orientation. Many sophisticated visual odometry approaches can be found in the literature [29]. An appropriate approach for real-time applications that uses a relatively high image frame rate is the one provided by the Library for Visual Odometry 2 (LibViso2) [30]. This approach has shown good performance and limited drift in translation and rotation, in controlled and in real underwater scenarios [31].

The main advantages derived from LibViso2 and that make the algorithm suitable for real-time underwater applications are three-fold: (1) it simplifies the feature detection and tracking process, accelerating the overall procedure; in our system, the odometer can be run at 10 fps using images with a resolution of 1024 × 768 pixels; (2) a pure stereo process is used for motion estimation, facilitating its integration and/or reutilization with the module that implements the 3D reconstruction; (3) the great number of feature matches found by the library at each consecutive stereo image makes it possible to deal with high resolution images, which is an advantage to increase the reliability of the motion estimates.

3.3. Point Clouds and 3D Reconstruction

3D models of natural sea-floor or underwater installations with strong relief are a very important source of information for scientists and engineers. Visual photo-mosaics are a highly valuable tool for building those models with a high degree of reliability [32]. However, they need a lot of time to be obtained, and they are rarely applicable online. On the contrary, the concatenation of successive point clouds registered by accurate pose estimates permits building and watching the environment online and in 3D, but with a lower level of detail. Thus, photo-mosaicking and point cloud concatenation can be viewed at present as complementary instruments for optic-based underwater exploration and study.

Dense stereo point clouds computed from the disparity maps [33] are provided by Fugu-C at the same rate as the image grabber. They are accumulated to build a dense 3D model of the underwater environment, visible online if a high rate connection between the vehicle and an external computer is established. The concatenation and meshing of the partial reconstructed regions must be done according to the robot pose. Although the odometric position (x, y, z) accumulates drift during relatively long routes, the system can reconstruct, with a high degree of reliability, static objects or short-medium stereo sequences, if no important errors in orientation are accumulated. However, if the orientation of the vehicle diverges from the ground truth, especially in roll and pitch, the successive point clouds will be concatenated with evident misalignments, presenting differences in inclination between them, thus distorting the final 3D result.

In order to increase the reliability of the reconstructed areas in longer trajectories and to minimize the effect of the orientation errors in the 3D models, we have implemented a generic ESKF, which corrects the vehicle attitude estimated by the visual odometer in the three rotation axis and minimizes the effect of the gyroscope drift.

4.1. Architecture Pipeline

The navigation architecture of Fugu-C is designed at three concatenated levels: (1) a twist controller; (2) a depth controller; and (3) a motion planner.

The twist control module is, in turn, implemented in three interdependent layers, namely:

• Motor layer: the orders directly sent to the motor board node, in charge of interacting with the four propellers.

• Wrench layer: translational (thrust) and angular (torque) forces calculated to move the vehicle in a desired direction with a certain speed are the inputs (wrench levels) of the thruster allocation module. Wrench levels are transformed into motor levels according to the affine thruster model and through a thruster allocation matrix (TAM) [34].

• Twist layer: twist includes the linear and angular velocities. The inputs of this module are the velocity setpoint and the current velocity. This module performs a basic proportional integral derivative (PID) controller for each degree of freedom (x, y, z; roll, pitch, yaw) which outputs a wrench level, containing the necessary force and torque to reach the requested twist.

During the experiments with Fugu-C in the pool and in the sea, the PID gains were experimentally adjusted in order to obtain an adequate response of the vehicle in each scenario, avoiding instabilities, oscillations or steady-state errors. A systematic calculation of the PID gains is included in an upcoming work, but excluded in the scope of this paper.

The depth module is also a basic PID that outputs a wrench level in the zaxis from two inputs: the current depth and the depth setpoint. The PID gains of these two additional modules are also tuned experimentally and online during each test.

The motion planner module is in charge of generating the twist set points for the twist controller. It can be either a joystick to control the vehicle remotely or a software module that generates automatically the needed twist, depending on the location of the goal point and the current vehicle state in terms of pose and velocity.

4.2. Sensor Fusion Using an Indirect Kalman Filter

4.2.3. Nominal State Prediction

The readings of the inertial sensors are used at this stage to estimate the relative vehicle displacement (translation and rotation) between two filter iterations. The inertial data contain a certain bias and noise, assumed to be additive and zero-mean Gaussian.

Let ω be the true vehicle angular rate given by [23]. At time k:

$${\mathit{\omega }}_m=\mathit{\omega }+\mathbf{\text{d}}+{\mathbf{\text{n}}}_{\mathit{\omega }}$$

(20)

where ω_m, d and n_ω denote, respectively, the sensor readings, the drift and the noise of the gyroscope.

The readings of the inertial sensors are usually referenced to the body frame, and the acceleration can include the locally-referenced gravity in the vertical axis. Let us assume, without loss of generality, that the inertial sensor is approximately located in the center of the vehicle, the centripetal acceleration being negligible. At time k:

$${\mathbf{\text{a}}}_m=(\mathbf{\text{a}}+{\text{g}}_L)+\mathbf{\text{b}}+{\mathbf{\text{n}}}_a$$

(21)

$${\text{g}}_L={\mathcal{\text{R}}}^{\top }\cdot \text{g}$$

(22)

where a_m is the sensor reading, b is the accelerometer bias, n_a is its noise and a is the real body acceleration at time t. The gravity, g_L, is expressed with respect to the body frame, and is the rotation matrix from local to global coordinates (corresponding to the quaternion q, the vehicle attitude); so ^T is the transposed rotation matrix of , permitting the transform from global to local coordinates, and g = (0, 0, −g), being g = 9.81 m/s².

Note that v_k, ω_k, a_k, d_k and b_k are referenced to the local coordinate system (e.g., the system of coordinates attached to the vehicle), and p_k and q_K are referenced to the inertial frame (e.g., global frame). The coordinate system conventions and notations are illustrated in Figure 4.

Figure 4. Coordinate frames and notations. {G} is the inertial global frame; {L} is the coordinate frame attached to the vehicle. u,v and d represent the position coordinates referenced to {L}.

In this particular implementation, the discrete equations that govern the nominal state are based on a general constant acceleration motion. Note that x_(k−1) ≡ x_{(k−1)|(k−1)} and that the estimated state at instant k dependent on the estimated state at instant k − 1 is denoted as x_(k)|(k−1).

$${\mathbf{\text{p}}}_{k|(k-1)}={\mathbf{\text{p}}}_{(k-1)}+{\mathcal{\text{R}}}_{(k-1)}{\mathbf{\text{v}}}_{(k-1)}\Delta t+\frac{1}{2}{\mathcal{\text{R}}}_{(k-1)}{\mathbf{\text{a}}}_{(k-1)}\Delta t^2$$

(23)

$${\mathbf{\text{q}}}_{k|(k-1)}={\left({\mathbf{\text{q}}}_m\ast \mathbf{\text{q}}\right)}_{(k-1)}$$

(24)

$${\mathbf{\text{v}}}_{k|(k-1)}={\mathbf{\text{v}}}_{k-1}+{\mathbf{\text{a}}}_{(k-1)}\Delta t$$

(25)

$${\mathit{\omega }}_{k|(k-1)}={\left({\mathit{\omega }}_m-\mathbf{\text{d}}\right)}_{(k-1)}$$

(26)

$${\mathbf{\text{a}}}_{k|(k-1)}={\left({\text{g}}_L-{\mathbf{\text{a}}}_m+\mathbf{\text{b}}\right)}_{(k-1)}$$

(27)

$${\mathbf{\text{d}}}_{k|(k-1)}={\mathbf{\text{d}}}_{(k-1)}$$

(28)

$${\mathbf{\text{b}}}_{k|(k-1)}={\mathbf{\text{b}}}_{(k-1)}$$

(29)

where Δt is the time interval between the current and the predicted nominal state, * represents the quaternion product and q_m is the quaternion that represents the vehicle angular motion occurring during Δt.

$${\mathbf{\text{q}}}_m=\text{quaternion}({\mathit{\omega }}_m\Delta t)={\left(\begin{array}{c}cos({\mathit{\omega }}_x\Delta t/2)\\ 0\\ 0\\ sin({\mathit{\omega }}_x\Delta t/2)\end{array}\right)}^{\top }\ast {\left(\begin{array}{c}cos({\mathit{\omega }}_y\Delta t/2)\\ 0\\ sin({\mathit{\omega }}_y\Delta t/2)\\ 0\end{array}\right)}^{\top }\ast {\left(\begin{array}{c}cos({\mathit{\omega }}_z\Delta t/2)\\ sin({\mathit{\omega }}_z\Delta t/2)\\ 0\\ 0\end{array}\right)}^{\top }$$

(30)

It is important to observe that, since the acceleration a_k and the linear velocity v_k are referenced to the local body frame, the term g_L present in the expression of a_k transforms the gravity expressed with respect to the inertial frame into the body frame. However, the accumulated position p_k must be given with respect to the inertial global frame. As a consequence, the terms _(k−1)v_(k−1) and _(k−1)a_(k−1) present in the expression of p_k denote the transformation of the velocity and the acceleration from the body frame to the inertial global frame.

4.2.4. Error State Prediction

The error is defined as the difference between the estimate of a certain variable and its real value. Let δx̃_k be the estimation of the error state vector at time k:

$$\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{x}}}}_{k|(k-1)}=f({\mathbf{\text{x}}}_{(k-1)})+{\mathit{ϵ}}_k$$

(31)

where f(x_k) is the general set of functions defined to predict the error state and ϵ_k = N(0, Q_k) is the zero-mean Gaussian error of the prediction model, Q_k being the model noise covariance.

Let δã_k, δω̃_k, δp̃_k, δq̃_k, δṽ_k, be the estimated errors in acceleration, angular velocity, position, attitude and linear velocity, all at time k. The discrete equations f(x_k|(k−1)) that govern the error state estimation are:

$$\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{p}}}}_{k|(k-1)}=\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{p}}}}_{(k-1)}+\mathcal{\text{R}}\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{v}}}}_{(k-1)}\Delta t-\mathcal{\text{R}}({\stackrel{\sim }{\mathbf{\text{v}}}}_{(k-1)}\otimes \mathit{\delta }{\stackrel{\sim }{\mathbf{\text{q}}}}_{(k-1)})\Delta t-\frac{1}{2}\mathcal{\text{R}}({\stackrel{\sim }{\mathbf{\text{a}}}}_{(k-1)}\otimes \mathit{\delta }{\stackrel{\sim }{\mathbf{\text{q}}}}_{(k-1)})\Delta t^2+\frac{1}{2}\mathcal{\text{R}}\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{a}}}}_{(k-1)}\Delta t^2$$

(32)

$$\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{v}}}}_{k|(k-1)}=\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{v}}}}_{(k-1)}+{\text{g}}_L\otimes \mathit{\delta }{\stackrel{\sim }{\mathbf{\text{q}}}}_{(k-1)}\Delta t+\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{a}}}}_{(k-1)}\Delta t$$

(33)

$$\mathit{\delta }{\stackrel{\sim }{\mathit{\omega }}}_{k|(k-1)}=\mathit{\delta }{\stackrel{\sim }{\mathit{\omega }}}_{k-1}$$

(34)

$$\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{a}}}}_{k|(k-1)}=\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{a}}}}_{k-1}$$

(35)

$$\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{q}}}}_{k|(k-1)}={\mathcal{\text{R}}}_m\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{q}}}}_{(k-1)}+\left(-{\mathbf{\text{I}}}_{3\times 3}\Delta t+\frac{1-cos(\left|{\mathit{\omega }}_{(k-1)}\right|\Delta t)}{{\left|{\mathit{\omega }}_{(k-1)}\right|}^2}\mathbf{\text{V}}-\frac{\left|{\mathit{\omega }}_{(k-1)}\right|\Delta t-sin(\left|{\mathit{\omega }}_{(k-1)}\right|\Delta t)}{{\left|{\mathit{\omega }}_{(k-1)}\right|}^3}{\mathbf{\text{V}}}^2\right)\mathit{\delta }{\stackrel{\sim }{\mathit{\omega }}}_{(k-1)}$$

(36)

where _m is the rotation matrix corresponding to ω_mΔt, also depending on time (k − 1). Note that δã_k|(k−1) and δω̃_k|(k−1) are supposed as constant, so that the filter can find the constant bias and drift of the sensors. V is the skew symmetric matrix of the velocity, in the form:

$$\mathbf{\text{V}}=\left(\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right)$$

(37)

I_3×3 is the three by three identity matrix and ⊗ represents the cross product.

Derivation of Equation (36) can be found in [35], and derivations of Equations (32) and (33) can be found in the Appendix of this paper.

As is exposed in Section 4.2.6, the values forming the error state are all reset after the nominal state has been corrected, and thereby, the values computed by Equations (32) to (36) are always zero. However, the corresponding Jacobian ${{\mathbf{F}}_k=\frac{\partial f(x_k)}{\partial \mathit{\delta }(x_k)}|}_{\mathit{\delta }{\stackrel{\sim }{\mathbf{x}}}_k}$, at iteration k is different from zero. Therefore, the system covariance matrix P_k can be propagated according to the Kalman equation:

$${\mathbf{\text{P}}}_{k|(k-1)}={\mathbf{\text{F}}}_k{\mathbf{\text{P}}}_{(k-1)|(k-1)}{\mathbf{\text{F}}}_k^{\top }+{\mathbf{\text{Q}}}_k$$

(38)

4.2.5. Error Kalman Update

Let us denote the measurement error δz_k as the difference between a sensor reading z_k and its prediction z̃_k contained in the nominal state:

$$\mathit{\delta }{\mathbf{\text{z}}}_k={\mathbf{\text{z}}}_k-{\stackrel{\sim }{\mathbf{\text{z}}}}_k$$

(39)

According to a standard EKF, the observation function h(δx̃_k) will be used to compute the predicted error measurement from the predicted error state. In this particular design of ESKF, and similarly to [21], the observation function is assumed to be a linear approximation and stated as h(δx̃_k) = H_kδx̃_k|(k−1) , H_k being the identity matrix. This is the same as assuming that, by definition, the measurement error is predicted as exactly the value of the estimated error state. Then, the innovation can be written as:

$${\stackrel{\sim }{\mathbf{\text{y}}}}_k=\mathit{\delta }{\mathbf{\text{z}}}_k-{\mathbf{\text{H}}}_k\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{x}}}}_{k|(k-1)}$$

(40)

being δz_k the vector of measurement errors.

In this step, the classical Kalman update equations are used to obtain an updated error state vector (δx̃_k) and its covariance, now both different from zero:

$${\mathbf{\text{S}}}_k={\mathbf{\text{H}}}_k{\mathbf{\text{P}}}_{k|(k-1)}{\mathbf{\text{H}}}_k^{\top }+{\text{R}}_k$$

(41)

$${\mathbf{\text{K}}}_k={\mathbf{\text{P}}}_{k|(k-1)}{\mathbf{\text{H}}}_k^{\top }{\mathbf{\text{S}}}_k^{-1}$$

(42)

$$\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{x}}}}_{k|k}=\mathit{\delta }{\stackrel{\sim }{\mathbf{\text{x}}}}_{k|(k-1)}+{\mathbf{\text{K}}}_k{\stackrel{\sim }{\mathbf{\text{y}}}}_k$$

(43)

$${\mathbf{\text{p}}}_{k|k}=(\mathbf{\text{I}}-{\mathbf{\text{K}}}_k{\mathbf{\text{H}}}_k){\mathbf{\text{P}}}_{k|(k-1)}$$

(44)

S_k being the innovation covariance, K_k the Kalman gain and R_k the measurements uncertainty. Note the difference between this R_k and the rotation matrix defined in Section 4.2.3.

4.2.7. Measurement Delays

The different sensors that equip Fugu-C supply their data at different rates. While the operational frequency of an standard IMU can range from 100 Hz to 1 KHz, a visual odometer or a pressure sensor can work, typically, between 10 Hz and 25 Hz (depending on the capacity of the used processor).

Motion predictions are done at the IMU transmission rate, while updates must be run when one exteroceptive measurement is received. The lack of synchronism in the integration of the data coming from the diverse sensors, that is delays between the reception and the processing of the IMU or odometry messages, and delays in the execution of the prediction and update steps can introduce additional errors in the filtering process.

To assure that all of the measurements are incorporated synchronously to the filter and that every update is launched synchronously with its corresponding motion prediction, all of the sensor timestamps are referred to the same global timer. Furthermore, similarly to [22], all of the measurements are stored in FIFO buffers with their timestamp. Although every measurement can really be processed with a certain delay, the updates are executed, aligned in time with the corresponding prediction, which is the one performed with the IMU reading that has a time stamp that immediately precedes the time stamp of the currently-processed visual sample.

It is worth commenting that, although the dynamics of Fugu-C are quite slow and this problem would not really affect the vehicle control, the filter has a general design, and it is prepared also to work in vehicles with six DOF with faster dynamics. This issue is crucial, for instance, in UAVs.

Our implementation of the MESKF and its ROS wrapper is available to the scientific community in a public repository [26]. Figure 5 shows a schematic overview of the ROS node-based navigation architecture described in this section.

Figure 5. The navigation and control architecture.

5.1. Navigation in a Controlled Environment

The first set of experiments to assess the navigation modules was carried out actuating the robot at a constant depth in a pool of 7 × 4 m. The bottom of this pool was covered with a printed digital image of a sea bottom environment, to simulate a marine-like context. In order to have a certain reference, the poster was also used to calculate the trajectory ground truth (see Figure 6). To that end, features of the online stereo images, captured from the down-looking camera, are matched with those features extracted offline from the original digital image of the poster. Afterwards, the 3D points computed from the online stereo pairs are re-projected onto the matching features of the original digital image, trying to minimize the global re-projection error. This is a particular application of the perspective-N-point (PNP) problem [39], and the result of the algorithm is the relative transformation between the current stereo image pair and the corresponding section in the original image of the poster. The algorithm assumes a flat ground and a perfect alignment of 90° between the lens axis of both cameras and the ground surface. The precision in the computation of the ground truth under the ideal conditions is high, since the outliers are eliminated using random sample consensus (RANSAC) [40], and errors in the re-projection and optimization process are minimized.

Figure 6. (a) The poster covering the pool bottom, with some artificial reliefs on it; (b) Fugu-C navigating in the pool.

Two representative experiments in the pool are shown in Figures 7, 8 (a loop trajectory) and 9 (a sweeping trajectory).

Figure 7. Experiments 1. (a) Robot trajectory in 3D; (b) depth; (c) robot trajectory in the x-y plane; (d) acceleration bias; (e) gyro drift.

Figure 8. Vehicle attitude corresponding to Experiment 1. (a) Ground truth; (b) visual odometry; (c) multiplicative error state Kalman filter (MESKF).

Figure 9. Experiment 2. A sweeping trajectory. (a) Robot trajectory in 3D; (b) depth; (c) robot trajectory in the x-y plane; (d) acceleration bias; (e) gyro drift.

The origin of the global coordinate system is always set at the beginning of the trajectory. This point is located at the water surface (z = 0), so the depth computed from the pressure sensor is equivalent to the z > 0 axis. The initial 15 × 15 covariance matrix of the error state vector was set to zero in all of the positions, except in the diagonal, where all elements were set to small values (10⁻²) to represent small uncertainties in the initial state vector.

Plot 7a shows the trajectory in (x,y,z) for the visual odometry (red), ground truth (blue) and for the filter estimates (green), and Plot 7c shows the same robot trajectory in the x-y plane.

Plot 7d shows the evolution of the accelerometer bias returned by the filter, and Plot 7e shows the evolution of the gyro drift.

Figure 8 shows the vehicle attitude during Experiment 1, corresponding to the ground truth (Plot 8a), the visual odometry (Plot 8b) and to the filter (Plot 8c).

Figure 9a,c shows, respectively, the 3D and 2D views of a sweeping trajectory run in the second experiment. Figure 9b shows the depth of the vehicle according to the visual odometry and the filter. Again, the origin of the trajectory is in the point (0, 0, 0). Plots 9d and 9e show the accelerometer bias and the gyro drift, respectively. Figure 10 shows the vehicle orientation in roll, pitch and yaw, according to the ground truth (Plot a), the visual odometry (Plot b) and the filter estimates (Plot c).

Figure 10. Vehicle attitude corresponding to Experiment 2. (a) ground truth; (b) visual odometry; (c) MESKF.

Results of both experiments point out the next conclusions:

• It can be observed from trajectories shown in Figures 7a and 9a that, although the data from the visual odometer present an important drift in z, the filter is able to correct this coordinate to a nearly constant value, using the pressure sensor information. The trajectory estimated by the filter is very close to the visual odometer, which is, in fact, much more reliable than the position estimated by integrating the inertial data. It is important to remark that the goal of the filter is not the pose correction. Since this is an approach for inertially-aided navigation, here, there are no external landmarks or loop closings used to improve the robot position further than the odometer estimates. Instead, the aim is to integrate all of the data coming from the navigation sensors in the most convenient way to, on the one hand, continuously obtain a vehicle pose that is as reliable as possible and, on the other hand, to stabilize the inertial data, compensating for their systematic biases.

• Acceleration bias and gyro drift stabilize quickly to constant values (some of them close to zero), meaning a practical sensor calibration and permitting the compensation of the inertial systematic errors. The highest value in the gyro drift corresponds to approximately 0.015 rad in its x component, an imperceptible value compared with the typical drifts that standard inertial sensors and odometers are prone to reach. Since the vehicle is moving at, approximately, a constant depth, (oscillating slightly in z due to the PID vertical hysteresis), the acceleration in z is very small and so is its drift in z.

• All of the trajectories estimated by the filter present, at their origin, a difference in orientation with respect to the ground truth. This difference is caused by the initial error in the angular rate. However, as this initial error is rapidly compensated for by the filter, it is not accumulated in the subsequent pose estimates.

• As Fugu-C is passively stable in pitch and roll, motion in these two DOF should result in being insignificant, while rotations in yaw should reflect the heading changes at every turn. In all of the trajectories, the vehicle always moved only in surge, heave and yaw. Consequently, the values corresponding to pitch and roll should be very small. However, the values of pitch and roll estimated by the ground truth and by the visual odometer in Experiments 1 and 2, as shown in Figures 8a,b and 10a,b, present values significantly different from zero (with maxima in the odometry of 0.5 radians), which is incorrect. The ground truth also presents, erroneously, non-negligible values of roll and pitch. This is because the condition of flat ground is not completely fulfilled due to a slight inclination of the pool bottom with respect to the water surface. Furthermore, the alignment of the lens axis of the down-looking camera with the normal to the ground plane is not perfect. Conversely, the filter approaches the values of roll and pitch to zero during both trajectories, which is much closer to the vehicle's real behavior. The ground truth in these experiments performs well in (x, y), and it is a reliable reference of the vehicle trajectory in the horizontal plane. However, unfortunately, it presents some errors in the orientation estimates and in the vertical direction.

5.2. Navigation in the Sea

Other experiments were conducted in a shallow water area off the coast of Mallorca (Balearic Islands, Spain). The robot was actuated only in surge, heave and yaw, without any significant motion in roll or pitch. Fugu-C was programmed to navigate at a constant depth in four different trajectories: one L-shaped, running close to a harbor breakwater, a loop and two sweeping trajectories covering around 40 meters of length. The sea bottom in the experiment area had no significant slope and was mainly covered by rocks and algae. Local reliefs caused significant and frequent changes in the distance from the vehicle to the ground. Even so, the experiments demonstrate that the filter responds properly, compensating for the visual odometer drift with the pressure sensor data. For these experiments, there was no possibility to calculate a ground truth. Instead, several markers were put on the sea bottom to control the difference between the real markers' position and the estimated pose of the vehicle when it passed over them. All trajectories started at the same point, indicated with a marker. Nonetheless, the goal of the filter is not a pose correction, but there is the self-calibration of the inertial sensors.

Besides, taking advantage of the rosbag [25] technology offered by the ROS middleware, the results of Experiments 3, 4 and 5, obtained with our MESKF, were compared with the results obtained using a standard approach of multiplicative extended Kalman filter (MEKF). The node ekf_localization of the ROS package robot_localization [41] was used to that end. This node implements an multiplicative (orientations represented with quaternions) EKF that permits the integration of six-DOF nominal data coming from several sensors (in our case, the acceleration and the angular rate of an IMU, the pose and twist of a visual odometer and the depth given by the pressure sensor) and returns a refined state vector composed of pose and twist. In the standard EKF, the nominal variables, all affected by additive errors, are inserted in the Kalman filter, while in the MESKF, only the error state is filtered, but its result is added to the nominal state. Both systems should lead to similar results, because both include, integrate, estimate and evolve the error of the system model and the nominal data in different ways. However, the main differences between both approaches are: (1) no error variables are included in the EKF state vector; consequently, the systematic errors of the inertial sensors are not compensated for; (2) there are no significant differences in the estimated 3D robot trajectory; but, since the drift of the gyroscope is not corrected, appreciable irregularities in the returned orientations are obtained with the standard EKF.

Figure 11 corresponds to an L-shaped trajectory (Experiment 3). The figure shows the trajectory in 3D and 2D, the depth, the acceleration bias, gyro drift and the robot orientation according to the visual odometer and the filter. This figure also shows the vehicle orientation returned by the ROS EKF package [41].

Figure 11. Experiment 3 in the sea using LibViso2 as the stereo odometer. An L-shaped trajectory. (a) Robot trajectory in 3D; (b) depth; (c) robot trajectory in the x-y plane; (d) acceleration bias; (e) gyro drift; (f) vehicle attitude according to the odometer; (g) vehicle attitude according to the robot operating system (ROS) EKF [41]; (h) vehicle attitude according to our MESKF.

Figures 12 and 13 correspond to the two sweeping trajectories conducted in the sea (Experiments 4 and 5). Both figures show the trajectory in 3D and 2D, the depth and the robot orientation, both according to LibViso2 and the MESKF, as well as the acceleration bias and the gyro drift given by the filter. These figures also show the vehicle orientation returned by the ROS EKF package [41]. The length of both experiments exceeds 40 meters. Figure 14 shows four images captured during Experiments 3, 4 and 5. Figure 14d shows the artificial marker deposited at the sea bottom to indicate the starting/end point of the trajectory.

Figure 12. Experiment 4 in the sea using LibViso2 as the stereo odometer. A sweeping trajectory. (a) Robot trajectory in 3D; (b) depth; (c) robot trajectory in the x-y plane; (d) acceleration bias; (e) gyro drift; (f) vehicle attitude according to the odometer; (g) vehicle attitude according to the ROS EKF [41]; (h) vehicle attitude according to our MESKF.

Figure 13. Experiment 5 in the sea using LibViso2 as the stereo odometer. A longer sweeping trajectory. (a) Robot trajectory in 3D; (b) depth; (c) robot trajectory in the x-y plane; (d) acceleration bias; (e) gyro drift; (f) vehicle attitude according to the odometer; (g) vehicle attitude according to the ROS EKF [41]; (h) vehicle attitude according to our MESKF.

Figure 14. Figures (a–d) show some images gathered during the experiments in the sea. (d) also shows the artificial marker that indicates the stating point.

The results of these experiments lead to the next discussion:

• The drift in z generated by the visual stereo odometer was corrected by including the pressure sensor data in the filter. In these experiments, the difference between the odometry and the filter estimates in the (x,y) plane is nearly zero. Some settings in the sea are slightly different than in the pool, for example the initial error state covariances and, especially, the odometry covariances (given directly by the visual odometer library and clearly influenced by the type of environment and, thus, the features found in it) and the covariances of the inertial data, which differ from those obtained in the pool, since the motion of the vehicle is conditioned by the slight sea current and the actions of the vehicle PID to compensate for this.

• Trajectories given by the ROS EKF have not been printed, because they are nearly equal to the trajectories estimated by the visual odometer; similar to the odometer, the vehicle attitude estimated by the ROS EKF has non-negligible values, with some maxima and minima close to 0.5 rad, which are not adjusted to its real behavior.

• The orientation of the vehicle in roll and pitch was clearly adjusted by the MESKF to values close to zero, thanks to the rapid compensation of the gyro drift.

• The acceleration biases given by the MESKF approximate a constant; the desirable situation would be to obtain biases stabilized to zero for motions at constant speed; however, in the sea, accelerations in the x axis, which is the direction of progress, are prone to appear periodically, since the vehicle is constantly trying to overcome the effects of the currents by applying, from time to time, in the forward direction, a certain force given by the PID controller; this problem is not so evident in the pool, where it is much easier to keep the vehicle moving at a constant velocity without interferences.

Figure 15 shows the evolution of the variables included in the error state vector during Experiment 4. All errors are bounded in a small range: position errors are bounded between ±10 cm; velocity errors are bounded between ±0.5 m/s (except a sample that reaches 1.5 m/s); orientation errors are bounded between approximately ±0.01 radians (0.6°); errors in angular rate are negligible; and errors in acceleration are bounded between approximately 0.02 rad/seg² and −0.015 rad/seg². Values of errors in angular rate and orientation (which include biases and noise) are remarkably small, which means that, according to the previous discussions, attitudes estimated by the filter are highly reliable and much closer to reality than those given by the odometry and by the ground truth algorithm used in this work.

Figure 15. Values of the error state variables during Experiment 4. (a) Error in position; (b) error in linear velocity; (c) error in attitude; (d) error in angular rate; (e) error in linear acceleration.

5.3. 3D Reconstruction

Stereo video sequences grabbed in the Port of Valldemossa in the ROS-bag [25] format were played offline to simulate the process of building online the 3D map of the environment by concatenating the successive point clouds. The dense point cloud generation was performed at the same rate as the image grabber (10 frames/s), permitting the reconstruction of the environment in real time. The correction in the vehicle estimated attitude increases the precision in the assembly of these point clouds, resulting in a realistic 3D view of the scenario where the robot is moving.

Figure 16a,b shows two different 3D views of the marine environment where Experiment 3 was performed. The successive point clouds were registered using the vehicle odometry pose estimates. Figure 16c,d shows two 3D views of the same environments, but registering the point clouds using the vehicle pose estimates provided by the MESKF. In all figures, the starting point and the direction of motion are indicated with a red circle and a red arrow, respectively. A marker was placed in the ground to indicate the starting/end point.

Figure 16. Experiment 3. (a,b) Two different views of the environment reconstructed in 3D, using the odometry to concatenate the point clouds; (c,d) two different 3D views of the environment concatenating the point clouds according to the MESKF estimates. In all cases, the red circle indicates the starting point where the marker was deposited, and the red arrow indicates the direction of motion.

Figure 17a,b shows two different 3D views built during Experiment 4, registering the successive point clouds with the vehicle odometry pose estimates. Figure 17c,d shows two 3D views of the same environments, but registering the point clouds using the vehicle pose estimates provided by the MESKF. Again, in all figures, the starting point and the direction of motion are indicated with a red circle and a red arrow, respectively.

Figure 17. Experiment 4. (a,b) Two different views of the environment reconstructed in 3D, using the odometry to concatenate the point clouds; (c,d) two different 3D views of the environment concatenating the point clouds according to the MESKF estimates. In all cases, the red circle indicates the starting point where the marker was deposited, and the red arrow indicates the direction of motion.

Figure 18 shows three images of Fugu-C navigating in this environment. In Figure 18c, the artificial marker deposited on the sea ground can be observed at the bottom of the image.

Figure 18. Images (a–c) show three different views of Fugu-C navigating in the Port of Valldemossa.

Raw point clouds are expressed with respect to the camera frame, but then, they are transformed to the global coordinates frame by composing their camera (local) coordinates with the estimated vehicle global coordinates. 3D maps of Figures 16a,b and 17a,b show clear misalignments between diverse point clouds. These misalignments are due to the values of the estimated roll, and pitch vehicle orientations are, at certain instants, significantly different from zero. As a consequence, the orientation of the point cloud has a value different from zero, causing them to be inclined and/or displaced with respect to those immediately contiguous and causing also the subsequent point clouds to be misaligned with respect to the horizontal plane. This effect is particularly evident in Figure 16a,b, where very few point clouds are parallel to the ground, most of them being displaced and oblique with respect to the ground.

However, as the vehicle orientations in roll and pitch estimated by the MESKF are all approximately zero, all of the point clouds are nearly parallel to the ground plane, without any significant inclination in pitch/roll or important misalignment, and providing a highly realistic 3D reconstruction. Notice how the 3D views shown in Figures 16c,d and 17c,d coincide with the trajectories shown in Figures 11 and 12, respectively.

The video uploaded in [42] shows different perspectives of the 3D map built from the dataset of Experiment 4, registering the point clouds with the odometry (at the left) and with the filter estimates (at the right). Observing the 3D reconstructions from different view points offers a better idea of how sloping, misaligned and displaced with respect to the ground, can be some of the point clouds, due to those values of roll and pitch different from zero. The improvement in the 3D map structure when using the filtered data is evident, as all of the point clouds are placed consecutively, totally aligned and parallel to the sea ground.