Distributed Motion Control for Multiple Connected Surface Vessels

Wei Wang, Zijian Wang^∗, Luis Mateos^∗, Kuan Wei Huang, Mac Schwager, Carlo Ratti, and Daniela Rus This work was supported by grant from the Amsterdam Institute for Advanced Metropolitan Solutions (AMS) in Netherlands.W. Wang, L. Mateos, K. Huang, and C. Ratti are with the SENSEable City Laboratory, Massachusetts Institute of Technology, Cambridge, MA 02139 USA. {wweiwang, lamateos, kwhuang, ratti}@mit.eduW. Wang, L. Mateos, and D. Rus are with the Computer Science and Artificial Intelligence Lab (CSAIL), Massachusetts Institute of Technology, Cambridge, MA 02139 USA. {wweiwang, lamateos, rus}@mit.eduZ. Wang, and M. Schwager are with the Department of Aeronautics and Astronautics, Stanford University, Stanford, CA 94305, USA. {zjwang, schwager}@stanford.edu^∗These authors contributed equally to this work.

Abstract

We propose a scalable cooperative control approach which coordinates a group of rigidly connected autonomous surface vessels to track desired trajectories in a planar water environment as a single floating modular structure. Our approach leverages the implicit information of the structure’s motion for force and torque allocation without explicit communication among the robots. In our system, a leader robot steers the entire group by adjusting its force and torque according to the structure’s deviation from the desired trajectory, while follower robots run distributed consensus-based controllers to match their inputs to amplify the leader’s intent using only onboard sensors as feedback. To cope with the nonlinear system dynamics in the water, the leader robot employs a nonlinear model predictive controller (NMPC), where we experimentally estimated the dynamics model of the floating modular structure in order to achieve superior performance for leader-following control. Our method has a wide range of potential applications in transporting humans and goods in many of today’s existing waterways. We conducted trajectory and orientation tracking experiments in hardware with three custom-built autonomous modular robotic boats, called Roboat, which are capable of holonomic motions and onboard state estimation. Simulation results with up to 65 robots also prove the scalability of our proposed approach.

I Introduction

Cooperative behavior is a remarkable phenomenon in biological systems, such as flocks of birds, schools of fish and colonies of ants. These swarm creatures can move in a coordinated fashion [1] to complete complex tasks such as foraging, transporting food and building massive structures. Some animals even physically link each other into one unit to facilitate moving in challenging environments. For example, fire ants can self-assemble into waterproof rafts for months to survive floods [2].

Loosely inspired by these astonishing swarm behaviors, cooperative control of multi-robot systems has also been receiving increasing attention by roboticists [3] and control theorists [4]. As it is now rapidly becoming economically viable to embed powerful computers and sensors into small robots, future vehicle could be composed of groups of physically connected smart modular robots. These modular vehicle can reconfigure and self-assemble into desired shapes and sizes to execute tasks such as transportation in certain environments. Although the complexity of multi-robot systems increases with the number of vehicle, additional vehicle can provide flexibility and robustness to handle various tasks and compensate for single vehicle failures. This paper presents the custom hardware, dynamics model and cooperative control of a floating structure consisting of physically linked robotic modules.

Modular robots [5, 6, 7] has emerged as the forefront of robotics research since the last several decades. An intriguing property of modular robots is the ability to re-assemble, re-configure and dynamically adapt the size of the robotic fleet to the changing environment during a mission [8]. Most of the existing work in the literature focused on rigid connection among modules for aerial robots [9, 10], wheeled robots [11], and maritime robots [12]. Modular structure control is similar to the problem of cooperative object transport [13] where a group of robots are physically attached to the object and they need to be coordinated to transport objects to a final destination [14, 15, 16, 17].

Some problems have yet to be fully solved on cooperative transport. First, most of the work assume a prior knowledge of the load and robot dynamics, which is not the case in real-world scenarios. Recently, a few algorithms have been proposed to estimate the inertial parameters of the load [18, 19]. However, the damping parameters of the load are ignored in these studies. Second, most works have focused on controlling the linear motion of the structure, while leaving the rotation uncontrolled [20, 21, 16]. However, rotation control is necessary in reaching an appropriate orientation to navigate through spatially constrained environments. Third, current work within the leader-follower scope typically focuses on the design of the follower controller, while the leader controller is always simple and doesn’t consider the robot dynamics. These model-free leader controllers cannot adapt to different sizes and configurations of the group and will not work well in highly nonlinear circumstances such as the water environment. The leader controller is critical in stabilizing and optimizing the task performance of the group. Fourth, current cooperative algorithms have rarely considered dynamic systems on the water [22, 23, 24]. Cooperative transport using a team of surface vehicles poses unique challenges not encountered in aerial or ground vehicles. For instance, inertia of the vehicles and the load become more significant factors leading the system harder to control.

Based on the above discussion, we propose a novel solution for cooperative control of a floating modular structure in this paper. We require only one leader robot in the group to know the desired trajectory and orientation, and the leader can steer the rest of the robots by adjusting its input. It is noted that our approach requires no communication between any two robots and yet all the robots will contribute positively to both the structure’s translation and rotation motion. Moreover, robots ignore relative positions in the structure and treat their local position and velocity as if being measured at the center of the structure. This reduces the amount of information that the designer has to provide to the robots about the modular structure and avoids providing robots with information about their positions in the structure.

