There has been an increasing interest in the use of autonomous underwater robots to monitor freshwater and marine environments. In particular, robots that propel and maneuver themselves like fish, often known as robotic fish, have emerged as mobile sensing platforms for aquatic environments. Highly nonlinear and often under-actuated dynamics of robotic fish present significant challenges in control of these robots. In this work, we propose a nonlinear model predictive control (NMPC) approach to path-following of a tail-actuated robotic fish that accommodates the nonlinear dynamics and actuation constraints while minimizing the control effort. Considering the cyclic nature of tail actuation, the control design is based on an averaged dynamic model, where the hydrodynamic force generated by tail beating is captured using Lighthill's large-amplitude elongated-body theory. A computationally efficient approach is developed to identify the model parameters based on the measured swimming and turning data for the robot. With the tail beat frequency fixed, the bias and amplitude of the tail oscillation are treated as physical variables to be manipulated, which are related to the control inputs via a nonlinear map. A control projection method is introduced to accommodate the sector-shaped constraints of the control inputs while minimizing the optimization complexity in solving the NMPC problem. Both simulation and experimental results support the efficacy of the proposed approach. In particular, the advantages of the control projection method are shown via comparison with alternative approaches.

## Introduction

Aquatic ecosystem sustainability is often at risk due to the increase of potential threats, such as oil spills, invasive species, and industrial and household waste. As a result, monitoring and understanding aquatic environments has become essential to ensuring the longevity of aquatic ecosystems and for securing water resources. In recent years, a type of aquatic robots that mimics the movement of fish (Fig. 1) has emerged as an attractive choice for the aforementioned applications. These robots have various actuation mechanisms, from oscillating caudal or pectoral fins to undulation of the entire body, and like fish, they are able to attain high maneuverability [1].

To be suitable for monitoring such ecosystems, these robots need to be able to sustain long field-operation time, and it is, thus, crucial for them to be highly energy-efficient. The latter makes optimal control an important problem for robotic fish. Much of the work done for robotic fish has been in robot development [2–11] and modeling [12–18]. There has also been extensive work on motion control of robotic fish, which has mainly been focused on the generation of fish-like swimming gaits, and on control to drive the robot to achieve a desired motion. In the case of swimming gait generation, several kinematics and dynamics-based schemes [19–27], as well as bioinspired approaches, such as central pattern generators [28–33], have been used to produce fish-like swimming. However, these approaches are typically open-loop in nature. Although some works have examined trajectory-tracking or stabilization problems [12,34], they have mainly been focused on heading or depth control.

There has been additional work done on model-based closed-loop motion control to achieve maneuvering or trajectory tracking [35–41]. In Ref. [35], a point-to-point control of a four-link robotic fish was implemented, where a classical proportional-integral derivative controller and a fuzzy logic controller were designed for speed and orientation control. The authors in Ref. [36] devised a control strategy for maneuvering an aquatic vehicle using an oscillating foil. The strategy consists of an optimal off-line motion planning step and an online feedback control step composed of a cascade of finite time, time-scalable linear quadratic control and input–output linearization, in combination with a sliding mode controller. Furthermore, in Ref. [37] a target-tracking and collision-avoidance algorithm for two autonomous robotic fish was implemented via a situated-behavior-based decentralized control approach, using a combination of an attractive force toward a target and a repulsive force for collision avoidance. In Ref. [38], a fuzzy control law for a pectoral-fin-driven robotic fish was developed to perform rendezvous and docking with an underwater post in water currents. Zou et al. developed a neural-network-based sliding mode control algorithm for cooperative trajectory tracking of multiple robotic fish [39]. In Ref. [40], three simplified linearized models of the decoupled fish dynamics were used for the design of linear quadratic regulators to achieve speed and orientation control and to stabilize the pitch and roll. Furthermore, a line-of-sight guidance scheme was implemented for way-point tracking. Finally, the authors in Ref. [41] designed a sliding mode controller for swimming, orientating, and way-point tracking of robotic fish in three-dimensional motion. Despite aforementioned progress in the control of robotic fish, a unified, systematic control approach that incorporates performance objectives and accommodates input constraints for such robots has not been proposed.

