Modeling of Cooperative Robotic Systems and Predictive Control Applied to Biped Robots and UAV-UGV Docking with Task Prioritization

Abstract

This paper studies a cooperative modeling framework to reduce the complexity in deriving the governing dynamical equations of complex systems composed of multiple bodies such as biped robots and unmanned aerial and ground vehicles. The approach also allows for an optimization-based trajectory generation for the complex system. This work also studies a fast–slow model predictive control strategy with task prioritization to perform docking maneuvers on cooperative systems. The method allows agents and a single agent to perform a docking maneuver. In addition, agents give different priorities to a specific subset of shared states. In this way, overall degrees of freedom to achieve the docking task are distributed among various subsets of the task space. The fast–slow model predictive control strategy uses non-linear and linear model predictive control formulations such that docking is handled as a non-linear problem until agents are close enough, where direct transcription is calculated using the Euler discretization method. During this phase, the trajectory generated is tracked with a linear model predictive controller and addresses the close proximity motion to complete docking. The trajectory generation and modeling is demonstrated on a biped robot, and the proposed MPC framework is illustrated in a case study, where a quadcopter docks on a non-holonomic rover using a leader–follower topology.

1. Introduction

Planning a trajectory for complex robots such as a biped is a complex task as the free-floating base of the robot is moved by the discontinuous contact forces acting on their feet. This propulsion method requires attention in planning the motion of the contact points and the contact forces while considering the dynamic effects on the robot [1]. In addition, a straightforward optimal control formulation results in intractable non-linear programs, as stated in the literature [2,3].

There are multiple causes of complexity in planning the trajectory of a floating base robot. First of all, the pose of the floating base is described as unactuated base coordinates with six degrees of freedom (DoF). Then, actuated joint coordinates of legs are added on top of that, which results in relatively large joint coordinates [1]. Therefore, the trajectory optimization of such a system should find optimal values for all these coordinates. Secondly, contact between the feet and the terrain must comply certain contact conditions such as unilateral contact forces [4]. In the literature, this problem is handled as a trajectory optimization problem using numerical optimization techniques such as direct optimal control methods [5], indirect optimal control methods [6], dynamic programming [7], and sequential methods [8]. In this way, trajectory of the robot, along with calculated joint coordinates, torques, and contact forces, are calculated by considering the upper-level constraints on the task.

The complexity of the problem is reduced in multiple ways. One way to carry this out is to use low-fidelity dynamic models for the robot such as single rigid body dynamics model, where lumped inertia is attached to the body frame and actuated links are assumed to be moving slowly and having low inertia [2]. Another way to reduce complexity is to split the the optimization into smaller sub-problems and to pre-define some portions of the trajectory such as footholds [9].

However, simplifications in robot model and optimization problems are compromised with the complexity of the motion that can be calculated with trajectory optimization. For this reason, having means to simplify full-body dynamics without compromising the fidelity of the model is vital. Recently, the trajectory optimization problem was distributed into smaller alternating sub-problems, where one first satisfies dynamic constraints of the problem by finding robot momentum and contact forces, and then one finds the leg kinematics that satisfy robot dynamics. This is denoted as centroidal and whole-body model splitting and was first introduced in the work [8], where a sequential optimal control formulation was represented. In another work based on the same splitting of whole-body motion and centroidal dynamics, the locomotion problem is cast into a mathematical framework based on Alternating Direction Method of Multipliers (ADMM) [10]. This paper exploits the natural splitting between centroidal and manipulator dynamics and ensures consensus between these models. Another recent example uses the same splitting and introduces an accelerated ADMM algorithm to solve the locomotion problem. [3].

Besides the centroidal and whole body splitting of the dynamic model of a legged robot, there is obviously another split between the portions of the body. This splitting is naturally defined by the design of legged robots and referred to as body and limbs. Depending on the design, they can contain a single body or a set of multi-bodies and interconnections between these sets maintained over a connection node, which is a joint. A formal way to describe the cooperation between these sets already exists in the literature within a cooperative system framework [11]. This framework defines a relationship between independent agents using communication graphs, which dictate a mapping to the information flow among agents. Cooperative system frameworks are used in many areas such as increasing performance of a sensory network in localization of data [12] or calculating the communication topology between dynamic agents within the cooperative system [13]. It is also used in designing controllers for agents within a network [14]. All the applications have one thing in common, which is the distributed modeling of the cooperative system and the distributed calculation of the variables to reach the desired objective.

Autonomous aerial systems have become essential in numerous practical fields, such as in public safety, as a surveillance tool and first response [15,16,17]. It has also been studied for the delivery of goods by the industry [18,19]. A critical task during the operation of an aerial system is docking maneuver, where it might be approaching a stationary or a moving platform [20,21] to drop/load cargo. In addition, aerial vehicles require refueling or recharging to extend their workspace [22]. Docking is an intricate maneuver that necessitates the awareness of the docking path’s constraints regarding flight safety and the docked platform’s attitude [23]. In addition, nonlinear effects such as the wake of the leading agent due to the proximity flight must be considered [24] as well as the ground effect [25]. Thus it is clear that the characteristic of this maneuver requires the agent to handle certain constraints and uncertainties.

A popular control method to handle previously described constraints and calculate control actions is model-predictive control (MPC). An MPC reaches the desired control action by minimizing a given objective using linear and non-linear optimization theory, as applied in the case of a tracking controller [26]. MPC also allows the integration of secondary tasks into decision-making, as in [27], which makes it a unified strategy to handle docking maneuvers without requiring ad-hoc integration of multiple frameworks that increase complexity. Implementing MPC-based docking strategies for space applications exists in case studies, where line of sight constraints are satisfied, energy-saving strategies are pursued, and docking on tumbling objects are executed [28,29,30].

Autonomous docking for aerial vehicles requires state information of the docked platform, which receives the docking agent. Therefore it certainly requires an on-board or external mechanism to sense and estimate states. On-board sensors such as cameras or LiDAR are widely implemented solutions (see [31]), yet they are bounded with range limitations. External mechanisms to obtain state information about the docked platform are sensors placed on the agents, except for the docking agent or external observers. As a result, this information is shared over a communication grid as illustrated in [32]. Besides state information, some of the autonomous docking literature can be grouped in terms of cooperation at the controller level. In one of the works, a central controller calculates lateral and longitudinal velocity commands for both of the docking maneuver devices: an uncrewed ground vehicle (UGV) and an uncrewed aerial vehicle (UAV). Then, these control signals are fed into these agents [33]. In the extension of this work, an MPC strategy is utilized on the same system, where both agents are cooperatively trying to execute docking maneuvers [34]. In another work, a multirotor docks onto a fixed-wing platform, where only the multirotor executes the docking maneuver [21]. The aerial refueling problem is addressed as a docking problem in [22]. However, only the docking platform and the boom are manipulated. Recent work studies an uncrewed sea surface vehicle (USV) landing and designs an MPC controller for a multirotor, where cooperative docking is executed [35]. However, the docking trajectory calculation is calculated on the multirotor and shared back to the USV.

• Motivation

In this context, this paper aims to keep the model fidelity of the biped robot higher than a single body dynamics and adopt a cooperative system framework in defining dynamics of the robot. As a result, the robot will be defined by multiple cooperating agents, where cooperation is introduced to the trajectory optimization by an adjacency matrix as a constraint. In this way, lumped trajectory optimization is converted to a separated objectives and constraints that are linearly related.

When multiple agents are communicating with each other and obeying specific rules, i.e., collision avoidance, velocity matching, and staying within the vicinity of the neighbors, the aggregate of these agents are called cooperative agents [36] and the application of these systems are vast due to the advantages. Popular applications include uncrewed vehicles [37] and space applications [38]. The nature of autonomous docking makes the system performing this maneuver a cooperative system. However, it is not formally addressed as such in the vast majority of the literature. This is due to the fact that in most scenarios, one of the vehicles is assumed to be tightly controlled and hence passive. The control laws are then derived for the remaining active vehicle. On the other hand, controllers cooperatively addressing this problem are mainly centralized on a ground controller or in one of the agents, which could suffer in performance or even fail due to uncertainties in the communication. Apart from the centralized implementation of cooperative docking, there are a few applications where both agents take part in the docking maneuver. Since the cooperative control framework allows various communication topologies, methods based on MPC with local neighbor state information can be applied. Besides the cooperative aspect, a prioritization of the states to track during docking is typically not studied in the literature. As summarized before, agents that are too far apart from each other first could first minimize the positional difference, which is carried out to be in a feasible solution set when the MPC for docking is initiated. Instead of handling this problem as two separate sub-problems, an automated prioritization of tracking certain states can be defined so that linear states can be given higher priority over the angular states (see [39]).

• Contributions

The key contributions of this paper are as follows. The methods introduced in this paper are used to divide the lumped multi-body model of a large robot-system into cooperative multi-bodies and address the trajectory optimization problem using this distributed model. In the case of the biped robot, although it has light weight legs compared to the shoulder and the floating base, the biped robot will be modeled as three cooperative agents, which are the floating base, right shoulder, and left shoulder. All agents are defined as multi-bodies, as defined in the following sections. The contributions of this paper are summarized as follows:

• This method divides the EoM of the biped robot into smaller cooperative agents, which has simpler EoMs. Agents with simpler dynamics result in simpler equality constraints for the trajectory optimization.
• The non-linear programming formulation given in this paper cast the trajectory optimization problem with single objective and single augmented Hamiltonian into split objectives and constraints.
• This paper also proposes a cooperative control strategy based on MPC for docking. The designed strategy implements a non-linear and a linear MPC for the coarse approach (long distance) and the delicate docking maneuver (short distance) based on the same objective function with tailored optimization strategies. A leader–follower type of topology is adopted, where the quadcopter docks on the UGV. As a showcase, this controller performs short- and long-distance docking of a quadcopter on a UGV.
• Formulation of the MPC includes task prioritization, which is based on a null-space projection of the tasks being ranked. The formulation is adopted from [40] by defining the docking task in terms of the docking agents’ Degrees of Freedom (DoF).

The organization of the paper is such that Section 2 expresses governing equations of agents and the underlying graph structure. Following that, Section 8 provides a task-prioritization and MPC strategy for docking. The simulation results are shared in Section 9. Finally, conclusions are provided in Section 11.

2. Notation and Preliminaries