More specifically, the contributions of this paper are outlined as follows. First, a new NMPC strategy is proposed for the leader which can adapt to different group sizes and configurations without re-tuning the parameters. Second, we derive a dynamical model for the rigidly attached modular structure with $N$ robots in different configurations, which serves as a realistic input rather than an assumption for the cooperative algorithm. Third, we extend prior works [20, 21, 16] which only considered translation control by incorporating rotation sensing and torque input, so that we can control the translation and orientation of the modular structure independently. Note that rotation control is also designed in [25]. However, the algorithm requires every robot to know its own position relative to the center of mass (CoM) of the structure, while our algorithm ignores the robot’s relative position to the CoM, and only needs local measurements for each robot. Fourth, we develop three autonomous robotic boats which are capable of onboard state estimation. The cooperative algorithms are verified by both the simulations and experiments with these surface modules. Some of our early results were briefly summarised as an extended two-page abstract in a conference workshop [26], and this paper adds substantial new contributions including the theoretical results, the NMPC formulation, controller analysis and extensive simulation studies.

II Modular Robot Design

Our design focuses on developing a modular robotic boat with the ability to estimate its pose, velocity and acceleration using onboard sensors. Its main components are described as follows.

II-A Surface Vessel

As shown in Fig. 1, the robot has four thrusters around the hull to achieve holonomic motions.

Refer to caption — Figure 1: The modular robotic boat. (a) The prototype picture (side view); (b) thruster configuration (bottom view). $f_{1}$ , $f_{2}$ , $f_{3}$ and $f_{4}$ denote the forces generated by left, right, anterior and rear thrusters, respectively.

In our latest hardware iteration, we mold the boat hull with fiber glass to improve its strength and water resistance. An Intel NUC is adopted as the main controller running the Robotic Operating System (ROS). Moreover, an auxiliary microprocessor (STM32F103) is used for real-time actuator control. The robot has multiple onboard sensors including a 3D LiDAR (Velodyne, Puck VLP-16), an IMU, a camera and a GPS sensor. The camera and GPS are not used in this paper. The vessel weighs around 15 kg, and its dimensions are 0.90 $\times$ 0.45 $\times$ 0.15 m. It is powered by a 11.1 V Li-Po battery which lasts around three hours. Each propeller is fixed and can generate forward and backward forces. More details of the robot hardware can be found in [27].

II-B Onboard State Estimation

We use an extended Kalman filter (EKF) [28] to fuse LiDAR with IMU measurements and obtain stable and precise state estimation for our surface vessels. In particular, we select LiDAR as the primary sensor for estimation because of its accuracy. Moreover, we choose the normal distributions transformation (NDT) matching algorithm [29] because it handles noisy measurements well thanks to its probabilistic modeling. The EKF algorithm [30] further integrates the outputs of the NDT algorithm and IMU odometry to estimate the state (including position, velocity and acceleration) of the surface vessel. More details of the EKF design for our vessel can be referred as to our previous studies [31].

III System Modeling

The dynamics equations of the robotic boat while in motion in the water environment are given in this section. We also connect multiple robots to form a rigid structure to achieve coordinated trajectory tracking without explicit communication among the robots.

Definition 1. (Modular Vessel): A robotic boat that can 1) move on the water surface by itself and 2) estimate its states including pose, velocity and acceleration by onboard sensors.

Definition 2. (Floating Modular Structure): A group of $N$ rigidly connected modular robots that move on the water as a single rigid body in two dimensional space. All modules are homogeneous, including shape, mass, inertia, sensors and actuators.

Three coordinate systems are used to describe the modular vessel and the rigidly connected floating modular structure, as illustrated in Fig. 2.

$W$ denotes the inertial coordinate frame which has its $z$ -axis pointing downwards. The body-fixed $K_{i}$ denotes the module coordinate frame, whose origin is the center of mass of the vessel. $S$ denotes the structure coordinate frame, whose origin is attached to the center of mass of the floating structure. Without loss of generality, we assume that the headings of all modules are aligned when connected in the structure as depicted in Fig. 2.

III-A Dynamical Model of a Modular Vessel

The kinematic equation correlates the velocity components in the inertial frame to those in the body-fixed frame, and the equation is described as follows

\displaystyle\dot{\bm{\eta}}=\bm{T}(\bm{\eta})\bm{v}

(1)

where $\bm{\eta}=[x~{}y~{}\psi]^{T}\in\mathbb{R}^{3\times 1}$ denotes the position and orientation of the vessel in the inertial frame and $\bm{T}(\bm{\eta})\in\mathbb{R}^{3\times 3}$ is the transformation matrix converting a state vector from the body-fixed frame to the inertial frame; $\bm{v}=[u~{}v~{}\omega]^{T}\in\mathbb{R}^{3\times 1}$ denotes the vessel velocity, which contains the vessel surge velocity ( $u$ ), sway velocity ( $v$ ) and angular velocity ( $\omega$ ) in the body-fixed frame;

The dynamics of a surface vessel can be described by a nonlinear differential equation [32]

\displaystyle\bm{M}\dot{\bm{v}}+\bm{C}(\bm{v})\bm{v}+\bm{D}(\bm{v})\bm{v}=\bm{d}

(2)