Nonlinear model predictive control (NMPC) presents a promising framework for dealing with uncertainties as well as input and state constraints. There is extensive work on NMPC for path-following of mobile robots [42–47], but little work has been reported on its application to control of robotic fish. In this work, we propose and implement a path-following NMPC scheme for a tail-actuated robotic fish. We consider a tail-actuated robotic fish particularly because of its simple mechanical design and low power consumption. On the other hand, the highly nonlinear and coupled dynamics, along with the under-actuated nature of the robot, pose significant challenges for the control design.

In this controller design, a high-fidelity averaged nonlinear dynamic model is used. Furthermore, the physical control inputs consist of two of the tail-beat parameters, the bias and the amplitude, while the other (angular frequency) is kept constant. We propose a framework to address the nonlinear input constraints. Specifically, to maximize the use of the admissible control and handle the nonlinear control constraints in a computationally efficient manner, we employ an analytical projection scheme for the control inputs. We further propose a novel estimation scheme to identify some key unknown parameters in the robotic fish model. In particular, inspired by the work in Ref. [48], we develop a parameter estimation method to empirically identify the hydrodynamic and scaling coefficients of the model instead of utilizing time-consuming computational fluid dynamics simulations, or relying on trial-and-error data fitting between dynamic simulation and experimental measurement. To implement the controller in real-time, we employ a framework using Visual C++, which consists of the ACADO toolkit [49] used to solve repeatedly the optimal control problem, and an image processing algorithm using OpenCV to provide feedback.

Some preliminary results of this work were presented at the 2016 ASME Dynamic Systems and Control Conference [50]. The improvement of this paper over [50] is extensive and significant. First, we have proposed a computation-efficient method to deal with the nonlinear constraints. Second, we have further formulated a parameter estimation scheme to identify crucial parameters in the model. Third, we have developed the experimental framework and implemented the proposed NMPC scheme experimentally.

The rest of the paper is organized as follows. We first review the dynamic and scaled averaging models of the tail-actuated robotic fish, followed by a simplified averaged model. In Sec. 3, we present the path-following problem formulation, followed by the NMPC design and the proposed control projection scheme. In Sec. 4, simulation results are discussed, and in Sec. 5, the experimental setup, the proposed parameter estimation scheme, and the experimental results are presented. Section 6 concludes the paper.

## Robotic Fish Model

### Dynamic Model.

As was done in Ref. [48], the tail-actuated robotic fish is modeled as a rigid body with a rigid tail that is actuated at its base, and it is assumed that the robot operates in an inviscid, irrotational, and incompressible fluid within an infinite domain. Define [**X**, **Y**, **Z**]^{T} and [**x**, **y**, **z**]^{T} as the inertial coordinate system and the body-fixed coordinate system, respectively, as illustrated in Fig. 2. The velocity of the center of mass in the body-fixed coordinates is expressed as $Vc=[Vcx,Vcy,Vcz]$, where $Vcx,\u2009Vcy$, and $Vcz$ denote the surge, sway, and heave velocities, respectively. Furthermore, let *β* denote the angle of attack, formed by the direction of **V*** _{c}* with respect to the

**x**-axis, and

*ψ*denote the heading angle, formed by the

**x**-axis relative to the

**X**-axis. The angular velocity expressed in the body-fixed coordinate system is given by $\omega =[\omega x,\omega y,\omega z]$, which is composed of roll (

*ω*), pitch (

_{x}*ω*), and yaw (

_{y}*ω*). Finally, let

_{z}*α*denote the tail deflection angle with respect to the negative

**x**-axis.

**xz**-plane and that the tail moves in the

**xy**-plane. As a result, the system only has three degrees-of-freedom, surge ($Vcx$), sway ($Vcy$), and yaw (

*ω*). It is further assumed that the inertial coupling between yaw, sway and surge motions is negligible, which leads to the following equations of planar motion:

_{z}*m*is the mass of the body,

_{b}*J*is the inertia of the body about the

_{bz}**z**-axis, $max$ and $may$ are the hydrodynamic derivatives that represent the added masses of the robotic fish along the

**x**and

**y**directions, respectively, and $Jaz$ represents the added inertia effect of the body about the

**z**-axis. The hydrodynamic forces and moment due to tail fin actuation and the interaction of the body itself with the fluid are captured by

*f*,

_{x}*f*, and