The notation of this work is as follows. $s\in \mathbb{R}$, $\mathit{v}\in {\mathbb{R}}^{n_v}$ and $\mathit{M}\in {\mathbb{R}}^{n_r\times n_c}$ represent the arbitrary scalar, vector, and matrix. ${\mathcal{F}}_I$ and ${\mathcal{F}}_B$ represent the inertial and body frames, respectively, where the expression of a vector for these frames is written as ${}_i{}^{ }\mathit{v}_{ }^{ },i=I,B$. Unit vectors in orthonormal frames are denoted as ${\mathit{u}}_1={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$, ${\mathit{u}}_2={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$ and ${\mathit{u}}_3={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$. The composite rotation matrix ${}_I{}^{ }\mathit{C}_B^{ }\in SO\left(3\right)$ is written from ${\mathcal{F}}_B$ to ${\mathcal{F}}_I$, the rotation sequence is depicted as $z-y-x$, and associated Euler angles are denoted as $\psi$, $\theta$ and $\varphi$. $g=9.81$ m/s² is the gravitational acceleration. $col{\{·\}}_{m=1}^M$, $row{\{·\}}_{l=1}^M$, and $diag{\{·\}}_{m=1}^M$ represent column, row and diagonal concatenation, respectively, of the entity within the parenthesis.

3. Underlying Graph Structure

The communication among agents in the cooperative systems are described by the graph $\mathcal{G}=(\mathcal{N},\mathcal{E})$, which consists of node set $\mathcal{N}$ and edge set $\mathcal{E}$ [41]. Edge set $\mathcal{E}\subset \mathcal{N}\times \mathcal{N}$ is given between nodes $i\in \mathcal{N}$ and $j\in \mathcal{N}$ such that $(j,i)\in \mathcal{E}$ denotes node i receives information from j. Let $n_w$ be an arbitrary signal dimension, and then the adjacency matrix; shared signal sizes among agents are assumed to be identical and equal to $n_w$. $\mathit{A}=\left[a_{ij}\right]\otimes {\mathit{I}}_{n_w}\in {\mathbb{R}}^{N·n_w\times N·n_w}$ of $\mathcal{G}$ is composed of weighting scalars $a_{ij}$, where $a_{ij}$ quantifies the strength of the connection from node j to node i. N is the number of agents in the cooperative system (CS). Formally, $A_{ij}$ is described by the following equation.

$${\mathit{A}}_{ij}=\left\{\begin{array}{cc}{a}_{ij}>0,\hfill & j\ne i,(j,i)\in \mathcal{E}\hfill \\ a_{ij}=0,\hfill & otherwise\hfill \end{array}\right.$$

(1)

4. ASLB—A Bipedal Robot for Dynamics Locomotion

4.1. ASLB System Composition

ASLB is a floating bipedal platform in which each leg is composed of hybrid structure with three degrees of freedom (DoFs). Specifically, starting from the body, the kinematic structure of legs are a revolutionjoint (at shoulder) followed by a parallel 5R mechanism. This kinematics result in three actuated and two passive joint coordinates in each leg. A rendered 3D model and manufactured prototype of ASLB is provided in Figure 1.

Table 1. Summary of key variables used for the description of the ASLB kinematics and dynamics.

${\mathbf{q}}_B$	set of unactuated joints
${\mathbf{q}}_{t,B}$	translational joints
${\mathbf{q}}_{r,B}$	rotational joints
${\mathbf{r}}_B\left(\right)$	translation matrix from one frame to another
${\mathbf{C}}_{IB}\left(\right)$	rotation matrix from frame I to frame B
${\mathit{\theta }}_i$	vector of joints for each leg
${\mathit{\theta }}_{a,i}$	vector of active joints for leg, i
${\mathit{\theta }}_{p,i}$	vector of passive joints for leg, i
${\mathbf{r}}_{v,i}$	position of point v
${\mathbf{R}}_{\ast }$	elementary rotation matrix for * axis
${\mathbf{v}}_{r,i}$	velocity relationship
${\mathbf{J}}_{r,i}$	Jacobian relating velocity to active and passive joints
$a_{.,.}$	lengths between joints
$\mathbf{M}\left(\right)$	generalized mass matrix
$\mathbf{C}(.,.)$	Coriolis and centrifugal terms
$\mathbf{G}\left(\right)$	gravitational terms
$\mathbf{S}$	control selection matrix for actuated joints of respective legs
$\mathit{\tau }$	actuated joint torques
${\mathit{F}}_{C,i}$	external force on tip of $i^{\mathrm{th}}$ leg
${\mathit{J}}_{C,i}$	geometric Jacobian of tip point for $i^{\mathrm{th}}$ leg
${\omega }_0$	angular speed for the linear inverted pendulum model (LIPM)
${}_{ }{}^p\overline{\mathbf{x}}_{c,K}^{ }$	contact state ${\overline{x}}_c$ at instant K in phase p

provides a summary of the key variables and terms used for the development of the ASLB dynamic model.

4.2. ASLB Kinematics

Kinematic model of the floating platform starts with a set of unactuated joints that gives six DoFs to the mobile platform. These joints are collected in a column matrix ${\mathit{q}}_B\in {\mathbb{R}}^3\times SO\left(3\right)$. Starting from the inertial frame, joints are located in a sequence such that first translational joints ${\mathit{q}}_{t,B}={[q_1,q_2,q_3]}^T\in {\mathbb{R}}^3$ are located in respective x, y and z directions. Assuming the euler sequence of $YZX$, rotational joints are ${\mathit{q}}_{r,B}={[q_4,q_5,q_6]}^T\in {\mathbb{R}}^3$. Body frame ${\mathcal{F}}_B$ is kinematically represented with respect to ${\mathcal{F}}_I$ by ${\mathit{q}}_B={[{\mathit{q}}_{t,B}^T,{\mathit{q}}_{r,B}^T]}^T$ such that ${\mathit{r}}_B\left({\mathit{q}}_{t,B}\right)$ and ${\mathit{C}}_{IB}\left({\mathit{q}}_{r,B}\right)$ are translation and rotation matrices from ${\mathcal{F}}_I$ to ${\mathcal{F}}_B$, respectively. There are two legs attached to the body, and respective joints are represented by ${\mathit{\theta }}_i\in {\mathbb{R}}^{n_i}$, where $n_i=5$ and $i=R,L$. In total, the combined DoF for legs are given as $n_a=10$. The composition of ${\mathit{\theta }}_i$ for each leg is given as ${\mathit{\theta }}_i={\left[{\theta }_{0,i}{\theta }_{1,i}{\theta }_{2,i}{\theta }_{3,i}{\theta }_{4,i}\right]}^T$, where there are three active and two passive joints, respectively ${\mathit{\theta }}_{a,i}={\left[{\theta }_{0,i}{\theta }_{1,i}{\theta }_{3,i}\right]}^T\in {\mathbb{R}}^3$ and ${\mathit{\theta }}_{p,i}\left({\mathit{\theta }}_{a,i}\right)={\left[{\theta }_{2,i}\left({\mathit{\theta }}_{a,i}\right){\theta }_{4,i}\left({\mathit{\theta }}_{a,i}\right)\right]}^T$. Passive joints can be written as a function of ${\mathit{\theta }}_{a,i}$ based on a velocity constraint as described in Section 4.2.1. As a result, total joint space of ASLB is given by $\mathit{q}={\left[{\mathit{q}}_B,{\mathit{\theta }}_{a,R},{\mathit{\theta }}_{a,L}\right]}^T\in {\mathbb{R}}^{12}$. The kinematic structure of the right leg is illustrated in Figure 2.

4.2.1. Passive—Active Joint Relation

As described in [42] starting from the origin of the body frame ${\mathcal{F}}_B$, there are two chains to reach point $\mathit{v}$ on both legs. Let ${\mathit{R}}_{\ast }$ represent elementary rotation matrix, where $\ast =x,y,z$ are active axes. Then these two chains can be written as in Equation (2).

$$\begin{array}{c}\hfill {\mathit{r}}_{v,1}={\mathit{a}}_{12}+{\mathit{R}}_z\left({\theta }_{1,R}\right){\mathit{a}}_2+{\mathit{R}}_z({\theta }_{1,R}+{\theta }_{2,R}){\mathit{a}}_2\\ \hfill {\mathit{r}}_{v,2}={\mathit{a}}_{14}+{\mathit{R}}_z\left({\theta }_{3,R}\right){\mathit{a}}_2+{\mathit{R}}_z({\theta }_{3,R}+{\theta }_{4,R}){\mathit{a}}_2\end{array}$$

(2)

Differentiating Equation (2) results in velocity equations of ${\mathit{v}}_{r,1}$ and ${\mathit{v}}_{r,2}$, which are equal. Writing this relationship as below relates the passive joints to active joints as described in Equation (3). Let ${\overline{\mathit{\theta }}}_{a,i}={\left[{\theta }_{1,i}{\theta }_{3,i}\right]}^T$ denote the set of active joints related to the closed loop, then ${\mathit{J}}_{a,i}\in {\mathbb{R}}^2$ becomes a square matrix.

$$\begin{array}{cc}\hfill {\mathit{v}}_{r,1}=& {\mathit{J}}_{r,1}{\left[\begin{array}{cc}{\overline{\mathit{\theta }}}_{a,i}^T& {\mathit{\theta }}_{p,i}^T\end{array}\right]}^T\hfill \\ \hfill {\mathit{v}}_{r,2}=& {\mathit{J}}_{r,2}{\left[\begin{array}{cc}{\overline{\mathit{\theta }}}_{a,i}^T& {\mathit{\theta }}_{p,i}^T\end{array}\right]}^T\hfill \\ \hfill \mathbf{0}=& ({\mathit{J}}_{r,1}-{\mathit{J}}_{r,2}){\left[\begin{array}{cc}{\overline{\mathit{\theta }}}_{a,i}^T& {\mathit{\theta }}_{p,i}^T\end{array}\right]}^T\hfill \\ \hfill =& \left[\begin{array}{cc}{\mathit{J}}_a& {\mathit{J}}_p\end{array}\right]{\left[\begin{array}{cc}{\overline{\mathit{\theta }}}_{a,i}^T& {\mathit{\theta }}_{p,i}^T\end{array}\right]}^T\hfill \end{array}$$

(3)

Finally, passive joints are related to active joints as given in Equation (4).

$$\begin{array}{c}\hfill {\mathit{\theta }}_{p,i}={\mathit{J}}_{pa}{\overline{\mathit{\theta }}}_{a,i}\\ \hfill {\mathit{J}}_{pa}=-{\mathit{J}}_{p,i}^{-1}{\mathit{J}}_{a,i}\end{array}$$

(4)

4.2.2. Forward Kinematics

Forward kinematics for each leg are used to calculate the position of the contact point c with respect to origin of ${\mathcal{F}}_B$, as illustrated in Figure 2. The aforementioned position vector is denoted as ${\mathit{r}}_c$. As mentioned in Section 4.2.1 the active and passive joint angles are related to each other and unless the passive joints are measured by sensors, they have to be calculated from this relationship. To do so, position vectors ${\mathit{r}}_{v,1}$ and and ${\mathit{r}}_{v,2}$ are arranged as shown below.   

$$\begin{array}{cc}\hfill & \left[\begin{array}{c}cos({\theta }_{3,R}+{\theta }_{4,R})\\ sin({\theta }_{3,R}+{\theta }_{4,R})\\ 0\end{array}\right]a_{5,x}=\hfill \\ \hfill & -\left[\begin{array}{c}cos({\theta }_{1,R}+{\theta }_{2,R})\\ sin({\theta }_{1,R}+{\theta }_{2,R})\\ 0\end{array}\right]a_{3,x}+\left[\begin{array}{c}{a}_{12,x}-a_{14,x}\\ a_{12,y}-a_{14,y}\\ a_{12,z}-a_{14,z}\end{array}\right]\hfill \\ \hfill & +\left[\begin{array}{c}cos\left({\theta }_{1,R}\right)\\ sin\left({\theta }_{1,R}\right)\\ 0\end{array}\right]a_{2,x}-\left[\begin{array}{c}cos\left({\theta }_{3,R}\right)\\ sin\left({\theta }_{3,R}\right)\\ 0\end{array}\right]a_{4,x}\hfill \end{array}$$

(5)

The first two rows of Equation (5) can be written in a minimal form as provided in Equation (6) such that terms including ${\theta }_{3,R}$, ${\theta }_{4,R}$ are left alone.

$$\begin{array}{cc}\hfill & \left[\begin{array}{c}cos({\theta }_{3,R}+{\theta }_{4,R})\\ sin({\theta }_{3,R}+{\theta }_{4,R})\end{array}\right]=\left[\begin{array}{c}{T}_x\\ T_y\end{array}\right]+\frac{a_{3,x}}{a_{5,x}}\left[\begin{array}{c}cos({\theta }_{1,R}+{\theta }_{2,R})\\ sin({\theta }_{1,R}+{\theta }_{2,R})\end{array}\right]\hfill \\ \hfill & T_x=\frac{1}{a_{5,x}}(a_{12,x}-a_{14,x}+cos\left({\theta }_{1,R}\right)+cos\left({\theta }_{3,R}\right))\hfill \\ \hfill & T_y=\frac{1}{a_{5,x}}(a_{12,y}-a_{14,y}+sin\left({\theta }_{1,R}\right)+sin\left({\theta }_{3,R}\right))\hfill \end{array}$$

(6)

Elements of Equation (6) are squared and summed to obtain Equation (7). Then trigonometric expressions are written in terms of tangents of the half angles, which leads to the a solution to ${\theta }_{12,R}={\theta }_{1,R}+{\theta }_{2,R}$ as provided in Equation (8).

$$\begin{array}{c}\hfill T_x^2+T_y^2+{\frac{a_{3,x}}{a_{5,x}}}^2+2T_x\frac{a_{3,x}}{a_{5,x}}cos\left({\theta }_{12,R}\right)+2Bsin\left({\theta }_{12,R}\right)=1\end{array}$$

(7)

$$\begin{array}{cc}\hfill {\theta }_{12,R}& =2{tan}^{-1}\left(T_{12}\right)\hfill \\ \hfill T_{12}& =\frac{-2C_3\pm \sqrt{{\left(2C_3\right)}^2-4(C_1-C_2)(C_1+C_2)}}{2(C_1-C_2)}\hfill \\ \hfill C_1& =T_x^2+T_y^2+{\frac{a_{3,x}}{a_{5,x}}}^2-1\hfill \\ \hfill C_2& =2T_x\frac{a_{3,x}}{a_{5,x}}\hfill \\ \hfill C_3& =2T_y\frac{a_{3,x}}{a_{5,x}}\hfill \end{array}$$

(8)

Using ${\theta }_{12,R}$, one can write Equation (9) and solve for ${\theta }_{34,R}={\theta }_{3,R}+{\theta }_{4,R}$. Finally, tip point location ${\mathit{r}}_c$ is calculated as provided in Equation (10).

$$\begin{array}{cc}\hfill {\theta }_{34,R}& =atan2(s_{34,R},c_{34,R})\hfill \\ \hfill \left[\begin{array}{c}{c}_{34,R}\\ s_{34,R}\end{array}\right]& =\left[\begin{array}{c}{T}_x+\frac{a_{3,x}}{a_{5,x}}cos\left({\theta }_{12,R}\right)\\ T_y+\frac{a_{3,x}}{a_{5,x}}sin\left({\theta }_{12,R}\right)\end{array}\right]\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {\mathit{r}}_c={\mathit{a}}_0+{\mathit{R}}_x\left({\theta }_{0,R}\right)({\mathit{a}}_{14}& +{\mathit{R}}_z\left({\theta }_{3,R}\right){\mathit{a}}_4\hfill \\ & +{\mathit{R}}_z\left({\theta }_{34,R}\right)({\mathit{a}}_5+{\mathit{a}}_6))\hfill \end{array}$$

(10)

4.2.3. Inverse Kinematics

Inverse kinematics is illustrated on right leg, and the calculations provided here can be duplicated for left leg. Inverse kinematics solutions in this work rely on the geometric calculation of ${\theta }_{0,R}$ as provided in Equation (11). The geometric entities are illustrated in Figure 3.

$$\begin{array}{cc}\hfill {\theta }_{0,R}& =atan2(sin\left({\theta }_{0,R}\right),cos\left({\theta }_{0,R}\right))\hfill \\ \hfill \left[\begin{array}{c}cos\left({\theta }_{0,R}\right)\\ sin\left({\theta }_{0,R}\right)\end{array}\right]& ={\left[\begin{array}{cc}{a}_{142,z}& -r_a\\ -r_a& -a_{142,z}\end{array}\right]}^{-1}\left[\begin{array}{c}{r}_z-a_{0,z}\\ r_y-a_{0,y}\end{array}\right]\hfill \\ \hfill {\mathit{r}}_1& =\mathit{r}-{\mathit{a}}_0\hfill \\ \hfill r_a& =\sqrt{r_{1,y}^2+r_{1,z}^2-a_{14,z}^2}\hfill \\ \hfill a_{142,z}& =a_{14,z}+a_{2,z}\hfill \end{array}$$

(11)

To calculate active joints ${\theta }_{1,R}$ and ${\theta }_{3,R}$x and y components of the position vector ${\mathit{r}}_{pc}$ as given in Equation (12) are expanded in Equation (13).

$$\begin{array}{cc}\hfill r_{pc,x}& =a_{4,x}cos\left({\theta }_{3,R}\right)+a_{56,x}cos({\theta }_{3,R}+{\theta }_{4,R})\hfill \\ \hfill r_{pc,y}& =a_{4,x}sin\left({\theta }_{3,R}\right)+a_{56,x}sin({\theta }_{3,R}+{\theta }_{4,R})\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill r_{pc,x}& =a_{4,x}cos\left({\theta }_{3,R}\right)+a_{56,x}(cos\left({\theta }_{3,R}\right)cos\left({\theta }_{4,R}\right)-sin\left({\theta }_{3,R}\right)sin\left({\theta }_{4,R}\right))\hfill \\ \hfill r_{pc,y}& =a_{4,x}sin\left({\theta }_{3,R}\right)+a_{56,x}(cos\left({\theta }_{3,R}\right)sin\left({\theta }_{4,R}\right)+sin\left({\theta }_{3,R}\right)cos\left({\theta }_{4,R}\right))\hfill \end{array}$$

(13)

Equation (13) is rewritten as in Equation (14), and each scalar equation can be squared and summed as in Equation (15).

$$\begin{array}{cc}\hfill cos\left({\theta }_{3,R}\right)& =(r_{pc,x}/a_{4,x})+(a_{56,x}/a_{4,x})cos({\theta }_{3,R}+{\theta }_{4,R})\hfill \\ \hfill & =C_4+C_{5}cos({\theta }_{3,R}+{\theta }_{4,R})\hfill \\ \hfill sin\left({\theta }_{3,R}\right)& =(r_{pc,y}/a_{4,x})+(a_{56,x}/a_{4,x})(sin({\theta }_{3,R}+{\theta }_{4,R})\hfill \\ \hfill & =C_6+C_{5}sin({\theta }_{3,R}+{\theta }_{4,R})\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill 1& =C_4^2+C_6^2+C_5^2+2C_4{C}_{5}cos({\theta }_{3,R}+{\theta }_{4,R})+2C_6{C}_{5}sin({\theta }_{3,R}+{\theta }_{4,R})\hfill \\ \hfill 0& =(C_4^2+C_6^2+C_5^2-1)+\left(2C_4{C}_5\right)cos({\theta }_{3,R}+{\theta }_{4,R})+\left(2C_6{C}_5\right)sin({\theta }_{3,R}+{\theta }_{4,R})\hfill \\ \hfill 0& =C_7+C_{8}cos({\theta }_{3,R}+{\theta }_{4,R})+C_{9}sin({\theta }_{3,R}+{\theta }_{4,R})\hfill \end{array}$$

(15)

Then, using tangents of the half angle, Equation (15) can be further manipulated in Equation (16). The solution to ${\theta }_{34,R}$ can be calculated by solving the quadratic problem for $T_{34}$ as illustrated in Equation (8).

$$\begin{array}{c}\hfill 0=(C_7-C_8)T_{34}^2+2C_9{T}_{34}+(C_7+C_8)\end{array}$$

(16)

After finding the solution to ${\theta }_{34,R}$, these values are inserted into Equation (12) to calculate ${\theta }_{3,R}$. This completes the solution to one chain of the leg mechanism.

Geometric definitions such as ${\mathit{r}}_{pcl}$, ${\mathit{r}}_{cl}$ are provided in Figure 4 to calculate the joint variables on the other chain that contains ${\mathit{a}}_2$ and ${\mathit{a}}_3$.

Using the known joint variables, point v is represented from origin of the joint $\mathbf{3}$ with a vector denoted as ${\mathit{r}}_{pcl}$. Alternatively, point v can also be represented from the origin of the joint $\mathbf{1}$ with a vector that is ${\mathit{r}}_{cl}$. These vectors are defined on $(P,R)$ plane, where the 5R mechanism lies, and provided in Equations (17) and (18), respectively.

$$\begin{array}{c}\hfill r_{pcl,x}=r_{pc,x}-a_{6,x}cos({\theta }_{3,R}+{\theta }_{4,R})\\ \hfill r_{pcl,y}=r_{pc,y}-a_{6,x}sin({\theta }_{3,R}+{\theta }_{4,R})\end{array}$$

(17)

$$\begin{array}{cc}\hfill r_{cl,x}& =a_{2,x}cos\left({\theta }_{1,R}\right)+a_{3,x}cos({\theta }_{1,R}+{\theta }_{2,R})\hfill \\ \hfill r_{cl,y}& =a_{2,x}sin\left({\theta }_{1,R}\right)+a_{3,x}sin({\theta }_{1,R}+{\theta }_{2,R})\hfill \end{array}$$

(18)

Scalar equations $r_{cl,x}$ and $r_{cl,y}$ of Equation (18) are squared, summed, and reorganized as provided in Equation (19) to calculate ${\beta }_R$, which leads to ${\theta }_{2,R}$.

$$\begin{array}{cc}\hfill cos\left({\beta }_R\right)& =-\left(\frac{r_{cl,x}^2+r_{cl,y}^2-a_{2,x}^2-a_{3,x}^2}{2a_{2,x}{a}_{3,x}}\right)\hfill \\ \hfill {\beta }_R& =acos\left(-\frac{r_{cl,x}^2+r_{cl,y}^2-a_{2,x}^2-a_{3,x}^2}{2a_{2,x}{a}_{3,x}}\right)\hfill \\ \hfill {\theta }_{2,R}& =\pi +{\beta }_R\hfill \end{array}$$

(19)

To calculate ${\theta }_{1,R}$, $r_{cl,x}$ and $r_{cl,y}$ are represented as a function of $r_{pcl}$. This is given in Equation (20) in matrix form, where terms with ${\theta }_{1,R}$ are the unknown variables. The solution to Equation (20) for ${\theta }_{1,R}$ finishes the inverse kinematic solution of the right leg. The procedure for the left leg is identical.

$$\begin{array}{c}\hfill \left[\begin{array}{c}{r}_{pcl,x}+(a_{14,x}-a_{12,x})\\ r_{pcl,y}\end{array}\right]=\\ \hfill \left[\begin{array}{cc}{a}_{2,x}+a_{3,x}cos\left({\theta }_{2,R}\right)& -a_{3,x}sin\left({\theta }_{2,R}\right)\\ a_{3,x}sin\left({\theta }_{2,R}\right)& a_{2,x}+a_{3,x}cos\left({\theta }_{2,R}\right)\end{array}\right]\left[\begin{array}{c}cos\left({\theta }_1\right)\\ sin\left({\theta }_1\right)\end{array}\right]\\ \hfill {\theta }_{1,R}=atan2(sin\left({\theta }_1\right),cos\left({\theta }_1\right))\end{array}$$

(20)

4.3. ASLB Dynamics

Based on the generalized coordinates, multi-body dynamics of ASLB are formulated as in (Equation (21))

$$\begin{array}{c}\hfill \mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}(\mathit{q},\dot{\mathit{q}})+\mathit{G}\left(\mathit{q}\right)=\mathit{S}\mathit{\tau }+{\mathit{J}}_{C,i}^T{\mathit{F}}_{C,i}\end{array}$$

(21)

where $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{12\times 12}$, $\mathit{C}(\mathit{q},\dot{\mathit{q}})\in {\mathbb{R}}^{12}$, and $\mathit{G}\left(\mathit{q}\right)\in {\mathbb{R}}^{12}$ are the generalized mass, Coriolis and centrifugal, and gravitational terms, respectively. $\mathit{S}\in {\mathbb{R}}^{12\times 6}$, $\mathit{\tau }\in {\mathbb{R}}^6$, ${\mathit{F}}_{C,i}\in {\mathbb{R}}^3$ and ${\mathit{J}}_{C,i}\in {\mathbb{R}}^{3\times 12}$ are the selection matrix for the actuated joints of respective legs, actuated joint torques, geometric Jacobian of the tip point of the $i^{th}$ leg, and external force on the tip of $i^{th}$ leg.

Let $\mathit{x}={\left[\mathit{q},\dot{\mathit{q}}\right]}^T\in {\mathbb{R}}^{24}$, ${\mathit{u}}_{\tau }=\mathit{\tau }$, and ${\mathit{u}}_c={\mathit{F}}_{C,i}\forall i$ be the states, inputs to motors and external forces acting on tip point of the legs, respectively; then, the non-linear dynamics of the robot can be written as in Equation (22).

$$\begin{array}{cc}\hfill \dot{\mathit{x}}& =\mathit{f}(\mathit{x},{\mathit{u}}_{\tau },\mathit{u})\hfill \\ \hfill \mathit{f}(\mathit{x},{\mathit{u}}_{\tau },\mathit{u})& =\left[\begin{array}{c}\dot{\mathit{q}}\\ \mathit{M}{\left(\mathit{q}\right)}^{-1}\mathbf{\Theta }\end{array}\right]\hfill \\ \hfill \mathit{\theta }& =-\mathit{C}(\mathit{q},\dot{\mathit{q}})-\mathit{G}\left(\mathit{q}\right)+\mathit{S}\mathit{\tau }+\sum _i{\mathit{J}}_{C,i}^T{\mathit{F}}_{C,i}\hfill \end{array}$$

(22)

The application of the graph-theoretic modeling for the ASLB by separating the legs, which are defined as Agent 2 and Agent 3, from the floating base introduces simplifications to the overall complexity of the model and optimization. One of the simplifications appears in modeling as leg dynamics does not necessarily need to be modeled with respect to ${\mathcal{F}}_I$ using ${\mathit{q}}_B$ but rather is better defined with respect to the ${\mathcal{F}}_B$. This local representation of leg dynamics leads to following outcomes for the dynamics of the agents:

• The leg is only used to find adjacency and contact forces on the floating base and ground,
• The state space representation of the leg dynamics can be kept at velocity level.

Note that the contact forces and geometric properties of contact point must be converted to ${\mathcal{F}}_B$.

For ASLB, the floating base of the robot is denoted as $\mathbf{Agent}\mathbf{1}$, the right leg is denoted as $\mathbf{Agent}\mathbf{2}$, and the left leg is denoted as $\mathbf{Agent}\mathbf{3}$. $\mathbf{Agent}\mathbf{1}$ is defined with two nodes, $\mathbf{node}\mathbf{1}$ and $\mathbf{node}\mathbf{2}$; $\mathbf{Agent}\mathbf{2}$ is defined with $\mathbf{node}\mathbf{3}$; and $\mathbf{Agent}\mathbf{3}$ is defined with $\mathbf{node}\mathbf{4}$. The locations of these nodes are illustrated in Figure 5, and it should be noted that $\mathbf{node}\mathbf{1}$ and $\mathbf{node}\mathbf{2}$ coincide. Without any interconnection constraint, agents are independent of each other; however, there are rigid joints connecting them. In this case there are two bi-directional edges and these are $(\mathbf{node}\mathbf{3},\mathbf{node}\mathbf{1})\in \mathcal{E}$ and $(\mathbf{node}\mathbf{2},\mathbf{node}\mathbf{4})\in \mathcal{E}$. Connections between nodes are assumed to be rigid, and then the adjacency matrix is composed of edge weights $a_{mn}=1$. This yields an adjacency matrix as in (23).

$$\mathbb{A}=\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$$

(23)

Finally, the relationships between cooperative agents are given in (24) by the Laplacian matrix. ${\mathit{I}}_p$ represents identity matrix with size p.

$$\mathbb{L}={\mathit{I}}_4-\mathbb{A}$$

(24)

Let $\mathit{W}\in {\mathbb{R}}^p$ be the signal that is being shared between agents; then, $\mathbb{L}$ can be expanded as in (25) to comply with the signal dimension. ⊗ is the Kronecker product operator.

$$\mathit{L}=\mathbb{L}\otimes {\mathit{I}}_p$$

(25)

Recalling that $\mathbf{node}\mathbf{1}$ and $\mathbf{node}\mathbf{2}$ are coincident, one can define the following adjacency constraints using the extended Laplacian definition given in (25)   

$$\begin{array}{cc}\hfill & {\mathit{L}}_x=\mathit{L}\left[\begin{array}{c}{\mathit{x}}_1\\ {\mathit{x}}_1\\ {\mathit{x}}_2\\ {\mathit{x}}_3\end{array}\right]=\mathbf{0}\hfill \\ \hfill & {\mathit{L}}_w=\mathit{L}\left[\begin{array}{c}{\mathit{W}}_{A,1}\\ {\mathit{W}}_{A,2}\\ {\mathit{W}}_{A,3}\\ {\mathit{W}}_{A,4}\end{array}\right]=\mathbf{0}\hfill \end{array}$$

(26)

Another aspect of splitting a lumped multi-body model into distributed cooperating multi-body models is generalized coordinates. This operation necessarily duplicates the generalized coordinates of the floating base in a lumped model to the distributed models. In addition to that, $\mathbf{Agent}\mathbf{2}$ and $\mathbf{Agent}\mathbf{3}$ have actuated joints of ${\mathit{q}}_{a,i},i=2,3$. Therefore, generalized coordinates of the agents are defined as ${\mathit{q}}_1={\left[{\mathit{q}}_{t,1}^T{\mathit{q}}_{r,1}^T\right]}^T\in {\mathbb{R}}^6$, ${\mathit{q}}_2={\left[{\mathit{q}}_{t,2}^T{\mathit{q}}_{r,2}^T{\mathit{q}}_{a,2}^T\right]}^T\in {\mathbb{R}}^9$ and ${\mathit{q}}_3={\left[{\mathit{q}}_{t,3}^T{\mathit{q}}_{r,3}^T{\mathit{q}}_{a,3}^T\right]}^T\in {\mathbb{R}}^9$, respectively, for $\mathbf{Agent}\mathbf{1}$, $\mathbf{Agent}\mathbf{2}$, and $\mathbf{Agent}\mathbf{3}$. This is illustrated in Figure 5.

4.4. Agent Kinematics

The YZX Euler sequence is used in defining the composite rotations ${\mathit{C}}_{Ii}\left({\mathit{q}}_{r,i}\right)$ from ${\mathcal{F}}_I$ to ${\mathcal{F}}_i$, where subscript i is the agent index and ${\mathcal{F}}_i$ is the local origin of the agent. The contact point position and velocity of $\mathbf{Agent}\mathbf{2}$ and $\mathbf{Agent}\mathbf{3}$ are calculated with respect to ${\mathcal{F}}_B$, which is denoted as in (27). ${\mathcal{K}}^{+}\left({\mathit{q}}_i\right)$ represents forward kinematics of leg i in Equation (27), which is explained in Section 4.2.

$$\begin{array}{c}\hfill {\mathit{r}}_{c,i}\left({\mathit{q}}_i\right)={\mathcal{K}}^{+}\left({\mathit{q}}_i\right)\\ \hfill {\dot{\mathit{r}}}_{c,i}({\mathit{q}}_i,{\dot{\mathit{q}}}_i)={\mathit{J}}_{c,i}{\dot{\mathit{q}}}_i\end{array}$$

(27)

4.5. Agent Dynamics

The dynamics of agents yield a similar equation as given in (22), and, for brevity, a representative Equation of Motion (EoM) is given in this section.

Generalized velocities are assigned to states of each agent as ${\mathit{x}}_i={\dot{\mathit{q}}}_i$. External forces in distributed notation are divided into two, where the first one is denoted as ${\mathit{F}}_{C,i}$ and acts on the agents as a result of ground contact. The second external force is denoted as ${\mathit{F}}_{A,m}$ and exerted on the agents from the adjacent nodes. Adjacent nodes also transmit moment, ${\mathit{M}}_{A,m}$; therefore it is convenient to collect forces and moments at adjacent nodes such as a wrench, denoted as ${\mathit{W}}_{A,m}={\left[{\mathit{F}}_{A,m}^T{\mathit{M}}_{A,m}^T\right]}^T$. As a result, the non-linear dynamics of each agent are written as given in (28). Let ${\mathit{x}}_i=\dot{\mathit{q}}\in {\mathbb{R}}^{n_{xi}}$, ${\mathit{u}}_{\tau ,i}=\mathit{\tau }\in {\mathbb{R}}^{n_{ti}}$, ${\mathit{u}}_{c,i}={\mathit{F}}_{C,i}\in {\mathbb{R}}^{n_{fi}}$, and ${\mathit{u}}_{w,i}={\mathit{W}}_{A,i}\in {\mathbb{R}}^{n_{wi}}$ be the states, torques, contact forces, and adjacency wrench, respectively; then, the non-linear dynamics of the robot can be written as in Equation (22). The signal sizes for Agent 1 are $n_{x1}=6$, $n_{t1}=0$, $n_{f1}=0$, $n_{w1}=6$, while Agent 2 and Agent 3 have signal sizes of $n_{xi}=9$, $n_{ti}=3$, $n_{fi}=3$, and $n_{wi}=6$ for $i=2,3$.

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_i=& {\mathit{f}}_i({\mathit{q}}_i,{\mathit{u}}_{\tau ,i},{\mathit{u}}_{c,i})\hfill \\ \hfill {\mathit{f}}_i=& {\mathit{M}}_i{\left({\mathit{q}}_i\right)}^{-1}{\mathbf{\Theta }}_i\hfill \\ \hfill {\mathbf{\Theta }}_i=& -{\mathit{C}}_i({\mathit{q}}_i,{\dot{\mathit{q}}}_i)-{\mathit{G}}_i\left({\mathit{q}}_i\right)\hfill \\ \hfill & +{\mathit{S}}_i{\mathit{\tau }}_i+{\mathit{J}}_{C,i}^T{\mathit{F}}_{C,i}+\sum _m{\mathit{J}}_{A,m}^T{\mathit{W}}_{A,m}\hfill \end{array}$$

(28)

5. Cooperative Graph-Theoretic Online Trajectory Generation for ASLB

Graph-theoretic online trajectory generation relies on the cooperative modeling of the robot and is composed of a series of optimizations. These are the contact phase, swing phase, and force optimizations, where contact optimization finds the optimal finite horizon for the current contact phase and a sequence of contact trajectories that will keep the robot states bounded for defined phase horizon. For this reason, the resultant contact phase trajectories, except for the one associated with the current contact leg, are not passed to the next optimization. Another cardinal data set that is passed to the next optimization from contact phase is the initial point of the subsequent contact phase. This information is required to generate a rough swing trajectory for the leg that is in the air. It should be noted that both contact and swing trajectories are calculated to provide an initial trajectory for the force optimization, where trajectories are refined using a cooperative system framework. Trajectories to previously defined optimizations are illustrated in Figure 6.

5.1. Contact Phase Optimization

The contact-phase optimization calculates a set of trajectories using the linear inverted pendulum model (LIPM) and contact constraints. The LIPM dynamics we used in this work are widely used in the vast majority of the literature. As shown in Section 5.3, the dynamics are written with respect to body frame ${\mathcal{F}}_B$. Besides that, the dynamics are also kept at the velocity level. Under these circumstances, the contact conditions for the leg in contact need to be defined accordingly.

5.1.1. Contact Condition

Under contact conditions with the no-slip assumption, ${\mathit{r}}_{c,i}$ has no relative motion with respect to the ground if this condition is observed from the inertial frame ${\mathcal{F}}_I$. This condition is observed from ${\mathcal{F}}_B$ as if ${\dot{\mathit{r}}}_{c,i}=-{\mathit{u}}_B$, which is illustrated in Equation (29) for agents $i=2,3$. Recall that states of agents $i=2,3$ are denoted as ${\mathit{x}}_i={\left[{\mathit{q}}_{t,i}^T{\mathit{q}}_{r,i}^T{\mathit{q}}_{a,i}^T\right]}^T,i=2,3$, and ${\dot{\mathit{q}}}_{t,i}$, where  $i=2,3$ is duplicate of ${\mathit{u}}_B$, assuming that the connections between nodes are as defined in Equation (26).

$$\begin{array}{cc}\hfill \mathbf{0}& =\left[\begin{array}{ccc}-\mathit{I}& -{\mathit{J}}_{r,i}& {\mathit{J}}_{a,i}\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{q}}}_{t,i}\\ {\dot{\mathit{q}}}_{r,i}\\ {\dot{\mathit{q}}}_{a,i}\end{array}\right]=\left[\begin{array}{ccc}-\mathit{I}& -{\mathit{J}}_{r,i}& {\mathit{J}}_{a,i}\end{array}\right]\left[\begin{array}{c}{\mathit{u}}_B\\ {\dot{\mathit{q}}}_{r,i}\\ {\dot{\mathit{q}}}_{a,i}\end{array}\right]\hfill \end{array}$$

(29)

Assuming the body is slowly rotating, ${\dot{\mathit{q}}}_{r,i}\approx \mathbf{0},i=2,3$

5.1.2. LIPM Model

The implementation of the LIPM model in this work has some nuances compared to the the work where it is proposed [43,44] and illustrated in Figure 7. Let ${\mathit{F}}_{xz}\in {\mathbb{R}}^2$ be the virtual force created on the $x-z$ plane due to the displacement between origin and contact locations ${}_{xz}{}^{ }\mathit{r}_{c,2}^{ }$ and ${}_{xz}{}^{ }\mathit{r}_{c,3}^{ }$. Note that the origin represents the center of mass (CoM) and is not the origin of the ${\mathcal{F}}_B$. Although CoM moves with respect to the origin of ${\mathcal{F}}_B$, in practice, it is assumed to be fixed with an offset from ${\mathcal{F}}_B$.

Under these assumptions, the LIPM dynamics are provided as in Equation (30) in state space form. States for this system are denoted as ${\mathit{x}}_c$ and defined as the contact point, and this point is denoted as $\mathbf{\Delta }{\mathit{r}}_{xz}$, as any of the two contact locations can be assigned to it, which are ${}_{xz}{}^{ }\mathit{r}_{c,2}^{ }$ and ${}_{xz}{}^{ }\mathit{r}_{c,3}^{ }$, respectively. Specifically, states are defined as ${\overline{\mathit{x}}}_{c,1}=\mathbf{\Delta }{\mathit{r}}_{xz}\in {\mathbb{R}}^2$, ${\overline{\mathit{x}}}_{c,2}=\frac{d}{dt}{\mathbf{\Delta }\mathit{r}}_{xz}\in {\mathbb{R}}^2$, which are combined as ${\overline{\mathit{x}}}_c={\left[{\overline{\mathit{x}}}_{c,1}^T{\overline{\mathit{x}}}_{c,2}^T\right]}^T$.

$${\dot{\overline{\mathit{x}}}}_c=\left[\begin{array}{c}{\dot{\overline{\mathit{x}}}}_{c,1}\\ {\dot{\overline{\mathit{x}}}}_{c,2}\end{array}\right]=\left[\begin{array}{cc}\mathbf{0}& \mathit{I}\\ {\omega }_0^2\mathit{I}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}{\overline{\mathit{x}}}_{c,1}\\ {\overline{\mathit{x}}}_{c,2}\end{array}\right]={\mathit{A}}_{lipm}{\overline{\mathit{x}}}_c$$

(30)

This system is an inherently unstable system; therefore, what is being pursued with this system is to find a set of initial conditions, denoted as ${}_{ }{}^0\overline{\mathit{x}}_c^{ }$, that will propel the CoM of the robot toward the desired velocity vector. While carrying that out, a set of state bounds are also satisfied. Equation (30) is discretized using Euler propagation as provided below, where $\Delta t$ is the sampling time.

$${\overline{\mathit{x}}}_{c,k+1}=(\mathit{I}+\Delta t{\mathit{A}}_{lipm}){\overline{\mathit{x}}}_{c,k}$$

(31)

5.1.3. Contact Phase Optimization

This method relies on finding a set of trajectories that will keep the proceeding steps within bounds; therefore, a phase horizon is defined as $N_p\in \mathbb{N}$, which represents the number of phases to be calculated during the optimization, including the current phase. A finite horizon for each phase is defined as $N_n\in \mathbb{N}$, which will be minimized for $N_p=1$ and kept at its nominal for $N_p>1$. The states of the LIPM dynamics in each phase are denoted as ${}_{ }{}^p\overline{\mathit{x}}_{c,k}^{ }$, where $p=\left[1,\cdots ,N_p\right]$ and $k=\left[1,\cdots ,N_n\right]$. The combined states for each phase are denoted as ${}_{ }{}^p\mathit{x}_{c,K}^{ }$ as provided below.

$$\begin{array}{cc}\hfill {}_{ }{}^p\overline{\mathit{x}}_{c,K}^{ }& =\left[\begin{array}{c}{}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ }\\ ⋮\\ {}_{ }{}^p\overline{\mathit{x}}_{c,N_n}^{ }\end{array}\right],\forall p\hfill \end{array}$$