where $\bm{M}\in\mathbb{R}^{3\times 3}$ denotes the positive-definite symmetric added mass and inertia matrix; $\bm{C}(\bm{v})\in\mathbb{R}^{3\times 3}$ denotes the skew-symmetric vehicle matrix of Coriolis and centripetal terms; $\bm{D}(\bm{v})\in\mathbb{R}^{3\times 3}$ denotes the positive-semi-definite drag matrix-valued function, $\bm{d}\in\mathbb{R}^{3\times 1}$ contains the 2D forces $\bm{F}$ and the 1D torque $\tau$ applied to the vessel. $\bm{M}$ is defined as: $\bm{M}=\text{diag}\{m_{11},m_{22},m_{33}\}$ , and $\bm{C}(\bm{v})$ is expressed as

\displaystyle\bm{C}(\bm{v})=\left[\begin{array}[]{ccc}0&0&-m_{22}v\\ 0&0&m_{11}u\\ m_{22}v&-m_{11}u&0\end{array}\right]

(6)

Furthermore, the drag matrix $\bm{D}(\bm{v})$ is simplified as $\bm{D}(\bm{v})=\text{diag}\{X_{u},Y_{v},N_{\omega}\}$ due to the low speed of the vessel in experiments. Furthermore, the applied force and torque vector $\bm{d}$ can be written as

\displaystyle\bm{d}=\bm{B}\bm{u}=\left[\begin{array}[]{cccc}1&1&0&0\\ 0&0&1&1\\ \dfrac{a}{2}&-\dfrac{a}{2}&\dfrac{b}{2}&-\dfrac{b}{2}\end{array}\right]\left(\begin{array}[]{c}f_{1}\\ f_{2}\\ f_{3}\\ f_{4}\end{array}\right)

(14)

where $\bm{B}$ is the control matrix describing the thruster configuration and $\bm{u}$ is the control input. $a$ is the distance between the transverse propellers, $b$ is the distance between the longitudinal propellers. $a$ and $b$ are also the length and width of the vessel, respectively. As shown in Fig. 1(b), $f_{1}$ , $f_{2}$ , $f_{3}$ and $f_{4}$ represent the forces generated by the left, right, anterior and rear propellers, respectively. Finally, (2) is rewritten as

\displaystyle\dot{\bm{v}}=\bm{M}^{-1}\bm{B}\bm{u}-\bm{M}^{-1}(\bm{C}(\bm{v})+\bm{D}(\bm{v}))\bm{v}

(15)

The unknown hydrodynamic parameters $\bm{M}$ , $\bm{C}$ , and $\bm{D}$ are identified by a nonlinear least squares method based on the trust-region-reflective algorithm [27].

III-B Modeling of Floating Modular Structure

The floating modular structure is composed of a group of rigidly attached $N$ robots, which are labelled as { $R_{1},R_{2},...,R_{N}$ }. We assign $R_{1}$ as the leader robot and the rest as the follower robots. We assume that only the leader $R_{1}$ knows the desired trajectory (e.g., position, orientation, etc.), while the followers have no information about the desired trajectory. Moreover, none of the robots within the group and are permitted to communicate with each other. Furthermore, no robot knows its relative positions in the group.

We now derive a dynamical model for the floating modular structure which contains $N$ rigidly connected vessels. The dynamical model of the floating structure is also described by a nonlinear differential equation

\displaystyle\bm{M}_{g}\dot{\bm{v}}+\bm{C}_{g}(\bm{v})\bm{v}+\bm{D}_{g}(\bm{v})\bm{v}=\bm{d}_{g}

(16)

where $\bm{M}_{g}\in\mathbb{R}^{3\times 3}$ , $\bm{C}_{g}\in\mathbb{R}^{3\times 3}$ and $\bm{D}_{g}\in\mathbb{R}^{3\times 3}$ represent the added mass and inertia matrix, the Coriolis and centripetal matrix and the drag matrix, respectively for the modular structure; $\bm{d}_{g}\in\mathbb{R}^{3\times 1}$ represents the total force and torque applied to the structure. For simplicity, we assume the shape of the modular structure as solid cuboid in this study. Moreover, we define the number of the robot $N$ as: $N=l\cdot w$ where $l$ is the number of the robot along the longitudinal direction of the structure, and $w$ is the number of the robot along the lateral direction of the structure, as shown in Fig. 3.

That is, the length of the floating platform is $l\cdot a$ , and the width of the floating platform is $w\cdot b$ .

In particular, $\bm{M}_{g}$ is roughly derived from the definition of mass and moment of inertia, and it is described as follows

\displaystyle\bm{M}_{g}=\text{diag}\{Nm_{11},Nm_{22},\dfrac{m_{33}(l^{2}a^{2}+w^{2}b^{2})}{12}\}

(17)

Note that in (17) we linearly scale the inertia tensor for the modular structure for simplicity. More accurate estimation should be investigated in the future. Then, according to (6) and (17), $\bm{C}_{g}(\bm{v})$ is defined mathematically as follow

\displaystyle\bm{C}_{g}(\bm{v})=N\bm{C}(\bm{v}),

(18)

Further, according to the definition of linear and angular drag in a perfect fluid [33], the damping of the floating structure is derived as follow

\displaystyle\bm{D}_{g}(\bm{v})=\text{diag}\{lX_{u},wY_{v},\dfrac{l^{3}N_{\omega}}{4}\}