_{y}*M*. To evaluate the hydrodynamic forces exerted by the tail, Lighthill's large amplitude elongated body theory is used [48]. The kinematic equations for the robotic fish are given by

_{z}*α*

_{0},

*α*, and

_{a}*ω*represent the bias, amplitude, and frequency of the tail beat, respectively. The original hydrodynamic force and moment terms in Eqs. (1)–(3) are scaled by some functions dependent on the tail beat parameters,

_{α}*α*

_{0},

*α*, and

_{a}*ω*, and classical averaging is then conducted over these scaled dynamics. In particular, we define the states $x1=Vcx,\u2009x2=Vcy$, and $x3=\omega z$, so that the averaged dynamics takes the following form:

_{α}where $m1=mb\u2212max,\u2009m2=mb\u2212may,\u2009J3=Jbz\u2212Jaz,\u2009c1=(1/2)\rho SCD,\u2009c2=(1/2)\rho SCL,\u2009c3=(1/2)mL2,\u2009c4=(1/(J3))KD,c5=(1/(2J3))L2mc,\u2009\u2009and\u2009c6=(1/(3J3))L3m$. Here *S* denotes the reference surface area for the robot body, *C _{D}*,

*C*, and

_{L}*K*represent the drag force coefficient, lift coefficient, and drag moment coefficient, respectively,

_{D}*ρ*is the density of water,

*L*is the tail length,

*c*is the distance from the body center to the pivot point of the actuated tail, and

*m*represents the mass of water displaced by the tail per unit length and is approximated by $(\pi /4)\rho d2$ with

*d*denoting the tail depth.

*K*is a scaling constant, and $Km(\alpha 0)$ is a scaling function affine in

_{f}*α*

_{0}. To further facilitate control design, in this paper

*K*is considered as a constant during the NMPC design. This term is found by taking the average of

_{m}*K*for a given range of

_{m}*α*

_{0}. The resulting model is called the simplified averaged model in this paper.

## Path-Following Control Algorithm

Considering that robotic fish are battery-powered, energy-efficient locomotion is highly desirable in order to prolong the field-operation time. It is important to design a controller that is able to meet performance objectives such as minimizing the path-tracking error while accommodating consideration of control effort. We are thus motivated to develop an NMPC scheme for path-following. NMPC is an attractive choice because it allows explicit consideration of state and input constraints, is capable of handling nonlinear models, and can optimize control performance [51,52].

### Path-Following Error Coordinates.

In contrast to trajectory tracking, in path-following one is interested in following a geometric reference parametrized by some scalar without any specified timing. The kinematic model of the robotic fish is expressed in a Frenet–Serret frame {F} that moves along the reference path according to some desired function of time. Figure 3 illustrates the path-following problem.

*l*denotes the length of the path, and the function $p:\mathbb{R}1\u2192\mathbb{R}2$ is twice differentiable. Let P denote a point on the path to be followed,

_{p}*θ*the tangential angle of the path at point P, and

_{p}*θ*the angle between the robotic fish velocity vector

_{c}**V**

*and the inertial*

_{c}**X**-axis, while the coordinate axes x

*and y*

_{p}*are directed along the tangential and normal directions at point P. We let point C denote the center of the robotic fish, and the vectors $C\xaf$ and $P\xaf$ describe the positions of C and P in the three-dimensional (3D) inertial frame {I}. Note that since we are only considering the planar case, the third component of the position vectors is taken as 0. Let $r=[Xe;Ye;0]T$ denote the position of the robotic fish center C with respect to the point P on the path expressed in {F}. Let $IRF$ denote the rotation matrix from {I} to {F} and $FRI$ denote the rotation matrix from {F} to {I}, with*

_{p}*α*=

_{e}*ψ*–

*θ*, and further expand $X\u02d9$ and $Y\u02d9$ with Eqs. (4) and (5), respectively, which results in the following error state model:

_{p}*V*is the magnitude of the robotic fish's translational velocity. The dynamical model of the robotic fish in the error state is then obtained by augmenting the previous equations with the simplified averaged scaled dynamics as seen in Eqs. (8)–(10). Ideally, one would like the robotic fish to not only converge to a desired path, but also move along the path with some desired surge velocity and desired angular velocity. Let $Vdx$ be the desired surge velocity, and let the velocity error states be defined by $\eta e=Vcx\u2212Vdx,\u2009\omega e=\omega z\u2212Cp(s)s\u02d9$, and $\zeta e=s\u02d9\u2212Vc\u2009cos(\alpha e+\beta )$ so that