(32)

Equation (33) represents the quadratic problem that runs in contact optimization phase, where ${\mathcal{L}}_c({}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ },{\mathit{u}}_{B,ref})$ is the objective function, and ${\mathcal{C}}_{cont}\left({}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ }\right)$ and ${\mathcal{C}}_{bounds}\left({}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ }\right)$ are equality and inequality constraints to ensure continuity of the states between phases and to keep the states within predefined bounds.

$$\begin{array}{ccccc}\hfill & \underset{{}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ }}{\mathrm{minimize}}\hfill & \hfill & \sum _{k=1}^M{\mathcal{L}}_c({}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ },{\mathit{u}}_{B,ref})\hfill & \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill & \hfill & {\mathcal{C}}_{cont}\left({}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ }\right)=\mathbf{0}\hfill \\ \hfill & \hfill & & {\mathcal{C}}_{bounds}\left({}_{ }{}^p\overline{\mathit{x}}_{c,1}^{ }\right)\le \mathbf{0}\hfill \end{array}$$

(33)

5.1.4. Contact Phase: Continuity Constraint

As explained earlier, contact optimization seeks to find several contact phase trajectories, and these trajectories should ideally be continuous. This is achieved by an equality constraint defined as in Equation (34) for $p=1,\cdots ,N_p-1$.

