Relative Vessel Motion Tracking using Sensor Fusion , Aruco Markers , and MRU Sensors

This paper presents a novel approach for estimating the relative motion between two moving offshore vessels. The method is based on a sensor fusion algorithm including a vision system and two motion reference units (MRUs). The vision system makes use of the open-source computer vision library OpenCV and a cube with Aruco markers placed onto each of the cube sides. The Extended Quaternion Kalman Filter (EQKF) is used for bad pose rejection for the vision system. The presented sensor fusion algorithm is based on the Indirect Feedforward Kalman Filter for error estimation. The system is self-calibrating in the sense that the Aruco cube can be placed in an arbitrary location on the secondary vessel. Experimental 6-DOF results demonstrate the accuracy and efficiency of the proposed sensor fusion method compared with the internal joint sensors of two Stewart platforms and the industrial robot. The standard deviation error was found to be 31mm or better when the Arcuo cube was placed at three different locations.


Introduction
Complex offshore load operations are usually carried out by the current industry state-of-the-art Active Heave Compensated (AHC) cranes, which are capable of decoupling the floating vessel's motion and the hanging load's motion by using a Motion Reference Unit (MRU) to measure the vessel's motion in real-time.As a result, the AHC crane is capable of controlling the load's height above the seabed for instance, which again will reduce the risk of destroying the hanging load when lowering it onto the seabed.These AHC cranes and MRU sensors have been tested in the offshore industry for years, and have been an important enabler for more advanced and efficient offshore operations in general.In future offshore load handling operations, an increased level of complexity is expected due to an increased level of offshore activities such as: floating wind turbines, remote fish farms and autonomous shipping, to mention some.A common challenge for all these operations is that cargo, equipment and person-nel have to be transferred between two floating installations.This is the main motivation for investigating the Vessel-to-Vessel Motion Compensation (VVMC) problem (see Figure 1 for illustration) which is seen as an extension of the AHC techniques used today.In VVMC, it is necessary to establish a method for measuring the relative motion between two floating vessels in realtime.As a result of measuring these motions precisely and efficiently, the load handling equipment such as a crane could be used to keep a hanging load in a fixed position relative to the secondary vessel.This is an important enabler for load transfer between two vessels during harsher weather conditions than allowed for today.Today such operations are limited by a so-called weather window, which only allows for any load or personnel to be transferred from one vessel to another if the significant wave height is below typically 2.5 meters (Kjelland (2016)).By introducing VVMC, load transfer between two vessels during harsher weather conditions may be allowed in the future.As a result, such operations can be carried out in a safer and more efficient way.Also, the operation cost may be reduced, given the fact that the time spent in waiting for better weather conditions to actually carry out the load transfer may be drastically reduced.
As an initial step towards solving the VVMC problem, a suitable method for measuring the relative motion between two floating vessels is needed.A method for measuring these relative motions using MRU sensors is motivated by the fact that MRUs have been successfully used for measuring vessel motions for many years in the industry.In addition, extensive research towards robust filtering algorithms using Inertial Measurement Units (IMU) and gyroscopes have been investigated by Küchler et al. (2011b); Richter et al. (2014); Küchler et al. (2011a) to mention some.It is therefore assumed that MRUs or IMUs with accompanying filtering algorithms are capable of measuring the motions of the two independent vessels with sufficient accuracy.However, in VVMC it is not enough to only measure the motion of each vessel independently, but rather to measure the relative motion between them.It is therefore believed from our previous experience and research presented by Tørdal et al. (2016), that a third sensor like a camera vision system capable of measuring the absolute motions between the two floating vessels can be used together with two wirelessly connected MRUs placed onto the two vessels in order to track the relative motions in real-time.Extensive research within the field of motion tracking using camera vision has already been carried out, and efficient programming libraries such as OpenCV (Itseez (2015)) provide a lot of functions which can be used for tracking rigid objects seen by a camera.One of the most recent methods which is capable of measuring all 6 Degrees of Freedom (DOF) of a fiducial marker is provided by the Aruco add-on library for OpenCV presented by Garrido-Jurado et al. (2014).By using this programming library together with a suitable camera and two MRUs, the authors believe that a proof-of-concept for tracking the relative vessel motions is realizable.
In this paper, a sensor fusion algorithm combining the measurements acquired from two independent MRUs placed onto two moving vessels, and a camera vision tracking system capable of measuring all 6 DOFs are presented.An extended Kalman filter has been used to improve the camera vision tracking performance, and a linear Kalman filter has been applied to estimate the error between the vision and the MRU measurements.In addition, the presented method is self-calibrating, meaning that the secondary MRU can be placed in any arbitrary location at the secondary vessel.As a final experiment, the equipment in the Norwegian Motion Laboratory is used to verify the effectiveness and accuracy of the proposed solution to the VVMC motion tracking problem.