_{c}Since we have formulated the problem with respect to the error dynamics, and have shifted the equilibrium point of the dynamic equations, our control objective has become a stabilization problem for the resultant error dynamics.

### Path-Following Control Design.

To steer the robotic fish to the desired path, and drive the error state vector $\Omega e$ to zero, we utilize an NMPC scheme using the robot's simplified averaged model. NMPC is an optimization-based method for feedback control of nonlinear systems, where the basic idea is to repeatedly solve a finite horizon optimization problem subject to state and input constraints. At a given time *t*, measurements are obtained, and using a model of the process, the controller predicts the behavior of the system over a prediction horizon *T _{p}* and then determines over the control horizon

*T*the input necessary to maximize the performance objective. The first part of the optimal control obtained is implemented until the next sampling instant, and then a new measurement is obtained and the process repeats [52].

_{c}*α*

_{0},

*α*, and

_{a}*ω*. By choosing the control in this manner, we allow the control inputs to appear linearly in the dynamic equation. In particular, we have chosen our control inputs as

_{α}*ω*. Furthermore, since the system dynamics are expressed in terms of the velocity errors states, by doing a change of variables we have essentially shifted the equilibrium point of the dynamical system, which means that there is also a shift on the control inputs $uf1$ and $uf2$. Let $u2ss$ and $u3ss$ represent the shifted control values, which are defined as follows:

_{α}where *u _{e}*

_{1}is essentially $\zeta \u02d9e$ as seen in Eq. (29).

where *Q* and *R* are positive definite weighting matrices that penalize deviations from the desired values.

*Q*for a terminal penalty of the following form:

_{T}The reader is referred to Ref. [53] for details on how to obtain this weighting matrix.

By solving the optimal control problem we obtain the optimal control sequence for $ue1,\u2009ue2$, and $ue3$. From $ue1$, we can obtain $\zeta \u02d9e$, and thus the state $\zeta e$ from which we can then solve for $s\u02d9$. Furthermore, from $ue2$ and $ue3$, along with Eqs. (31)–(34), one can solve for the actual robotic fish control variables *α*_{0} and *α _{a}*.

### Control Projection.

Given that the NMPC inputs, *u _{e}*

_{2}and

*u*

_{e}_{3}, consist of functions of the actual robotic fish control variables

*α*

_{0}and

*α*, the NMPC input constraints are nonlinear in nature. As an illustration, Fig. 4 plots the admissible control inputs in terms of $uf1$ and $uf2$ when the tail beat bias and amplitude have the following limits:

_{a}$\alpha 0min=\u221240\u2009deg$

$\alpha 0max=40\u2009deg$

$\alpha amin=0\u2009deg$

$\alpha amax=30\u2009deg$

where $\alpha 0min,\u2009\alpha 0max,\u2009\alpha amin$, and $\alpha amax$ are the physical limits on the tail-beat bias and amplitude, respectively.

Although NMPC is able to handle nonlinear control constraints, defining the constraints in this manner leads to an increase in computational time and complexity which in turn makes it challenging to implement in real-time. It is thus desirable to define boxed-constraints since this can reduce significantly the complexity of the optimization problem and thus lower the computational time. One way of handling the irregular sector-shaped admissible control region as shown in Fig. 4, is to choose a rectangular area that lies inside this sector-shaped region as depicted by the light gray box in Fig. 5. However, this deprives one of fully utilizing the admissible control. To overcome this problem, we propose to employ a projection method, where we define the NMPC boxed constraint to be such that it encompasses the admissible control region as depicted in Fig. 6, and then project the computed values onto the true region depicted by the red sector-shaped section.

*U*is convex, this problem is then well defined and $ProjU(z)$ is unique. Instead of relying on an iterative optimization algorithm to determine the projected value, one can directly obtain an analytical solution that will simplify the projection and minimize computational complexity, as explained next. By taking advantage of the symmetry of the admissible control set, one can restrict the analysis to the left-half plane. To characterize the boundaries of the admissible control region, we obtain the relationship between $uf1,\u2009uf2$, and