$$\begin{array}{cc}\hfill {}_{ }{}^p\overline{\mathit{x}}_{c,N_n}^{ }& ={}_{ }{}^{p+1}\overline{\mathit{x}}_{c,1}^{ }\hfill \\ \hfill \left[\begin{array}{cc}\mathbf{0}& \mathit{I}\end{array}\right]{}_{ }{}^p\overline{\mathit{x}}_{c,N_n}^{ }& =\left[\begin{array}{cc}\mathbf{0}& \mathit{I}\end{array}\right]{}_{ }{}^{p+1}\overline{\mathit{x}}_{c,1}^{ }\hfill \end{array}$$

(34)

5.1.5. Contact Phase: Constraint for State Bounds

State bounds are defined based on the leg in contact; therefore, state bounds are switching between the bounds of Agent 2 and Agent 3. Let $c_2=\left[0,1\right]$ and $c_3=\left[0,1\right]$ be the contact indicators of Agent 2 and Agent 3, respectively. Under contact $c_i=1$; otherwise, $c_i=0$ for $i=2,3$ at phase p. The bounds for Agent 2 and Agent 3 are defined as ${\mathit{S}}_i={\left[{\mathit{S}}_{i,UB}^T,{\mathit{S}}_{i,LB}^T\right]}^T$ and assigned to ${\mathit{S}}_p$ such that ${\mathit{S}}_p={\mathit{S}}_i$ if $c_i=1$. Note that we are assuming a single point of contact. Using the bounds, the state boundary constraints are defined as in Equation (35).

$$\begin{array}{c}\hfill \left[\begin{array}{c}\mathit{I}\\ -\mathit{I}\end{array}\right]\left[\begin{array}{cc}\mathit{I}& \mathbf{0}\end{array}\right]{}_{ }{}^p\overline{\mathit{x}}_{c,k}^{ }\le \left[\begin{array}{c}{\mathit{S}}_{p,UB}\\ {\mathit{S}}_{p,LB}\end{array}\right]\end{array}$$

(35)

5.1.6. Contact Phase: Cost Function

Contact phase optimization aims to reach the $x-z$ projection of reference velocity that is provided by the user ${\mathit{u}}_{B,ref}$, which is ${\mathit{u}}_{B,xz}$. Recall that ${\dot{\mathit{r}}}_{c,i}=-{\mathit{u}}_B$; therefore, the cost function is written as in Equation (36).

$$\begin{array}{cc}\hfill J_{contact}=& {\left[{\mathit{u}}_{B,xz}+\left[\begin{array}{cc}\mathbf{0}& \mathit{I}\end{array}\right]{}_{ }{}^p\overline{\mathit{x}}_{c,K}^{ }\right]}^T{\mathit{Q}}_s{\left[{\mathit{u}}_{B,xz}+\left[\begin{array}{cc}\mathbf{0}& \mathit{I}\end{array}\right]{}_{ }{}^p\overline{\mathit{x}}_{c,K}^{ }\right]}^T+\hfill \\ \hfill & {\left[\left[\begin{array}{cc}\mathbf{0}& \mathit{I}\end{array}\right]{}_{ }{}^p\overline{\mathit{x}}_{c,K}^{ }\right]}^T{\mathit{Q}}_p{\left[\left[\begin{array}{cc}\mathbf{0}& \mathit{I}\end{array}\right]{}_{ }{}^p\overline{\mathit{x}}_{c,K}^{ }\right]}^T\hfill \end{array}$$

(36)

5.2. Swing Phase Optimization

Swing phase optimization calculates a rough trajectory for the swinging leg by connecting the current position of the tip of the swinging leg to the initial point of the proceeding contact phase trajectory with a bezier curve. Instances of the swing trajectory are denoted as ${}_{ }{}^p\mathit{x}_{s,k}^{ }$, and the current and final positions of the swing trajectory are denoted as ${}_{ }{}^p\mathit{x}_{s,1}^{ }$ and ${}_{ }{}^p\mathit{x}_{s,N_n}^{ }$, respectively. ${}_{ }{}^p\overline{\mathit{x}}_{s,1}^{ }$ and ${}_{ }{}^p\overline{\mathit{x}}_{s,N_n}^{ }$ are the projections of the vectors on the $x-z$ plane, and the initial point of the proceeding contact phase trajectory is ${}_{ }{}^{p+1}\overline{\mathit{x}}_{c,1}^{ }$. Note that the contact phase optimization generates a planar trajectory. Therefore, the implementation of swing-phase trajectory optimization requires a modification of these vectors by adding the height of the CoM to the y axis of any projected vector if it needs to be passed to the swing trajectory. Figure 8 illustrates previously mentioned vectors. Dashed lines represent the projection of the swing trajectory, while solid lines show the contact trajectory. ${\mathit{n}}_0$, ${\mathit{n}}_f$, and $\mathit{L}$ represent the unit vector to CoM, the unit vector from CoM, and the straight line on the $x-z$ plane between initial and final positions of the swing trajectory. Formal definitions for ${\mathit{n}}_0$, ${\mathit{n}}_f$ and $\mathit{L}$ are provided in Equation (37).

$$\begin{array}{cc}\hfill {\mathit{n}}_0& =-\frac{{}_{ }{}^{p=1}\mathit{x}_{s,1}^{ }}{|{}_{ }{}^{p=1}\mathit{x}_{s,1}^{ }|}\hfill \\ \hfill {\mathit{n}}_f& =\frac{{}_{ }{}^{p=1}\mathit{x}_{s,N_n}^{ }}{|{}_{ }{}^{p=1}\mathit{x}_{s,N_n}^{ }|}\hfill \\ \hfill \mathit{L}& ={}_{ }{}^{p=1}\mathit{x}_{s,N_n}^{ }-{}_{ }{}^{p=1}\mathit{x}_{s,1}^{ }\hfill \end{array}$$

(37)

5.2.1. Swing Phase Optimization

Swing phase optimization is run for the current phase; therefore, unlike for the contact phase optimization, $p=1$. The finite horizon for this phase is the $N_n$ at $p=1$. Note that at $p=1$, $N_n$ is optimized at the contact phase optimization.

Let $b_{x,j}$, $b_{y,j}$, and $b_{z,j}$ be the coefficients of the bezier curve, where $j=1,\cdots ,nb$. ${\mathit{c}}_b$ and ${\mathit{v}}_b$, which are sorted collections of $b_{x,j}$, $b_{y,j}$, and $b_{z,j}$, are defined. This classification collects coefficients related to initial and final positions of the curve under ${\mathit{c}}_b$ and coefficients that are being optimized under ${\mathit{v}}_b$. Specifically ${\mathit{c}}_b$ is defined as ${\mathit{c}}_b={\left[\begin{array}{cc}{}_{ }{}^{p=1}\mathit{x}_{s,1}^T& {}_{ }{}^{p=1}\mathit{x}_{s,N_n}^{ }\end{array}\right]}^T$. Based on thepreviously described notation, bezier curves for swing trajectory are defined as in Equation (39), where ${\mathit{J}}_{b,c}$ and ${\mathit{J}}_{b,v}$ are matrix-valued functions of k, which can be populated for $k=1<\cdots ,N_n$ and maps ${\mathit{c}}_b$ and ${\mathit{v}}_b$ to ${}_{ }{}^{p=1}\mathit{x}_{s,k}^{ }\in {\mathbb{R}}^3$. Similarly, ${\mathit{c}}_b$ and ${\mathit{v}}_b$ are mapped to ${}_{ }{}^{p=1}\dot{\mathit{x}}_{s,k}^{ }\in {\mathbb{R}}^3$ using ${}_d{}^{ }\mathit{J}_{b,c}^{ }$ and ${}_d{}^{ }\mathit{J}_{b,v}^{ }$.

Equation (38) represents the quadratic problem that runs in the swing optimization phase, where ${\mathcal{L}}_s\left({\mathit{v}}_b\right)$ is the objective function and ${\mathcal{S}}_{bounds}\left({\mathit{v}}_b\right)$ is the set of inequality constraints to keep states within predefined bounds.

$$\begin{array}{ccccc}\hfill & \underset{{\mathit{v}}_b}{\mathrm{minimize}}\hfill & \hfill & \sum _{k=1}^M{\mathcal{L}}_s\left({\mathit{v}}_b\right)\hfill & \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill & \hfill & {\mathcal{C}}_{bounds}\left({\mathit{v}}_b\right)\le \mathbf{0}\hfill \end{array}$$

(38)

$$\begin{array}{c}\hfill {}_{ }{}^{p=1}\mathit{x}_{s,k}^{ }=\left[\begin{array}{cc}{\mathit{J}}_{b,c}\left(k\right)& {\mathit{J}}_{b,v}\left(k\right)\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\\ \hfill {}_{ }{}^{p=1}\dot{\mathit{x}}_{s,k}^{ }=\left[\begin{array}{cc}{}_d{}^{ }\mathit{J}_{b,c}^{ }\left(k\right)& {}_d{}^{ }\mathit{J}_{b,v}^{ }\left(k\right)\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\end{array}$$

(39)

The implementation of swing phase optimization requires the calculation of ${}_d{}^0\mathit{J}_{b,c}^{ }$, ${}_d{}^f\mathit{J}_{b,c}^{ }$, ${}_d{}^0\mathit{J}_{b,v}^{ }$, ${}_d{}^f\mathit{J}_{b,v}^{ }$ using Equation (39) at $k=1$ and $k=N_n$.

5.2.2. Swing Phase: Constraint for State Bounds

State bounds are defined based on the leg in the swing; therefore, state bounds switch between the bounds of Agent 2 and Agent 3. Bounds for Agent 2 and Agent 3 are defined as ${\mathit{S}}_i={\left[{\mathit{S}}_{i,UB}^T,{\mathit{S}}_{i,LB}^T\right]}^T$ and assigned to ${\mathit{S}}_p$ such that ${\mathit{S}}_p={\mathit{S}}_i$ if $c_i=0$. Using these bounds, state boundary constraints are defined as in Equation (40).

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\mathit{J}}_{b,c}\left(k\right){\mathit{c}}_b+{\mathit{J}}_{b,v}\left(k\right){\mathit{v}}_b\\ -{\mathit{J}}_{b,c}\left(k\right){\mathit{c}}_b-{\mathit{J}}_{b,v}\left(k\right){\mathit{v}}_b\end{array}\right]& \le {\mathit{S}}_p\hfill \\ \hfill \left[\begin{array}{c}{\mathit{J}}_{b,v}\left(k\right)\\ -{\mathit{J}}_{b,v}\left(k\right)\end{array}\right]{\mathit{v}}_b& \le {\mathit{S}}_p-\left[\begin{array}{c}{\mathit{J}}_{b,c}\left(k\right)\\ -{\mathit{J}}_{b,c}\left(k\right)\end{array}\right]{\mathit{c}}_b\hfill \end{array}$$

(40)

5.2.3. Swing Phase: Cost Function

Swing phase optimization aims to pull the swinging leg to the CoM in the beginning of the swing motion and then pushes it toward the final position. Along with these, it also tries to approach the straight line $\mathit{L}$. Therefore, the cost function of the swing phase optimization is written as in Equation (41).