(19)

Finally, the total force and torque $\bm{d}_{g}$ is described as follow

\displaystyle\bm{d}_{g}=\sum_{i=1}^{N}\bm{d}_{i}+[0~{}0~{}||\sum_{i=1}^{N}\bm{r}_{i}\times\bm{F}_{i}||]^{T},

(20)

where $\bm{d}_{i}=[\bm{F}_{i}~{}\tau_{i}]^{T}\in\mathbb{R}^{3\times 1}$ denotes the 2D force and direct 1D torque applied by $R_{i}$ , and $\bm{r}_{i}$ denotes the position vector pointing from the center of the mass of the structure to the center of mass of $R_{i}$ .

IV Control of Floating Modular Structure

In this section we present our decentralized leader-following controller which guarantees the coordinated motion among all robot modules without explicit communication. A few assumptions are required for the robots to achieve the force and torque coordination with no communication. They are stated formally below.

Assumption 1. All robots know the number of the vehicles ( $N$ ), the length of the floating structure ( $la$ ) and the width of the structure ( $wb$ );

Assumption 2. The center of mass of the robots are centrosymmetric around the center of mass of the floating structure, meaning that for any robot $R_{i}$ , there exists another robot $j\neq i$ such that $\bm{r}_{i}=-\bm{r}_{j}$ .

IV-A Decentralized Follower’s Controller

The following force and torque controller is used by every follower robot which will lead to synchronization to the leader’s input:

	$\displaystyle\dot{\bm{d}}_{i}$	$\displaystyle=$	$\displaystyle\sum_{j=1,j\neq i}^{N}(\bm{d}_{j}-\bm{d}_{i})=\sum_{j=1}^{N}\bm{d}_{j}-N\bm{d}_{i}$		(21)
		$\displaystyle=$	$\displaystyle\bm{M}_{g}\dot{\bm{v}}+\bm{C}_{g}(\bm{v})\bm{v}+\bm{D}_{g}(\bm{v})\bm{v}-N\bm{d}_{i}.$		(22)

(21) is a notable implementation of the famous consensus law [4] and will lead to all robots applying equal forces and torques to the modular structure. Traditional use of this protocol would require robot $i$ to communicate with its neighbors to obtain $\bm{d}_{j}$ . Our novel implementation of the consensus protocol in (22) eliminates the need for communication, because the sum of all neighbors’ $\bm{d}_{j}$ plus robot $i$ ’s own $\bm{d}_{i}$ is a known quantity by (16), assuming that robot $i$ can measure $\bm{v}$ and $\dot{\bm{v}}$ . Therefore the follower’s control law (22) can be calculated locally without communication. Note that we do not consider the second term $[0~{}0~{}||\sum_{j=1}^{N}\bm{r}_{j}\times\bm{F}_{j}||]$ of (20) in the distributed controller (21) because this term will diminish to zero when (21) converges under Assumption 2.

Convergence Analysis for (22). Olfati-Saber and Murray in [4] proved that the values of the all followers will converge to the leader’s if there is one leader who does not change its value. If the leader’s input force is changing, we can prove that the the followers’ forces converge to the leader’s exponentially using the similar method described by (31) in [17]. The convergence for a varying leader’s torque will be analyzed by (IV-A) in the following. $\hfill{\Box}$

In practice, it is unrealistic for the followers to directly measure $\dot{\bm{v}}$ and $\bm{v}$ in (22) at the center of mass of the structure. Here we show that the control law still works when the followers use the local measurements instead of the unrealistic $\dot{\bm{v}}$ and $\bm{v}$ . The control law with local sensing is described as

\displaystyle\dot{\bm{d}}_{i}

\displaystyle=

\displaystyle\bm{M}_{g}\dot{\bm{v}}_{i}+\bm{C}_{g}(\bm{v}_{i})\bm{v}_{i}+\bm{D}_{g}(\bm{v}_{i})\bm{v}_{i}-N\bm{d}_{i},

(23)

where $\bm{v}_{i}=[u_{i}~{}v_{i}~{}\omega_{i}]^{T}$ is the local velocity of $R_{i}$ . After $\bm{d}_{i}$ is obtained, the propeller input $\bm{u}_{i}$ of $R_{i}$ can be calculated using (14). Note that () ignores the relative positions of the robots in the group.

Convergence Analysis for (23). We first analyze the convergence for the rotation control. Extracting the third row in (23), the control law for rotation can be written as

$\displaystyle\!\!\!\!\!\!\!\!\!\!\dot{\tau}_{i}=\!\!\!\!\!\!\!\!\!\!$	$\displaystyle M_{g}^{33}\dot{\omega}+Nm_{11}(u_{c}+\omega r_{i}^{y})-$
	$\displaystyle Nm_{22}(v_{c}+\omega r_{i}^{x})+D_{g}^{33}r-N\tau_{i}$
$\displaystyle\!\!\!\!\!\!\!\!\!\!=\!\!\!\!\!\!\!\!\!\!$	$\displaystyle M_{g}^{33}\dot{\omega}+Nm_{11}u_{c}-Nm_{22}v_{c}+D_{g}^{33}r-N\tau_{i}+$
	$\displaystyle N(m_{11}\omega r_{i}^{y}-m_{22}\omega r_{i}^{x})$
