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 , where , and denote the surge, sway, and heave velocities, respectively. Furthermore, let β denote the angle of attack, formed by the direction of Vc 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 , which is composed of roll (ωx), pitch (ωy), and yaw (ωz). Finally, let α denote the tail deflection angle with respect to the negative x-axis.
where . Here S denotes the reference surface area for the robot body, CD, CL, and KD represent the drag force coefficient, lift coefficient, and drag moment coefficient, respectively, ρ 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 with d denoting the tail depth. Kf is a scaling constant, and is a scaling function affine in α0. To further facilitate control design, in this paper Km is considered as a constant during the NMPC design. This term is found by taking the average of Km for a given range of α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.
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 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 Tp and then determines over the control horizon Tc 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].
where ue1 is essentially as seen in Eq. (29).
where Q and R are positive definite weighting matrices that penalize deviations from the desired values.
The reader is referred to Ref. [53] for details on how to obtain this weighting matrix.
Control Projection.
Given that the NMPC inputs, ue2 and ue3, consist of functions of the actual robotic fish control variables α0 and αa, the NMPC input constraints are nonlinear in nature. As an illustration, Fig. 4 plots the admissible control inputs in terms of and when the tail beat bias and amplitude have the following limits:
where , and 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.
By taking the partial derivative of with respect to and setting it to zero, we can obtain a unique real root for that would minimize this distance. Finally, is obtained with Eq. (41).
By taking the partial derivative of with respect to and setting it to zero, we can obtain a unique real root for , and consequently 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:
Parameter | Value |
---|---|
mb | 0.725 kg |
max | −0.217 kg |
may | −0.7888 kg |
Jbz | 2.66 × 10–3 kg/m2 |
Jaz | −7.93 × 10–4 kg/m2 |
L | 0.071 m |
d | 0.04 m |
c | 0.105 m |
ρ | 1000 kg/m3 |
S | 0.03 m2 |
CD | 0.97 |
CL | 3.9047 |
KD | 4.5 × 10–3 kg/m2 |
Kf | 0.7 |
Km (averaged) | 0.45 |
Parameter | Value |
---|---|
mb | 0.725 kg |
max | −0.217 kg |
may | −0.7888 kg |
Jbz | 2.66 × 10–3 kg/m2 |
Jaz | −7.93 × 10–4 kg/m2 |
L | 0.071 m |
d | 0.04 m |
c | 0.105 m |
ρ | 1000 kg/m3 |
S | 0.03 m2 |
CD | 0.97 |
CL | 3.9047 |
KD | 4.5 × 10–3 kg/m2 |
Kf | 0.7 |
Km (averaged) | 0.45 |
where is the maximum velocity the robotic fish can achieve, is the maximum speed the point s can move along the path with, and , and 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 xp and yp represent the position of the point P in the {I} frame. This path has a curvature of , and we chose to require the robotic fish to move with a constant velocity 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 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.
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 ts, 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.
Using Eqs. (47)–(52), we formulate the following algorithm to obtain the hydrodynamic coefficients CD, CL, and KD, as well as the scaling coefficients Km and Kf for the averaged model.
Using the previous equation, one can obtain the numerical value of the ratio R1 for a given set of tail beat parameters and the corresponding measured . 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 for a set of experiments.
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 CD, CL, and Kf using the definitions established previously. Since the proposed estimation method only provides the ratio , to obtain the values for Km and KD, we run simulations with the original dynamical model and choose KD such that the angular velocity of the dynamic model matches that of the averaged model.
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 (), and amplitudes () while holding the frequency at 1 Hz. The values obtained for the parameters Kf, Km, CD, CL, and KD 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.
(αa, α0) | Turning radius error (%) | Turning period error (%) |
---|---|---|
(15 deg, 45 deg) | 11.23 | 8.80 |
(15 deg, 50 deg) | 7.57 | 11.65 |
(20 deg, 45 deg) | 8.46 | 3.82 |
(20 deg, 50 deg) | 8.74 | 12.55 |
(25 deg, 45 deg) | 2.90 | 0.97 |
(25 deg, 50 deg) | 11.07 | 15.88 |
(αa, α0) | Turning radius error (%) | Turning period error (%) |
---|---|---|
(15 deg, 45 deg) | 11.23 | 8.80 |
(15 deg, 50 deg) | 7.57 | 11.65 |
(20 deg, 45 deg) | 8.46 | 3.82 |
(20 deg, 50 deg) | 8.74 | 12.55 |
(25 deg, 45 deg) | 2.90 | 0.97 |
(25 deg, 50 deg) | 11.07 | 15.88 |
Experimental Results on Path-Following.
The parameters used to solve the optimization problem and implement the NMPC were as follows:
where xp and yp 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.
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).