$$\begin{array}{cc}\hfill J_{swing}& ={\left({\mathit{n}}_0-\left[\begin{array}{cc}{}_d{}^0\mathit{J}_{b,c}^{ }& {}_d{}^0\mathit{J}_{b,v}^{ }\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\right)}^T{\mathit{Q}}_0\left({\mathit{n}}_0-\left[\begin{array}{cc}{}_d{}^0\mathit{J}_{b,c}^{ }& {}_d{}^0\mathit{J}_{b,v}^{ }\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\right)+\hfill \\ \hfill & {\left({\mathit{n}}_f-\left[\begin{array}{cc}{}_d{}^f\mathit{J}_{b,c}^{ }& {}_d{}^f\mathit{J}_{b,v}^{ }\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\right)}^T{\mathit{Q}}_f\left({\mathit{n}}_f-\left[\begin{array}{cc}{}_d{}^f\mathit{J}_{b,c}^{ }& {}_d{}^f\mathit{J}_{b,v}^{ }\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\right)+\hfill \\ \hfill & {\left(\mathit{L}-\left[\begin{array}{cc}{\mathit{J}}_{b,c}& {\mathit{J}}_{b,v}\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\right)}^T{\mathit{Q}}_t\left(\mathit{L}-\left[\begin{array}{cc}{\mathit{J}}_{b,c}& {\mathit{J}}_{b,v}\end{array}\right]\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{v}}_b\end{array}\right]\right)\hfill \end{array}$$

(41)

5.3. Cooperative Force Optimization

Approximate trajectories are obtained in contact and swing phase optimizations, and these trajectories are used in the cooperative force optimization as the initial trajectory. In order to follow the method easily, agent dynamics are rewritten in Equation (43), where ${\mathit{M}}_h$ and ${\mathit{C}}_h$ are partitioned as given in Equation (42). In addition, ${\mathit{M}}_i$, ${\mathit{C}}_i$, and ${\mathit{G}}_i$ for $i=2,3$ are assigned to ${\mathit{M}}_h$, ${\mathit{C}}_h$ and ${\mathit{G}}_h$, where $h=S$ represents swing, $h=C$ represents contact, and $h=B$ represents floating base matrices. Minimal representation of the dynamics are provided in Equation (44). Floating base dynamics do not switch; however, Agent 2 and Agent 3 dynamics are assigned to $h=C$ or $h=S$ depending on $c_i$ for $i=2,3$. Similarly, wrenches ${\mathit{W}}_{A,m}$ are assigned to ${\mathit{W}}_{A,h}$ based on $c_i$ such that if $c_2=1$, then ${\mathit{W}}_{A,C}={\mathit{W}}_{A,2}$ and if $c_3=1$, then ${\mathit{W}}_{A,C}={\mathit{W}}_{A,3}$ for $i=2,3$. Finally, ${\tilde{\mathit{r}}}_C$ is the tip point position ${\mathit{r}}_{c,i}$ of the leg with $c_i=1$. ${\mathit{F}}_C$ is the interaction between the contact leg and the ground.

$$\begin{array}{cc}\hfill {\mathit{M}}_h& =\left[\begin{array}{cc}{}_{bb}{}^{ }\mathit{M}_h^{ }& {}_{bq}{}^{ }\mathit{M}_h^{ }\\ {}_{qb}{}^{ }\mathit{M}_h^{ }& {}_{qq}{}^{ }\mathit{M}_h^{ }\end{array}\right]\hfill \\ \hfill {\mathit{C}}_h& =\left[\begin{array}{cc}{}_{bb}{}^{ }\mathit{C}_h^{ }& {}_{bq}{}^{ }\mathit{C}_h^{ }\\ {}_{qb}{}^{ }\mathit{C}_h^{ }& {}_{qq}{}^{ }\mathit{C}_h^{ }\end{array}\right]\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill {}_{bb}{}^{ }\mathit{M}_B^{ }{\dot{\mathit{x}}}_B+{}_{bb}{}^{ }\mathit{C}_B^{ }{\mathit{x}}_B+{}_{bb}{}^{ }\mathit{G}_B^{ }& ={\overline{\mathit{W}}}_{A,C}+{\overline{\mathit{W}}}_{A,S}\hfill \\ \hfill \left[\begin{array}{cc}{}_{bb}{}^{ }\mathit{M}_S^{ }& {}_{bq}{}^{ }\mathit{M}_S^{ }\end{array}\right]{\dot{\mathit{x}}}_S+\left[\begin{array}{cc}{}_{bb}{}^{ }\mathit{C}_S^{ }& {}_{bq}{}^{ }\mathit{C}_S^{ }\end{array}\right]{\mathit{x}}_S+{}_{bb}{}^{ }\mathit{G}_S^{ }& ={\mathit{W}}_{A,S}\hfill \\ \hfill \left[\begin{array}{cc}{}_{bb}{}^{ }\mathit{M}_C^{ }& {}_{bq}{}^{ }\mathit{M}_C^{ }\end{array}\right]{\dot{\mathit{x}}}_C+\left[\begin{array}{cc}{}_{bb}{}^{ }\mathit{C}_C^{ }& {}_{bq}{}^{ }\mathit{C}_C^{ }\end{array}\right]{\mathit{x}}_S+{}_{bb}{}^{ }\mathit{G}_C^{ }& ={\mathit{W}}_{A,C}+\left[\begin{array}{c}\mathit{I}\\ {\tilde{\mathit{r}}}_C\end{array}\right]{\mathit{F}}_C\hfill \end{array}$$

(43)

$$\begin{array}{cc}\hfill {\widehat{\mathit{M}}}_B{\dot{\mathit{x}}}_B+{\widehat{\mathit{C}}}_B{\mathit{x}}_B+{\widehat{\mathit{G}}}_B& ={\overline{\mathit{W}}}_{A,C}+{\overline{\mathit{W}}}_{A,S}\hfill \\ \hfill {\widehat{\mathit{M}}}_S{\dot{\mathit{x}}}_S+{\widehat{\mathit{C}}}_S{\mathit{x}}_S+{\widehat{\mathit{G}}}_S& ={\mathit{W}}_{A,S}\hfill \\ \hfill {\widehat{\mathit{M}}}_C{\dot{\mathit{x}}}_C+{\widehat{\mathit{C}}}_C{\mathit{x}}_S+{\widehat{\mathit{G}}}_C& ={\mathit{W}}_{A,C}+\left[\begin{array}{c}\mathit{I}\\ {\tilde{\mathit{r}}}_C\end{array}\right]{\mathit{F}}_C\hfill \end{array}$$

(44)

Then, continuous models in Equation (43) are converted into discrete models using Euler discretization, and Equation (45) provides the discrete system model that is used in cooperative force optimization. The current states are denoted as ${\mathit{x}}_{h,k}$, where h and k represent the model identifier and the prediction step, respectively.

$$\begin{array}{c}\hfill \left[\begin{array}{cc}\Delta t{\widehat{\mathit{C}}}_{B,k}-{\widehat{\mathit{M}}}_{B,k}& {\widehat{\mathit{M}}}_{B,k}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{B,k}\\ {\mathit{x}}_{B,k+1}\end{array}\right]-\Delta t{\overline{\mathit{W}}}_{S,k}-\Delta t{\overline{\mathit{W}}}_{C,k}=\Delta t{\widehat{\mathit{G}}}_B\\ \hfill \left[\begin{array}{cc}\Delta t{\widehat{\mathit{C}}}_{S,k}-{\widehat{\mathit{M}}}_{S,k}& {\widehat{\mathit{M}}}_{S,k}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{S,k}\\ {\mathit{x}}_{S,k+1}\end{array}\right]-\Delta t{\mathit{W}}_{S,k}=\Delta t{\widehat{\mathit{G}}}_{S,k}\\ \hfill \left[\begin{array}{cc}\Delta t{\widehat{\mathit{C}}}_{C,k}-{\widehat{\mathit{M}}}_{C,k}& {\widehat{\mathit{M}}}_{C,k}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{C,k}\\ {\mathit{x}}_{C,k+1}\end{array}\right]-\Delta t{\mathit{W}}_{C,k}-\Delta t{\widehat{\mathit{J}}}_{C,k}{\mathit{F}}_{C,k}=\Delta t{\widehat{\mathit{G}}}_{C,k}\end{array}$$

(45)

For brevity, Equations (45) are represented with a minimal representation as follows.

$$\begin{array}{cc}\hfill {\tilde{\mathit{M}}}_{h,k}& =\left[\begin{array}{cc}{\tilde{\mathit{M}}}_{h_1,k}& {\tilde{\mathit{M}}}_{h_2,k}\end{array}\right]=\left[\begin{array}{cc}\Delta t{\widehat{\mathit{C}}}_{B,k}-{\widehat{\mathit{M}}}_{B,k}& {\widehat{\mathit{M}}}_{B,k}\end{array}\right]\hfill \\ \hfill {\tilde{\mathit{J}}}_{C,k}& =-\Delta t{\widehat{\mathit{J}}}_{C,k}\hfill \\ \hfill {\tilde{\mathit{G}}}_{h,k}& =\Delta t{\widehat{\mathit{G}}}_{h,k}\hfill \\ \hfill {\tilde{\mathit{P}}}_{h,k}& =-\Delta t\widehat{\mathit{I}}\hfill \end{array}$$

(46)

5.3.1. Cooperative Force Optimization Problem

This method relies on initially provided trajectories that is provided by contact and swing phase. In this phase, decision variables are defined as corrections to the nominal trajectories, and a complete trajectory is defined as such. Nominal trajectories for states are denoted with ${}_0{}^{ }\mathit{x}_{h,k}^{ }$, and corrections to the nominal trajectories at every instant are denoted as $\Delta {\mathit{x}}_{h,k}$. Similarly, force trajectories are defined in the same fashion such that ${}_0{}^{ }\mathit{F}_{C,k}^{ }$ and ${}_0{}^{ }\mathit{W}_{A,m}^{ }$ are the nominal force trajectories, while $\Delta {\mathit{F}}_{C,k}$ and $\Delta {\mathit{W}}_{A,m}$ are the corrections to the relevant trajectories.

$$\begin{array}{c}\hfill {\mathit{x}}_{h,k}^T={}_0{}^{ }\mathit{x}_{h,k}^{ }+\Delta {\mathit{x}}_{h,k}\\ \hfill {\mathit{F}}_{C,k}^T={}_0{}^{ }\mathit{F}_{C,k}^{ }+\Delta {\mathit{F}}_{C,k}\\ \hfill {\mathit{W}}_{h,k}^T={}_0{}^{ }\mathit{W}_{h,k}^{ }+\Delta {\mathit{W}}_{h,k}\end{array}$$

(47)

States, contact forces, and wrenches for the entire trajectory are combined as follows.

$$\begin{array}{cc}\hfill {\mathit{x}}_{h,K}^T& ={\left[\begin{array}{ccc}{\mathit{x}}_{h,1}^T& \cdots & {\mathit{x}}_{h,N_n}^T\end{array}\right]}^T\hfill \\ \hfill {\mathit{F}}_{C,K}^T& ={\left[\begin{array}{ccc}{\mathit{F}}_{C,1}^T& \cdots & {\mathit{F}}_{C,N_n}^T\end{array}\right]}^T\hfill \\ \hfill {\mathit{W}}_{h,K}^T& ={\left[\begin{array}{ccc}{\mathit{W}}_{h,1}^T& \cdots & {\mathit{W}}_{h,N_n}^T\end{array}\right]}^T\hfill \end{array}$$

(48)

The relationship between ${\mathit{x}}_{h,K}$, ${\mathit{F}}_{C,K}$, and ${\mathit{W}}_{h,K}$ can be written for the entire trajectory using combined matrices as provided in Equation (49).

$$.\begin{array}{cc}\hfill {\tilde{\mathit{M}}}_{h,K}& =\left[\begin{array}{cccc}{\tilde{\mathit{M}}}_{h_1,1}& {\tilde{\mathit{M}}}_{h_2,2}& \mathbf{0}& \cdots \\ \mathbf{0}& {\tilde{\mathit{M}}}_{h_1,2}& {\tilde{\mathit{M}}}_{h_2,3}& \\ ⋮& & \ddots & \ddots \end{array}\right]\hfill \\ \hfill {\tilde{\mathit{J}}}_{C,K}& =\left[\begin{array}{ccc}-\Delta t{\widehat{\mathit{J}}}_{C,1}& \mathbf{0}& \cdots \\ \mathbf{0}& -\Delta t{\widehat{\mathit{J}}}_{C,2}& \\ ⋮& & \ddots \end{array}\right]\hfill \\ \hfill {\tilde{\mathit{G}}}_{h,K}& =\left[\begin{array}{c}{\tilde{\mathit{G}}}_{h,1}\\ {\tilde{\mathit{G}}}_{h,2}\\ ⋮\end{array}\right]\hfill \\ \hfill {\tilde{\mathit{P}}}_{h,K}& =\left[\begin{array}{ccc}{\tilde{\mathit{P}}}_{h,1}& \mathbf{0}& \cdots \\ \mathbf{0}& {\tilde{\mathit{P}}}_{h,2}& \\ ⋮& & \ddots \end{array}\right]\hfill \end{array}$$

(49)

The dynamics for the floating base, contacting, and swinging bodies are written as provided in Equation (50).

$$\begin{array}{c}\hfill \left[\begin{array}{ccc}{\tilde{\mathit{M}}}_{B,K}& {\tilde{\mathit{P}}}_{S,K}& {\tilde{\mathit{P}}}_{C,K}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{B,K}\\ {\overline{\mathit{W}}}_{S,K}\\ {\overline{\mathit{W}}}_{C,K}\end{array}\right]={\mathit{G}}_{B,K}\\ \hfill \left[\begin{array}{cc}{\tilde{\mathit{M}}}_{S,K}& {\tilde{\mathit{P}}}_{S,K}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{S,K}\\ {\mathit{W}}_{S,K}\end{array}\right]={\mathit{G}}_{S,K}\\ \hfill \left[\begin{array}{ccc}{\tilde{\mathit{M}}}_{C,K}& {\tilde{\mathit{P}}}_{C,K}& {\tilde{\mathit{J}}}_{C,K}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{C,K}\\ {\mathit{W}}_{C,K}\\ {\mathit{F}}_{C,K}\end{array}\right]={\mathit{G}}_{C,K}\end{array}$$

(50)

The optimization for the three agents is represented in a single objective Equation (51) and a set of constraints in this work; however, the problem is readily available for distributed optimization.

$$\begin{array}{ccccc}\hfill & \underset{\Delta {\mathit{x}}_{h,k},\Delta {\mathit{F}}_{C,k},\Delta {\mathit{W}}_{h,k}}{\mathrm{minimize}}\hfill & \hfill & \sum _{k=1}^M{\mathcal{L}}_c({\mathit{x}}_{h,k},{\mathit{F}}_{C,k},{\mathit{W}}_{h,k})\hfill & \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill & \hfill & {\mathcal{C}}_{dyn}(\Delta {\mathit{x}}_{h,k},\Delta {\mathit{F}}_{C,k},\Delta {\mathit{W}}_{h,k})\hfill & \hfill =\mathbf{0}\\ \hfill & \hfill & & {\mathcal{C}}_{coop}(\Delta {\mathit{x}}_{h,k},\Delta {\mathit{F}}_{C,k},\Delta {\mathit{W}}_{h,k})\hfill & \hfill =\mathbf{0}\\ \hfill & \hfill & & {\mathcal{C}}_{cntct}(\Delta {\mathit{x}}_{h,k})\hfill & \hfill =\mathbf{0}\\ \hfill & \hfill & & {\mathcal{C}}_{fc}(\Delta {\mathit{F}}_{C,k})\hfill & \hfill \le \mathbf{0}\end{array}$$

(51)

Based on the dynamics given in Equation (49) and the representation of the trajectories provided in Equation (47), the matrices for the equality constraints are denoted as ${\mathit{A}}_{h,dyn}$ and ${\mathit{B}}_{h,dyn}$. The equality constraint is provided for only the floating base for brevity.   

$$\begin{array}{c}\hfill \left[\begin{array}{ccc}{\tilde{\mathit{M}}}_{B,K}& {\tilde{\mathit{P}}}_{S,K}& {\tilde{\mathit{P}}}_{C,K}\end{array}\right]\left[\begin{array}{c}\Delta {\mathit{x}}_{B,K}\\ \Delta {\overline{\mathit{W}}}_{S,K}\\ \Delta {\overline{\mathit{W}}}_{C,K}\end{array}\right]={\mathit{G}}_{B,K}-\left[\begin{array}{ccc}{\tilde{\mathit{M}}}_{B,K}& {\tilde{\mathit{P}}}_{S,K}& {\tilde{\mathit{P}}}_{C,K}\end{array}\right]\left[\begin{array}{c}{}_0{}^{ }\mathit{x}_{B,K}^{ }\\ {}_0{}^{ }\overline{\mathit{W}}_{S,K}^{ }\\ {}_0{}^{ }\overline{\mathit{W}}_{C,K}^{ }\end{array}\right]\end{array}$$

(52)

5.3.2. Cooperative Force Optimization: Contact Constraint

The contact constraint was provided previously in Equation (29). This condition is modified for the definition of the trajectory provided in Equation (47). Put simply, the contact point velocity has to be equal to the body velocity in the opposite direction in the no-slip condition, and Equation (53) projects the relationship on the decision variables for the quadratic optimization.

$$\begin{array}{cc}\hfill \mathbf{0}& =\left[\begin{array}{cc}-\mathit{I}& {\mathit{J}}_{a,i}\end{array}\right]{\mathit{x}}_{B,K}\hfill \\ \hfill \mathbf{0}& =\left[\begin{array}{cc}-\mathit{I}& {\mathit{J}}_{a,i}\end{array}\right]\Delta {\mathit{x}}_{B,K}+\left[\begin{array}{cc}-\mathit{I}& {\mathit{J}}_{a,i}\end{array}\right]{}_0{}^{ }\mathit{x}_{B,K}^{ }\hfill \\ \hfill -\left[\begin{array}{cc}-\mathit{I}& {\mathit{J}}_{a,i}\end{array}\right]\Delta {\mathit{x}}_{B,K}& =\left[\begin{array}{cc}-\mathit{I}& {\mathit{J}}_{a,i}\end{array}\right]{}_0{}^{ }\mathit{x}_{B,K}^{ }\hfill \end{array}$$

(53)

5.3.3. Cooperative Force Optimization: Force Cone Constraint

The coooperative force optimization phase is designed as a quadratic problem; therefore, constraints have to be set accordingly. Contact constraints are dedicated to keep tangential forces small so that no slipping occurs. To do so, a friction pyramid is created inside a friction cone. The friction cone is a geometric interpretation of the magnitude of the allowable tangential force that can be applied on the ground. The allowable limit is calculated by simply multiplying the normal component of the applied force by the contact point with the friction coefficient. The nrmal component of the force is denoted as ${}_B{}^{ }f_{c,n}^{ }$ and the tangential components of the applied force are denoted as ${}_B{}^{ }f_{c,t}^{ }$, ${}_B{}^{ }f_{c,s}^{ }$, respectively. It should be noted that the applied force is defined with respect to ${\mathcal{F}}_B$. The friction coefficient is denoted as $\mu$. The radius of the friction cone is defined as ${\mathit{r}}_{fc}=\mu {}_B{}^{ }f_{c,n}^{ }$. The friction pyramid is defined such that it is always upper-bounded by the ${\mathit{r}}_{fc}$, and this is achieved by setting linear bounds that are denoted as $r_{fc,s}$ and $r_{fc,t}$. These bounds are calculated such that $|r_{fc,s}|\approx 0.707{\mathit{r}}_{fc}$ and $|r_{fc,s}|\approx 0.707{\mathit{r}}_{fc}$. It should be noted that the bounding ${\mathit{r}}_{fc}$ creates a non-linear relationship and makes optimization a non-linear problem; however, bounds that are defined for tangential components can be implemented in a linear fashion. The geometric interpretation of the friction cone (red solid line) and pyramid (blue solid line) is provided in Figure 9.

Based on the linear and conservative bounds, the following linear constraints are defined for the tangential forces.

$$\begin{array}{c}\hfill -0.707\mu {}_B{}^{ }f_{c,n}^{ }\le {}_B{}^{ }f_{c,t}^{ }\le 0.707\mu {}_B{}^{ }f_{c,n}^{ }\\ \hfill -0.707\mu {}_B{}^{ }f_{c,n}^{ }\le {}_B{}^{ }f_{c,s}^{ }\le 0.707\mu {}_B{}^{ }f_{c,n}^{ }\\ \hfill -2\times 0.707\mu {}_B{}^{ }f_{c,n}^{ }\le {}_B{}^{ }f_{c,t}^{ }+{}_B{}^{ }f_{c,s}^{ }\le 2\times 0.707\mu {}_B{}^{ }f_{c,n}^{ }\end{array}$$

(54)

The relationship given in Equation (54) is written compactly as provided in Equation (55), where ${}_B{}^{ }\mathit{F}_{C,k}^{ }$ is the vector containing the decision variables. ${}_B{}^{ }\mathit{n}_{ }^{ }$, ${}_B{}^{ }\mathit{t}_{ }^{ }$ and ${}_B{}^{ }\mathit{s}_{ }^{ }$ are the unit vectors attached on the contact points and defined in ${\mathcal{F}}_B$. As a practical note, calculating these unit vectors with respect to ${\mathcal{F}}_B$ is a straightforward calculation when there are passive joints at the ankle of the contact legs. Depending on the kinematic structure of the leg, certain unit vectors can be assumed to be in the same direction with the axes of the body frame.

$$\begin{array}{c}\hfill \left(-2\times 0.707\mu {}_B{}^{ }\mathit{n}_{ }^T-{}_B{}^{ }\mathit{t}_{ }^T-{}_B{}^{ }\mathit{s}_{ }^T\right){}_B{}^{ }\mathit{F}_{C,k}^{ }\le \mathbf{0}\\ \hfill \left(2\times 0.707\mu {}_B{}^{ }\mathit{n}_{ }^T+{}_B{}^{ }\mathit{t}_{ }^T+{}_B{}^{ }\mathit{s}_{ }^T\right){}_B{}^{ }\mathit{F}_{C,k}^{ }\le \mathbf{0}\end{array}$$

(55)

Within the current work, ${}_B{}^{ }\mathit{n}_{ }^{ }$, ${}_B{}^{ }\mathit{t}_{ }^{ }$ and ${}_B{}^{ }\mathit{s}_{ }^{ }$ are assumed to be constant and defined as ${}_B{}^{ }\mathit{n}_{ }^T={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$, ${}_B{}^{ }\mathit{t}_{ }^{ }={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$ and ${}_B{}^{ }\mathit{s}_{ }^{ }={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$.

5.3.4. Cooperative Force Optimization: Cost Function

Force phase optimization tries to keep the commanded motion intact while minimizing the disturbance injected on the system due to the joint accelerations. The joint accelerations affect each agent due to the ${\mathcal{C}}_{coop}$, which is provided in Equation (26). The constraints that are introduced in this chapter previously maintained contact, and force cone constraints are satisfied. The cost function for this optimization is defined in Equation (56).

$$\begin{array}{cc}\hfill & {\mathcal{L}}_c({\mathit{x}}_{h,k},{\mathit{F}}_{C,k},{\mathit{W}}_{h,k})=\hfill \\ & {\left(-{\mathit{u}}_{B,xz}+\left[\begin{array}{ccccc}1& 0& 0& \mathbf{0}& \mathbf{0}\\ 0& 0& 1& \mathbf{0}& \mathbf{0}\end{array}\right]{\mathit{x}}_{B,k}\right)}^T{\mathit{Q}}_T\left(-{\mathit{u}}_{B,xz}+\left[\begin{array}{ccccc}1& 0& 0& \mathbf{0}& \mathbf{0}\\ 0& 0& 1& \mathbf{0}& \mathbf{0}\end{array}\right]{\mathit{x}}_{B,k}\right)+\hfill \\ & {\mathit{x}}_{B,k}^T{\mathit{I}I}_Q^T{\mathit{Q}}_W{\mathit{I}I}_Q{\mathit{x}}_{B,k}\hfill \end{array}$$

(56)

where ${\mathit{I}I}_Q$ is a matrix that selects the states that are the joint velocities of contact and swing legs and approximate the acceleration of these selected states. The joint velocities within states ${\mathit{x}}_{h,k}$ are denoted as ${\mathit{q}}_{h,k}$ for $h=C,S$, and the selection of these states is defined in Equation (57).

$$\begin{array}{cc}\hfill {\dot{\mathit{q}}}_{h,k}=& \left[\begin{array}{ccccc}\mathbf{0}& \mathbf{0}& 1& 0& 0\\ \mathbf{0}& \mathbf{0}& 0& 1& 0\\ \mathbf{0}& \mathbf{0}& 0& 0& 1\end{array}\right]{\mathit{x}}_{h,k}\hfill \\ \hfill {\dot{\mathit{q}}}_{h,k}=& {\mathit{I}I}_q{\mathit{x}}_{h,k}\hfill \end{array}$$

(57)

The approximation for acceleration is given below, where states are assumed to propagate with the first-order Euler method.

$${\ddot{\mathit{q}}}_{h,k}={\mathit{I}I}_q\frac{\left({\mathit{x}}_{h,k+1}-{\mathit{x}}_{h,k}\right)}{\Delta t}$$

(58)

Finally, accelerations ${\ddot{\mathit{q}}}_{h,k}$ for $h=C,S$ for $k=1,\cdots ,N_n$ is given as provided in Equation (59)   

$$\begin{array}{cc}\hfill {\ddot{\mathit{q}}}_{h,k}=& \left[\begin{array}{cccc}-\Delta t{\mathit{I}I}_q& \Delta t{\mathit{I}I}_q& & \\ & -\Delta t{\mathit{I}I}_q& \Delta t{\mathit{I}I}_q& \\ & & & \ddots \end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{h,1}\\ {\mathit{x}}_{h,2}\\ ⋮\\ {\mathit{x}}_{h,N_n}\end{array}\right]\hfill \\ \hfill {\ddot{\mathit{q}}}_{h,k}=& {\mathit{I}I}_Q{\mathit{x}}_{h,K}\hfill \end{array}$$

(59)

6. Preliminary Results for the ASLB Platform

This section presents the simulation of the algorithm for the proposed method. The simulation is not executed in a physics environment, and trajectories illustrated in this section are estimated trajectories only. Solutions are obtained on Intel(R) Core i7-4720HQ CPU @2.60 GHz 16GB RAM PC with MATLAB 2019b software.

The sampling time $\Delta t$ for the discrete model is selected to be $\Delta t=0.05$s. Nominal values for $N_n$ and $N_p$ are selected as $N_n=20$ and $N_p=4$, respectively. ${\mathit{Q}}_s$ and ${\mathit{Q}}_p$ for contact phase optimization is selected to be ${\mathit{Q}}_s=5$ and ${\mathit{Q}}_p=2$. ${\mathit{Q}}_0$, ${\mathit{Q}}_f$, and ${\mathit{Q}}_t$ for swing phase optimization is selected to be ${\mathit{Q}}_0=12.5$, ${\mathit{Q}}_f=12.5$, and ${\mathit{Q}}_t=30$. Finally, ${\mathit{Q}}_u$ and ${\mathit{Q}}_w$ for cooperative force optimization are selected to be ${\mathit{Q}}_u=100$ and ${\mathit{Q}}_w=5$.

Based on these settings, and from the initial conditions of ${\mathit{u}}_B(t=0)={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$, ${\mathit{r}}_2(t=0)={\left[\begin{array}{ccc}0.01& -0.344& 0.063\end{array}\right]}^T$, and ${\mathit{r}}_3(t=0)={\left[\begin{array}{ccc}0.045& -0.344& -0.239\end{array}\right]}^T$, ASLB is asked to move forward by ${\mathit{u}}_{B,ref}={\left[\begin{array}{ccc}0.1& 0& 0\end{array}\right]}^T$. The results are provided in the following figures.

Figure 10 illustrates the calculated swing and contact trajectories. $N_n$ for the first optimization is minimized to $N_n=6$ from a nominal 20 as the contact initial position for ${}_{ }{}^{p=1}\mathit{r}_{c,1}^{ }$ is already provided by the sensor information. For a feasible finite horizon, $N_n=6$, and contact leg, which is the right leg or Agent 2, is used to calculate an approximate trajectory for ${\mathit{r}}_c$ starting from ${\mathit{r}}_2(t=0)$ and diverge from the CoM toward the (+) x and (+) z directions. Note that this trajectory is calculated with respect to ${\mathcal{F}}_B$. The swing phase optimization connects ${\mathit{r}}_3(t=0)$ to the ${}_{ }{}^{p=2}\mathit{r}_{c,1}^{ }$ without violating the kinematic bounds.

Figure 11 illustrates the nominal force trajectory for the contact leg as it interacts with the ground. ${}_0{}^{ }\mathit{F}_C^{ }$ is the initial trajectory that is calculated by substituting ${\mathit{q}}_{a,i}$ and ${\mathit{x}}_i$ into the agent dynamics. ${}_{opt}{}^{ }\mathit{F}_C^{ }$ is the resultant contact force trajectory. It is visible from the figure that there is not a significant change in the y-axis. However, ${}_{opt}{}^{ }\mathit{F}_C^{ }$ is shifted in the (+) x direction by approximately 0.4 N.

Figure 12 shows the contact and swing trajectories for the second step, where $N_n$ is minimized to $N_n=16$. Similar to the first step, ${}_{ }{}^{p=1}\mathit{r}_{c,1}^{ }$ for contact phase optimization is the initial position of the current contact leg, which is Agent 3. ${\mathit{r}}_c$ in Figure 12 converges to the CoM in the beginning of this phase and then pushes away toward the (-) x and (-) z directions.

A smaller correction occurs in the contact force as ${}_0{}^{ }\mathit{F}_C^{ }$ and ${}_{opt}{}^{ }\mathit{F}_C^{ }$ have slight differences in all three directions as seen in Figure 13.

Figure 14 provides the ${\mathit{u}}_B$ trajectory throughout the walking simulation along with the ${\mathit{r}}_{c,i}$ for $i=2,3$. This figure illustrates the characteristic difference in the method, which calculates the contact positions and forces them to track a reference velocity ${\mathit{u}}_{B,ref}$. The black circle in Figure 14 indicates the CoM and decision variables are defined with respect to that. The trajectory of ${\mathit{u}}_B$, which is given with the blue line, settles in a cyclic pattern. The mean of the magnitude in x direction is approximately 0.28 m/s, while it is almost zero for z direction.

Figure 15 provides a more intuitive illustration of the motion of the robot as ${}_I{}^{ }\mathit{r}_B^{ }$ is calculated from ${\mathit{u}}_B$. ${}_I{}^{ }\mathit{r}_B^{ }$ gives the position of the origin of the ${\mathcal{F}}_B$ with respect to the ${\mathcal{F}}_I$. The trajectory of ${}_I{}^{ }\mathit{r}_B^{ }$ reveals that body drifts away from the line $z=0$ while achieving forward motion, as desired. The drift is due to the first contact leg, which is the Agent 2, and cannot be corrected.

With the proposed method, a velocity command could be tracked with some offset. During the simulation of the algorithm, it was observed that relaxation of the state bounds decreases the offsets between velocity commands and actual velocity. In addition, since this method is designed for velocity level dynamics, a drift in the walking does occur, which can be mitigated by an appropriate control scheme. That said, extracting the dynamics from the kinematics and developing a controller for this simpler set equations allows faster calculation of the future steps.

7. Cooperative UAV-UGV Docking with Task Prioritization

Motivated by the results from the preceding section, we extend the framework to study systems that are not on the same platform for example a cooperative UAV-UGV system similar to that in [39], with the objective to have the UAV (quadcopter) dock on the moving UGV (rover). Note that this framework further illustrates that it can accommodate very different dynamical systems, unlike the ASLB system, where the left and right leg dynamics were identical.

Table 2. Summary of key variables used for description of the quadcopter and rover kinematics and dynamics.

${\mathcal{F}}_B$	body fixed reference frame (quadcopter)
$m_q$	mass of the quadcopter
${\mathbf{J}}_q$	moment of inertia of the quadcopter
${\mathit{x}}_q$	state vector of quadcopter
${}_I{}^{ }\mathit{C}_q^{ }\left({\mathit{\theta }}_q\right)={}_I{}^{ }\mathit{C}_q^{ }$	rotation matrix for quadcopter
$f_q$	total thrust generated by motors in the body frame
${\mathit{t}}_q$	moments generated on the body
${\mathit{u}}_q$	input vector for the quadcopter model
g	acceleration due to gravity
${\left[{\mathit{u}}_1{\mathit{u}}_2{\mathit{u}}_3\right]}^T$	unit vectors representing the body frame
$m_r$	mass of the rover
${\mathit{J}}_r$	moment of inertia of the rover
${\mathit{x}}_r$	state vector for rover dynamics
${}_I{}^{ }\mathit{C}_r^{ }$	rotation matrix of the rover
${\mathit{u}}_r$	inputs to the rover

provides a summary of the key variables and terms used for the development of the ASLB dynamical model.

7.1. Quadcopter Dynamics

The six-degree-of-freedom (DoF) rigid body dynamics with mass ($m_q$) and moment of inertia (${\mathit{J}}_q$) of the quadcopter are represented in (60). The model’s states are denoted as ${\mathit{x}}_q={\left[\begin{array}{cccc}{\mathit{p}}_q^T& {\mathit{v}}_q^T& {\mathit{\theta }}_q^T& {\mathit{w}}_q^T\end{array}\right]}^T\in {\mathbb{R}}^{12}$, where ${\mathit{p}}_q={}_I{}^{ }\mathit{p}_q^{ }\in {\mathbb{R}}^3$, ${\mathit{v}}_q={}_I{}^{ }\mathit{v}_q^{ }\in {\mathbb{R}}^3$, ${\mathit{\theta }}_q\in {\mathbb{R}}^3$ and ${\mathit{w}}_q={}_B{}^{ }\mathit{w}_q^{ }\in {\mathbb{R}}^3$ are inertial position, inertial velocity, Euler angles, and angular velocity, respectively. Given the Euler angles, the rotation matrix for the quadcopter is denoted as ${}_I{}^{ }\mathit{C}_q^{ }\left({\mathit{\theta }}_q\right)={}_I{}^{ }\mathit{C}_q^{ }$. The inputs of the system are denoted as ${\mathit{u}}_q={\left[\begin{array}{cc}{f}_q& {\mathit{t}}_q^T\end{array}\right]}^T\in {\mathbb{R}}^4$, where $f_q$ is the total thrust generated by the motors on ${\mathcal{F}}_B$ and ${\mathit{t}}_q\in {\mathbb{R}}^3$ is the column matrix of moments generated on the body defined in ${\mathcal{F}}_B$. ${\mathit{E}}_q\left({\mathit{\theta }}_q\right)$ is the mapping between ${\mathit{w}}_q$ and ${\dot{\mathit{\theta }}}_q$ such that ${\mathit{w}}_q={\mathit{E}}_q\left({\mathit{\theta }}_q\right){\dot{\mathit{\theta }}}_q$ for pre-defined rotation sequence. The quadcopter properties are taken from the work [45].

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_q& =\left[\begin{array}{c}{\dot{\mathit{p}}}_q\\ {\dot{\mathit{v}}}_q\\ {\dot{\mathit{e}}}_q\\ {\dot{\mathit{w}}}_q\end{array}\right]=\mathit{f}({\mathit{x}}_q,{\mathit{u}}_q)\hfill \\ \hfill & =\left[\begin{array}{c}{\mathit{v}}_q\\ g{\mathit{u}}_3-(1/m_q){}_I{}^{ }\mathit{C}_q^{ }{f}_q{\mathit{u}}_3\\ {\mathit{E}}^{-1}\left({\mathit{\theta }}_q\right){\mathit{w}}_q\\ {\mathit{J}}_q^{-1}(-{\mathit{w}}_q\times {\mathit{J}}_q{\mathit{w}}_q+{\mathit{t}}_q)\end{array}\right]\hfill \end{array}$$

(60)

7.2. Rover Dynamics

The dynamics of the rover are calculated with mass $m_r$ and moment of inertia ${\mathit{J}}_r$, assuming that it runs on a flat surface and provided in (61). Based on this assumption, model states are denoted as ${\mathit{x}}_r={\left[p_{r,x}{p}_{r,y}{\mathbf{\Psi }}_r{v}_{r,x}{w}_{r,z}\right]}^T$, where $p_{r,x}$ and $p_{r,y}$ represent inertial position on the $x-y$ plane, ${\mathbf{\Psi }}_r$ is the Euler angle about the z axis, ${}_B{}^{ }v_{r,x}^{ }=v_{r,x}$ is the horizontal velocity of the rover, and ${}_B{}^{ }w_{r,z}^{ }=w_{r,z}$ is the angular velocity of the rover. The composite rotation matrix for the rover is denoted as ${}_I{}^{ }\mathit{C}_r^{ }={}_I{}^{ }\mathit{C}_r^{ }\left({\mathbf{\Psi }}_r\right)$. Inputs to the system are denoted as ${\mathit{u}}_r={\left[f_{r,1}{f}_{r,2}\right]}^T$, which represent the traction forces applied on the surface by a set of wheels on the right- and left-hand sides of the rover, respectively. $m_r$ is taken as 1 kg, and ${\mathit{J}}_r=diag\left\{0.1\right\}$ kg·m².

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_r& =\left[\begin{array}{c}{\dot{p}}_{r,x}\\ {\dot{p}}_{r,y}\\ {\dot{\mathbf{\Psi }}}_r\\ {\dot{v}}_{r,x}\\ {\dot{w}}_{r,z}\end{array}\right]=\mathit{f}({\mathit{x}}_r,{\mathit{u}}_r)\hfill \\ \hfill & =\left[\begin{array}{c}{\mathit{u}}_1^T{}_I{}^{ }\mathit{C}_r^{ }{v}_{r,x}{\mathit{u}}_1\\ {\mathit{u}}_2^T{}_I{}^{ }\mathit{C}_r^{ }{v}_{r,x}{\mathit{u}}_1\\ w_{r,z}\\ (1/m_r)(f_{r,1}+f_{r,2})\\ {\mathit{J}}^{-1}({\mathit{r}}_1\times f_{r,1}+{\mathit{r}}_2\times f_{r,2})\end{array}\right]\hfill \end{array}$$

(61)

8. Cooperative Model-Predictive Control Methodology

This section introduces a unified MPC strategy to maintain a docking approach for the long range and a finer docking maneuver in the short range by accommodating a non-linear and a linear MPC designed with edge weight information and task prioritization. This section is divided into four subsections, where non-linear MPC (NMPC), linear MPC (LMPC), cooperative task prioritization, and implementation of the control strategy are described in Section 8.1, Section 8.2, Section 8.3, and Section 8.4, respectively.

The introduced method can be applied on all of the agents as the formulation only considers local neighbor information; therefore, formulations will be provided for the agent denoted as i, which is defined by the states ${\mathit{x}}_i\left(t\right)\in {\mathbb{R}}^{n_x}$, inputs ${\mathit{u}}_i\left(t\right)\in {\mathbb{R}}^{n_u}$, and outputs ${\mathit{y}}_i\left(t\right)\in {\mathbb{R}}^{n_y}$. Given the states and inputs, the non-linear dynamics of agent i are given in the following form:

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_i\left(t\right)& ={\mathit{f}}_i({\mathit{x}}_i\left(t\right),{\mathit{u}}_i\left(t\right))\hfill \\ \hfill {\mathit{y}}_i& ={\mathit{h}}_i({\mathit{x}}_i\left(t\right),{\mathit{u}}_i\left(t\right))\hfill \end{array}$$

(62)

Equation (63) provides the discrete linear representation of the agents’ dynamics. This is obtained by linearizing the current state and input, which is denoted as $({\mathit{x}}_{i,c},{\mathit{u}}_{i,c})$ and then by converting the continuous time system in a discrete system using Euler discretization (see [46]).

$$\begin{array}{cc}\hfill \mathbf{\Delta }{\mathit{x}}_{i,k+1}& ={\mathit{A}}_i\mathbf{\Delta }{\mathit{x}}_{i,k}+{\mathit{B}}_i\mathbf{\Delta }{\mathit{u}}_{i,k}\hfill \\ \hfill \mathbf{\Delta }{\mathit{y}}_{i,k}& ={\mathit{C}}_i\mathbf{\Delta }{\mathit{x}}_{i,k}+{\mathit{D}}_i\mathbf{\Delta }{\mathit{u}}_{i,k}\hfill \end{array}$$

(63)

where k is the current sample such that $\mathbf{\Delta }{\mathit{x}}_{i,k}={\mathit{x}}_{i,k}-{\mathit{x}}_{i,c}$, and $\mathbf{\Delta }{\mathit{u}}_{i,k}={\mathit{u}}_{i,k}-{\mathit{u}}_{i,c}$. In this work, we assumed that full state information is shared among the agents. Therefore, matrices ${\mathit{C}}_i$ and ${\mathit{D}}_i$ are assumed to be $\mathit{I}$ and $\mathbf{0}$, respectively, with compatible sizes.

8.1. Non-Linear MPC Formulation

Equation (64) represents the non-linear problem that runs in NMPC where ${\mathcal{L}}_i({\mathit{x}}_{i,k},{\mathit{u}}_{i,k},{\mathit{x}}_{i,ref})$ is the objective function, and ${\mathcal{C}}_{eq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})$ and ${\mathcal{C}}_{ineq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})$ are equality and inequality constraints, respectively. M in (64) denotes the prediction horizon for the NMPC.

$$\begin{array}{ccccc}\hfill & \underset{{\mathit{x}}_{i,k},{\mathit{u}}_{i,k}}{\mathrm{minimize}}\hfill & \hfill & \sum _{k=1}^M{\mathcal{L}}_i({\mathit{x}}_{i,k},{\mathit{u}}_{i,k},{\mathit{x}}_{i,ref})\hfill & \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill & \hfill & {\dot{\mathit{x}}}_i\left(t\right)-{\mathit{f}}_i({\mathit{x}}_i\left(t\right),{\mathit{u}}_i\left(t\right))=\mathbf{0}\hfill \\ \hfill & \hfill & & {\mathcal{C}}_{eq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})=\mathbf{0}\hfill \\ \hfill & \hfill & & {\mathcal{C}}_{ineq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})\le \mathbf{0}\hfill \end{array}$$

(64)

The objective function implemented in this work has the following quadratic form.

$$\begin{array}{cc}\hfill & {\mathcal{L}}_i({\mathit{x}}_{i,k},{\mathit{u}}_{i,k},{\mathit{x}}_{i,ref})=\hfill \\ \hfill & \sum _{k=1}^M{\mathcal{L}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,k})+{\mathit{u}}_{i,k}^T\mathit{R}{\mathit{u}}_{i,k}+{\mathit{x}}_{i,k}^T\mathit{Q}{\mathit{x}}_{i,k}\hfill \end{array}$$

(65)

where $\mathit{Q}\in {\mathbb{R}}^{n_x\times n_x}$ and $\mathit{R}\in {\mathbb{R}}^{n_u\times n_u}$ are square matrices. This objective function drives the system to achieve the cooperative task defined by ${\mathcal{L}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,k})$, which brings a nuanced cooperativeness with task prioritization to the tracking and is explained in Section 8.3.

The equality and inequality constraints serve the following purposes. ${\mathcal{C}}_{eq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})$ is introduced to ensure the initial conditions, which are ${\mathit{x}}_{i,c}={\mathit{x}}_i(t=0)$ and ${\mathit{u}}_{i,c}={\mathit{u}}_i(t=0)$, and ${\mathcal{C}}_{ineq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})$ is introduced to enforce states and inputs to stay in the predefined ranges. These constraints are provided in [45].