Problem Formulation and Experimental Lab Setup
In VVMC, the main goal is to safely transport either personnel or cargo between two floating vessels which can be two ships, offshore platforms, floating wind turbines etc.The detection of the relative motions between the two floating vessels in real-time is considered a crucial task which has to be solved in order to carry out the motion compensation task using offshore cranes or other robot-like equipment.The relative motion between the two floating ships consists of a positional offset, and an orientation offset.In robotics, these motions are often described using homogeneous transformation matrices, which describe the geometric transformation between the two body coordinates of body-A {b A } relative to body-B {b B }.An illustration of two floating vessels laying alongside each other at sea is given by Figure 1.
Figure 1: The VVMC problem where a load is supposed to be transported from vessel/ship A onto vessel/ship B.
The figure illustrates both the body-fixed coordinate systems of vessels A and B, a load handling equipment (industrial robot), and a suspended load hanging in the end of the industrial robot (end-effector).The industry standard equipment used to detect the motion of floating vessels relies heavily on the use of MRU sensors to measure the heave, roll and pitch motions of the vessel.These sensors are proven to give precise and reliable measurements and have been used for many years in the application of offshore AHC cranes.However, in VVMC, it is desired to keep the hanging load in some desired height above the secondary vessel's ship deck, or even in a given orientation relative to some coordinate system defined on the secondary floating vessel.To experimentally test and investigate the effectiveness of the proposed method, the lab setup shown in Figure 4 is used.Figure 2 illustrates the experimental lab setup used to carry out the VVMC experiments presented in this paper.As seen from the figure, the camera (Logitech C930e) is mounted onto the biggest Stewart platform (E-Motion 8000) in front of the industrial robot (Comau SMART-5 NJ 110-3.0)used to simulate the load handling equipment.One of the MRUs, namely MRU1, is placed on top of the biggest Stewart platform, and a secondary MRU (MRU2) is placed inside the Aruco cube.By placing the second MRU inside the Aruco cube it is possible to use the camera to measure the absolute orientation and position offset between the two body-fixed coordinates of MRU1 and MRU2.This absolute measurement is an important enabler for the proposed sensor fusion system which is combining the MRUs and the vision system.The proposed sensor fusion algorithm is illustrated in Figure 3.The algorithm relies on the measurements acquired from two MRUs and a vision tracking system used to measure the absolute position and orientation offset between the two body-fixed coordinates of the MRUs.In addition, an important feature of the proposed solution is the fact that the Aruco cube can be placed at any location onto the secondary vessel's ship deck.This enables for easier installation and more flexible operation, since the proposed solution has the ability for automatic self-calibration.
To carry out the VVMC lab-experiment, a common control and logging interface is needed.Figure 4 illustrates how the laboratory equipment is connected to the main control unit delivered by Beckhoff Automation (CX2040).The communication network consists of three different types of communication: SERCOS, POWERLINK, and Ethernet.While SERCOS and POWERLINK are both hard real-time and deterministic communication protocols, the UDP communication protocol using a conventional Ethernet connection is not proven to be real-time, nor deterministic.However, the author's experience shows that the UDP interface has demonstrated to give satisfactory performance since the wave motions to be simulated in the laboratory are found in the range of 8-20s in typical wave periods.