$\displaystyle\!\!\!\!\!\!\!\!\!\!=\!\!\!\!\!\!\!\!\!\!$	$\displaystyle(\sum_{j=1}^{N}\tau_{j}-N\tau_{i})+N(m_{11}\omega r_{i}^{y}-m_{22}\omega r_{i}^{x})$	(24)

where $M_{g}^{33}$ is the element in the third row and third column of $\bm{M}_{g}$ , $D_{g}^{33}$ represents the same element of $\bm{D}_{g}$ , $u_{c}$ and $v_{c}$ are the surge and sway speeds at the center of the structure respectively, $r_{i}^{x}$ and $r_{i}^{y}$ are the the $x$ and $y$ component of $\bm{r}_{i}$ respectively. The first term in (IV-A) by itself would lead to a consensus, as in (21). The second term in (IV-A) can be regarded as disturbance, which denotes the additional Coriolis and centripetal effects caused by the rotation. However, under Assumption 2, the second term in (IV-A) will not have any influence on the consensus because it will be canceled out by the paired symmetric robots.

Now we analyze the convergence of the translation control in $x$ axis. Extracting the first row in (23), the control law can be written as follow

$\displaystyle\dot{F}_{i}^{x}=\!\!\!\!\!\!\!\!\!\!$	$\displaystyle M_{g}^{11}(\dot{u}_{c}+\dot{\omega}r_{i}^{y}+\omega(\omega r_{i}^{y}))+$
	$\displaystyle Nm_{22}(u_{c}+\omega r_{i}^{y})(v_{c}+\omega r_{i}^{x})+$
	$\displaystyle D_{g}^{11}(u_{c}+\omega r_{i}^{y})-NF_{i}^{x}$
$\displaystyle=\!\!\!\!\!\!\!\!\!\!$	$\displaystyle(M_{g}^{11}\dot{u}_{c}+Nm_{22}u_{c}v_{c}+D_{g}^{11}u_{c}-NF_{i}^{x})+\dot{\omega}r_{i}^{y}+$	(25)
	$\displaystyle Nm_{22}(u_{c}\omega r_{i}^{x}+v_{c}\omega r_{i}^{y}+(\omega r_{i}^{y})(\omega r_{i}^{x}))+$
	$\displaystyle D_{g}^{11}\omega r_{i}^{y}+\omega(\omega r_{i}^{y})$

Similar to (IV-A), the first term in (IV-A) equals to $\sum_{j=1}^{N}F_{j}^{x}-NF_{i}^{x}$ which would lead to a consensus, and the other terms in (IV-A) are treated as disturbances. Under Assumption 2, the third, fourth and fifth terms in (IV-A) will be canceled out by the paired symmetric robots. We need to further study the second term in (IV-A). Using (16), we can write $\dot{\omega}$ as follow

	$\displaystyle\dot{\omega}=$		$\displaystyle\dfrac{\tau_{g}}{M_{g}^{33}}+\dfrac{\|\|\sum\limits_{j=1}^{N}\bm{r}_{j}\times\bm{F}_{j}\|\|}{M_{g}^{33}}+$
			$\displaystyle\dfrac{\omega(Nm_{22}v_{c}-Nm_{11}u_{c}-D_{g}^{33})}{M_{g}^{33}}$

The first and third term in (IV-A) will be cancelled out under Assumption 2 after the cross product with $r_{i}^{y}$ . The second term $||\sum_{j=1}^{N}\bm{r}_{j}\times\bm{F}_{j}||$ will diminish to zero when the force convergence is achieved under Assumption 2. Note that Assumption 2 does not guarantee that this term will always be zero especially during the turning process. In this case, we treat the torque term as the disturbance, which can be effectively rejected by our controller. The convergence analysis for the translation control in the $y$ axis is the same as that of the $x$ axis. $\hfill{\Box}$

IV-B Leader’s Controller

The leader controller largely affects the tracking performance of the group. An optimal leader controller is desired to minimize the tracking errors while conforming to the constraints of the dynamics and actuator limitations. We revised a NMPC strategy [27] for the leader which could adapt to various sizes and configurations without re-tuning the parameters.

The NMPC is used to track the whole state of the leader $\bm{q}_{1}$ where $\bm{q}_{1}=[x_{1}~{}y_{1}~{}\psi_{1}~{}u_{1}~{}v_{1}~{}\omega_{1}]^{T}$ . The reference state for the leader is defined as $\bm{q}_{r}=[x_{r}~{}y_{r}~{}\psi_{r}~{}u_{r}~{}v_{r}~{}\omega_{r}]^{T}$ . Then, the tracking error between the actual and reference states is defined as $\bm{e}=\bm{q}_{1}-\bm{q}_{r}$ . By combining (1) and (15), the dynamical model of the leader is then reformulated as

\displaystyle\dot{\bm{q}}_{1}=g(\bm{q}_{1},\bm{u}_{1})

(27)

The NMPC leader generates feasible inputs to guide the group to follow a set of predefined states. In particular, the input applied to the system is given by the solution to the following receding horizon optimal control problem, which is solved at every time step,

\displaystyle\underset{\bm{q}_{1}(\cdot),\bm{u}_{1}(\cdot)}{\text{min}}=\int_{t}^{t+T}F(\bm{q}_{1}(\tau),\bm{u}_{1}(\tau))d\tau+E(\bm{q}_{1}(t+T))