8.2. Linear MPC Formulation

The LMPC method implemented in this work is the implicit LMPC applied in [26,45], and details can be found therein. Therefore, the method is summarized here for completeness. The quadratic problem for the LMPC is provided in (66).

$$\begin{array}{ccccc}\hfill & \underset{{\mathit{u}}_{i,K}}{\mathrm{minimize}}\hfill & \hfill & {\overline{\mathcal{L}}}_i({\mathit{x}}_{i,K},\mathbf{\Delta }{\mathit{u}}_{i,K},{\mathit{x}}_{i,ref})\hfill & \\ \hfill & \hfill & & {\overline{\mathcal{C}}}_{eq}({\mathit{x}}_{i,K},{\mathit{u}}_{i,K})=\mathbf{0}\hfill \\ \hfill & \hfill & & {\overline{\mathcal{C}}}_{ineq}({\mathit{x}}_{i,K},{\mathit{u}}_{i,K})\le \mathbf{0}\hfill \end{array}$$

(66)

The quadratic objective function is denoted as ${\overline{\mathcal{L}}}_i({\mathit{x}}_{i,K},\mathbf{\Delta }{\mathit{u}}_{i,K},{\mathit{x}}_{i,ref})$ and given in (67), where the decision variable is $\mathbf{\Delta }{\mathit{u}}_{i,K}$ and ${\mathit{x}}_{i,K}$ is a function of $\mathbf{\Delta }{\mathit{u}}_{i,K}$.

