Artur WolekJames McMahonDepartment of Mechanical Engineering and Engineering Science, University of North Carolina at Charlotte, Charlotte, NC, 28223 USA (e-mail: awolek@charlotte.edu).Physical Acoustics Branch, Code 7135, Naval Research Laboratory, Washington, DC, 20375 USA (e-mail: james.w.mcmahon8.civ@us.navy.mil)
Abstract
This paper presents three batch estimation methods that use noisy ground velocity and heading measurements from a vehicle executing a circular orbit (or similar large heading change maneuver) to estimate the speed and direction of a steady, uniform, flow-field. The methods are based on a simple kinematic model of the vehicle’s motion and use curve-fitting or nonlinear least-square optimization. A Monte Carlo simulation with randomized flow conditions is used to evaluate the batch estimation methods while varying the measurement noise of the data and the interval of unique heading traversed during the maneuver. The methods are also compared using experimental data obtained with a Bluefin-21 unmanned underwater vehicle performing a series of circular orbit maneuvers over a five hour period in a tide-driven flow.
keywords:
marine vehicle, velocity triangle, current estimation, least-square regression
††thanks: This work was supported in part by NSF Grant No. 2301475.
1 Introduction
Autonomy and control algorithms can leverage estimates of the flow-field to improve the operation of marine vehicles, for example, in path planning to achieve greater efficiency and robustness. This paper is motivated by the authors’ prior work (Wolek etal., 2021) that investigated time-optimal path planning for an underwater vehicle to re-inspect points of interest along circular orbits with a sonar sensor. In such applications, the presence of a flow-field impacts the vehicle motion and the pointing angle of a directional sonar sensor (due to sideslip). The methods presented here can be used to estimate flow direction and magnitude on-the-fly (e.g., after executing each circular inspection orbit) to support path re-planning.
For a vehicle operating in a steady, uniform, flow-field the flow-relative velocity , the flow velocity , and the inertial (ground) velocity are related by the equation , as illustrated in Fig.1. When two of the three quantities are known, the remaining one can be determined by direct computation. For marine vehicles, ground velocity, , is provided by the navigation system by processing GPS position measurements, acoustic ranging measurements and/or a Doppler velocity log (DVL) sensor operating in bottom-lock mode. The vehicle flow-relative velocity can also be measured with appropriate instrumentation (e.g., pitot tubes or a DVL operating in water current profiling mode).The flow velocity may be inferred using various filtering techniques based on the vehicle’s kinematics (Rhudy etal., 2015) or a dynamic model (Hegrenaes and Hallingstad, 2011). Non-uniform flow-fields can be estimated, for example, using motion tomography (Chang etal., 2017).
The contributions of this paper are three batch estimation techniques to estimate water current direction and magnitude for a marine craft using noisy measurements of kinematic variables and a basic model of the vehicle’s motion. The methods do not require knowledge of the vehicle’s flow-relative speed (since it is estimated as part of the procedure) and they are relatively simple to implement with minimal tuning required. The first method is based on quadratic curve fitting to speed-over-ground magnitude and heading angle data when the vehicle executes a 360 degree heading change maneuver (e.g., a circular orbit). The second method relies on a constrained nonlinear least-square optimization to match speed-over-ground velocity components and heading data. The third method is similar to the second but uses speed-over-ground magnitude, rather than velocity components, for optimization. The approaches are compared to each other, as well as to another existing method in the literature based on circular curve fitting, using both simulated and experimental data.
The remainder of the paper is organized as follows. Section2 describes two curve-fitting based methods. Section3 describes two optimization-based methods. Sections4 and 5 provide a comparison with simulated and experimental data, respectively. Section6 concludes the paper.
2 Curve-Fitting-Based Methods
Consider the following kinematic equations describing the motion of a marine craft in the horizontal plane:
(1)
(2)
where is the planar position, is the flow-relative speed, is the magnitude of the current, and is the current direction. The speed is assumed constant (e.g., corresponding to a fixed propeller speed). The current magnitude and direction, , is assumed to be steady and uniform over the space in which a heading change maneuver is performed.This section describes two methods for estimating in (1)–(2) based on curve fitting to noisy data of either ground velocity components or ground velocity magnitude as a function of heading.
2.1 Circular Curve Fit with Data
In (McLaren, 2008) it was shown that given a minimum of three unique ground velocity component measurements, from (1)–(2), the parameters can be estimated. Here, we summarize this approach and use it for comparisons later on in Secs.4 and 5.
Consider the sketch in Fig.1 (right panel) showing the velocity triangle and a triplet of velocity triangles for three different flow-relative velocities. A circle that passes through the three ground velocity vectors can be inscribed and its center corresponds to the current velocity . The radius of the circle is the vehicle’s flow-relative speed .One can confirm, by substituting (1)–(2), that all points on the circle satisfy
Suppose that measurements are recorded while the vehicle performs a maneuver.In the absence of noise, the data satisfies , where
(11)
and . If, instead, the data is corrupted by zero-mean additive Gaussian random variables with variance , then the system of equations can only be satisfied in a least-square sense (i.e., minimizing ) where is estimated as and is the matrix pseudo-inverse. The flow-field parameters are then:
(12)
(13)
(14)
An illustrative example of fitting a circular curve to data using this method is shown in Fig.2 for a maneuver where the heading completes one full revolution (i.e., ). The approach can also be applied for maneuvers with .
2.2 Quadratic Curve Fit with Data
Now, suppose that the vehicle has access to the ground speed magnitude (rather than the velocity components) along with heading angle information. That is, the data is where
(15)
(16)
Again, the data is assumed to be corrupted by additive zero-mean Gaussian noise. In general this assumption is made throughout this work, however, it is worth mentioning that for a vessel continuously rotating in one direction that is not aft-fore symmetric there may be a bias present based on the direction of rotation. To mitigate this bias, the dataset could include maneuvers performed in both clockwise and counter-clockwise orientations.If the heading undergoes a full revolution, then the curve contains a maximum and a minimum (see Fig.3). These minima and maxima can be approximated with a quadratic curve fit to estimate as described next.
Differentiating (16) with respect to gives the condition for a critical point:
(17)
which is satisfied at and . The speed over ground is a maximum when the vehicle heading is aligned with the current, , and a minimum when they are opposed, .Now consider a Taylor series approximation at these two points. At the maximum:
(18)
(19)
where . Similarly, at the minimum
(20)
(21)
(22)
where . Since the curvature at the minimum is greater. Let denote a guess for the heading angle of the maximum. Applying a second-order polynomial least-square fit to the the data on the interval to give the coefficients . Equating these coefficients to the Taylor series approximation one can solve for an estimate . A similar approach at the minimum gives the coefficients and another estimate. To utilize more of the available data, both sets of parameters can be used. Let the coordinate of the maximum and minimum be
(23)
(24)
respectively.Then, the estimates are:
(25)
(26)
(27)
The estimate in (27) averages and while considering angle wrap-around. The approach applied to example data is shown in Fig.3.
3 Optimization-Based Methods
This section proposes two optimization-based approaches to estimate the vehicle speed and current magnitude and direction. The first method applies to data and the second method applies to data.
Suppose that the vehicle obtains noise corrupted data of the form and consider the cost function:
(28)
Now define the following constants that depend on the data collected:
The cost function is then rewritten as
(29)
and notice that (29) is linear in the data-dependent variables . These terms only need to be computed once before the optimization proceeds.The gradient with respect to the parameters is:
(31)
where
(32)
(33)
(34)
The symmetric Hessian matrix is:
(35)
with entries
where , , and .
The cost, gradient, and Hessian are essential for optimization (e.g., via Newton-Rhapson’s method). By deriving the analytical forms of the gradient and Hessian the use of finite difference techniques can be avoided to improve optimization efficiency and robustness. In this work, we supply and to an interior-point optimizer implemented by the fmincon function in MATLAB. A constrained optimization problem is formulated subject to the linear inequality constraints
(36)
where and are user-supplied lower and upper bounds on the vehicle’s flow-relative speed. The constraints (36) also encode for the assumption that is required for the problem to be well posed. Local minima can occur along the boundaries of the constraint set since the topology of is not considered in (36). Additionally, local minima may occur with noisier datasets or when the maneuver contains a smaller range of heading angles. To improve robustness the optimization is run from multiple start points and the lowest cost solution is selected. The initial starting points are selected as the corners of the polytope represented by the constraints and as the centroid of the polytope. An example of the cost function and optimization scheme is shown in Fig.4. The level sets of the cost function are indicative of the uncertainty in the parameter space around the estimated point.
3.2 Least-square Optimization with Data
Lastly, we consider an optimization-based estimation algorithm that assumes speed-over-ground measurements and heading data are available. Consider the cost function
(37)
The gradient with respect to the parameters has the same form as (31) with entries
(38)
(39)
(40)
where .The symmetric Hessian matrix is the same as in (35)with entries
where and and with , , and . Both the gradient and the Hessian are well-defined only if . In practice, the case occurs rarely and during implementation we check for and remove any such instances from the dataset.
Unlike the previous optimization case, the cost function, gradient, and Hessian depend non-linearly on the data. The summations involving must be recomputed at each iteration. As described earlier, the analytical forms of and are provided to fmincon along with constraints (36). An example of the cost function and optimization sequence is shown in Fig.5.
4 Comparison using Simulated Data
To compare the performance of the four methods described in Secs. 2 and 3, a Monte Carlo experiment was conducted using simulated data. The data was simulated for three different noise settings, m/s, and three different heading angle changes, . For each combination of and a total of 250 unique datasets of noisy samples were generated and transformed to the appropriate form for each algorithm. Each dataset was generated by randomizing the true parameters and drawing them uniformly at random from the interior of the polytope represented by the constraints with and m/s. For each dataset the circular fit algorithm, optimization with data, and optimization with data were tested. The quadratic fit approach was evaluated only for the case. The estimate each algorithm produced was compared to the actual value and the absolute error was recorded. The mean of the absolute error, average over 250 trials, is shown in Fig.6.
As expected, the error for each algorithm increased as the measurement noise is increased and as the range of data is reduced. The quadratic fit method produced the largest errors (e.g., almost 18 deg in mean current direction error for the highest noise setting). The optimization with the data was not as accurate as the optimization with data. This is not surprising since more information is available when the components of the ground velocity are known, rather than the magnitude. The circle fit, which uses data, in most cases had an intermediate error in comparison to the the two optimization-based methods. The optimization method using data produced the most accurate results — for example, in the case of data collected for maneuvers the vehicle and current speed errors were 0.01 m/s and the current direction error was 5 degrees.
5 Comparison using Experimental Data
The four methods described in Secs.2 and 3 were also evaluated using experimental data obtained by a Bluefin-21 unmanned underwater vehicle operated by the U.S. Naval Research Laboratory. The Bluefin-21 is a 20 ft length, 21 inch diameter vehicle developed by Bluefin Robotics that features a fiber-optic gyroscope inertial navigation system and Doppler velocity log (DVL) navigation suite. The vehicle was deployed on June 22, 2016 near Boston Harbor southeast of Nahant Bay. The vehicle was programmed to execute a series of circular orbits around points of interest to test an onboard sonar system. The ground track of the vehicle is shown in Fig.7 along with the flow direction estimated by the optimization method with data.
The radii of the smaller and larger orbits were 70 and 120 meters, respectively. The data under analysis was truncated to be within 10 meters of these orbits and input into all four estimation algorithms. An example of the dataset along one such orbit is shown in Fig.8. The parameters estimated by each algorithm were used to superimpose a curve over the data points. The dataset exhibits outliers that do not conform to the model (e.g., due to transient motions as the vehicle enters and exits each orbit). To address this issue the optimization algorithms are wrapped in a random sample consensus (RANSAC) algorithm outer loop.
The estimated current direction and magnitude was also compared to historical data from an instrumented buoy (identifier: BOS1132) deployed by the National Oceanic and Atmospheric Administration (NOAA) located in Stellwagen Bank about 51 km away directly east from the testing area (15nm NNE of Race Point at a depth of 27ft). The results show good agreement with the ebb and flow tidal directions reported by the buoy. The current magnitude matches the general trend and variations may be due to the spatial separation of the buoy and vehicle leading to different tidal flows closer to shore.
6 Conclusion
Three batch estimation methods were presented that determine the direction and magnitude of a steady, uniform, flow-field, and the vehicle’s speed, using noisy kinematic measurements during heading change maneuvers, such as circular orbits or 180 degree turns. The three methods proposed included a quadratic curve fitting approach with data and least-square optimization methods with either or data. The methods were compared through a Monte Carlo experiment with simulated data to illustrate the impact of measurement noise and heading angle change during the maneuver. The comparison included an existing circular curve fitting method from the literature that uses data. The results indicated that the optimization with led to the lowest mean errors. The methods were also evaluated using a experimental data obtained by an underwater vehicle performing a series of circular orbits during a tidal change. The estimated values had modest agreement with data recorded by a nearby buoy.
The advantages of the proposed methods are that they are relatively simple to implement, depend on only a small number of parameters, and do not require a vehicle model or assumption of vehicle flow-relative speed. The optimization methods also allow visualizing the uncertainty in the parameter space as level sets of a corresponding cost function. Future work may consider running the estimators onboard a vehicle, comparing to dynamic state estimators (e.g., Kalman filters that estimate flow-field conditions), and using flow-field estimates to optimize mission plans.
References
Chang etal. [2017]Chang, D., Wu, W., Edwards, C.R., and Zhang, F. (2017).Motion tomography: Mapping flow fields using autonomous underwatervehicles.The International Journal of Robotics Research, 36(3),320–336.
Hegrenaes and Hallingstad [2011]Hegrenaes, Ø. and Hallingstad, O. (2011).Model-aided INS with sea current estimation for robust underwaternavigation.IEEE Journal of Oceanic Engineering, 36(2), 316–337.
McLaren [2008]McLaren, S.A. (2008).Velocity estimate following air data system failure.Master’s thesis, Air Force Institute of Technology.
Rhudy etal. [2015]Rhudy, M.B., Fravolini, M.L., Gu, Y., Napolitano, M.R., Gururajan, S., andChao, H. (2015).Aircraft model-independent airspeed estimation without pitot tubemeasurements.IEEE Transactions on Aerospace and Electronic Systems, 51(3),1980–1995.
Wolek etal. [2021]Wolek, A., McMahon, J., Dzikowicz, B.R., and Houston, B.H. (2021).The orbiting Dubins traveling salesman problem: planning inspectiontours for a minehunting AUV.Autonomous Robots, 45(1), 31–49.
Introduction: My name is Mrs. Angelic Larkin, I am a cute, charming, funny, determined, inexpensive, joyous, cheerful person who loves writing and wants to share my knowledge and understanding with you.
We notice you're using an ad blocker
Without advertising income, we can't keep making this site awesome for you.