Vision Tracking of Aruco Cube
In order to use the acquired measurements from both the MRUs placed onto the two vessels, a third sensor is needed (Tørdal et al. (2016)) to measure the absolute position and orientation H 1 2 between the two body-fixed coordinate frames {b 1 } and {b 2 } of MRU1 and MRU2 (see Figure 2).A camera vision tracking method capable of measuring all 6 DOFs between the camera body fixed coordinate system {b c } and the sec-ondary MRU's body-fixed coordinate system {b 2 } is proposed as a possible solution.A picture showing the lab set-up used to simulate the suggested VVMC sensor fusion algorithm is given in Figure 5.

Aruco Marker Detection using OpenCV
To measure all 6 DOFs using the camera placed in front of the robot, it was chosen to use a square cube and place an Aruco marker on each of the cube sides except the side which is facing down.The reason for having a cube is motivated by the fact that it is possible to detect one of the markers given any orientation of the cube, as long as the bottom of the cube is facing downwards.The considered Aruco cube which is a square cube with an edge length of 39 cm is illustrated in Figure 6.By using the Aruco add-on library supported by OpenCV (Garrido-Jurado et al. ( 2014)), it is fairly straightforward to make a C++ program capable of tracking each of the Aruco markers: identification, position and pose relative to the camera at a typical update cycle ranging in between 40-100ms.The update speed of the algorithm is like for most vision applications, heavily influenced by the selected image resolution.The position and orientation of the i'th Aruco marker can be defined as: where H c i is the homogeneous transformation matrix between the i'th marker and the camera (also known as the extrinsic parameters), R q (q i ) and q i is the i'th rotation matrix and the quaternion respectively, describing the i'th marker's pose given in the camera coordinate system, p i is the positional vector describing the position of the i'th marker, and N m is the number of Aruco markers detected in the processed picture.R(q i ) is the function converting a quaternion into a rotation matrix as: 2   1 2 − q 2 y + q 2 z q x q y − q 0 q z q 0 q y + q x q z q 0 q z + q x q y −q 2 x − q 2 z + 1 2 q y q z − q 0 q x q x q z − q 0 q y q 0 q x + q y q z −q (2) where q 0 , q x , q y , q z are the quaternion coefficients describing the quaternion.The quaternion is defined by: where i, j, k are the fundamental quaternion units which all square to -1.To further understand how the quaternion can be used to describe the orientation of a rigid body relative to a given coordinate system, the following definition is used: where n is a unit axis in SO(3) and θ is the rotation of the rigid body around axis n in radians.The quaternion has many useful and superior properties compared to Euler-angles especially.However, the reason for using quaternions is motivated by the fact that several (N m ) Aruco markers are measured and an average of these measurements are desired.The averaging method is presented in the next subsection.

Position and Quaternion Averaging
Given that we have N m measurements of the Aruco marker position and orientation relative to the camera, it is straightforward to calculate the average position p as: where a i is the area of the i'th marker in the picture frame, p i is the measured position vector to marker number i, and A is the total area of all the markers found in the current picture.The area of the i'th marker is found by using the shoelace formula (Braden (1986)) which calculates the area formed by four image points as: where  As for the positions p i to each of the Aruco markers, also the orientation of each marker q i is processed to obtain the average quaternion q of all N m detected markers.However, averaging a quaternion is not as straightforward as for the positions since quaternions are not linearly independent.The quaternion averaging method of Markley et al. (2007) has therefore been applied to solve this problem.The principle used is that all the measured quaternions are stacked into a measurement matrix Q using the following expression: where q i q T i is defined by Equation ( 9).
q 2 0 q 0 q x q 0 q y q 0 q z q 0 q x q 2 x q x q y q x q z q 0 q y q x q y q 2 y q y q z q 0 q z q x q z q y q z q 2 To find the averaged quaternion q, the eigenvalue problem of measurements matrix Q has to be solved using the eigenvalue decomposition.The eigenvalue problem is defined as: where the average quaternion q is given by the eigenvector v corresponding to the smallest eigenvalue λ.
As an enabler to carry out the quaternion averaging in real-time, the Eigen library (Guennebaud and Jacob (2010)) supported by C++ is used to implement the quaternion averaging algorithm.