$$\begin{array}{cc}\hfill & {\overline{\mathcal{L}}}_i({\mathit{x}}_{i,K},\mathbf{\Delta }{\mathit{u}}_{i,K},{\mathit{x}}_{i,ref})=\hfill \\ \hfill & {\overline{\mathcal{L}}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,K})+{\mathit{u}}_{i,K}^T\overline{\mathit{R}}{\mathit{u}}_{i,K}+{\mathit{x}}_{i,K}^T\overline{\mathit{Q}}{\mathit{x}}_{i,K}\hfill \end{array}$$

(67)

where $\overline{\mathit{R}}=diag{\left\{\mathit{R}\right\}}_1^M$, $\overline{\mathit{Q}}=diag{\left\{\mathit{Q}\right\}}_1^M$ and ${\overline{\mathcal{L}}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,K})$ is calculated in Section 8.3.

Due to the implicit formulation, state predictions are described with respect to the decision variables and combined in a lumped representation. Similarly, decision variables, which are the inputs, are also collected under a lumped term. These lumped states and inputs are denoted as $\mathbf{\Delta }{\mathit{x}}_{i,K}$ and $\mathbf{\Delta }{\mathit{u}}_{i,K}$ and provided in (68).

$$\begin{array}{cc}\hfill \mathbf{\Delta }{\mathit{x}}_{i,K}& =\left[\begin{array}{c}\mathbf{\Delta }{\mathit{x}}_{i,k}\\ ⋮\\ \mathbf{\Delta }{\mathit{x}}_{i,k+M-1}\end{array}\right]\hfill \\ \hfill \mathbf{\Delta }{\mathit{u}}_{i,K}& =\left[\begin{array}{c}\mathbf{\Delta }{\mathit{u}}_{i,k}\\ ⋮\\ \mathbf{\Delta }{\mathit{u}}_{i,k+M-1}\end{array}\right]\hfill \end{array}$$

(68)

The relationship between $\mathbf{\Delta }{\mathit{x}}_{i,K}$ and $\mathbf{\Delta }{\mathit{u}}_{i,K}$ is provided below with matrices ${\mathit{F}}_i\in {\mathbb{R}}^{M{n}_x\times n_x}$ and ${\mathit{H}}_i\in {\mathbb{R}}^{M{n}_x\times M{n}_u}$ for the prediction horizon of M.

$$\begin{array}{cccc}\hfill \mathbf{\Delta }{\mathit{x}}_{i,K}& ={\mathit{F}}_i{\mathit{x}}_{i,c}+{\mathit{H}}_i\mathbf{\Delta }{\mathit{u}}_{i,K}\hfill & \hfill & \\ \hfill {\mathit{F}}_i& =col{\left\{{\mathit{F}}_{i,m}\right\}}_{m=1}^M\hfill & \hfill & \\ \hfill {\mathit{F}}_{i,m}& ={\mathit{A}}_i^{(m-1)}\hfill & \hfill ,m& =\left[1,\cdots ,M\right]\hfill \\ \hfill {\mathit{H}}_i& =col{\left\{{\mathit{H}}_{i,m}\right\}}_{m=1}^M\hfill & \hfill & \\ \hfill {\mathit{H}}_{i,m}& =\mathbf{0}\hfill & \hfill m& =1\hfill \\ \hfill {\mathit{H}}_{i,m}& =row\left\{{\mathit{h}}_{i,l}\right\}\hfill & \hfill m& =\left[2,\cdots ,M\right]\hfill \\ \hfill {\mathit{h}}_{i,l}& ={\mathit{A}}_i^{(m-l-1)}{\mathit{B}}_i\hfill & \hfill ,l& =\left[1,\cdots ,m-1\right]\hfill \end{array}$$

(69)

Terminal states are given as a function of ${\mathit{x}}_{i,k}$ and $\mathbf{\Delta }{\mathit{u}}_{i,K}$ below.

$$\begin{array}{cc}\hfill \mathbf{\Delta }{\mathit{x}}_{i,k+M}& ={\mathit{A}}_i^M{\mathit{x}}_{i,k}+{\overline{\mathit{B}}}_i\mathbf{\Delta }{\mathit{u}}_{i,K}\hfill \\ \hfill {\overline{\mathit{B}}}_i& =\left[\begin{array}{cccc}{\mathit{A}}_i^{M-1}{\mathit{B}}_i& \cdots & {\mathit{A}}_i^1{\mathit{B}}_i& {\mathit{B}}_i\end{array}\right]\hfill \end{array}$$

(70)

${\overline{\mathcal{C}}}_{eq}({\mathit{x}}_{i,K},{\mathit{u}}_{i,K})$ and ${\overline{\mathcal{C}}}_{ineq}({\mathit{x}}_{i,K},{\mathit{u}}_{i,K})$ serve the same purpose with ${\mathcal{C}}_{eq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})$ and ${\mathcal{C}}_{ineq}({\mathit{x}}_{i,k},{\mathit{u}}_{i,k})$; however, they are modified to comply with LMPC notation. The calculations of these constraints are provided in [26].

8.3. Cooperative Task Prioritization

Let ${\mathit{ϵ}}_{i,t}\in {\mathbb{R}}^{n_{ϵt}}$, as provided in (71), represent a cooperative task for agent i based on local neighbor state information that is received from neighboring agents, denoted as j. Assume that ${\mathit{ϵ}}_{i,t}\in {\mathbb{R}}^{n_{ϵt}}$ is defined for a subset of states denoted as ${\widehat{\mathit{x}}}_i\subset {\mathit{x}}_i$ and ${\widehat{\mathit{x}}}_j\subset {\mathit{x}}_j$, where ${\widehat{\mathit{x}}}_i\in {\mathbb{R}}^{n_s}$ and ${\widehat{\mathit{x}}}_j\in {\mathbb{R}}^{n_s}$. It should be noted that ${\widehat{\mathit{x}}}_i$ and ${\widehat{\mathit{x}}}_j$ are assumed to be measured with respect to the same coordinate frame. Otherwise, the task ${\mathit{ϵ}}_{i,t}$ becomes non-linear.

$${\mathit{ϵ}}_{i,t}=a_{ij}({\widehat{\mathit{x}}}_j-{\widehat{\mathit{x}}}_i),t=1,\cdots ,T$$

(71)

where T is the maximum number of tasks. The first derivative of ${\mathit{ϵ}}_{i,t}$ is calculated as follows:

$$\begin{array}{cc}\hfill {\dot{\mathit{ϵ}}}_{i,t}& =a_{ij}({\dot{\widehat{\mathit{x}}}}_j-{\dot{\widehat{\mathit{x}}}}_i)\hfill \\ \hfill {\dot{\mathit{ϵ}}}_{i,t}& =\left[\begin{array}{cc}{a}_{ij}{\mathit{I}}_{n_s}& -a_{ij}{\mathit{I}}_{n_s}\end{array}\right]\left[\begin{array}{c}{\dot{\widehat{\mathit{x}}}}_j\\ {\dot{\widehat{\mathit{x}}}}_i\end{array}\right]\hfill \end{array}$$

(72)

Collecting the terms on the right-hand side ${\mathit{M}}_i\left(a_{ij}\right)=\left[\begin{array}{cc}{a}_{ij}{\mathit{I}}_{n_s}& -a_{ij}{\mathit{I}}_{n_s}\end{array}\right]$ and ${\dot{\mathit{S}}}_{i,t}={\left[\begin{array}{cc}{\dot{\widehat{\mathit{x}}}}_j^T& {\dot{\widehat{\mathit{x}}}}_i^T\end{array}\right]}^T\in {\mathbb{R}}^{n_{st}}$, the mapping in (72) between task space (TS) velocities and local state space (LSS) velocities takes the form

$${\dot{\mathit{ϵ}}}_{i,t}={\mathit{M}}_i\left(a_{ij}\right){\dot{\mathit{S}}}_{i,t}$$

(73)

Following that, the inverse mapping from TS to LSS is calculated as

$${\dot{\mathit{S}}}_{i,t}={\mathit{M}}_i^{+}{\dot{\mathit{ϵ}}}_{i,t}$$

(74)

where ${\mathit{M}}_i^{+}$ is the pseudo inverse of ${\mathit{M}}_{i,t}$ and $a_{ij}$ is dropped from the expression for brevity. Note that ${\mathit{x}}_j$ is the state of a neighboring agent.

As illustrated in [47] for robots, excessive (redundant) LSS can be utilized to manage secondary tasks. Let ${\mathit{ϵ}}_{i,1}$ and ${\mathit{S}}_{i,1}$ denote the first TS and LSS and ${\mathit{ϵ}}_{i,t}$, and let ${\mathit{S}}_{i,t},t=2,\cdots ,T$ denotes the remaining ones; then, following task management methodology can be used [40]:

$$\begin{array}{cc}\hfill {\dot{\mathit{S}}}_{i,1}& ={\mathit{M}}_{i,1}^{+}{\dot{\mathit{ϵ}}}_1\hfill \\ \hfill {\dot{\mathit{S}}}_{i,t}& ={\dot{\mathit{S}}}_{i,t-1}+{\left({\mathit{M}}_{i,t}{\mathbf{\Phi }}_{i,t-1}\right)}^{+}({\dot{\mathit{ϵ}}}_{i,t}-{\mathit{M}}_{i,t}{\dot{\mathit{S}}}_{i,t-1})\hfill \end{array}$$

(75)

where ${\mathbf{\Phi }}_{i,t}$ is the null space projection matrix and calculated is as follows:

$$\begin{array}{c}\hfill {\mathbf{\Phi }}_{i,1}={\mathit{I}}_{n_{st}}-{\mathit{M}}_{i,1}^{+}{\mathit{M}}_{i,1}\\ \hfill {\mathbf{\Phi }}_{i,t}={\mathbf{\Phi }}_{i,t-1}-{\left({\mathit{M}}_{i,t}{\mathbf{\Phi }}_{i,t-1}\right)}^{+}\left({\mathit{M}}_{i,t}{\mathbf{\Phi }}_{i,t-1}\right)\end{array}$$

(76)

The formulation defined in (75) is collected in a minimal representation as in (77).

$$\dot{\mathit{S}}=\mathbf{\Psi }\dot{\mathit{ϵ}}$$

(77)

where ${\dot{\mathit{S}}}_i={\left[\begin{array}{cccc}{\dot{\mathit{S}}}_{i,1}^T& {\dot{\mathit{S}}}_{i,t}^T& \cdots & {\dot{\mathit{S}}}_{i,T}^T\end{array}\right]}^T$ and ${\dot{\mathit{ϵ}}}_i={\left[\begin{array}{cccc}{\dot{\mathit{ϵ}}}_{i,1}^T& {\dot{\mathit{ϵ}}}_{i,t}^T& \cdots & {\dot{\mathit{ϵ}}}_{i,T}^T\end{array}\right]}^T$. Thus, a quadratic function can be written as in (78) based on (77).

$$\begin{array}{cc}\hfill L\left({\dot{\mathit{ϵ}}}_i\right):={\dot{\mathit{S}}}^T\dot{\mathit{S}}& ={\dot{\mathit{ϵ}}}_i^T{\mathbf{\Psi }}^T\mathbf{\Psi }{\dot{\mathit{ϵ}}}_i\hfill \end{array}$$

(78)

Finally, based on (78), ${\mathcal{L}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,k})$ and ${\overline{\mathcal{L}}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,K})$ are calculated as given in (79) and (83), respectively, where ${\mathit{Q}}_s^T={\mathit{Q}}_s\ge \mathbf{0}\in {\mathbb{R}}^{n_{ϵt}\times n_{ϵt}}$.

$$\begin{array}{cc}\hfill {\mathcal{L}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,k})& ={\dot{\mathit{ϵ}}}_i^T{\mathbf{\Psi }}^T\left[\begin{array}{cc}{\mathit{Q}}_s& \mathbf{0}\\ \mathbf{0}& {\mathit{Q}}_s\end{array}\right]\mathbf{\Psi }{\dot{\mathit{ϵ}}}_i\hfill \\ \hfill & ={\dot{\mathit{ϵ}}}_i^T{\mathit{Q}}_{ϵ}{\dot{\mathit{ϵ}}}_i\hfill \end{array}$$

(79)

Remark 1.

If states of the agents i and j are linearly related, then ${\widehat{\mathit{x}}}_i$ and ${\widehat{\mathit{x}}}_j$ are linearly related; therefore ${\mathit{M}}_{i,1}$ is constant. As a result,

• $L\left({\dot{\mathit{ϵ}}}_i\right)$ outputs a quadratic function of ${\dot{\mathit{ϵ}}}_{i,t}$ with scalars ${\gamma }_t$ such that $L\left({\dot{\mathit{ϵ}}}_i\right)={\sum }_{t=1}^T{\gamma }_t{\dot{\mathit{ϵ}}}_{i,t}^2$, where ${\gamma }_1\ge \cdots \ge {\gamma }_t\ge \cdots \ge {\gamma }_T$.
• The relationship given for derivatives of state space and task space in (78) is also valid for the state space and task space as follows:

  $$\begin{array}{cc}\hfill L\left({\mathit{ϵ}}_i\right):={\mathit{S}}^T\mathit{S}& ={\mathit{ϵ}}_i^T{\mathbf{\Psi }}^T\mathbf{\Psi }{\mathit{ϵ}}_i\hfill \end{array}$$

  (80)

  which results in

  $$\begin{array}{cc}\hfill {\mathcal{L}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,k})& ={\mathit{ϵ}}_i^T{\mathbf{\Psi }}^T\left[\begin{array}{cc}{\mathit{Q}}_s& \mathbf{0}\\ \mathbf{0}& {\mathit{Q}}_s\end{array}\right]\mathbf{\Psi }{\mathit{ϵ}}_i\hfill \\ \hfill & ={\mathit{ϵ}}_i^T{\mathit{Q}}_{ϵ}{\mathit{ϵ}}_i\hfill \end{array}$$

  (81)

Let ${\mathit{Q}}_{ϵ}$ be partitioned as in (82), and let ${\mathit{X}}_{i,ref}:=\overline{\mathbf{1}}\otimes {\mathit{x}}_{i,ref}$, where $\overline{\mathbf{1}}={\left[1\cdots 1\right]}^T\in {\mathbb{R}}^M$. Then, the overall task matrix ${\overline{\mathit{ϵ}}}_i$ can be written as an affine function of ${\mathit{X}}_{i,ref}$, and ${\mathit{x}}_{i,K})$ and ${\overline{\mathcal{L}}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,K})$ is calculated as provided in (83).

$${\mathit{Q}}_{ϵ}=\left[\begin{array}{cc}{\mathit{Q}}_{ϵ,11}& {\mathit{Q}}_{ϵ,12}\\ {\mathit{Q}}_{ϵ,21}& {\mathit{Q}}_{ϵ,22}\end{array}\right]$$

(82)

$${\overline{\mathcal{L}}}_{i,tp}({\mathit{x}}_{i,ref},{\mathit{x}}_{i,K})={\overline{\mathit{ϵ}}}_i^T{\overline{\mathit{Q}}}_{ϵ}{\overline{\mathit{ϵ}}}_i$$

(83)

where ${\overline{\mathit{Q}}}_{ϵ,★\ast }=diag{\left\{{\mathit{Q}}_{ϵ,★\ast }\right\}}_1^M$ and $★,\ast =\{1,2\}$.

$${\overline{\mathit{Q}}}_{ϵ}=\left[\begin{array}{cc}{\overline{\mathit{Q}}}_{ϵ,11}& {\overline{\mathit{Q}}}_{ϵ,12}\\ {\overline{\mathit{Q}}}_{ϵ,21}& {\overline{\mathit{Q}}}_{ϵ,22}\end{array}\right]$$

(84)

8.4. Implementation of the MPCs

The docking controller is designed to have several layers to decide how to perform the docking maneuver based on the distance between the agent i and the neighboring agents. The layers, as mentioned earlier, are the selector NMPC and LMPC. Essentially, the selector works as a governor and decides the reference trajectory that the LMPC tracks. If the absolute distance between agent i and neighboring agents is greater than a threshold, the NMPC generates a trajectory toward the neighboring agents using neighboring state information. Note that NMPC does not aim to achieve the terminal state constraint, but it repeatedly minimizes the state error within the given prediction horizon. Then, the generated trajectory, which creeps toward the neighbors, is passed to LMPC. If the distance is smaller than the threshold, LMPC receives the neighboring state information to generate control inputs without needing NMPC. The distance and threshold are denoted as $\mathit{e}=\parallel \left[{\mathit{p}}_j\right]-\left[{\mathit{p}}_i\right]{\parallel }_2^2\in \mathbb{R}$ and $e_d\in \mathbb{R}$. The currently available state and input information are denoted as ${\mathit{x}}_i(t-1)$, ${\mathit{x}}_j(t-1)$ and ${\mathit{u}}_i(t-1)$, where t represents current sample and $(t-1)$ represents previous sample. The pipeline for the control strategy described above is given in Figure 16.

When triggered, NMPC generates a rough trajectory toward agent j for a given prediction horizon M based on the formulation given in (64). Then, a portion of this trajectory that is determined by the control horizon $M_u$ is extracted, and this is denoted as ${\mathit{x}}_{nmpc}$. The optimization parameters for NMPC such as M, $t_f$, and $M_u$ are selected empirically to achieve a rough trajectory and allow a time interval to queue a new trajectory to guide the agent i to the vicinity of other agents. The selection of the optimization parameters with the right-hand-side sparsity template described in [46] provides a longer time to queue a new trajectory. Before the LMPC receives the output of the NMPC or the neighboring state information, i.e., ${\mathit{x}}_{nmpc}$ ir ${\mathit{x}}_j(t-1)$, respectively, a line or a set of line segments are populated based on the prediction horizon of the LMPC. Finally, the tracking based on the optimization provided in (66) is performed by the LMPC.

9. Cooperative MPC Simulation and Results

This section presents the simulation results for the proposed strategy based on the simulation setup of a quadcopter and a rover, as illustrated in Figure 17.

The NMPC and LMPC parameters are provided in Figure 18, and the quadcopter properties are given in [45] in Table 1. Two scenarios are illustrated—(1) proximity docking on a moving agent and (2) large range docking on a moving agent—where LMPC and NMPC-LMPC strategies are tested, respectively. Let ${\mathcal{P}}_s=\{1,2,3,4\}$ represent the priority of states, where 1 is the highest priority and 4 is the lowest priority, and $s=p,v,e,w$ represents the subset of states that $\mathcal{P}$ is representing. On this basis, LMPC uses the priority map of $\{{\mathcal{P}}_p,{\mathcal{P}}_v,{\mathcal{P}}_e,{\mathcal{P}}_w\}=\{2,3,1,4\}$. Priority mapping is used to arrange tasks defined as ${\mathit{ϵ}}_{i,t}$, where $i\in \{1,2,3,4\}$. This mapping is valid for both LMPC and NMPC.

9.1. Case Study 1: Proximity Docking on the Rover

The initial conditions for the quadcopter and the rover are ${\mathit{x}}_{q,0}=$${\left[00.5-10000000000\right]}^T$ and ${\mathit{x}}_{q,0}={\left[0000.500000000\right]}^T$, respectively.

The initial inputs for the quadcopter are ${\mathit{u}}_{i,c}={\left[9.81m_{q}000\right]}^T$ to temper the behavior of the ${\mathit{f}}_q$. Given the initial values, the quadcopter is guided to the rover with $-0.1$ m offset in the z direction, as illustrated in Figure 19. As shown in Figure 19, the quadcopter makes a free fall in $3/4^{th}$ of a second, and then it recovers. During this phase, it loses altitude and gains speed in the w component up to $6\mathrm{m}/\mathrm{s}$. The duration of the free fall is correlated to $\mathit{R}$ in the cost function and inequality constraints provided to the LMPC, which is ${\overline{\mathcal{C}}}_{ineq}({\mathit{x}}_{i,K},{\mathit{u}}_{i,K})$. After the free fall, after approximately $t=1$ (s), the controller makes a correction in $\mathit{e}$ and starts approaching the rover in the x- and y-directions.

As mentioned above, free fall can also be seen in Figure 20 in the $f_q$ subplot, where $f_q$ stays at 0 N for that period. During the maneuver, $f_q$ is upper-bounded by 12 N. The maximums for ${\mathit{t}}_q$ are reached in the x-direction, where −$0.52$ and $0.05$ Nm of toque are observed at the extremes.

Figure 21 provides an overview of the performed trajectory along with the heading direction of the quadcopter, which is aligned with the ${\mathit{u}}_1$ axis of ${\mathcal{F}}_B$.

9.2. Case Study 2: Long-Range Docking on the Rover

The initial conditions for the quadcopter and the rover are ${\mathit{x}}_{q,0}={\left[00-10000000000\right]}^T$ and ${\mathit{x}}_{r,0}={\left[101000.500000000\right]}^T$, respectively.

The initial inputs for the quadcopter are ${\mathit{u}}_{i,c}={\left[9.81m_{q}000\right]}^T$. This case illustrates the long-range capability of the proposed control strategy. The overview of the scenario is provided as a set snapshot in Figure 17, and the video of the performed scenario can be found in https://www.youtube.com/watch?v=QJ-NuwZQ8zw (accessed on 12 May 2024).

In this scenario, LMPC tracks the ${\mathit{x}}_{nmpc}$ that the NMPC generates until $\mathit{e}$ is less than ${\mathit{e}}_d$. At about 41 s, a transition happens, and LMPC starts tracking the ${\mathit{x}}_j(t-1)$. Figure 22 shows the states of the quadcopter. An important feature of this motion is that the signal oscillates at a velocity level in both the linear and angular motion. The same oscillatory behavior is visible for inputs $f_q$ and ${\mathit{t}}_q$, as provided in Figure 23, which is because the quadcopter’s current states are changing as the ${\mathit{x}}_{nmpc}$ is generated on the NMPC side, sent to the LMPC, and populated as line segments. Figure 22 and Figure 24 reveal the difference in proximity and long-range docking maneuvers, to which LMPC is more sensitive and can be seen from the magnitude of the tracking error $\mathit{e}$.

The positional errors between the rover and the quadcopter in Figure 24 illustrate that the positional errors in the $x-$ and $y-$directions approach zero, while the error in the $z-$direction approach $-0.1$ m.

Figure 25 illustrates the calculation time of each NMPC trigger. Initially, the generation costs approximately 0.1 s, and as the quadcopter settles on the trajectory trajectory generation, the times decrease to 0.05 s. For a given elapsed time profile, the overall calculation time is calculated to be 13.91 s. The upper bound line illustrates the control horizon $M_u$ for the NMPC; see [39]).

10. Discussion

The central premise of this paper is to show that a complex dynamical system and/or a control system can be decomposed into parts. These parts can be constrained to develop the governing equations to study the dynamics or to synthesize a control strategy for the composite system using the the interconnection between the sub-systems as constraints. Note thatwhile these are two separate applications, they can be studied using the same framework. In the biped dynamics, the legs (left and right) and the torso are considered as sub-systems, which are coupled via constraints and thus modeled using the cooperative system framework. The trajectory for the biped robot is computed using a sequence of optimizations. Given a desired amount of forward movement of the center of mass, one would begin from the contact phase optimization, the results of which then feed into the swing phase optimization, hwich provides the necessary smooth trajectories for moving to the new location. This is finally passed on to the cooperative force optimization, which computes the necessary generalized forces to accomplish the movement.

While there are promising early results from this approach, further study is warranted to investigate the sensitivity of the optimization approaches to parameters such as the number of phases, the number of sample instants, the sampling period, the contact force models (friction cone), and the asymmetry in the legs, sensor noise, and other unmodeled terms in the dynamics.

Similarly, in the case of a cooperative docking scenario, the quadcopter and ground rover are two separate sub-systems of the overall system and are constrained to work together in the cooperative setup. In this context, the framework also allows one to choose different control schemes and combine them together for the overall cooperative mission. For example, a nonlinear MPC was chosen to bring the quadcopter closer to the ground rover faster because we wish for a faster approach to the goal; however, when one is close to the goal, a linear MPC controller is activated since when it is closer to the desired state, the linear terms typically dominate higher-order polynomial terms and/or harmonic functions.

In the cooperative task decomposition for the UAV-UGV problem, one would have to further study the impact of communication delays and environmental disturbances such as unstructured terrain models to fully understand the efficacy of the proposed cooperative control formulation.

11. Summary and Conclusions

This paper presents a cooperative modeling framework that is shown to reduce the complexity of deriving the governing dynamical equations of complex systems composed of multiple bodies. The approach also allows for an optimization-based trajectory generation for the complex system.

The results show that such cooperative modeling enables online trajectory generation through a series of optimizations that generate approximate trajectories and pass them to a final optimization that refines the motion with the cooperative system framework.

The paper also proposes a unified MPC control strategy capable of handling long-range to proximity docking maneuvers. A rough but fast NMPC method is run to propagate the agent to a closer vicinity, and then the LMPC handles more sensitive proximity motion. The sensitivity of the LMPC is apparent in Figure 22 and Figure 24, where a smooth approach suddenly becomes relatively violent. Based on the simulations, the selection of the matrices $\mathit{Q}$, $\mathit{R}$ and ${\mathit{Q}}_s$ makes the transition from NMPC-LMPC to LMPC smoother. Note that the docking of the quadcopter is almost tangential to the $x-y$ plane, and during the final section of the maneuver, it penetrates the docking platform. The latter problem can be resolved with an approach constraint to the MPC controllers. Most importantly, the proposed method is readily applicable among dynamic systems, including non-linear systems, since the controller’s structure stays the same. Using local neighbor information instead of external sensors or observers naturally makes this control strategy decentralized.