*α*

_{0}by solving for

*α*from Eq. (32) and then substituting that into Eq. (31). Similarly, by solving for

_{a}*α*

_{0}from Eq. (32), we can obtain an equation that captures the relationship between $uf1,\u2009uf2$, and

*α*. These equations are given as follows:

_{a}*A*), the point to be projected is inside or on the boundary of the convex set

*U*and no projection is needed. For case (D), the point to be projected would be outside of the box encompassing the constraint set

*U*and thus does not need to be considered. For case (

*B*), the point to be projected is above the arc, and therefore $ProjU(z)$ can be found by finding the minimum distance from the point

*z*to the arc described by Eq. (40). Let

*z*= (

*p*,

*q*) and $u*=(uf1*,uf2*)=ProjU(z)$. The relationship between $uf1*$ and $uf2*$ is given by

By taking the partial derivative of $g2(uf2*)$ with respect to $uf2*$ and setting it to zero, we can obtain a unique real root for $uf2*$ that would minimize this distance. Finally, $uf1*$ is obtained with Eq. (41).

By taking the partial derivative of $g3(uf2*)$ with respect to $uf2*$ and setting it to zero, we can obtain a unique real root for $uf2*$, and consequently $uf1*$ with Eq. (43).

## Simulation Results

To evaluate the effectiveness of the designed controller, simulation was carried out using ACADO model predictive control toolkit. The parameters used (Table 1) were based on a robotic fish developed by Smart Microsystems Lab at Michigan State University. Furthermore, while the input constraints are the same as those presented in the experiment section, the parameters used to solve the optimization problem and implement the NMPC are as follows:

$Length\u2009of\u2009optimization\u2009horizon:Tc=Tp=12\u2009\u2009s\u2009$

$Sampling\u2009interval:ts=1\u2009\u2009s$

$Weighting\u2009matrix:Q=diag(7,7,0.3,1,1,7)$

$Control\u2009weighting\u2009matrix:R=0.001I3$

$Vc\u2009max=0.04\u2009\u2009m/\u2009sec\u2009$

$s\u02d9max=0.04\u2009m/\u2009sec\u2009$

$\alpha 0min=\u221240\u2009deg$

$\alpha 0max=40\u2009deg$

$\alpha amin=0\u2009deg$

$\alpha amax=30\u2009deg$

where $Vc\u2009max$ is the maximum velocity the robotic fish can achieve, $s\u02d9max$ is the maximum speed the point *s* can move along the path with, and $\alpha 0min,\u2009\alpha 0max,\u2009\alpha amin$, and $\alpha amax$ are the physical limits on the tail-beat bias and amplitude, respectively. Note that all of the following simulation was run with the same set of parameters and initial conditions. Furthermore, the terminal penalty weighting matrix was determined as described in Sec. 3.2. Though the controller was designed using the simplified averaged model, the simulation was performed using the original dynamic model. In other words, the model of the process was based on the simplified averaged dynamics as described by Eqs. (8)–(10), and the inputs obtained from solving the optimization problem were applied to the system described by Eqs. (1)–(3).

where *x _{p}* and

*y*represent the position of the point P in the {I} frame. This path has a curvature of $cp(s)=0$, and we chose to require the robotic fish to move with a constant velocity $Vc=0.03\u2009$ m/s. In Figs. 7–9 we compare the desired path and the closed-loop trajectory of the robotic fish for three cases. In particular, Fig. 7 shows the simulation results of the NMPC utilizing the projection scheme, while Fig. 8 shows the results for the case when no projection was employed and a boxed constraint within

_{p}*U*was chosen instead (as shown in Fig. 5). Finally, Fig. 9 shows the results when the nonlinear constraints for the set

*U*were directly defined. Note that in this work the blue dashed line represents the closed-loop trajectory of the robotic fish while the solid red line represents the desired path, and the arrowheads point in the direction of progression. Furthermore, the red diamond represents the starting position of the robotic fish, the green dot represents the starting point of the path, and the magenta box represents the imaginary boundaries of the fish tank. Moreover, Fig. 10 shows the computed physical inputs from solving the NMPC with the larger boxed constraint and their final values after the proposed projection.