Image Model and Camera Calibration
To obtain correct physical measurements of the Aruco cube position and orientation (extrinsic parameters), the camera's intrinsic parameters have to be calibrated properly.These parameters describe the relation between the 2D image pixels (x i , y i ) and the real world coordinates (x, y, z).The relationship between the image pixels and the world coordinates is modeled using the pinhole camera model given by: where f x and f y are the camera focal lengths in the xand y-direction, (c x , c y ) is the optical center expressed in pixels, and K is known as the camera calibration matrix which can be found from taking several pictures of a checkerboard and process them using the Matlab camera calibration toolbox.The OpenCV library also features a camera calibration functionality, but it is not as user friendly as the Matlab toolbox.One of the pictures used to calibrate the camera is given in Figure 8.
Figure 8: One of the pictures used to calibrate the camera using the Matlab camera calibration toolbox.
In addition to calculate the camera calibration matrix K, it is also necessary to remove the radial image distortion.The mathematical relationship between the corrected image pixels (x i , y i ) and the radial distorted pixels (x d , y d ) is given by: where {k 1 , k 2 , k 3 } are the radial distortion coefficients which are also found using the Matlab calibration toolbox.In addition to the radial distortion, tangential distortion may occur if the camera lens is not perfectly parallel to the image sensor plane.However, it is in general common to ignore this since it can be assumed that the lens is more or less parallel to the image sensor.

Extended Quaternion Kalman Filter (EQKF) for Bad Pose Rejection
Subsection 3.2 presented an averaging method of all the detected markers in the camera picture.The observed noise in the averaged measurements p and q was not considered small enough to be used directly in the sensor fusion algorithm which will be discussed later.Since the orientation problem is described using a quaternion, an Extended Quaternion Based Kalman Filter (EQKF) is used to estimate the Aruco cube position and orientation.The approach presented here is inspired by the approach used by Kraft (2003), Pawlus et al. (2016) and Marins et al. (2001).The state-vector of the combined position and orientation estimation problem is: where p, ṗ and p are the position, velocity and acceleration of the Aruco cube, q is the orientation quaternion, and ω, ω are the rotational velocities and accelerations in the body-fixed coordinate system of the Aruco cube.The vector components of p, q and ω are given by Equation ( 16).
It is also required that the orientation quaternion is kept unitary at all times, meaning that both the estimated quaternion q and the measured quaternion q are constrained as:

Process and Measurement Model
The state transition and observation model is given as: where x k is the current state, w k−1 is the previous process noise, z k is the current measurement vector, h(x k ) is the current measurement model, and v k is the current measurement noise.Both the process and the measurement noise are assumed to have covariances Q k and R k with zero mean value.The non-linear state transition function is defined by the following equation: where ∆t is the time step of the proposed vision algorithm (65 ms), and q is the time-differentiated quaternion which is defined by: q = 1 2 where q w is the rotational velocity vector ω treated as a quaternion with zero scalar component, and ⊗ represents the quaternion product.The measurement function h(x) is fairly simple since both the position p and the orientation q are measured directly as: where both the measured position and the orientation are obtained from the averaged measurements as they are defined in Subsection 3.2.