(28)

subject to

\displaystyle\dot{\bm{q}}_{1}(\tau)=g(\bm{q}_{1}(\tau),\bm{u}_{1}(\tau)),~{}\bm{q}_{1}(t)=\bm{q}_{1}^{t},

(29)

\displaystyle\bm{q}_{1}(\tau)\in Q_{1},\bm{u}_{1}(\tau)\in U_{1},\forall\tau\in[t,t+T]

(30)

where $T$ is the prediction horizon, $Q_{1}$ and $U_{1}$ are the state space and actuator force space respectively, $F$ is the cost function defining the desired performance objective and $E$ is the terminal cost. In particular, $F$ and $E$ are defined as follows


$\displaystyle F(\bm{q}_{1},\bm{u}_{1})$	$\displaystyle=$	$\displaystyle\bm{e}(t)^{T}Q\bm{e}(t)+\bm{u}_{1}(t)^{T}R\bm{u}_{1}(t)$	(31a)
$\displaystyle E(\bm{q}_{1})$	$\displaystyle=$	$\displaystyle\bm{e}(t+T)^{T}Q_{N}\bm{e}(t+T)$	(31b)

where $Q$ and $R$ are the positive definite weight matrices that penalize deviations from the desired values; $Q_{N}$ is the terminal penalty matrix which can improve the stability of the NMPC algorithm. To adapt the leader controller to different sizes and configurations,we experimentally found a rough relationship between the fleet size and the good weight parameters $Q$ , $R$ and $Q_{N}$ : $Q=NQ_{m}$ , $R=R_{m}$ and $Q_{N}=NQ_{N_{m}}$ where $Q_{m}$ , $R_{m}$ and $Q_{N_{m}}$ are the weight parameters for an individual robot.

V Simulations

Simulations are conducted in Matlab to verify the proposed distributed controller for the followers and the NMPC for the leader. ACADO Model Predictive Control Toolkit [34] is used to solve the constrained nonlinear optimization problem in the NMPC. The numerical Runge-Kutta-Fehlberg (RKF45) method is adopted to calculate the distributed force and torque controller. The numerical Gauss-Legendre integration method is employed to simulate the dynamics of the floating modular structure. The sampling rate of the simulation is set to 10 Hz.

In the simulation, the modular structure consists of 65 robots. The 64 followers are arranged into a 8 $\times$ 8 array and the leader is attached to front of the array. The dimensions and the dynamics of the simulated robots are the same as the physical roboat described in Section II. According to the propeller configuration and force limitation for each propeller, each robot can apply a force up to 16.97 N and a torque up to 8.10 N $\cdot$ m. Noisy sensing and actuation are added to the simulated robots. The measured velocities of the structure at the local frame of the robots, are corrupted by zero-mean Gaussian noise $\mathcal{N}(0,0.1I_{3})$ , where $I_{3}$ is a three-dimensional identity matrix. Similarly, we add $\mathcal{N}(0,0.1I_{3})$ to the desired force and torque actuation, and add $\mathcal{N}(0,0.03I_{3})$ to the local acceleration measurements. The goal is to navigate the floating structure through a narrow canal where both the position and orientation of the structure are controlled at all times to avoid collision with the canal edges. Moreover, the desired velocities of the structure are also controlled during the whole process. The NMPC leader controller in (28)-(30) guides the whole structure to track these desired states of the structure. The parameters and constraints of the leader controller are listed as follows: $f^{\text{min}}_{i}\leq f_{i}\leq f^{\text{max}}_{i}$ , where $f^{\text{min}}_{i}=-6$ N, $f^{\text{max}}_{i}=6$ N, and $i=1,~{}2,~{}3,~{}4$ ; $H=4$ s, $N=65$ $Q_{m}=\text{diag}\{20,20,20,40,40,40\}$ , $R_{m}=\text{diag}\{1,1,1,1\}$ , and $Q_{Nm}=\text{diag}\{20,20,20,40,40,40\}$ .

Fig. 4 and 5 shows that the modular structure successfully tracks the desired trajectories, orientations, and velocities at the same time without direct communication among the robots.

It can be observed that the measured linear velocities of the robots are clearly different when the structure is rotating from 60 s to 133 s, verifying the use of local measurements. Fig. 6 shows the forces and torques of the robots during the simulation.

It is evident in Fig. 6 that both the forces and torques of the followers converge to those of the leader when the force and torque of the leader are changing, as proved in Section IV-A. Compared to the controllers in [17, 16, 21], our controller is able to accurately track the trajectory, orientation, linear and angular velocities of the structure simultaneously. Moreover, compared to the controller in [25], our controller does not need the relative position of the structure and uses only the local measurements to achieve the force and torque alignment. Furthermore, it is notable that tuning the NMPC weight parameters is straightforward because each weight parameter has an explicit physical meaning. By contrast, the PID controllers always require troublesome parameter tuning.

VI Experiments

In this section, the effectiveness and robustness of the proposed cooperative controllers are verified through experiments with custom-built robots. We built three Roboats for the experiments in this study, as shown in Fig. 7.