*c*(

_{p}*s*) = 3.33. In Figs. 11–13, we compare the desired path trajectory with those obtained by the robotic fish using the three aforementioned control schemes, respectively.

From the simulation results, one can see that the proposed NMPC scheme with projection outperforms the other two schemes in both line-tracking and arc-tracking cases; in particular, it results in smaller tracking error at the steady-state. Compared with the case with boxed constraint within the set *U*, the proposed scheme offers larger control authority. The better tracking results from the proposed scheme compared to the case using direct, nonlinear constraints, however, were somewhat surprising. We conjecture that this is because the latter algorithm cannot reach an optimal solution within the allotted computing time. In particular, directly defining the nonlinear constraints requires the optimization algorithm to conduct more iterations in order to find the solution, which also makes it difficult to implement in real-time.

## Experimental Results

In order to evaluate the effectiveness of the designed controller, experiments were carried out using the robotic fish depicted in Fig. 1. The robot consisted of a rigid-shell body and a relatively rigid tail, which were both 3D-printed. The tail was actuated using a Hitec digital micro waterproof servo (HS-5086WP) (Poway, CA), while a microchip digital signal processors and controller (DSPIC30F6014, Chandler, AZ) was used to control the tail actuation. Furthermore, an XBee-PRO module (Hopkins, MN) was used for communication with a computer. Two Tenergy Li-Ion rechargeable batteries (7.4V, 3350 mAh) (Fremont, CA) were used to power the robot. For the experiments, the robotic fish was run in a 1.38 m by 0.8 m tank equipped with an overhead Logitech camera (Newark, CA) as seen in Fig. 14. Furthermore, to obtain the robotic fish's position and orientation in the tank, two markers were attached to the anterior and posterior of the robotic fish body. We then captured an overhead video of the robotic fish swimming in the tank using the camera, and utilized Visual C++ and the OpenCV library to implement an image processing algorithm. The algorithm detected the positions of the two markers and then used their average to obtain the center position of the robotic fish. The heading angle of the robot was estimated using the positions of the two markers. Additionally, the Kalman filter function in OpenCV was used to estimate the linear and angular velocities of the robot based on the measured position and heading. During every sampling time *t _{s}*, the OpenCV algorithm was used to obtain measurements for NMPC, which were then passed to the nonlinear optimization tool ACADO to solve the optimal control problem. In particular, we ran the software on a Surface Pro tablet with an Intel(R) Core(TM) i5 central processing unit @ 2.50 GHz with 4.0 GB of DDR3 RAM. Once the control inputs were calculated, the bias and amplitude values for the tail beat were obtained and then transmitted to the robotic fish wirelessly, and the process was repeated.

### Model Parameter Identification.

*C*,

_{D}*C*, and

_{L}*K*) of the robotic fish model (8)–(16) typically requires extensive effort in fitting dynamic simulation data to experimental data by scanning the parameter space [15]. Furthermore, the determination of the scaling coefficients of

_{D}*K*and

_{f}*K*requires scanning the parameter space for multiple sets of tail beat patterns and matching the simulated average model data to the simulated dynamic model [48], which is time-consuming. In this paper, we propose an efficient and systematic way to identify the model parameters by exploiting the approximate, analytical relationship between the steady-state turning parameter (turning radius, turning period, etc.) and the model parameters established in Ref. [48]. With the assumption $|x1|\u226b|x2|$, which is reasonable in general, we can obtain the unique equilibrium of the system (8)–(10), under a given tail beat pattern, as

_{m}Using Eqs. (47)–(52), we formulate the following algorithm to obtain the hydrodynamic coefficients *C _{D}*,

*C*, and

_{L}*K*, as well as the scaling coefficients

_{D}*K*and

_{m}*K*for the averaged model.

_{f}Using the previous equation, one can obtain the numerical value of the ratio *R*_{1} for a given set of tail beat parameters and the corresponding measured $x\xaf1$. In particular, we found this ratio by averaging the different values obtained for each set of measurements.

Using Eq. (54) can then estimate the numerical values for *θ*_{0} and *θ*_{1} by utilizing, for example, the constrained linear least squares (lsqlin) function in matlab, based on the tail beat parameters and the corresponding measurement of $x\xaf3$ for a set of experiments.

*R*