State Estimation and Update Equations
The discrete-time model of the process and measurement model is defined as as: where F k−1 is the linearized state transition matrix evaluated at the previous state estimate xk−1 , and H k is the linearized measurement matrix evaluated at the current predicted state x− k .The state transition matrix and measurement matrix are given by the following two equations: Given these equations it is possible to predict and correct the state estimation.The prediction equations are defined as: where x− k is the current state prediction, and P − k is the predicted covariance.Based on the prediction, the model may be corrected given that the measurement z k is occurring.The measurement correction equations are defined as: where K k is the current Kalman gain, xk is the current state estimate, and P k is the current covariance matrix.
The process and measurement covariance matrices are constant for all k and are defined as identity matrices with a constant gain on each of the diagonal elements.
The covariance matrices Q k and R k are defined as: where the process variance σ 2 q and the measurement variance σ 2 r are manually tuned to give satisfactory performance and noise rejection.The actual parameter values for σ 2 q and σ 2 r are given in the Appendix.The C++ source files for the Aruco box tracking algorithm are available at: https://github.com/sondre1988/vision-tracker/tree/master.

Hand-Eye Camera Calibration
In addition to the Aruco marker detection and the proposed EQKF algorithm, it is also necessary to know where the camera is relative to the robot base coordinate system {b b }.To find these calibration parameters, a two-step procedure has to be carried out; first is to find the location of a marker relative to the end-effector coordinate system {b t } of the robot, second is to find the camera coordinate system {b c } relative to the robot base {b b }.The first problem is known as the hand-eye calibration problem, and has been investigated using both closed form solutions and iterative approaches to successfully solve the matrix equation: where A n is the n'th movements of the robot end effector, B n is the n'th movements of the marker attached to the robot end-effector, and X is the unknown location and orientation of the marker relative to the robot end-effector.By measuring k robot poses H b t,k and k camera observations H c m,k of the marker attached to the robot end-effector.The matrix A n and B n are found by using the following two equations: To actually solve the hand-eye calibration problem given that k measurements are carried out, the closedform solution presented by Park and Martin (1994) is used since it serves as an efficient and elegant solution when several measurements are present.Later on, also Baillot et al. (2003) have demonstrated the efficiency and practical use of the hand-eye calibration algorithm presented by Park and Martin.The unknown homogeneous transformation matrix describing the marker location relative to the robot end-effector is given by: where R X is the unknown rotation matrix, and t X is the unknown translation vector.The proposed leastsquares solution presented by Park and Martin utilizes the following equations to solve the hand-eye calibration problem.The logarithm of a matrix is defined as: where log(R) is defined as the logarithm of an orthogonal rotation matrix R. The angle θ is defined as: where Tr(R) is the trace of matrix R. In order to solve for the rotation matrix R X , it is necessary to take the logarithm of the rotation matrices R An and R Bn and stack them into a measurement matrix, according to: where M is the measurement matrix used to solve for the unknown rotation matrix R X using: where (M T M ) − 1 2 can be found using the eigenvalue decomposition or the singular value decomposition (SVD).The eigenvalue decomposition is used as described by (M T M ) − 1 2 = V D − 1 2 V −1 .When the rotation matrix R X has been determined, the unknown position t X can be found from applying the least-squares solution described as: where matrix C and vector d are given by the following equations: A Matlab function of the hand-eye calibration algorithm has been developed and is available at https: //github.com/sondre1988/matlab-functions/blob/master/src/HandEyeParkMartin.m

MRU and Vision Sensor Fusion
As motivated in both the introduction and the problem formulation, the desire of tracking the relative motions between two floating vessels in real-time is of high importance when it comes to solving the VVMC problem.
In this section, a sensor fusion system utilizing the visual tracking system described in Section 3 and the MRU sensors will be presented.