Each Roboat is able to estimate its states including the position, heading, velocity and acceleration by its onboard sensors. During the experiments, only the leader knows the desired trajectory, heading and velocities of the structure while each follower run a consensus-based controller (23) to match the forces and torque of the leader’s using only onboard sensors as feedback. In particular, two configurations are studied in the experiments. One configuration is three robots are connected in series, the other one is three robots are connected in parallel. The experiment was conducted in 12 $\times$ 6 m arena within a swimming pool. The position, heading angle, force, torque, velocity and acceleration measurements of the robots are recorded at 10 Hz during the experiments and then uploaded for analysis. All the experiments are run 5 times to ensure repeatability.

Fig. 8 and 9 illustrate the tracking performance of the cooperative controllers when the floating structure is in series and in parallel, respectively.

It can be seen that the coordinated group is able to track various trajectories and orientations in both configurations. We can see that the cooperative controller performs better when the group is configured in parallel compared with the tandem configuration. This is possibly due to the fact that the local measurements of the robots in tandem configuration have more discrepancies with respect to the center of the structure. Fig. 10 and 11 illustrate the forces and torques of all the three robots in series and parallel configuration, respectively.

From Fig. 10(a) and 11(a) we can clearly see that the magnitudes of followers’ forces contribute positively, indicating that the follower robots help the leader in the process. Moreover, we can see from Fig. 11(a) that the followers contribute almost equally with the leader, implying that the cooperation is very efficient. Furthermore, similar to the simulation in Fig. 6, there is a slight time delay between the forces/torques of the leader and those of the followers, because it takes time for the followers to converge. In addition, from Fig. 10(c) and 11(c), we can also see that the torques of the followers also contribute positively especially when the floating structure is rotating from 40 s to 67 s, which indicates that the followers collaborated with the leader to rotate the structure.

VII Conclusion and Future Work

In this paper, we have developed cooperative algorithms that enable multiple connected surface vessels to form a floating modular structure and cooperatively track desired trajectories, orientations and velocities on the water without any cross-robot communication. An NMPC strategy is first formulated for the leader which is subject to control inputs and dynamics constraints. A dynamical model for the floating modular structure with $N$ modules is then derived. This model-based leader controller is able to adapt to different sizes and configurations of the group without parameter re-tuning. We further propose a decentralized force and torque controller for the followers, which requires only local measurements (e.g., velocities and accelerations). Furthermore, we build three autonomous modular vessels, which are capable of holonomic motions and onboard state estimation using an extended Kalman filter. We have theoretically proved the convergence of the cooperative controller. Extensively simulations and experiments with the robots have verified that the connected modules can successfully coordinate their force and torque to complete the tracking tasks relying on their local measurements, rather than using explicit communication.

In the future, we can use machine learning to online estimate the key parameters the robots which are now roughly calculated in Section III-B. We are also interested in adaptive controllers that allow for the dynamics change to the structure when meaningful objects are placed on the structure, such as the work in [35]. Moreover, we are extending our approach to realistic outdoor water environments where large disturbances such as currents and waves exist.

References