_{1}and the ratio $(Km/KD)$, we have reduced the number of parameters to be estimated from 5 ($Kf,Km,CD,CL,KD$) to 3 ($R1,(Km/KD),CL$). In order to obtain the particular values for

*C*,

_{D}*K*,

_{f}*K*, and

_{m}*K*, and to estimate the remaining parameter

_{D}*C*, we utilize Eqs. (53) and (54) along with Eqs. (48) and (51). By letting $c0=CD+CL$ and substituting

_{L}*R*

_{1},

*θ*

_{0}and

*θ*

_{1}into Eq. (51), one obtains

With a set of collected data, the parameters *ϕ*_{1} through *ϕ*_{3} can be estimated readily using techniques, such as the constrained linear-least square method. We can then solve for *C _{D}*,

*C*, and

_{L}*K*using the definitions established previously. Since the proposed estimation method only provides the ratio $(Km/KD)$, to obtain the values for

_{f}*K*and

_{m}*K*, we run simulations with the original dynamical model and choose

_{D}*K*such that the angular velocity of the dynamic model matches that of the averaged model.

_{D}For the implementation of the earlier parameter estimation scheme, we first ran experiments to obtain the steady-state turning radii and periods for different tail biases ($0\u2009deg,\u200925\u2009deg,\u200940\u2009deg$), and amplitudes ($15\u2009deg,\u200920\u2009deg,\u200925\u2009deg$) while holding the frequency at 1 Hz. The values obtained for the parameters *K _{f}*,

*K*,

_{m}*C*,

_{D}*C*, and

_{L}*K*are listed in Table 1. Furthermore, to validate the models we ran experiments with the same set of biases and amplitudes as previously stated while holding the frequency at 1.5 Hz. Table 2 lists the errors in turning radius and period between those obtained from experiments and those obtained from simulation using the parameters estimated earlier. The comparison indicates that the estimated model has acceptable accuracy.

_{D}### Experimental Results on Path-Following.

The parameters used to solve the optimization problem and implement the NMPC were as follows:

$Length\u2009of\u2009optimization\u2009horizon:Tc=Tp=7s\u2009$

$Sampling\u2009interval:ts=1s$

$Weighting\u2009matrix:Q=0.9I5$

$Control\u2009weighting\u2009matrix:R=0.001I3$

$Vc\u2009max=0.04\u2009\u2009m/s$

$s\u02d9max=0.04\u2009m/s$

$\alpha 0min=\u221240\u2009deg$

$\alpha 0max=40\u2009deg$

$\alpha amax=30\u2009deg$

$\alpha amin=0\u2009deg$

where *x _{p}* and

*y*represent the position of the point P in the {I} frame. The desired velocity for the robotic fish was set to be 0.03 m/s.

_{p}In Figs. 15 and 16 we compare the desired path and the closed-loop robotic fish trajectory, obtained by using the NMPC with the proposed control projection scheme, and with a boxed constraint inside the nonlinear constraint set *U*, respectively. We do not report the case of NMPC with nonlinear constraints *U* directly, because it could not be implemented in real-time due to its long computation time.

## Conclusion

In this paper, we proposed and implemented in real-time a path-following NMPC scheme for a tail-actuated robotic fish. A high-fidelity averaged nonlinear dynamic model was used for controller design. A parameter estimation scheme was employed to empirically identify the hydrodynamic parameters and scaling coefficients of the model. Furthermore, given that the control inputs were functions of two of the tail-beat parameters, specifically the tail bias and tail amplitude, a control projection strategy was implemented to handle these nonlinear input constraints and maximize the use of the admissible control region in a computationally efficient manner. Finally, simulation and experimental results demonstrated the effectiveness of the proposed scheme.

For future work, the proposed NMPC algorithm will be evaluated in an environmental sensing application, where there will be an upper-level path planning scheme integrated with the NMPC-based path-tracking scheme. Furthermore, in another direction, we plan to extend this work to robotic fish with more sophisticated dynamics, such as robotic fish actuated by both pectoral fins and caudal fin [18], and underwater robots like the gliding robotic fish [55].

## Funding Data

National Science Foundation (Grant Nos. DGE1424871, ECCS 1446793, and IIS 1715714; Funder ID: 10.13039/100000001).