Kinematic Model
A kinematic model of the camera relative to the MRUs and the robot base is needed in order to utilize the acquired measurements from the camera and the MRUs in a suitable way. Figure 9 is used to illustrate the kinematic model and the accompanying coordinate systems which are involved.
Figure 9: Coordinate systems as they are used in the sensor fusion algorithm to describe the relative position and orientation between the MRUs.
The following notation is assumed in Figure 9: {b 1 } and {b 2 } are the body-fixed coordinates of MRU1 and MRU2, {h 1 } and {h 2 } are the heading coordinates of MRU1 and MRU2 which are equal to the body-fixed coordinates when both the roll and pitch angles are equal to zero.The z-axis of both heading coordinates is always pointing downwards.The camera body coordinate is denoted as {b c }, the robot base {b b } and the robot end-effector {b t } are only denoted using their body-fixed coordinates.The offset position and orientation between the two heading frames {h 1 } and {h 2 } is given by: where ∆x, ∆y and ∆z are the offset positions, and ∆ψ is the offset heading angle.To fully determine the body-fixed location and orientation between the two MRUs, the roll and pitch measurements from both the MRUs are simply added as: where H 1 2 is the offset between the two body-fixed MRU coordinates.

Sensor Fusion Algorithm
The objective of the sensor fusion algorithm is to estimate the homogeneous transformation matrix X 1 2 between the two heading frames as precisely as possible combining the measurements of the two MRUs and the camera vision system presented in Section 3. The sensor fusion algorithm which is shown in Figure 3 estimates X 1 2 using the measurements z 1 and z 2 .This filter structure is named Indirect Feedforward Kalman Filter for error estimation (Sasiadek and Hartana (2000)).The filter algorithm utilizes two measurements: z 1 which represents the changes in X 1 2 and z 2 which serves as an absolute measurement of X 1 2 .By simply integrating z 1 it is possible to define an error measure z e as: and use a linear Kalman filter to find an estimated error ê and add this to measurement z 1 such as: where X1 2 represents the estimated position and orientation offset between the two MRU heading frames.The Kalman filter state-vector is then defined as: where ∆x e , ∆y e and ∆z e are the position errors and ∆ψ e is the heading angle error.It is assumed that a linear process model describing the error measurements is satisfactory since the change in ∆ψ e is most likely to be smaller than ±5 • .The error process model is given by: where ∆t is the time step used in the sensor fusion algorithm (∆t = 4ms).The measurement model is described as: indicating that the error can be measured directly using the camera and the MRUs.However, this is not completely true if the kinematic equations describing X 1 2 and H 1 2 are considered in detail.Anyway, as an engineering approach it is assumed that the measured roll and pitch angles from both the MRU sensors are very accurate and reliable, and hence the following equations can be used to calculate the two measurements z 1 and z 2 which are defined as: where ∆ ẋ1 , ∆ ẏ1 and ∆ ż1 can be calculated as: where v 1 and v 2 are the measured velocity vectors of MRU1 and MRU2 represented in the first heading coordinate system {h 1 } of MRU1.These two velocities are found by the following two equations: where v 1 and v 2 are the linear velocities measured along the body-fixed coordinate axes of MRU1 and MRU2, respectively.The turn rate difference ∆ ψ1 between the two heading coordinate systems is given by: where ψ1 and ψ2 are the two MRU measured turn rates as illustrated in Figure 9.
The second measurement used in the sensor fusion algorithm is found by measuring X 1 2 directly using the following equation: where H 1 c is the known camera location and orientation relative to MRU1, and Ĥc 2 is the EQKF estimated orientation and position of MRU2 relative to the camera body-fixed coordinate system.Using Eq. ( 41) it is possible to write X 1 2 as: By comparing the coefficients of Eq. ( 53) and Eq. ( 54) it is fairly straightforward to calculate the second measurement z 2 as: The resulting equations is not given in detail since they are found by utilizing the symbolic toolbox of Matlab and are fairly long.By using the proposed linear process model, measurement model, and the error measurements z e it is possible to find the estimated version of X1 2 using the following prediction and correction equations: where the state transition matrix A and the measurement matrix H are found from taking the partial derivative of Eq. ( 46) and Eq. ( 47) respectively: The prediction equations are given as: and the corresponding correction equations as: where Q and R are the diagonal process and measurement covariance matrices given by: The actual values for σ 2 q and σ 2 r are manually tuned to give satisfactory performance and the resulting values are given in the Appendix.