[1] I. D. Couzin and J. Krause, “Self-organization and collective behavior in vertebrates,” Advances in the Study of Behavior, vol. 32, pp. 1–75, 2003.
[2] N. J. Mlot, C. A. Tovey, and D. L. Hu, “Fire ants self-assemble into waterproof rafts to survive floods,” Proceedings of the National Academy of Sciences, vol. 108, no. 19, pp. 7669–7673, 2011.
[3] J. Alonso-Mora, E. Montijano, T. Nägeli, O. Hilliges, M. Schwager, and D. Rus, “Distributed multi-robot formation control in dynamic environments,” Autonomous Robots, 2018.
[4] R. Olfati-Saber and R. Murray, “Consensus problems in networks of agents with switching topology and time-delays,” IEEE Transactions on Automatic Control, vol. 49, no. 9, pp. 1520 – 1533, 2004.
[5] K. Gilpin and D. Rus, “Modular robot systems,” IEEE Robotics Automation Magazine, vol. 17, no. 3, pp. 38–55, Sep. 2010.
[6] S. Murata and H. Kurokawa, “Self-reconfigurable robots,” IEEE Robotics Automation Magazine, vol. 14, no. 1, pp. 71–78, March 2007.
[7] M. Yim, W. Shen, B. Salemi, D. Rus, M. Moll, H. Lipson, E. Klavins, and G. S. Chirikjian, “Modular self-reconfigurable robot systems [grand challenges of robotics],” IEEE Robotics Automation Magazine, vol. 14, no. 1, pp. 43–52, March 2007.
[8] J. Daudelin, G. Jing, T. Tosun, M. Yim, H. Kress-Gazit, and M. Campbell, “An integrated system for perception-driven autonomy with modular robots,” Science Robotics, vol. 3, no. 23, 2018. [Online]. Available: https://robotics.sciencemag.org/content/3/23/eaat4983
[9] R. Oung and R. D’Andrea, “The distributed flight array: Design, implementation, and analysis of a modular vertical take-off and landing vehicle,” The International Journal of Robotics Research, vol. 33, no. 3, pp. 375–400, 2014.
[10] D. Saldaña, B. Gabrich, G. Li, M. Yim, and V. Kumar, “Modquad: The flying modular structure that self-assembles in midair,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), May 2018, pp. 691–698.
[11] G. Baldassarre, V. Trianni, M. Bonani, F. Mondada, M. Dorigo, and S. Nolfi, “Self-organized coordinated motion in groups of physically connected robots,” IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), vol. 37, no. 1, pp. 224–239, Feb 2007.
[12] J. Paulos, N. Eckenstein, T. Tosun, J. Seo, J. Davey, J. Greco, V. Kumar, and M. Yim, “Automated self-assembly of large maritime structures by a team of robotic boats,” IEEE Transactions on Automation Science and Engineering, vol. 12, no. 3, pp. 958–968, 2015.
[13] E. Tuci, M. H. M. Alkilabi, and O. Akanyeti, “Cooperative object transport in multi-robot systems: A review of the state-of-the-art,” Frontiers in Robotics and AI, vol. 5, p. 59, 2018.
[14] D. Rus, B. Donald, and J. Jennings, “Moving furniture with teams of autonomous robots,” in Proceedings 1995 IEEE/RSJ International Conference on Intelligent Robots and Systems. Human Robot Interaction and Cooperative Robots, vol. 1, Aug 1995, pp. 235–242 vol.1.
[15] D. Mellinger, M. Shomin, N. Michael, and V. Kumar, Cooperative Grasping and Transport Using Multiple Quadrotors. Berlin, Heidelberg: Springer Berlin Heidelberg, 2013, pp. 545–558.
[16] Z. Wang and M. Schwager, “Multi-robot manipulation with no communication using only local measurements,” in 2015 54th IEEE Conference on Decision and Control (CDC), Dec 2015, pp. 380–385.
[17] Z. Wang and M. Schwager, “Force-amplifying N-robot transport system (Force-ANTS) for cooperative planar manipulation without communication,” The International Journal of Robotics Research, vol. 35, no. 13, pp. 1564–1586, 2016.
[18] A. Franchi, A. Petitti, and A. Rizzo, “Distributed estimation of state and parameters in multi-agent cooperative manipulation,” IEEE Transactions on Control of Network Systems, pp. 1–1, 2018.
[19] M. Corah and N. Michael, “Active estimation of mass properties for safe cooperative lifting,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), May 2017, pp. 4582–4587.
[20] Z. Wang and M. Schwager, “Multi-robot manipulation without communication,” in Distributed autonomous robotic systems. Springer, 2016, pp. 135–149.
[21] Z. Wang and M. Schwager, “Kinematic multi-robot manipulation with no communication using force feedback,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), May 2016, pp. 427–432.
[22] J. Esposito, M. Feemster, and E. Smith, “Cooperative manipulation on the water using a swarm of autonomous tugboats,” in 2008 IEEE International Conference on Robotics and Automation, May 2008, pp. 1501–1506.
[23] H. Hajieghrary, D. Kularatne, and M. A. Hsieh, “Differential geometric approach to trajectory planning: Cooperative transport by a team of autonomous marine vehicles,” in 2018 Annual American Control Conference (ACC), June 2018, pp. 858–863.
[24] ——, “Cooperative transport of a buoyant load: A differential geometric approach,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Sep. 2017, pp. 2158–2163.
[25] Z. Wang, G. Yang, X. Su, and M. Schwager, “Ouijabots: Omnidirectional robots for cooperative object transport with rotation control using no communication,” in Distributed Autonomous Robotic Systems. Springer, 2018, pp. 117–131.
[26] W. Wang, L. Mateos, Z. Wang, K. W. Huang, M. Schwager, C. Ratti, and D. Rus, “Cooperative control of an autonomous floating modular structure without communication,” in 2019 International Symposium on Multi-Robot and Multi-Agent Systems (MRS). IEEE, 2019, pp. 44–46.
[27] W. Wang, L. Mateos, S. Park, P. Leoni, B. Gheneti, F. Duarte, C. Ratti, and D. Rus, “Design, modeling, and nonlinear model predictive tracking control of a novel autonomous surface vehicle,” in Proc. 2018 IEEE Int. Conf. Robot. Autom, 2018, pp. 6189–6196.
[28] G. Bishop, G. Welch et al., “An introduction to the kalman filter,” Proc of SIGGRAPH, Course, vol. 8, no. 27599-23175, p. 41, 2001.
[29] P. Biber and W. Strasser, “The normal distributions transform: a new approach to laser scan matching,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2003, pp. 2743 – 2748.
[30] T. Moore and D. Stouch, “A generalized extended kalman filter implementation for the robot operating system,” in Intelligent Autonomous Systems 13. Springer, 2016, pp. 335–348.
[31] W. Wang, B. Gheneti, L. A. Mateos, F. Duarte, C. Ratti, and D. Rus, “Roboat: An autonomous surface vehicle for urban waterways,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Nov 2019, pp. 6340–6347.
[32] T. I. Fossen, Guidance and control of ocean vehicles. West Sussex PO19 1UD, England: John Wiley & Sons Ltd, 1994.
[33] Y. Nakayama, Introduction to fluid mechanics. Butterworth-Heinemann, 2018.
[34] B. Houska, H. Ferreau, F. Logist, and M. Diehl, “Acado toolkit-an automatic control and dynamic optimization,” 2012, optimization in Engineering Center (OPTEC), http://acado.github.io/.
[35] P. Culbertson and M. Schwager, “Decentralized adaptive control for collaborative manipulation,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), 2018, pp. 278–285.