End-effector Position Relative to the Secondary Vessel
Recalling Figure 9, it is seen from the coordinate systems that the robot end-effector is supposed to be kept in a constant position relative to the secondary MRU placed onto the secondary vessel's ship deck.The kinematic control of the robot is not considered the main contribution in this work, a simplified user input H 2 t is therefore suggested and used for the final experimental verification.The user input is given as: where x u , y u and z u are the user defined positions of the robot end-effector relative to the body-fixed coordinate of MRU2 .The roll and pitch movements of MRU2 are used to always ensure that the last defined user input z u is placing the end-effector in a desired height above the secondary ship deck in the direction of the world fixed z-axis.To finally carry out the VVMC task, the robot pose H b t has to be calculated using: where Ĥ1 2 is found from Eq. ( 42) where X 1 2 is substituted with X1 2 which is a result of the proposed sensor fusion algorithm.Finally, the joint space angles q of the robot are found through applying the Inverse Kinematics (IK) algorithm given by: The IK algorithm is not described in detail since it depends on the specific load handling equipment used to carry out the VVMC task.

Experimental Results
In this section, the result of applying the proposed sensor fusion algorithm is to be investigated.The motions of both the Stewart platforms are prescribed by stochastic wave motions defined by the Pierson-Moskowitz wave spectrum as presented by Pierson and Moskowitz (1964).The two Stewart platforms move asynchronously including all 6 DOF according to the wave spectrum prescribed by a typical wave period ranging in between 8 to 14 s.A more complex model to describe the motions of both the Stewart platforms could be investigated.However, it is assumed that the asynchronous stochastic wave motions is sufficient to describe the vessel motions.

Camera Filter Performance
The proposed EQKF algorithm presented in Section 3 is supposed to reduce the noise in the averaged position p and orientation q measurement of the Aruco cube.As a verification of the proposed algorithm, several plots comparing the averaged and the estimated curves are acquired from the physical experiments.The resulting x-, y-and z-direction EQKF performance curves are given in Figures 10, 11 and 12.As seen from the figures, it is clear that both the xand y-positional measurements are significantly more precise than the z-position measurements.This result is expected since the measurement accuracy in computer vision systems depends on the image pixel density.Therefore, the z-position contains more noise since small changes in the z-direction will cause the marker edges to move across more pixels in the image if compared to x-and y-movements.The image resolution used in our experiments was set to 960 × 540 pixels since this resolution gave an acceptable update rate at 65ms and sufficient measurement accuracy.The filter performance for the orientation quaternion is also analyzed in the same way as for the positions, and is shown in Figures 13, 14 By analyzing the curves representing the averaged quaternion q and the estimated quaternion q it is clear that also the orientation contains more noise due to the somewhat low resolution used in the proposed method.A higher camera resolution would give more accurate measurements for both the z-position and the orientation quaternion, but the resulting slower update rate would reduce the overall performance.However, an FPGA implementation of the vision tracking algorithm could result in both high measurement resolution and fast computation.

Sensor Fusion Performance
The sensor fusion algorithm presented in Section 4 heavily depends on the vision tracking of the Aruco cube.It is therefore interesting to investigate how much of this noise it is possible to remove using the proposed sensor fusion algorithm.To validate and investigate the sensor fusion performance, the estimated measurements and the unfiltered measurements are compared in Figures 17,18,19

Compensation Performance
The final goal of the proposed method is to see whether the Aruco box could be placed anywhere on the secondary vessel ship deck, where the only constraint is that the body-fixed z-axis of the secondary MRU sensor is pointing towards the vessel ship deck.Using the resulting measurements, the robot end-effector should be kept in a given height above the plane spanned by the ship deck.The Aruco cube was therefore placed in three different locations in order to verify that the proposed solution was actually capable of self-calibrating.The resulting curves indicate that the three different experimental locations of the Aruco cube give somewhat the same result in terms of overall compensation accuracy.Anyway, a more quantitative measure of the overall compensation accuracy is given in Table 1, where the accuracy is summarized in terms of RMS error, absolute maximum deviation, standard deviation and mean deviation.From the table, it can be seen that the standard deviation is almost the same for all three test locations, while the mean deviation is indicating that there is a constant offset which is much likely to originate from some imperfections in the calibration matrices H 1 b used to describe the offset between MRU1 and the robot base.This calibration matrix was measured by hand, and is therefore more likely to contain some errors.In future work, a more sophisticated method for determining these calibration parameters can be utilized, such as the one presented by Mirzaei and Roumeliotis (2008) for instance.

Discussion and Conclusions
In this paper, a sensor fusion algorithm capable of measuring all 6 DOF between two floating vessels is investigated.The proposed method utilized two MRU sensors and a camera vision system to track the relative motions in real-time.The camera vision system using an Aruco cube was able to provide an absolute measurement between the body-fixed coordinates of the two MRUs.This information was useful in terms of: ability to self-calibrate the motion tracking system, remove drift in the MRU measurements, and provide a visual feedback to the operator through the augmented reality indicating the detected coordinate system of the Aruco cube.The overall accuracy for all three test locations resulted in a maximum standard deviation of 31 mm.In addition, the sensor fusion algorithm demonstrated the ability to self-calibrate since the Aruco cube was placed in three different locations onto the secondary Stewart platform.However, it is observed that, the results suffer from a constant mean error of roughly 30 mm, which may indicate that some small errors might occur in the calibration parameters such as MRU1's location relative to camera, for instance.In addition to the calibration error, the overall time-delay of approximately 65 ms due to the camera processing is also contributing to the overall compensation error.
Future work in improving the relative motion tracking between two vessels should focus on reducing the overall time-delay in the camera processing algorithm and use a higher camera resolution for higher accuracy.Likewise, the proposed sensor fusion algorithm should be extended to also estimate the MRU drift in addition to only estimate the error between the MRUs and the camera vision system.This might enable for motion compensation even when the camera is not capable of detecting the Aruco cube for some small time periods.

Figure 2 :
Figure 2: The Norwegian Motion Laboratory located at the University of Agder.

Figure 3 :
Figure 3: Illustration of the proposed sensor fusion algorithm combining the camera and the MRU measurements.

Figure 4 :
Figure 4: Communication network used to operate all the lab equipment from a common control unit (Beckhoff CX2040).

Figure 5 :
Figure 5: Experimental lab setup for VVMC in the Norwegian Motion Laboratory.

Figure 6 :
Figure 6: Aruco cube with an MRU placed inside the cube.
are the x-and y-pixel coordinates of the Aruco corner points returned by the Aruco detection function in OpenCV.The location of the Aruco corner points is further illustrated in Figure 7.

Figure 7 :
Figure 7: Corner points of each Aruco marker as they are represented in the OpenCV Aruco library.

Figure 12 :
Figure 10: Positional comparison in the x-direction.

Figure 20 :
Figure 18: Sensor fusion performance in y-direction.
The robot end-effector was then controlled to carry out the VVMC task based on the resulting sensor fusion measurements.The three different test locations are presented in Figures 21, 22 and 23.

Figure 21 :
Figure 21: Experimental location number 1 seen from the camera.

Figure 22 :
Figure 22: Experimental location number 2 seen from the camera.

Figure 23 :
Figure 23: Experimental location number 3 seen from the camera.